From 7fb3dce87fbb8ab3775e45a8f4e62f8df354247d Mon Sep 17 00:00:00 2001 From: OceanCing <3419311969@qq.com> Date: Sat, 5 Sep 2026 22:18:15 +0800 Subject: [PATCH] packages for engineer2026 --- auto_exchange_planner/CMakeLists.txt | 79 + ...to_exchange_planner_plugin_description.xml | 7 + .../auto_exchange_context.h | 35 + .../auto_exchange_planner.h | 37 + auto_exchange_planner/package.xml | 22 + .../src/auto_exchange_context.cpp | 72 + .../src/auto_exchange_manager.cpp | 85 + .../src/auto_exchange_planner.cpp | 171 + dist_sensor/CMakeLists.txt | 107 + dist_sensor/config/dist_sensor_config.yaml | 3 + dist_sensor/include/dist_sensor/common/data.h | 119 + .../include/dist_sensor/common/protocol.h | 17 + dist_sensor/include/dist_sensor/dist_sensor.h | 57 + dist_sensor/launch/load.launch | 4 + dist_sensor/package.xml | 16 + dist_sensor/src/dist_sensor.cpp | 61 + dist_sensor/src/main.cpp | 21 + engineer2_arm_config/.setup_assistant | 6 +- .../config/auto_exchange_planning.yaml | 1 + engineer2_arm_config/config/engineer2.srdf | 122 +- .../config/engineer2_moveit_controllers.yaml | 13 + .../config/fake_controllers.yaml | 9 +- .../config/gazebo_engineer2.urdf | 2908 +++-------------- engineer2_arm_config/config/joint_limits.yaml | 36 +- engineer2_arm_config/config/kinematics.yaml | 3 + .../config/ompl_planning.yaml | 36 +- .../config/ros_controllers.yaml | 73 +- engineer2_arm_config/config/sensors_3d.yaml | 1 + engineer2_arm_config/config/servo.yaml | 14 +- .../config/simple_moveit_controllers.yaml | 3 +- .../config/stomp_planning.yaml | 41 +- ...auto_exchange_planning_pipeline.launch.xml | 23 + .../launch/default_warehouse_db.launch | 15 + engineer2_arm_config/launch/demo.launch | 66 + .../launch/demo_gazebo.launch | 21 + .../launch/empty_world.launch | 26 + ...neer2_moveit_controller_manager.launch.xml | 13 +- .../fake_moveit_controller_manager.launch.xml | 2 +- engineer2_arm_config/launch/gazebo.launch | 34 + .../launch/joystick_control.launch | 17 + .../launch/load_controllers.launch | 7 +- .../launch/load_move_group.launch | 2 +- engineer2_arm_config/launch/move_group.launch | 83 +- engineer2_arm_config/launch/moveit.rviz | 303 +- .../ompl_chomp_planning_pipeline.launch.xml | 21 + .../launch/ompl_planning_pipeline.launch.xml | 1 + ...otion_planner_planning_pipeline.launch.xml | 15 + .../launch/planning_pipeline.launch.xml | 2 +- ...ntrol_moveit_controller_manager.launch.xml | 4 + .../launch/ros_controllers.launch | 11 + .../launch/run_benchmark_ompl.launch | 21 + .../launch/trajectory_execution.launch.xml | 15 +- engineer2_arm_config/launch/warehouse.launch | 15 + .../launch/warehouse_settings.launch.xml | 16 + engineer2_arm_config/package.xml | 8 +- engineer_arm_config/.setup_assistant | 6 +- engineer_arm_config/CMakeLists.txt | 2 +- .../config/cartesian_limits.yaml | 5 + .../config/chomp_planning.yaml | 6 +- engineer_arm_config/config/engineer.srdf | 234 +- .../config/engineer_moveit_controllers.yaml | 34 + .../config/fake_controllers.yaml | 62 +- .../config/gazebo_controllers.yaml | 4 + .../config/gazebo_engineer.urdf | 1095 +++++++ engineer_arm_config/config/joint_limits.yaml | 97 +- engineer_arm_config/config/kinematics.yaml | 20 +- engineer_arm_config/config/ompl_planning.yaml | 150 +- .../config/ros_controllers.yaml | 143 +- engineer_arm_config/config/sensors_3d.yaml | 2 +- .../config/simple_moveit_controllers.yaml | 25 + .../config/stomp_planning.yaml | 156 + .../launch/chomp_planning_pipeline.launch.xml | 26 +- .../launch/default_warehouse_db.launch | 15 + engineer_arm_config/launch/demo.launch | 66 + engineer_arm_config/launch/demo_gazebo.launch | 21 + engineer_arm_config/launch/empty_world.launch | 25 + ...ineer_moveit_controller_manager.launch.xml | 13 +- .../fake_moveit_controller_manager.launch.xml | 5 +- engineer_arm_config/launch/gazebo.launch | 34 + .../launch/joystick_control.launch | 17 + .../launch/load_controllers.launch | 34 +- .../launch/load_move_group.launch | 2 +- engineer_arm_config/launch/move_group.launch | 150 +- engineer_arm_config/launch/moveit.rviz | 451 +-- engineer_arm_config/launch/moveit_rviz.launch | 15 + .../launch/ompl_planning_pipeline.launch.xml | 35 +- .../launch/planning_context.launch | 35 +- .../launch/planning_pipeline.launch.xml | 8 +- ...ntrol_moveit_controller_manager.launch.xml | 4 + .../launch/ros_controllers.launch | 11 + .../launch/run_benchmark_ompl.launch | 21 + .../launch/sensor_manager.launch.xml | 17 + .../launch/setup_assistant.launch | 16 + ...imple_moveit_controller_manager.launch.xml | 8 + .../launch/stomp_planning_pipeline.launch.xml | 23 + .../launch/trajectory_execution.launch.xml | 30 +- engineer_arm_config/launch/warehouse.launch | 15 + .../launch/warehouse_settings.launch.xml | 16 + engineer_arm_config/package.xml | 56 +- engineer_custom_controller/CMakeLists.txt | 118 + .../config/engineer_custom_controller.yaml | 18 + .../engineer_custom_controller/common/data.h | 107 + .../common/protocol.h | 86 + .../custom_controller_data_sender.h | 58 + .../engineer_custom_controller.h | 44 + engineer_custom_controller/launch/load.launch | 4 + engineer_custom_controller/package.xml | 22 + .../src/custom_controller_data_sender.cpp | 94 + .../src/engineer_custom_controller.cpp | 87 + engineer_custom_controller/src/main.cpp | 21 + engineer_middleware/CMakeLists.txt | 3 + engineer_middleware/config/engineer.yaml | 1 + .../engineer2_simulation_controllers.yaml | 154 +- .../config/engineer2_steps_list.yaml | 2342 +++---------- .../engineer_simulation_controllers.yaml | 230 +- .../config/engineer_steps_list.yaml | 2886 +++++++--------- .../engineer_middleware/chassis_interface.h | 12 +- .../include/engineer_middleware/middleware.h | 21 +- .../include/engineer_middleware/motion.h | 488 ++- .../include/engineer_middleware/step.h | 150 +- .../include/engineer_middleware/step_queue.h | 21 +- engineer_middleware/launch/load.launch | 2 +- engineer_middleware/launch/sim.launch | 12 +- engineer_middleware/mesh/units_studio.stl | Bin 0 -> 428884 bytes engineer_middleware/package.xml | 1 + engineer_middleware/src/middleware.cpp | 33 +- engineer_planning_adapters/CMakeLists.txt | 46 + .../special_joint_constraint_adapter.h | 31 + engineer_planning_adapters/package.xml | 23 + .../planning_adapter_plugins.xml | 5 + .../src/special_joint_constraint_adapter.cpp | 86 + engineer_trajectory_teaching/CMakeLists.txt | 121 + .../config/config.yaml | 3 + .../engineer_trajectory_teaching.h | 63 + .../launch/load.launch | 4 + engineer_trajectory_teaching/package.xml | 66 + .../src/engineer_trajectory_teaching.cpp | 285 ++ engineer_trajectory_teaching/src/main.cpp | 21 + .../trajectory/big_change_small.yaml | 57 + .../trajectory/big_island1.yaml | 42 + .../trajectory/big_island2.yaml | 16 + .../trajectory/big_island_left.yaml | 27 + .../trajectory/big_island_left2.yaml | 9 + .../trajectory/big_island_mid.yaml | 20 + .../trajectory/big_island_mid2.yaml | 9 + .../trajectory/big_island_right.yaml | 16 + .../trajectory/big_island_right2.yaml | 9 + .../trajectory/big_island_right_ready.yaml | 6 + .../trajectory/get_second_stone.yaml | 23 + .../trajectory/get_second_stone_side.yaml | 30 + .../trajectory/get_transfer_stored.yaml | 26 + .../trajectory/ground_stone1.yaml | 10 + .../trajectory/ground_stone2.yaml | 13 + .../trajectory/small_island1.yaml | 40 + .../trajectory/small_island2.yaml | 6 + .../trajectory/test.yaml | 10 + .../trajectory/transfer.yaml | 49 + .../trajectory/transfer_store_stone.yaml | 41 + 158 files changed, 8843 insertions(+), 7515 deletions(-) create mode 100644 auto_exchange_planner/CMakeLists.txt create mode 100644 auto_exchange_planner/auto_exchange_planner_plugin_description.xml create mode 100644 auto_exchange_planner/include/auto_exchange_planner/auto_exchange_context.h create mode 100644 auto_exchange_planner/include/auto_exchange_planner/auto_exchange_planner.h create mode 100644 auto_exchange_planner/package.xml create mode 100644 auto_exchange_planner/src/auto_exchange_context.cpp create mode 100644 auto_exchange_planner/src/auto_exchange_manager.cpp create mode 100644 auto_exchange_planner/src/auto_exchange_planner.cpp create mode 100644 dist_sensor/CMakeLists.txt create mode 100644 dist_sensor/config/dist_sensor_config.yaml create mode 100644 dist_sensor/include/dist_sensor/common/data.h create mode 100644 dist_sensor/include/dist_sensor/common/protocol.h create mode 100644 dist_sensor/include/dist_sensor/dist_sensor.h create mode 100644 dist_sensor/launch/load.launch create mode 100644 dist_sensor/package.xml create mode 100644 dist_sensor/src/dist_sensor.cpp create mode 100644 dist_sensor/src/main.cpp create mode 100644 engineer2_arm_config/config/auto_exchange_planning.yaml create mode 100644 engineer2_arm_config/config/engineer2_moveit_controllers.yaml create mode 100644 engineer2_arm_config/launch/auto_exchange_planning_pipeline.launch.xml create mode 100644 engineer2_arm_config/launch/default_warehouse_db.launch create mode 100644 engineer2_arm_config/launch/demo.launch create mode 100644 engineer2_arm_config/launch/demo_gazebo.launch create mode 100644 engineer2_arm_config/launch/empty_world.launch create mode 100644 engineer2_arm_config/launch/gazebo.launch create mode 100644 engineer2_arm_config/launch/joystick_control.launch create mode 100644 engineer2_arm_config/launch/ompl_chomp_planning_pipeline.launch.xml create mode 100644 engineer2_arm_config/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml create mode 100644 engineer2_arm_config/launch/ros_control_moveit_controller_manager.launch.xml create mode 100644 engineer2_arm_config/launch/ros_controllers.launch create mode 100644 engineer2_arm_config/launch/run_benchmark_ompl.launch create mode 100644 engineer2_arm_config/launch/warehouse.launch create mode 100644 engineer2_arm_config/launch/warehouse_settings.launch.xml create mode 100644 engineer_arm_config/config/cartesian_limits.yaml create mode 100644 engineer_arm_config/config/engineer_moveit_controllers.yaml create mode 100644 engineer_arm_config/config/gazebo_controllers.yaml create mode 100644 engineer_arm_config/config/gazebo_engineer.urdf create mode 100644 engineer_arm_config/config/simple_moveit_controllers.yaml create mode 100644 engineer_arm_config/config/stomp_planning.yaml create mode 100644 engineer_arm_config/launch/default_warehouse_db.launch create mode 100644 engineer_arm_config/launch/demo.launch create mode 100644 engineer_arm_config/launch/demo_gazebo.launch create mode 100644 engineer_arm_config/launch/empty_world.launch create mode 100644 engineer_arm_config/launch/gazebo.launch create mode 100644 engineer_arm_config/launch/joystick_control.launch create mode 100644 engineer_arm_config/launch/moveit_rviz.launch create mode 100644 engineer_arm_config/launch/ros_control_moveit_controller_manager.launch.xml create mode 100644 engineer_arm_config/launch/ros_controllers.launch create mode 100644 engineer_arm_config/launch/run_benchmark_ompl.launch create mode 100644 engineer_arm_config/launch/sensor_manager.launch.xml create mode 100644 engineer_arm_config/launch/setup_assistant.launch create mode 100644 engineer_arm_config/launch/simple_moveit_controller_manager.launch.xml create mode 100644 engineer_arm_config/launch/stomp_planning_pipeline.launch.xml create mode 100644 engineer_arm_config/launch/warehouse.launch create mode 100644 engineer_arm_config/launch/warehouse_settings.launch.xml create mode 100644 engineer_custom_controller/CMakeLists.txt create mode 100644 engineer_custom_controller/config/engineer_custom_controller.yaml create mode 100644 engineer_custom_controller/include/engineer_custom_controller/common/data.h create mode 100644 engineer_custom_controller/include/engineer_custom_controller/common/protocol.h create mode 100644 engineer_custom_controller/include/engineer_custom_controller/custom_controller_data_sender.h create mode 100644 engineer_custom_controller/include/engineer_custom_controller/engineer_custom_controller.h create mode 100644 engineer_custom_controller/launch/load.launch create mode 100644 engineer_custom_controller/package.xml create mode 100644 engineer_custom_controller/src/custom_controller_data_sender.cpp create mode 100644 engineer_custom_controller/src/engineer_custom_controller.cpp create mode 100644 engineer_custom_controller/src/main.cpp create mode 100644 engineer_middleware/mesh/units_studio.stl create mode 100644 engineer_planning_adapters/CMakeLists.txt create mode 100644 engineer_planning_adapters/include/engineer_planning_adapters/special_joint_constraint_adapter.h create mode 100644 engineer_planning_adapters/package.xml create mode 100644 engineer_planning_adapters/planning_adapter_plugins.xml create mode 100644 engineer_planning_adapters/src/special_joint_constraint_adapter.cpp create mode 100644 engineer_trajectory_teaching/CMakeLists.txt create mode 100644 engineer_trajectory_teaching/config/config.yaml create mode 100644 engineer_trajectory_teaching/include/engineer_trajectory_teaching/engineer_trajectory_teaching.h create mode 100644 engineer_trajectory_teaching/launch/load.launch create mode 100644 engineer_trajectory_teaching/package.xml create mode 100644 engineer_trajectory_teaching/src/engineer_trajectory_teaching.cpp create mode 100644 engineer_trajectory_teaching/src/main.cpp create mode 100644 engineer_trajectory_teaching/trajectory/big_change_small.yaml create mode 100644 engineer_trajectory_teaching/trajectory/big_island1.yaml create mode 100644 engineer_trajectory_teaching/trajectory/big_island2.yaml create mode 100644 engineer_trajectory_teaching/trajectory/big_island_left.yaml create mode 100644 engineer_trajectory_teaching/trajectory/big_island_left2.yaml create mode 100644 engineer_trajectory_teaching/trajectory/big_island_mid.yaml create mode 100644 engineer_trajectory_teaching/trajectory/big_island_mid2.yaml create mode 100644 engineer_trajectory_teaching/trajectory/big_island_right.yaml create mode 100644 engineer_trajectory_teaching/trajectory/big_island_right2.yaml create mode 100644 engineer_trajectory_teaching/trajectory/big_island_right_ready.yaml create mode 100644 engineer_trajectory_teaching/trajectory/get_second_stone.yaml create mode 100644 engineer_trajectory_teaching/trajectory/get_second_stone_side.yaml create mode 100644 engineer_trajectory_teaching/trajectory/get_transfer_stored.yaml create mode 100644 engineer_trajectory_teaching/trajectory/ground_stone1.yaml create mode 100644 engineer_trajectory_teaching/trajectory/ground_stone2.yaml create mode 100644 engineer_trajectory_teaching/trajectory/small_island1.yaml create mode 100644 engineer_trajectory_teaching/trajectory/small_island2.yaml create mode 100644 engineer_trajectory_teaching/trajectory/test.yaml create mode 100644 engineer_trajectory_teaching/trajectory/transfer.yaml create mode 100644 engineer_trajectory_teaching/trajectory/transfer_store_stone.yaml diff --git a/auto_exchange_planner/CMakeLists.txt b/auto_exchange_planner/CMakeLists.txt new file mode 100644 index 0000000..dee6355 --- /dev/null +++ b/auto_exchange_planner/CMakeLists.txt @@ -0,0 +1,79 @@ +cmake_minimum_required(VERSION 3.10) +project(auto_exchange_planner) + +set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +add_definitions(-Wall -Werror) + +find_package(catkin REQUIRED COMPONENTS + moveit_core + pluginlib + roscpp + std_msgs +) + +find_package(Eigen3 REQUIRED) + +catkin_package( + INCLUDE_DIRS + include + ${EIGEN3_INCLUDE_DIR} + LIBRARIES + CATKIN_DEPENDS + roscpp + std_msgs + DEPENDS +) + +include_directories(include) +include_directories( + SYSTEM + ${catkin_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} +) + +#add_executable(test_auto_exchange src/test_auto_exchange.cpp) +#target_link_libraries(test_auto_exchange +# ${catkin_LIBRARIES} +# ${Boost_LIBRARIES} +#) + +add_library(moveit_auto_exchange_planner_plugin + src/auto_exchange_manager.cpp + src/auto_exchange_planner.cpp + src/auto_exchange_context.cpp) +set_target_properties(moveit_auto_exchange_planner_plugin PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +target_link_libraries(moveit_auto_exchange_planner_plugin ${catkin_LIBRARIES} ${Boost_LIBRARIES}) + +# Mark executables and/or libraries for installation +install( + TARGETS + moveit_auto_exchange_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 + DESTINATION + ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) + +# Mark roslaunch files for installation +install( + DIRECTORY + launch + DESTINATION + ${CATKIN_PACKAGE_SHARE_DESTINATION} +) +#catkin_lint ignore uninstalled_plugin +install(FILES auto_exchange_planner_plugin_description.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + diff --git a/auto_exchange_planner/auto_exchange_planner_plugin_description.xml b/auto_exchange_planner/auto_exchange_planner_plugin_description.xml new file mode 100644 index 0000000..f926cac --- /dev/null +++ b/auto_exchange_planner/auto_exchange_planner_plugin_description.xml @@ -0,0 +1,7 @@ + + + + The motion planner plugin which plans a particular path for RM exchanger. + + + diff --git a/auto_exchange_planner/include/auto_exchange_planner/auto_exchange_context.h b/auto_exchange_planner/include/auto_exchange_planner/auto_exchange_context.h new file mode 100644 index 0000000..9cfd159 --- /dev/null +++ b/auto_exchange_planner/include/auto_exchange_planner/auto_exchange_context.h @@ -0,0 +1,35 @@ +// +// Created by ch on 24-12-6. +// + +#pragma once + +#include +#include "auto_exchange_planner/auto_exchange_planner.h" + +namespace auto_exchange_planner +{ + MOVEIT_CLASS_FORWARD(AutoExchangeContext); + + class AutoExchangeContext : public planning_interface::PlanningContext + { + public: + AutoExchangeContext(const std::string& name, const std::string& ns, const std::string& group, + const moveit::core::RobotModelConstPtr& model); + ~AutoExchangeContext() 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_; + moveit::core::RobotStatePtr robot_state_; + AutoExchangePlannerPtr auto_exchange_planner_; + }; + +} // namespace auto_exchange_planner diff --git a/auto_exchange_planner/include/auto_exchange_planner/auto_exchange_planner.h b/auto_exchange_planner/include/auto_exchange_planner/auto_exchange_planner.h new file mode 100644 index 0000000..347af48 --- /dev/null +++ b/auto_exchange_planner/include/auto_exchange_planner/auto_exchange_planner.h @@ -0,0 +1,37 @@ +// +// Created by ch on 24-12-1. +// + +#pragma once + +#include +#include + +namespace auto_exchange_planner +{ + MOVEIT_CLASS_FORWARD(AutoExchangePlanner); + + class AutoExchangePlanner + { + public: + AutoExchangePlanner(const ros::NodeHandle& nh = ros::NodeHandle("~")); + + bool solve(const planning_scene::PlanningSceneConstPtr& planning_scene, + const planning_interface::MotionPlanRequest& req, moveit_msgs::MotionPlanDetailedResponse& res); + + protected: + ros::NodeHandle nh_; + std::string name_; + int num_steps_; + int dof_; + double trajectory2_length_; + + private: + void interpolate(const std::vector& joint_names, moveit::core::RobotStatePtr& robot_state, + const moveit::core::JointModelGroup* joint_model_group, const std::vector& start_joint_vals, + const std::vector& goal_joint_vals, trajectory_msgs::JointTrajectory& joint_trajectory); + void auto_exchange_interpolate(const std::vector& joint_names, moveit::core::RobotStatePtr& robot_state, + const moveit::core::JointModelGroup* joint_model_group, const geometry_msgs::Pose& start_pose, + const geometry_msgs::Pose& goal_pose, trajectory_msgs::JointTrajectory& joint_trajectory); + }; +} // namespace auto_exchange_planner diff --git a/auto_exchange_planner/package.xml b/auto_exchange_planner/package.xml new file mode 100644 index 0000000..4318caa --- /dev/null +++ b/auto_exchange_planner/package.xml @@ -0,0 +1,22 @@ + + + auto_exchange_planner + 0.0.0 + The auto_exchange_planner package + ch + BSD + catkin + pluginlib + moveit_core + roscpp + std_msgs + pluginlib + moveit_core + roscpp + std_msgs + + + + + + diff --git a/auto_exchange_planner/src/auto_exchange_context.cpp b/auto_exchange_planner/src/auto_exchange_context.cpp new file mode 100644 index 0000000..02b0350 --- /dev/null +++ b/auto_exchange_planner/src/auto_exchange_context.cpp @@ -0,0 +1,72 @@ +// +// Created by ch on 24-12-6. +// +#include +#include +#include +#include + +#include "auto_exchange_planner/auto_exchange_context.h" +#include "auto_exchange_planner/auto_exchange_planner.h" +namespace auto_exchange_planner +{ +AutoExchangeContext::AutoExchangeContext(const std::string& context_name, const std::string& ns, + const std::string& group_name, const moveit::core::RobotModelConstPtr& model) + : planning_interface::PlanningContext(context_name, group_name), robot_model_(model) +{ + auto_exchange_planner_ = AutoExchangePlannerPtr(new AutoExchangePlanner(ros::NodeHandle(ns))); +} + +bool AutoExchangeContext::solve(planning_interface::MotionPlanDetailedResponse& res) +{ + moveit_msgs::MotionPlanDetailedResponse res_msg; + bool auto_exchange_solved = auto_exchange_planner_->solve(planning_scene_, request_, res_msg); + + if (auto_exchange_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_); + moveit::core::robotStateMsgToRobotState(res_msg.trajectory_start, start_state); + + res.trajectory_[0]->setRobotTrajectoryMsg(start_state, res_msg.trajectory[0]); + res.description_.push_back("plan"); + res.processing_time_ = res_msg.processing_time; + res.error_code_ = res_msg.error_code; + + return true; + } + + res.error_code_ = res_msg.error_code; + return false; +}; + +bool AutoExchangeContext::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 AutoExchangeContext::terminate() +{ + return true; +} + +void AutoExchangeContext::clear() +{ + // This planner has no state, so has nothing to clear +} + +} // namespace auto_exchange_planner diff --git a/auto_exchange_planner/src/auto_exchange_manager.cpp b/auto_exchange_planner/src/auto_exchange_manager.cpp new file mode 100644 index 0000000..42e06b8 --- /dev/null +++ b/auto_exchange_planner/src/auto_exchange_manager.cpp @@ -0,0 +1,85 @@ +// +// Created by ch on 24-12-6. +// + +#include +#include +#include +#include +#include +#include "auto_exchange_planner/auto_exchange_context.h" + +namespace auto_exchange_planner +{ +class AutoExchangeManager : public planning_interface::PlannerManager +{ +public: + AutoExchangeManager() : planning_interface::PlannerManager() + { + } + + bool initialize(const moveit::core::RobotModelConstPtr& model, const std::string& ns) override + { + for (const std::string& gpName : model->getJointModelGroupNames()) + { + planning_contexts_[gpName] = + AutoExchangeContextPtr(new AutoExchangeContext("auto_exchange_context", ns, 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 "AutoExchange"; + } + + void getPlanningAlgorithms(std::vector& algs) const override + { + algs.clear(); + algs.push_back("auto_exchange"); + } + + planning_interface::PlanningContextPtr getPlanningContext(const planning_scene::PlanningSceneConstPtr& planning_scene, + const planning_interface::MotionPlanRequest& req, + moveit_msgs::MoveItErrorCodes& error_code) const override + { + 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(); + } + + // retrieve and configure existing context + const AutoExchangeContextPtr& context = planning_contexts_.at(req.group_name); + ROS_INFO_STREAM_NAMED("auto_exchange_manager", "===>>> context is made "); + + context->setPlanningScene(planning_scene); + context->setMotionPlanRequest(req); + + error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS; + return context; + } + +protected: + std::map planning_contexts_; +}; + +} // namespace auto_exchange_planner + +// register the AutoExchangeManager class as a plugin +CLASS_LOADER_REGISTER_CLASS(auto_exchange_planner::AutoExchangeManager, planning_interface::PlannerManager); diff --git a/auto_exchange_planner/src/auto_exchange_planner.cpp b/auto_exchange_planner/src/auto_exchange_planner.cpp new file mode 100644 index 0000000..6af5fde --- /dev/null +++ b/auto_exchange_planner/src/auto_exchange_planner.cpp @@ -0,0 +1,171 @@ +// +// Created by ch on 24-12-1. +// + +#include +#include +#include + +#include + +#include + +#include +#include +#include +#include + +#include "auto_exchange_planner/auto_exchange_planner.h" + +namespace auto_exchange_planner +{ + AutoExchangePlanner::AutoExchangePlanner(const ros::NodeHandle& nh) : nh_(nh),name_("AutoExchangePlanner"), num_steps_(1), dof_(0) + { + } + + bool AutoExchangePlanner::solve(const planning_scene::PlanningSceneConstPtr& planning_scene, + const planning_interface::MotionPlanRequest& req, + moveit_msgs::MotionPlanDetailedResponse& res) + { + // Load the planner-specific parameters + nh_.param("num_steps", num_steps_, 50); + + ros::Time start_time = ros::Time::now(); + moveit::core::RobotModelConstPtr robot_model = planning_scene->getRobotModel(); + moveit::core::RobotStatePtr start_state(new moveit::core::RobotState(robot_model)), middle_state(new moveit::core::RobotState(robot_model)); + *start_state = planning_scene->getCurrentState(); + const moveit::core::JointModelGroup* joint_model_group = start_state->getJointModelGroup(req.group_name); + const moveit::core::JointModelGroup* mid_joint_model_group = middle_state->getJointModelGroup(req.group_name); + std::vector joint_names = joint_model_group->getVariableNames(); + dof_ = joint_names.size(); + std::vector start_joint_values; + start_state->copyJointGroupPositions(joint_model_group, start_joint_values); + trajectory_msgs::JointTrajectory joint_trajectory; + + if (req.goal_constraints.size() > 1) + { + geometry_msgs::Pose goal_pose_final; + goal_pose_final.position = req.goal_constraints[0].position_constraints[0].constraint_region.primitive_poses[0].position; + goal_pose_final.orientation = req.goal_constraints[0].orientation_constraints[0].orientation; + + geometry_msgs::Pose goal_pose_mid; + goal_pose_mid.position = req.goal_constraints[1].position_constraints[0].constraint_region.primitive_poses[0].position; + goal_pose_mid.orientation = req.goal_constraints[1].orientation_constraints[0].orientation; + + std::vector middle_joint_values; + bool found_mid_ik = middle_state->setFromIK(mid_joint_model_group, goal_pose_mid); + if (found_mid_ik) + { + middle_state->copyJointGroupPositions(mid_joint_model_group, middle_joint_values); + } + else + { + ROS_INFO("Did not find middle_state's IK solution for middle state!"); + return false; + } + + interpolate(joint_names, start_state, joint_model_group, start_joint_values, middle_joint_values, joint_trajectory); + auto_exchange_interpolate(joint_names, start_state, joint_model_group, goal_pose_mid, goal_pose_final, joint_trajectory); + + } + else + { + const std::vector& goal_constraints = req.goal_constraints; + const std::vector& goal_joint_constraint = goal_constraints[0].joint_constraints; + + std::vector goal_joint_values; + goal_joint_values.reserve(goal_joint_constraint.size()); + + for (const auto& constraint : goal_joint_constraint) + { + goal_joint_values.push_back(constraint.position); + } + + interpolate(joint_names, start_state, joint_model_group, start_joint_values, goal_joint_values, joint_trajectory); + + } + + res.trajectory.resize(1); + res.trajectory[0].joint_trajectory.joint_names = joint_names; + res.trajectory[0].joint_trajectory.header = req.start_state.joint_state.header; + res.trajectory[0].joint_trajectory = joint_trajectory; + + res.error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS; + res.processing_time.push_back((ros::Time::now() - start_time).toSec()); + + res.group_name = req.group_name; + res.trajectory_start.joint_state.name = joint_names; + res.trajectory_start.joint_state.position = start_joint_values; + + return true; + } + + void AutoExchangePlanner::interpolate(const std::vector& joint_names, moveit::core::RobotStatePtr& rob_state, + const moveit::core::JointModelGroup* joint_model_group, + const std::vector& start_joint_vals, const std::vector& goal_joint_vals, + trajectory_msgs::JointTrajectory& joint_trajectory) + { + std::vector dt_vector, m_start_joint_vals; + for (int joint_index = 0; joint_index < dof_; ++joint_index) + { + double dt = (goal_joint_vals[joint_index] - start_joint_vals[joint_index]) / num_steps_; + dt_vector.push_back(dt); + } + + for (double start_joint_val : start_joint_vals) + m_start_joint_vals.push_back(start_joint_val + 0.001 ); + + for (int step = 0; step <= num_steps_; ++step) + { + std::vector joint_values; + for (int k = 0; k < dof_; ++k) + { + double joint_value = m_start_joint_vals[k] + step * dt_vector[k]; + joint_values.push_back(joint_value); + } + rob_state->setJointGroupPositions(joint_model_group, joint_values); + rob_state->update(); + + joint_trajectory.joint_names = joint_names; + trajectory_msgs::JointTrajectoryPoint trajectory_point; + trajectory_point.positions = joint_values; + joint_trajectory.points.push_back(trajectory_point); + } + } + + void AutoExchangePlanner::auto_exchange_interpolate(const std::vector& joint_names, moveit::core::RobotStatePtr& rob_state, + const moveit::core::JointModelGroup* joint_model_group, const geometry_msgs::Pose& start_pose, + const geometry_msgs::Pose& goal_pose, trajectory_msgs::JointTrajectory& joint_trajectory) + { + double dt_x = (goal_pose.position.x - start_pose.position.x)/num_steps_, + dt_y = (goal_pose.position.y - start_pose.position.y)/num_steps_, + dt_z = (goal_pose.position.z - start_pose.position.z)/num_steps_; + + for (int step = 0; step <= num_steps_; ++step) + { + std::vector joint_values; + geometry_msgs::Pose waypoint_pose; + waypoint_pose.orientation = start_pose.orientation; + waypoint_pose.position.x = start_pose.position.x + dt_x * step; + waypoint_pose.position.y = start_pose.position.y + dt_y * step; + waypoint_pose.position.z = start_pose.position.z + dt_z * step; + // ROS_INFO("waypoint%d x:%f y:%f z:%f",step,waypoint_pose.position.x,waypoint_pose.position.y,waypoint_pose.position.z); + bool found_waypoint_ik = rob_state->setFromIK(joint_model_group, waypoint_pose); + if (found_waypoint_ik) + { + rob_state->copyJointGroupPositions(joint_model_group, joint_values); + rob_state->update(); + + joint_trajectory.joint_names = joint_names; + trajectory_msgs::JointTrajectoryPoint trajectory_point; + trajectory_point.positions = joint_values; + joint_trajectory.points.push_back(trajectory_point); + } + else + { + ROS_INFO("Did not find IK solution in step %d !", step); + } + } + } + +} // namespace auto_exchange_planner \ No newline at end of file diff --git a/dist_sensor/CMakeLists.txt b/dist_sensor/CMakeLists.txt new file mode 100644 index 0000000..56642f2 --- /dev/null +++ b/dist_sensor/CMakeLists.txt @@ -0,0 +1,107 @@ +cmake_minimum_required(VERSION 3.0.2) +project(dist_sensor) + +## Use C++14 +set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +## By adding -Wall and -Werror, the compiler does not ignore warnings anymore, +## enforcing cleaner code. +add_definitions(-Wall -Werror -Wno-address-of-packed-member) + +## Find catkin macros and libraries +find_package(serial REQUIRED) + +find_package(catkin REQUIRED + COMPONENTS + roscpp + rm_msgs + serial + rm_common +) + +## Find system libraries +#find_package(Eigen3 REQUIRED) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( + INCLUDE_DIRS + include + LIBRARIES + CATKIN_DEPENDS + roscpp + rm_msgs + rm_common + DEPENDS +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + +## Declare cpp executables +FILE(GLOB ALL_SOURCES "src/*.cpp" "src/common/*.cpp" ) +add_executable(${PROJECT_NAME} ${ALL_SOURCES}) + +## Add dependencies to exported targets, like ROS msgs or srvs +add_dependencies(${PROJECT_NAME} + ${catkin_EXPORTED_TARGETS} +) + +## Specify libraries to link executable targets against +target_link_libraries(${PROJECT_NAME} + ${catkin_LIBRARIES} +) + +############# +## Install ## +############# + +# 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" +) + +## Mark other files for installation +install( + DIRECTORY launch + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +############# +## Testing ## +############# + +#if (${CATKIN_ENABLE_TESTING}) +# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread") +# ## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test +# test/test_ros_package_template.cpp +# test/AlgorithmTest.cpp) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}_core) +#endif () diff --git a/dist_sensor/config/dist_sensor_config.yaml b/dist_sensor/config/dist_sensor_config.yaml new file mode 100644 index 0000000..3032ce1 --- /dev/null +++ b/dist_sensor/config/dist_sensor_config.yaml @@ -0,0 +1,3 @@ +sensor_interval: 0.44 +dist_lower: 0.1 +dist_upper: 2.5 \ No newline at end of file diff --git a/dist_sensor/include/dist_sensor/common/data.h b/dist_sensor/include/dist_sensor/common/data.h new file mode 100644 index 0000000..f2c7404 --- /dev/null +++ b/dist_sensor/include/dist_sensor/common/data.h @@ -0,0 +1,119 @@ +// +// Created by ch on 25-6-4. +// + +#pragma once + +#include +#include +#include + +namespace dist_sensor +{ +class Base +{ +public: + serial::Serial serial_; + bool sensor_is_online_ = false; + + void initSerial(const std::string &port) + { + serial::Timeout timeout = serial::Timeout::simpleTimeout(50); + serial_.setPort(port); + serial_.setBaudrate(115200); + serial_.setTimeout(timeout); + if (serial_.isOpen()) + return; + try + { + serial_.open(); + } + catch (serial::IOException& e) + { + ROS_ERROR("Cannot open port %s", port.c_str()); + } + } + + // CheckSum + uint8_t getChecksum(unsigned char* pch_message, unsigned int dw_length) + { + uint8_t checksum = 0; + while (dw_length--) + { + checksum += *pch_message++; + } + return checksum; + } + + uint32_t verifyCheckSum(unsigned char* pch_message, unsigned int dw_length) + { + unsigned char expected_checksum; + if ((pch_message == nullptr) || (dw_length <= 2)) + return 0; + expected_checksum = getChecksum(pch_message, dw_length - 1); + return (expected_checksum == pch_message[dw_length - 1]); + } + + void appendCheckSum(unsigned char* pch_message, unsigned int dw_length) + { + unsigned char checksum; + if ((pch_message == nullptr) || (dw_length <= 2)) + return; + checksum = getChecksum(pch_message, dw_length - 1); + pch_message[dw_length - 1] = checksum; + } +}; + +class SensorData +{ +public: + explicit SensorData(ros::NodeHandle& nh, const std::string& sensor_name, const std::string& port) + : dist_lower_(nh.param("dist_lower", 0.2)), + dist_upper_(nh.param("dist_upper", 2.0)) + { + base_.initSerial(port); + publisher_ = nh.advertise(sensor_name, 1); + } + void setDistance(const double distance) + { + if (getAmp() >= 200 && getAmp() <= 60000 && distance >= dist_lower_ && distance <= dist_upper_) + { + dist_sensor_data_.distance = distance; + error_ = false; + } + else + error_ = true; + } + void setAmp(const double amp) + { + dist_sensor_data_.amp = amp; + } + void setTemperature(const double temperature) + { + dist_sensor_data_.temperature = temperature; + } + void publish() + { + publisher_.publish(dist_sensor_data_); + } + double getDistance() + { + return dist_sensor_data_.distance; + } + double getAmp() + { + return dist_sensor_data_.amp; + } + bool getError() + { + return error_; + } + Base base_; + uint8_t unpack_buffer_[256]{}; +private: + double dist_lower_, dist_upper_; + ros::Publisher publisher_; + rm_msgs::EngineerDistSensor dist_sensor_data_; + bool error_ = true; +}; +} // namespace dist_sensor diff --git a/dist_sensor/include/dist_sensor/common/protocol.h b/dist_sensor/include/dist_sensor/common/protocol.h new file mode 100644 index 0000000..75d25be --- /dev/null +++ b/dist_sensor/include/dist_sensor/common/protocol.h @@ -0,0 +1,17 @@ +// +// Created by ch on 25-6-4. +// + +#pragma once +#define __packed __attribute__((packed)) + +#include +namespace dist_sensor +{ +typedef struct +{ + uint8_t dist[2]; + uint8_t amp[2]; + uint8_t temp[2]; +} __packed DistanceSensorData; +} // namespace dist_sensor \ No newline at end of file diff --git a/dist_sensor/include/dist_sensor/dist_sensor.h b/dist_sensor/include/dist_sensor/dist_sensor.h new file mode 100644 index 0000000..ad9ccfd --- /dev/null +++ b/dist_sensor/include/dist_sensor/dist_sensor.h @@ -0,0 +1,57 @@ +// +// Created by ch on 25-6-4. +// + +#pragma once + +#include +#include +#include +#include + +#include "dist_sensor/common/data.h" +#include "dist_sensor/common/protocol.h" + +namespace dist_sensor +{ + class DistSensor + { + public: + explicit DistSensor(ros::NodeHandle& nh) : + sensor_l_(nh, "dist_sensor_l", "/dev/usbDistSensorLeft"), + sensor_r_(nh, "dist_sensor_r", "/dev/usbDistSensorRight"), + last_get_data_time_(ros::Time::now()) + { + sensor_interval_ = nh.param("sensor_interval", 0.6); + yaw_angle_pub_ = nh.advertise("yaw_angle",1); + ROS_INFO("Dist sensor load."); + } + + void read(SensorData& sensor); + void clearRxBuffer() + { + rx_buffer_.clear(); + rx_len_ = 0; + } + void calculateYawAngle() + { + std_msgs::Float64 yaw_angle; + if (sensor_l_.getError() || sensor_r_.getError()) + yaw_angle.data = 0; + else + yaw_angle.data = atan((sensor_r_.getDistance() - sensor_l_.getDistance()) / sensor_interval_); + yaw_angle_pub_.publish(yaw_angle); + } + SensorData sensor_l_, sensor_r_; + std::vector rx_buffer_; + int rx_len_; + + private: + int unpack(uint8_t* rx_data, SensorData& sensor); + ros::Time last_get_data_time_; + ros::Publisher yaw_angle_pub_; + double sensor_interval_; + const int k_frame_length_ = 128, k_header_length_ = 2, k_data_length_ = 6, k_tail_length_ = 1; + const int k_unpack_buffer_length_ = 256; + }; +} // namespace dist_sensor diff --git a/dist_sensor/launch/load.launch b/dist_sensor/launch/load.launch new file mode 100644 index 0000000..ec2df19 --- /dev/null +++ b/dist_sensor/launch/load.launch @@ -0,0 +1,4 @@ + + + + diff --git a/dist_sensor/package.xml b/dist_sensor/package.xml new file mode 100644 index 0000000..b0ae83b --- /dev/null +++ b/dist_sensor/package.xml @@ -0,0 +1,16 @@ + + + dist_sensor + 0.0.0 + The dist_sensor package + + BSD + ch + + catkin + + roscpp + rm_msgs + rm_common + serial + diff --git a/dist_sensor/src/dist_sensor.cpp b/dist_sensor/src/dist_sensor.cpp new file mode 100644 index 0000000..26585d7 --- /dev/null +++ b/dist_sensor/src/dist_sensor.cpp @@ -0,0 +1,61 @@ +// +// Created by ch on 25-6-4. +// + +#include "dist_sensor/dist_sensor.h" + +namespace dist_sensor +{ +void DistSensor::read(SensorData& sensor) +{ + if (sensor.base_.serial_.available()) + { + rx_len_ = static_cast(sensor.base_.serial_.available()); + sensor.base_.serial_.read(rx_buffer_, rx_len_); + } + else + return; + uint8_t temp_buffer[256] = { 0 }; + if (ros::Time::now() - last_get_data_time_ > ros::Duration(0.1)) + sensor.base_.sensor_is_online_ = false; + if (rx_len_ < k_unpack_buffer_length_) + { + for (int k_i = 0; k_i < k_unpack_buffer_length_ - rx_len_; ++k_i) + temp_buffer[k_i] = sensor.unpack_buffer_[k_i + rx_len_]; + for (int k_i = 0; k_i < rx_len_; ++k_i) + temp_buffer[k_i + k_unpack_buffer_length_ - rx_len_] = rx_buffer_[k_i]; + for (int k_i = 0; k_i < k_unpack_buffer_length_; ++k_i) + sensor.unpack_buffer_[k_i] = temp_buffer[k_i]; + } + for (int k_i = 0; k_i < k_unpack_buffer_length_ - k_frame_length_; ++k_i) + { + if (sensor.unpack_buffer_[k_i] == 0x59 && sensor.unpack_buffer_[k_i + 1] == 0x59) + { + int frame_len = unpack(&sensor.unpack_buffer_[k_i], sensor); + if (frame_len != -1) + k_i += frame_len; + } + } + clearRxBuffer(); +} + +int DistSensor::unpack(uint8_t* rx_data, SensorData& sensor) +{ + int frame_len = k_header_length_ + k_data_length_ + k_tail_length_; + if (sensor.base_.verifyCheckSum(rx_data, frame_len) == 1) + { + dist_sensor::DistanceSensorData dist_sensor_data_ref; + memcpy(&dist_sensor_data_ref, rx_data + 2, sizeof(dist_sensor::DistanceSensorData)); + sensor.setDistance((dist_sensor_data_ref.dist[1] << 8 | dist_sensor_data_ref.dist[0]) / 1000.0); + sensor.setAmp(dist_sensor_data_ref.amp[1] << 8 | dist_sensor_data_ref.amp[0]); + sensor.setTemperature((dist_sensor_data_ref.temp[1] << 8 | dist_sensor_data_ref.temp[0]) / 8.0 - 256); + sensor.publish(); + + sensor.base_.sensor_is_online_ = true; + last_get_data_time_ = ros::Time::now(); + return frame_len; + } + return -1; +} + +} // namespace dist_sensor \ No newline at end of file diff --git a/dist_sensor/src/main.cpp b/dist_sensor/src/main.cpp new file mode 100644 index 0000000..10066b3 --- /dev/null +++ b/dist_sensor/src/main.cpp @@ -0,0 +1,21 @@ +// +// Created by ch on 25-6-4. +// + +#include "dist_sensor/dist_sensor.h" + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "dist_sensor"); + ros::NodeHandle nh("~"); + dist_sensor::DistSensor dist_sensor(nh); + ros::Rate loop_rate(100); + while (ros::ok()) + { + ros::spinOnce(); + dist_sensor.read(dist_sensor.sensor_l_); + dist_sensor.read(dist_sensor.sensor_r_); + dist_sensor.calculateYawAngle(); + loop_rate.sleep(); + } +} diff --git a/engineer2_arm_config/.setup_assistant b/engineer2_arm_config/.setup_assistant index 7ab7a69..9c88099 100644 --- a/engineer2_arm_config/.setup_assistant +++ b/engineer2_arm_config/.setup_assistant @@ -6,6 +6,6 @@ moveit_setup_assistant_config: SRDF: relative_path: config/engineer2.srdf CONFIG: - author_name: Chonghong Cai - author_email: 792166012@qq.com - generated_timestamp: 1720344342 + author_name: ch + author_email: 3631676002@qq.com + generated_timestamp: 1775277360 \ No newline at end of file diff --git a/engineer2_arm_config/config/auto_exchange_planning.yaml b/engineer2_arm_config/config/auto_exchange_planning.yaml new file mode 100644 index 0000000..d82580e --- /dev/null +++ b/engineer2_arm_config/config/auto_exchange_planning.yaml @@ -0,0 +1 @@ +num_steps: 40 \ No newline at end of file diff --git a/engineer2_arm_config/config/engineer2.srdf b/engineer2_arm_config/config/engineer2.srdf index 12140bf..3de3820 100644 --- a/engineer2_arm_config/config/engineer2.srdf +++ b/engineer2_arm_config/config/engineer2.srdf @@ -10,27 +10,119 @@ - + + + + + + + + + + - + - - + + + + + - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/engineer2_arm_config/config/engineer2_moveit_controllers.yaml b/engineer2_arm_config/config/engineer2_moveit_controllers.yaml new file mode 100644 index 0000000..c4b157e --- /dev/null +++ b/engineer2_arm_config/config/engineer2_moveit_controllers.yaml @@ -0,0 +1,13 @@ +controller_list: + - name: controllers/gravity_compensation_controller + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + default: True + joints: + - joint1 + - joint2 + - joint3 + - joint4 + - joint5 + - joint6 + - joint7 \ No newline at end of file diff --git a/engineer2_arm_config/config/fake_controllers.yaml b/engineer2_arm_config/config/fake_controllers.yaml index 42c4827..5debc52 100644 --- a/engineer2_arm_config/config/fake_controllers.yaml +++ b/engineer2_arm_config/config/fake_controllers.yaml @@ -1,5 +1,5 @@ controller_list: - - name: arm_trajectory_controller + - name: fake_engineer_arm_controller type: $(arg fake_execution_type) joints: - joint1 @@ -8,6 +8,11 @@ controller_list: - joint4 - joint5 - joint6 + - joint7 + - name: fake_gripper_controller + type: $(arg fake_execution_type) + joints: + - gripper_joint initial: # Define initial robot poses per group - group: engineer_arm - pose: home + pose: home \ No newline at end of file diff --git a/engineer2_arm_config/config/gazebo_engineer2.urdf b/engineer2_arm_config/config/gazebo_engineer2.urdf index e04abdc..9d5e13a 100644 --- a/engineer2_arm_config/config/gazebo_engineer2.urdf +++ b/engineer2_arm_config/config/gazebo_engineer2.urdf @@ -1,14 +1,14 @@ - + - - - + + + @@ -33,7 +33,13 @@ + + + + + + @@ -46,2583 +52,691 @@ - - - - + + - + - + - - + + - - - - - - - - - - - - - - Gazebo/Black - - / - - - - - - - + - - - - - - - - - - - + + + + + + + + + - - - - - - Gazebo/Black - - - - - + + - + - + - - + + - - - - - - - - - - - - - - Gazebo/Black - - - - - - + - - - - - - - - - - - + + + + + + + + + - - - - - - Gazebo/Black - - - - - + + - + - + - - + + - - - - - - - - - - - - - - Gazebo/Black - - - - - - + - - - - - - - - - - - + + + + + + + + + - - - - - - Gazebo/Black - + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + 19.2032 + + + hardware_interface/EffortJointInterface + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + -19.2032 + + + hardware_interface/EffortJointInterface + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + 19.2032 + + + hardware_interface/EffortJointInterface + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + -19.2032 + + + hardware_interface/EffortJointInterface + + - - - - + + + - + - + - - - + + + - - - - - - - - - - - - - - Gazebo/Black - - - - - - + - - - - - - - - - - - - - - - - - - Gazebo/Black - - - - - - + + + - + - + - - - + + + - - - - - - - - - - - - - - Gazebo/Black - - - - - - + - - - - - - - - - - - - - - - - - - Gazebo/Black - - - - - - + + + - + - + - - - + + + - - - - - - - - - - - - - - Gazebo/Black - - - - - + - + - - - - - - - - - - - - - - - - - - Gazebo/Black - - - - - - + + + - + - + - - - + + + - - - - - - - - - - - - - - Gazebo/Black - - - - - + - + - - - - - - - - - - - - - - - - - - Gazebo/Black - - - + + - + - - + + + - - - - - - - - - - - - - - - + - + - - - - - - - - - - - - - - - - - - Gazebo/Black - - - - - - + + + - + - + - - - + + + - - - - - - - - - - - - - - Gazebo/Black - - - - - + - + - - - - - - - - - - - - - - - - - - Gazebo/Black - - - - - - + + + - + - + - - - - - - - - - - - - - - - - - - Gazebo/Black - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Black - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Black - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Black - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Black - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Black - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Black - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Black - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Black - - - - - - - - - - - - - - + + + - - - - - - - - - - - - - - Gazebo/Black - - - - - + - + - - - - - - - - - - - - - - - - - - Gazebo/Black - - - + + - + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Black - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Black - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Black - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Black - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Black - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Black - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Black - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Black - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Black - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Black - - - - - - - - - - - - - - + + + - - - - - - - - - - - - - - Gazebo/Black - - - - - + - - - - - - - - - - - - - - - - - - - - - - Gazebo/Black - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Black - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Black - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Black - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Black - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Black - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Black - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Black - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Black - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Black - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Black - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Black - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Black - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Black - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Black - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Black - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Black - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - 19.2032 - - - hardware_interface/EffortJointInterface - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - -19.2032 - - - hardware_interface/EffortJointInterface - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - 19.2032 - - - hardware_interface/EffortJointInterface - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - -19.2032 - - - hardware_interface/EffortJointInterface - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - transmission_interface/SimpleTransmission - - 421.49 - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 0. - - - - transmission_interface/SimpleTransmission - - 1 - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 0. - - - - transmission_interface/SimpleTransmission - - 1 - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 0 - - - - transmission_interface/SimpleTransmission - - 1 - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 0 - - - - transmission_interface/DifferentialTransmission - - actuator1 - 1 - - - actuator2 - 1 - - - hardware_interface/EffortJointInterface - 0 - joint1 - - - hardware_interface/EffortJointInterface - 0 - joint2 - - - - - - - - - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - + + 492.488 + hardware_interface/EffortJointInterface - 1 - - - transmission_interface/SimpleTransmission - + hardware_interface/EffortJointInterface + 0.552 - - hardware_interface/EffortJointInterface - 1 - - + transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface + 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - hardware_interface/EffortJointInterface - 1 - - - transmission_interface/SimpleTransmission - + hardware_interface/EffortJointInterface + -0.588 - - hardware_interface/EffortJointInterface - 1 - - + transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - + + -1 hardware_interface/EffortJointInterface - 1 - - - transmission_interface/SimpleTransmission - + hardware_interface/EffortJointInterface + 0.514 - - hardware_interface/EffortJointInterface - 1 - - + transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface + 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - hardware_interface/EffortJointInterface - 1 - - - transmission_interface/SimpleTransmission - + hardware_interface/EffortJointInterface + -0.539 - - hardware_interface/EffortJointInterface - 1 - - + transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - + + -1 hardware_interface/EffortJointInterface - 1 - - - transmission_interface/SimpleTransmission - + hardware_interface/EffortJointInterface + 0.323 - - hardware_interface/EffortJointInterface - 1 - - + transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface + 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - hardware_interface/EffortJointInterface - 1 - - - transmission_interface/SimpleTransmission - + hardware_interface/EffortJointInterface + -0.036 - - hardware_interface/EffortJointInterface - 1 - - + transmission_interface/SimpleTransmission - + + -1 hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - transmission_interface/SimpleTransmission - + hardware_interface/EffortJointInterface + 0.419 - - hardware_interface/EffortJointInterface - 1 - - + transmission_interface/SimpleTransmission - + + -1 hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - transmission_interface/SimpleTransmission - + hardware_interface/EffortJointInterface + 0.0 - - hardware_interface/EffortJointInterface - 1 - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - + + 984.976 hardware_interface/EffortJointInterface - 1 - - - transmission_interface/SimpleTransmission - + hardware_interface/EffortJointInterface + 0.0 - - hardware_interface/EffortJointInterface - 1 - - + transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - + + -984.976 hardware_interface/EffortJointInterface - 1 - - - transmission_interface/SimpleTransmission - + hardware_interface/EffortJointInterface + 0.0 - - hardware_interface/EffortJointInterface - 1 - - + transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - + + 36 hardware_interface/EffortJointInterface - 1 - - - transmission_interface/SimpleTransmission - + hardware_interface/EffortJointInterface + 0.0 - - hardware_interface/EffortJointInterface - 1 - - + transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - + + -36 hardware_interface/EffortJointInterface - 1 - - - transmission_interface/SimpleTransmission - + hardware_interface/EffortJointInterface + 0.0 - - hardware_interface/EffortJointInterface - 1 - + + + + + + + + + / + + + diff --git a/engineer2_arm_config/config/joint_limits.yaml b/engineer2_arm_config/config/joint_limits.yaml index 7031779..52d5010 100644 --- a/engineer2_arm_config/config/joint_limits.yaml +++ b/engineer2_arm_config/config/joint_limits.yaml @@ -2,39 +2,49 @@ # For beginners, we downscale velocity and acceleration limits. # You can always specify higher scaling factors (<= 1.0) in your motion requests. # Increase the values below to 1.0 to always move at maximum speed. -default_velocity_scaling_factor: 0.1 -default_acceleration_scaling_factor: 0.1 +default_velocity_scaling_factor: 0.5 +default_acceleration_scaling_factor: 0.4 # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] joint_limits: + gripper_joint: + has_velocity_limits: true + max_velocity: 20.94 + has_acceleration_limits: true + max_acceleration: 10 joint1: has_velocity_limits: true - max_velocity: 1.25 + max_velocity: 1.929 has_acceleration_limits: true - max_acceleration: 1 + max_acceleration: 1.5 joint2: has_velocity_limits: true - max_velocity: 10.46 + max_velocity: 3.76 has_acceleration_limits: true max_acceleration: 10 joint3: has_velocity_limits: true - max_velocity: 5.44 + max_velocity: 10.46 has_acceleration_limits: true - max_acceleration: 15 + max_acceleration: 10 joint4: has_velocity_limits: true - max_velocity: 20.94 + max_velocity: 3.76 has_acceleration_limits: true - max_acceleration: 15 + max_acceleration: 10 joint5: has_velocity_limits: true - max_velocity: 10 + max_velocity: 20.94 has_acceleration_limits: true - max_acceleration: 15 + max_acceleration: 10 joint6: has_velocity_limits: true - max_velocity: 10 + max_velocity: 20.94 + has_acceleration_limits: true + max_acceleration: 10 + joint7: + has_velocity_limits: true + max_velocity: 20.94 has_acceleration_limits: true - max_acceleration: 15 + max_acceleration: 10 \ No newline at end of file diff --git a/engineer2_arm_config/config/kinematics.yaml b/engineer2_arm_config/config/kinematics.yaml index c50afb8..7d6dacf 100644 --- a/engineer2_arm_config/config/kinematics.yaml +++ b/engineer2_arm_config/config/kinematics.yaml @@ -2,3 +2,6 @@ engineer_arm: kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin kinematics_solver_search_resolution: 0.005 kinematics_solver_timeout: 0.005 + goal_joint_tolerance: 0.001 + goal_position_tolerance: 0.001 + goal_orientation_tolerance: 0.01 \ No newline at end of file diff --git a/engineer2_arm_config/config/ompl_planning.yaml b/engineer2_arm_config/config/ompl_planning.yaml index 205b9ad..1dd9651 100644 --- a/engineer2_arm_config/config/ompl_planning.yaml +++ b/engineer2_arm_config/config/ompl_planning.yaml @@ -195,4 +195,38 @@ engineer_arm: - ABITstar - BITstar projection_evaluator: joints(joint1,joint2) - longest_valid_segment_fraction: 0.005 + longest_valid_segment_fraction: 0.01 +gripper: + planner_configs: + - AnytimePathShortening + - SBL + - EST + - LBKPIECE + - BKPIECE + - KPIECE + - RRT + - RRTConnect + - RRTstar + - TRRT + - PRM + - PRMstar + - FMT + - BFMT + - PDST + - STRIDE + - BiTRRT + - LBTRRT + - BiEST + - ProjEST + - LazyPRM + - LazyPRMstar + - SPARS + - SPARStwo + - AITstar + - ABITstar + - BITstar +special_joint_constraint: + enabled: true + joint_a: joint4 + joint_b: joint3 + constant: -0.27 \ No newline at end of file diff --git a/engineer2_arm_config/config/ros_controllers.yaml b/engineer2_arm_config/config/ros_controllers.yaml index a4837dc..fda1626 100644 --- a/engineer2_arm_config/config/ros_controllers.yaml +++ b/engineer2_arm_config/config/ros_controllers.yaml @@ -1,13 +1,5 @@ -# Simulation settings for using moveit_sim_controllers -moveit_sim_hw_interface: - joint_model_group: engineer_arm - joint_model_group_pose: home -# Settings for ros_control_boilerplate control loop -generic_hw_control_loop: - loop_hz: 300 - cycle_time_error_threshold: 0.01 -# Settings for ros_control hardware interface -hardware_interface: +engineer_arm_controller: + type: effort_controllers/JointTrajectoryController joints: - joint1 - joint2 @@ -15,17 +7,50 @@ hardware_interface: - joint4 - joint5 - joint6 - sim_control_mode: 1 # 0: position, 1: velocity - -controller_list: - - name: controllers/arm_trajectory_controller - action_ns: follow_joint_trajectory - default: True - type: FollowJointTrajectory - joints: - - joint1 - - joint2 - - joint3 - - joint4 - - joint5 - - joint6 + - joint7 + constraints: + goal_time: 5.0 + stopped_velocity_tolerance: 0.05 + joint1: { trajectory: 0.08, goal: 0.05 } + joint2: { trajectory: 0.08, goal: 0.05 } + joint3: { trajectory: 0.08, goal: 0.05 } + joint4: { trajectory: 0.08, goal: 0.05 } + joint5: { trajectory: 0.08, goal: 0.05 } + joint6: { trajectory: 0.08, goal: 0.05 } + joint7: { trajectory: 0.08, goal: 0.05 } + gains: + joint1: + p: 16000 + d: 500 + i: 100 + i_clamp: 10 + joint2: + p: 100 + d: 10 + i: 5 + i_clamp: 5 + joint3: + p: 1000 + d: 20 + i: 100 + i_clamp: 10 + joint4: + p: 500 + d: 5 + i: 1 + i_clamp: 1 + joint5: + p: 200 + d: 2 + i: 1 + i_clamp: 1 + joint6: + p: 100 + d: 1 + i: 1 + i_clamp: 1 + joint7: + p: 100 + d: 1 + i: 1 + i_clamp: 1 \ No newline at end of file diff --git a/engineer2_arm_config/config/sensors_3d.yaml b/engineer2_arm_config/config/sensors_3d.yaml index b29057e..51010a3 100644 --- a/engineer2_arm_config/config/sensors_3d.yaml +++ b/engineer2_arm_config/config/sensors_3d.yaml @@ -1 +1,2 @@ sensors: + [] \ No newline at end of file diff --git a/engineer2_arm_config/config/servo.yaml b/engineer2_arm_config/config/servo.yaml index 76270c2..e0c53fe 100644 --- a/engineer2_arm_config/config/servo.yaml +++ b/engineer2_arm_config/config/servo.yaml @@ -8,10 +8,10 @@ command_in_type: "speed_units" # "unitless"> in the range [-1:1], as if from joy scale: # Scale parameters are only used if command_in_type=="unitless" linear: 0.4 # Max linear velocity. Meters per publish_period. Unit is [m/s]. Only used for Cartesian commands. - rotational: 0.2 # Max angular velocity. Rads per publish_period. Unit is [rad/s]. Only used for Cartesian commands. + rotational: 1.5 # 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.005 -low_pass_filter_coeff: 1. # Larger --> trust the filtered data more, trust the measurements less. +low_pass_filter_coeff: 2. # Larger --> trust the filtered data more, trust the measurements less. ## Properties of outgoing commands publish_period: 0.5 # 1/Nominal publish rate [seconds] @@ -25,15 +25,15 @@ 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_accelerations: false +publish_joint_accelerations: true ## MoveIt properties move_group_name: engineer_arm # Often 'manipulator' or 'arm' planning_frame: base_link # The MoveIt planning frame. Often 'base_link' or 'world' ## Other frames -ee_frame_name: link6 # The name of the end effector link, used to return the EE pose -robot_link_command_frame: link6 # commands must be given in the frame of a robot link. Usually either the base or end effector +ee_frame_name: link7 # The name of the end effector link, used to return the EE pose +robot_link_command_frame: link7 # 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 @@ -44,7 +44,7 @@ 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) hard_stop_singularity_threshold: 30 # Stop when the condition number hits this -joint_limit_margin: 0.001 # added as a buffer to joint limits [radians]. If moving quickly, make this larger. +joint_limit_margin: 0.005 # added as a buffer to joint limits [radians]. If moving quickly, make this larger. ## Topic names cartesian_command_in_topic: delta_twist_cmds # Topic for incoming Cartesian twist commands @@ -54,7 +54,7 @@ status_topic: status # Publish status to this topic command_out_topic: /controllers/arm_trajectory_controller/command # Publish outgoing commands here ## Collision checking for the entire robot body -check_collisions: false # Check collisions? +check_collisions: true # Check collisions? 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. diff --git a/engineer2_arm_config/config/simple_moveit_controllers.yaml b/engineer2_arm_config/config/simple_moveit_controllers.yaml index 1da75d2..a4ca2dc 100644 --- a/engineer2_arm_config/config/simple_moveit_controllers.yaml +++ b/engineer2_arm_config/config/simple_moveit_controllers.yaml @@ -1,5 +1,5 @@ controller_list: - - name: arm_trajectory_controller + - name: engineer_arm_controller action_ns: follow_joint_trajectory type: FollowJointTrajectory default: True @@ -10,3 +10,4 @@ controller_list: - joint4 - joint5 - joint6 + - joint7 \ No newline at end of file diff --git a/engineer2_arm_config/config/stomp_planning.yaml b/engineer2_arm_config/config/stomp_planning.yaml index d0e6815..71ef836 100644 --- a/engineer2_arm_config/config/stomp_planning.yaml +++ b/engineer2_arm_config/config/stomp_planning.yaml @@ -11,7 +11,7 @@ stomp/engineer_arm: task: noise_generator: - class: stomp_moveit/NormalDistributionSampling - stddev: [0.05, 0.05, 0.05, 0.05, 0.05, 0.05] + stddev: [0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05] cost_functions: - class: stomp_moveit/CollisionCheck collision_penalty: 1.0 @@ -37,3 +37,42 @@ stomp/engineer_arm: publish_intermediate: True marker_topic: stomp_trajectory marker_namespace: optimized +stomp/gripper: + group_name: gripper + optimization: + num_timesteps: 60 + num_iterations: 40 + num_iterations_after_valid: 0 + num_rollouts: 30 + max_rollouts: 30 + initialization_method: 1 # [1 : LINEAR_INTERPOLATION, 2 : CUBIC_POLYNOMIAL, 3 : MININUM_CONTROL_COST] + control_cost_weight: 0.0 + task: + noise_generator: + - class: stomp_moveit/NormalDistributionSampling + stddev: [0.05] + cost_functions: + - class: stomp_moveit/CollisionCheck + collision_penalty: 1.0 + cost_weight: 1.0 + kernel_window_percentage: 0.2 + longest_valid_joint_move: 0.05 + noisy_filters: + - class: stomp_moveit/JointLimits + lock_start: True + lock_goal: True + - class: stomp_moveit/MultiTrajectoryVisualization + line_width: 0.02 + rgb: [255, 255, 0] + marker_array_topic: stomp_trajectories + marker_namespace: noisy + update_filters: + - class: stomp_moveit/PolynomialSmoother + poly_order: 6 + - class: stomp_moveit/TrajectoryVisualization + line_width: 0.05 + rgb: [0, 191, 255] + error_rgb: [255, 0, 0] + publish_intermediate: True + marker_topic: stomp_trajectory + marker_namespace: optimized \ No newline at end of file diff --git a/engineer2_arm_config/launch/auto_exchange_planning_pipeline.launch.xml b/engineer2_arm_config/launch/auto_exchange_planning_pipeline.launch.xml new file mode 100644 index 0000000..eba37b8 --- /dev/null +++ b/engineer2_arm_config/launch/auto_exchange_planning_pipeline.launch.xml @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + diff --git a/engineer2_arm_config/launch/default_warehouse_db.launch b/engineer2_arm_config/launch/default_warehouse_db.launch new file mode 100644 index 0000000..2188279 --- /dev/null +++ b/engineer2_arm_config/launch/default_warehouse_db.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/engineer2_arm_config/launch/demo.launch b/engineer2_arm_config/launch/demo.launch new file mode 100644 index 0000000..123a14c --- /dev/null +++ b/engineer2_arm_config/launch/demo.launch @@ -0,0 +1,66 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + [move_group/fake_controller_joint_states] + + + + [move_group/fake_controller_joint_states] + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/engineer2_arm_config/launch/demo_gazebo.launch b/engineer2_arm_config/launch/demo_gazebo.launch new file mode 100644 index 0000000..0ef8f95 --- /dev/null +++ b/engineer2_arm_config/launch/demo_gazebo.launch @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/engineer2_arm_config/launch/empty_world.launch b/engineer2_arm_config/launch/empty_world.launch new file mode 100644 index 0000000..bf2e5a9 --- /dev/null +++ b/engineer2_arm_config/launch/empty_world.launch @@ -0,0 +1,26 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/engineer2_arm_config/launch/engineer2_moveit_controller_manager.launch.xml b/engineer2_arm_config/launch/engineer2_moveit_controller_manager.launch.xml index db614c0..c730acd 100644 --- a/engineer2_arm_config/launch/engineer2_moveit_controller_manager.launch.xml +++ b/engineer2_arm_config/launch/engineer2_moveit_controller_manager.launch.xml @@ -1,10 +1,7 @@ + + - - - - - - - + + + \ No newline at end of file diff --git a/engineer2_arm_config/launch/fake_moveit_controller_manager.launch.xml b/engineer2_arm_config/launch/fake_moveit_controller_manager.launch.xml index 4d5bbea..64a5b70 100644 --- a/engineer2_arm_config/launch/fake_moveit_controller_manager.launch.xml +++ b/engineer2_arm_config/launch/fake_moveit_controller_manager.launch.xml @@ -7,6 +7,6 @@ - + diff --git a/engineer2_arm_config/launch/gazebo.launch b/engineer2_arm_config/launch/gazebo.launch new file mode 100644 index 0000000..e0006af --- /dev/null +++ b/engineer2_arm_config/launch/gazebo.launch @@ -0,0 +1,34 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/engineer2_arm_config/launch/joystick_control.launch b/engineer2_arm_config/launch/joystick_control.launch new file mode 100644 index 0000000..9411f6e --- /dev/null +++ b/engineer2_arm_config/launch/joystick_control.launch @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/engineer2_arm_config/launch/load_controllers.launch b/engineer2_arm_config/launch/load_controllers.launch index 2b45884..f1aa6bd 100644 --- a/engineer2_arm_config/launch/load_controllers.launch +++ b/engineer2_arm_config/launch/load_controllers.launch @@ -21,13 +21,8 @@ controllers/robot_state_controller controllers/joint_state_controller controllers/arm_trajectory_controller - controllers/gimbal_controller + controllers/gravity_compensation_controller controllers/chassis_controller - controllers/silver_lifter_controller - controllers/silver_pusher_controller - controllers/silver_rotator_controller - controllers/gold_lifter_controller - controllers/gold_pusher_controller " /> diff --git a/engineer2_arm_config/launch/load_move_group.launch b/engineer2_arm_config/launch/load_move_group.launch index a66a2a9..d4c3309 100644 --- a/engineer2_arm_config/launch/load_move_group.launch +++ b/engineer2_arm_config/launch/load_move_group.launch @@ -9,7 +9,7 @@ - + diff --git a/engineer2_arm_config/launch/move_group.launch b/engineer2_arm_config/launch/move_group.launch index c8771cc..0cb8840 100644 --- a/engineer2_arm_config/launch/move_group.launch +++ b/engineer2_arm_config/launch/move_group.launch @@ -1,52 +1,89 @@ - - - - + + + + - - - - + + + + - + + + + + - + - - - + + + + + + + + + + + + + + + + - - - - - + + + + + + + + + + + + + + + + - + - + diff --git a/engineer2_arm_config/launch/moveit.rviz b/engineer2_arm_config/launch/moveit.rviz index 2d517dd..6a876dd 100644 --- a/engineer2_arm_config/launch/moveit.rviz +++ b/engineer2_arm_config/launch/moveit.rviz @@ -4,12 +4,9 @@ Panels: Name: Displays Property Tree Widget: Expanded: - - /Global Options1 - /MotionPlanning1 - - /TF1 - - /TF1/Frames1 Splitter Ratio: 0.5 - Tree Height: 1301 + Tree Height: 226 - Class: rviz/Help Name: Help - Class: rviz/Views @@ -17,10 +14,6 @@ Panels: - /Current View1 Name: Views Splitter Ratio: 0.5 -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 Visualization Manager: Class: "" Displays: @@ -30,7 +23,7 @@ Visualization Manager: Color: 160; 160; 164 Enabled: true Line Style: - Line Width: 0.029999999329447746 + Line Width: 0.03 Value: Lines Name: Grid Normal Cell Count: 0 @@ -42,61 +35,30 @@ Visualization Manager: Plane Cell Count: 10 Reference Frame: Value: true - - Acceleration_Scaling_Factor: 0.1 - Class: moveit_rviz_plugin/MotionPlanning - Enabled: false - Move Group Namespace: "" - MoveIt_Allow_Approximate_IK: false - MoveIt_Allow_External_Program: false - MoveIt_Allow_Replanning: false - MoveIt_Allow_Sensor_Positioning: false - MoveIt_Planning_Attempts: 10 - MoveIt_Planning_Time: 5 - MoveIt_Use_Cartesian_Path: false - MoveIt_Use_Constraint_Aware_IK: false - MoveIt_Workspace: - Center: - X: 0 - Y: 0 - Z: 0 - Size: - X: 2 - Y: 2 - Z: 2 + - Class: moveit_rviz_plugin/MotionPlanning + Enabled: true Name: MotionPlanning Planned Path: - Color Enabled: false - Interrupt Display: false - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order + Links: ~ Loop Animation: true Robot Alpha: 0.5 - Robot Color: 150; 50; 150 Show Robot Collision: false Show Robot Visual: true Show Trail: false State Display Time: 0.05 s - Trail Step Size: 1 Trajectory Topic: move_group/display_planned_path - Use Sim Time: false Planning Metrics: Payload: 1 Show Joint Torques: false Show Manipulability: false Show Manipulability Index: false Show Weight Limit: false - TextHeight: 0.07999999821186066 Planning Request: Colliding Link Color: 255; 0; 0 Goal State Alpha: 1 Goal State Color: 250; 128; 0 Interactive Marker Size: 0 Joint Violation Color: 255; 0; 255 - Planning Group: engineer_arm Query Goal State: true Query Start State: false Show Workspace: false @@ -106,236 +68,19 @@ Visualization Manager: Robot Description: robot_description Scene Geometry: Scene Alpha: 1 - Scene Color: 50; 230; 50 - Scene Display Time: 0.009999999776482582 Show Scene Geometry: true Voxel Coloring: Z-Axis Voxel Rendering: Occupied Voxels Scene Robot: Attached Body Color: 150; 50; 150 - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order + Links: ~ Robot Alpha: 0.5 - Show Robot Collision: false - Show Robot Visual: true - Value: false - Velocity_Scaling_Factor: 0.1 - - Class: rviz/TF - Enabled: true - Filter (blacklist): "" - Filter (whitelist): "" - Frame Timeout: 15 - Frames: - All Enabled: false - base_link: - Value: true - fake_link4: - Value: false - gold_lifter_link: - Value: false - gold_pusher_link: - Value: false - left_back_wheel: - Value: false - left_front_wheel: - Value: false - link1: - Value: false - link2: - Value: false - link3: - Value: false - link4: - Value: false - link5: - Value: true - link6: - Value: true - map: - Value: true - odom: - Value: true - pitch: - Value: true - right_back_wheel: - Value: false - right_front_wheel: - Value: false - silver_gripper_link: - Value: false - silver_lifter_link: - Value: false - silver_pusher_link: - Value: false - tools_link: - Value: false - yaw: - Value: false - Marker Alpha: 1 - Marker Scale: 1 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: true - Tree: - map: - odom: - base_link: - gold_pusher_link: - gold_lifter_link: - {} - left_back_wheel: - {} - left_front_wheel: - {} - link1: - link2: - link3: - fake_link4: - {} - link4: - link5: - link6: - tools_link: - {} - yaw: - pitch: - {} - right_back_wheel: - {} - right_front_wheel: - {} - silver_lifter_link: - silver_pusher_link: - silver_gripper_link: - {} - Update Interval: 0 + Show Scene Robot: true Value: true - - Alpha: 1 - Class: rviz/RobotModel - Collision Enabled: false - Enabled: true - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - fake_link4: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - gold_lifter_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - gold_pusher_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - left_back_wheel: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - left_front_wheel: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link1: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link2: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link3: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link4: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link5: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link6: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - pitch: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - right_back_wheel: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - right_front_wheel: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - silver_gripper_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - silver_lifter_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - silver_pusher_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - tools_link: - Alpha: 1 - Show Axes: false - Show Trail: false - yaw: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - Name: RobotModel - Robot Description: robot_description - TF Prefix: "" - Update Interval: 0 - Value: true - Visual Enabled: true Enabled: true Global Options: Background Color: 48; 48; 48 - Default Light: true - Fixed Frame: odom - Frame Rate: 30 + Fixed Frame: base_link Name: root Tools: - Class: rviz/Interact @@ -346,37 +91,41 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 1.3873012065887451 + Distance: 2.0 Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 + Stereo Eye Separation: 0.06 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Field of View: 0.75 Focal Point: - X: 0.22694315016269684 - Y: -0.3219256103038788 - Z: 0.15961049497127533 + X: -0.1 + Y: 0.25 + Z: 0.30 Focal Shape Fixed Size: true - Focal Shape Size: 0.05000000074505806 + Focal Shape Size: 0.05 Invert Z Axis: false Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 1.3847957849502563 + Near Clip Distance: 0.01 + Pitch: 0.5 Target Frame: base_link - Yaw: 4.408130168914795 + Yaw: -0.6232355833053589 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 1536 + Height: 848 Help: collapsed: false Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd0000000100000000000001f3000005a6fc0200000007fb000000100044006900730070006c006100790073010000003d000005a6000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006700000002f4000002ef0000000000000000000007bf000005a600000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + MotionPlanning: + collapsed: false + MotionPlanning - Trajectory Slider: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000100000000000001f0000002f6fc0200000007fb000000100044006900730070006c006100790073010000003d00000173000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000001b60000017d0000017d00ffffff00000315000002f600000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Views: collapsed: false - Width: 2488 - X: 72 - Y: 27 + Width: 1291 + X: 454 + Y: 25 diff --git a/engineer2_arm_config/launch/ompl_chomp_planning_pipeline.launch.xml b/engineer2_arm_config/launch/ompl_chomp_planning_pipeline.launch.xml new file mode 100644 index 0000000..c1e6c0c --- /dev/null +++ b/engineer2_arm_config/launch/ompl_chomp_planning_pipeline.launch.xml @@ -0,0 +1,21 @@ + + + + + + + + + + + + diff --git a/engineer2_arm_config/launch/ompl_planning_pipeline.launch.xml b/engineer2_arm_config/launch/ompl_planning_pipeline.launch.xml index ed05662..b2aa188 100644 --- a/engineer2_arm_config/launch/ompl_planning_pipeline.launch.xml +++ b/engineer2_arm_config/launch/ompl_planning_pipeline.launch.xml @@ -5,6 +5,7 @@ default="default_planner_request_adapters/LimitMaxCartesianLinkSpeed default_planner_request_adapters/AddTimeParameterization default_planner_request_adapters/ResolveConstraintFrames + engineer_planning_adapters/SpecialJointConstraintAdapter default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision diff --git a/engineer2_arm_config/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml b/engineer2_arm_config/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml new file mode 100644 index 0000000..c7c4cf5 --- /dev/null +++ b/engineer2_arm_config/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + diff --git a/engineer2_arm_config/launch/planning_pipeline.launch.xml b/engineer2_arm_config/launch/planning_pipeline.launch.xml index 0929525..4b4d0d6 100644 --- a/engineer2_arm_config/launch/planning_pipeline.launch.xml +++ b/engineer2_arm_config/launch/planning_pipeline.launch.xml @@ -5,6 +5,6 @@ - + diff --git a/engineer2_arm_config/launch/ros_control_moveit_controller_manager.launch.xml b/engineer2_arm_config/launch/ros_control_moveit_controller_manager.launch.xml new file mode 100644 index 0000000..9ebc91c --- /dev/null +++ b/engineer2_arm_config/launch/ros_control_moveit_controller_manager.launch.xml @@ -0,0 +1,4 @@ + + + + diff --git a/engineer2_arm_config/launch/ros_controllers.launch b/engineer2_arm_config/launch/ros_controllers.launch new file mode 100644 index 0000000..7565713 --- /dev/null +++ b/engineer2_arm_config/launch/ros_controllers.launch @@ -0,0 +1,11 @@ + + + + + + + + + + diff --git a/engineer2_arm_config/launch/run_benchmark_ompl.launch b/engineer2_arm_config/launch/run_benchmark_ompl.launch new file mode 100644 index 0000000..8873abe --- /dev/null +++ b/engineer2_arm_config/launch/run_benchmark_ompl.launch @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/engineer2_arm_config/launch/trajectory_execution.launch.xml b/engineer2_arm_config/launch/trajectory_execution.launch.xml index ae4b5af..fa6ef00 100644 --- a/engineer2_arm_config/launch/trajectory_execution.launch.xml +++ b/engineer2_arm_config/launch/trajectory_execution.launch.xml @@ -2,7 +2,7 @@ - + @@ -10,15 +10,14 @@ - + - + - + - - - + + diff --git a/engineer2_arm_config/launch/warehouse.launch b/engineer2_arm_config/launch/warehouse.launch new file mode 100644 index 0000000..0712e67 --- /dev/null +++ b/engineer2_arm_config/launch/warehouse.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/engineer2_arm_config/launch/warehouse_settings.launch.xml b/engineer2_arm_config/launch/warehouse_settings.launch.xml new file mode 100644 index 0000000..e473b08 --- /dev/null +++ b/engineer2_arm_config/launch/warehouse_settings.launch.xml @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/engineer2_arm_config/package.xml b/engineer2_arm_config/package.xml index 8c33136..3464459 100644 --- a/engineer2_arm_config/package.xml +++ b/engineer2_arm_config/package.xml @@ -5,14 +5,14 @@ An automatically generated package with all the configuration and launch files for using the engineer2 with the MoveIt Motion Planning Framework - Chonghong Cai - Chonghong Cai + ch + ch BSD http://moveit.ros.org/ - https://github.com/ros-planning/moveit/issues - https://github.com/ros-planning/moveit + https://github.com/moveit/moveit/issues + https://github.com/moveit/moveit catkin diff --git a/engineer_arm_config/.setup_assistant b/engineer_arm_config/.setup_assistant index 27ac3a9..2cebde3 100644 --- a/engineer_arm_config/.setup_assistant +++ b/engineer_arm_config/.setup_assistant @@ -6,6 +6,6 @@ moveit_setup_assistant_config: SRDF: relative_path: config/engineer.srdf CONFIG: - author_name: QiayuanLiao - author_email: liaoqiayuan@gmail.com - generated_timestamp: 1617294211 + author_name: CH + author_email: 3631676002@qq.com + generated_timestamp: 1784690695 \ No newline at end of file diff --git a/engineer_arm_config/CMakeLists.txt b/engineer_arm_config/CMakeLists.txt index 287bc11..0cbbfdf 100644 --- a/engineer_arm_config/CMakeLists.txt +++ b/engineer_arm_config/CMakeLists.txt @@ -6,5 +6,5 @@ find_package(catkin REQUIRED) catkin_package() install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} - PATTERN "setup_assistant.launch" EXCLUDE) + PATTERN "setup_assistant.launch" EXCLUDE) install(DIRECTORY config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) diff --git a/engineer_arm_config/config/cartesian_limits.yaml b/engineer_arm_config/config/cartesian_limits.yaml new file mode 100644 index 0000000..7df72f6 --- /dev/null +++ b/engineer_arm_config/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/engineer_arm_config/config/chomp_planning.yaml b/engineer_arm_config/config/chomp_planning.yaml index 0831fda..eb9c912 100644 --- a/engineer_arm_config/config/chomp_planning.yaml +++ b/engineer_arm_config/config/chomp_planning.yaml @@ -7,12 +7,12 @@ learning_rate: 0.01 smoothness_cost_velocity: 0.0 smoothness_cost_acceleration: 1.0 smoothness_cost_jerk: 0.0 -ridge_factor: 0.01 +ridge_factor: 0.0 use_pseudo_inverse: false pseudo_inverse_ridge_factor: 1e-4 joint_update_limit: 0.1 collision_clearance: 0.2 collision_threshold: 0.07 use_stochastic_descent: true -enable_failure_recovery: true -max_recovery_attempts: 15 +enable_failure_recovery: false +max_recovery_attempts: 5 diff --git a/engineer_arm_config/config/engineer.srdf b/engineer_arm_config/config/engineer.srdf index c49a334..b6855d6 100644 --- a/engineer_arm_config/config/engineer.srdf +++ b/engineer_arm_config/config/engineer.srdf @@ -9,48 +9,200 @@ - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/engineer_arm_config/config/engineer_moveit_controllers.yaml b/engineer_arm_config/config/engineer_moveit_controllers.yaml new file mode 100644 index 0000000..138f592 --- /dev/null +++ b/engineer_arm_config/config/engineer_moveit_controllers.yaml @@ -0,0 +1,34 @@ +controller_list: + - name: controllers/left_arm_controller + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + default: True + joints: + - left_joint1 + - left_joint2 + - left_joint3 + - left_joint4 + - left_joint5 + - left_joint6 + - left_joint7 + - name: controllers/right_arm_controller + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + default: True + joints: + - right_joint1 + - right_joint2 + - right_joint3 + - right_joint4 + - right_joint5 + - right_joint6 + - right_joint7 + - name: controllers/gimbal_arm_controller + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + default: True + joints: + - main_pitch + - gimbal_joint1 + - gimbal_joint2 + - gimbal_joint3 \ No newline at end of file diff --git a/engineer_arm_config/config/fake_controllers.yaml b/engineer_arm_config/config/fake_controllers.yaml index 82506df..cdcbecf 100644 --- a/engineer_arm_config/config/fake_controllers.yaml +++ b/engineer_arm_config/config/fake_controllers.yaml @@ -1,14 +1,56 @@ controller_list: - - name: fake_engineer_arm_controller + - name: fake_left_arm_controller type: $(arg fake_execution_type) joints: - - joint1 - - joint2 - - joint3 - - joint4 - - joint5 - - joint6 - - joint7 + - left_joint1 + - left_joint2 + - left_joint3 + - left_joint4 + - left_joint5 + - left_joint6 + - left_joint7 + - name: fake_right_arm_controller + type: $(arg fake_execution_type) + joints: + - right_joint1 + - right_joint2 + - right_joint3 + - right_joint4 + - right_joint5 + - right_joint6 + - right_joint7 + - name: fake_gimbal_arm_controller + type: $(arg fake_execution_type) + joints: + - main_pitch + - gimbal_joint1 + - gimbal_joint2 + - gimbal_joint3 + - name: fake_engineer_arms_controller + type: $(arg fake_execution_type) + joints: + - main_pitch + - gimbal_joint1 + - gimbal_joint2 + - gimbal_joint3 + - left_joint1 + - left_joint2 + - left_joint3 + - left_joint4 + - left_joint5 + - left_joint6 + - left_joint7 + - right_joint1 + - right_joint2 + - right_joint3 + - right_joint4 + - right_joint5 + - right_joint6 + - right_joint7 initial: # Define initial robot poses per group - - group: engineer_arm - pose: home + - group: left_arm + pose: left_arm_home + - group: right_arm + pose: right_arm_home + - group: gimbal_arm + pose: gimbal_arm_home \ No newline at end of file diff --git a/engineer_arm_config/config/gazebo_controllers.yaml b/engineer_arm_config/config/gazebo_controllers.yaml new file mode 100644 index 0000000..e4d2eb0 --- /dev/null +++ b/engineer_arm_config/config/gazebo_controllers.yaml @@ -0,0 +1,4 @@ +# Publish joint_states +joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 50 diff --git a/engineer_arm_config/config/gazebo_engineer.urdf b/engineer_arm_config/config/gazebo_engineer.urdf new file mode 100644 index 0000000..c7cca2b --- /dev/null +++ b/engineer_arm_config/config/gazebo_engineer.urdf @@ -0,0 +1,1095 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Black + + / + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Black + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Black + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Black + + + transmission_interface/SimpleTransmission + + -1 + hardware_interface/EffortJointInterface + + + -1.815 + hardware_interface/EffortJointInterface + + + + transmission_interface/SimpleTransmission + + 19.2032 + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + + + + transmission_interface/SimpleTransmission + + -1 + hardware_interface/EffortJointInterface + + + 0.472 + hardware_interface/EffortJointInterface + + + + transmission_interface/SimpleTransmission + + -19.2032 + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + + + + transmission_interface/SimpleTransmission + + -1 + hardware_interface/EffortJointInterface + + + -0.312 + hardware_interface/EffortJointInterface + + + + transmission_interface/SimpleTransmission + + 19.2032 + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + + + + transmission_interface/SimpleTransmission + + -1 + hardware_interface/EffortJointInterface + + + 1.856 + hardware_interface/EffortJointInterface + + + + transmission_interface/SimpleTransmission + + -19.2032 + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + 1 + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 0.940 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + 1 + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + -0.402 + + + + transmission_interface/SimpleTransmission + + -1 + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 2.320 + + + + transmission_interface/SimpleTransmission + + 1 + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + -0.641 + + + + transmission_interface/SimpleTransmission + + -1 + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + -0.298 + + + + transmission_interface/SimpleTransmission + + 1 + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1.279 + + + + transmission_interface/SimpleTransmission + + -1 + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + -1.283 + + + + transmission_interface/SimpleTransmission + + 1 + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1.515 + + + + transmission_interface/SimpleTransmission + + -1 + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1.705 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + -1 + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 0.003 + + + + transmission_interface/SimpleTransmission + + 1 + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 3.420 + + + + transmission_interface/SimpleTransmission + + -1 + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 0.638 + + + + transmission_interface/SimpleTransmission + + 1 + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + -0.369 + + + + transmission_interface/SimpleTransmission + + -1 + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 0.832 + + + + transmission_interface/SimpleTransmission + + 1 + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + -1.697 + + + + transmission_interface/SimpleTransmission + + -1 + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 2.02 + + + + transmission_interface/SimpleTransmission + + 1 + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 2.14 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + 1 + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 0.0 + + + + transmission_interface/SimpleTransmission + + 1 + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 0.0 + + + + transmission_interface/SimpleTransmission + + 1 + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 0.0 + + + + + + + + + + + + + + + + diff --git a/engineer_arm_config/config/joint_limits.yaml b/engineer_arm_config/config/joint_limits.yaml index a1ea2d6..ba51a1d 100644 --- a/engineer_arm_config/config/joint_limits.yaml +++ b/engineer_arm_config/config/joint_limits.yaml @@ -8,38 +8,93 @@ default_acceleration_scaling_factor: 0.1 # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] joint_limits: - joint1: + left_joint1: has_velocity_limits: true - max_velocity: 1.80 + max_velocity: 10.46 has_acceleration_limits: true - max_acceleration: 3.5 - joint2: + max_acceleration: 10 + left_joint2: has_velocity_limits: true - max_velocity: 2.75 + max_velocity: 10.46 has_acceleration_limits: true - max_acceleration: 6 - joint3: + max_acceleration: 10 + left_joint3: has_velocity_limits: true - max_velocity: 1.80 + max_velocity: 3.76 has_acceleration_limits: true - max_acceleration: 5 - joint4: + max_acceleration: 10 + left_joint4: has_velocity_limits: true - max_velocity: 19 + max_velocity: 3.76 has_acceleration_limits: true - max_acceleration: 50 - joint5: + max_acceleration: 10 + left_joint5: has_velocity_limits: true - max_velocity: 15 + max_velocity: 12.56 has_acceleration_limits: true - max_acceleration: 20 - joint6: + max_acceleration: 10 + left_joint6: has_velocity_limits: true - max_velocity: 50 + max_velocity: 12.56 has_acceleration_limits: true - max_acceleration: 90 - joint7: + max_acceleration: 10 + left_joint7: has_velocity_limits: true - max_velocity: 50 + max_velocity: 12.56 has_acceleration_limits: true - max_acceleration: 90 + max_acceleration: 10 + right_joint1: + has_velocity_limits: true + max_velocity: 10.46 + has_acceleration_limits: true + max_acceleration: 10 + right_joint2: + has_velocity_limits: true + max_velocity: 10.46 + has_acceleration_limits: true + max_acceleration: 10 + right_joint3: + has_velocity_limits: true + max_velocity: 3.76 + has_acceleration_limits: true + max_acceleration: 10 + right_joint4: + has_velocity_limits: true + max_velocity: 3.76 + has_acceleration_limits: true + max_acceleration: 10 + right_joint5: + has_velocity_limits: true + max_velocity: 10.46 + has_acceleration_limits: true + max_acceleration: 10 + right_joint6: + has_velocity_limits: true + max_velocity: 10.46 + has_acceleration_limits: true + max_acceleration: 10 + right_joint7: + has_velocity_limits: true + max_velocity: 10.46 + has_acceleration_limits: true + max_acceleration: 10 + main_pitch: + has_velocity_limits: true + max_velocity: 10.46 + has_acceleration_limits: true + max_acceleration: 10 + gimbal_joint1: + has_velocity_limits: true + max_velocity: 3.76 + has_acceleration_limits: true + max_acceleration: 10 + gimbal_joint2: + has_velocity_limits: true + max_velocity: 10.46 + has_acceleration_limits: true + max_acceleration: 10 + gimbal_joint3: + has_velocity_limits: true + max_velocity: 10.46 + has_acceleration_limits: true + max_acceleration: 10 \ No newline at end of file diff --git a/engineer_arm_config/config/kinematics.yaml b/engineer_arm_config/config/kinematics.yaml index b298b37..fc7bb29 100644 --- a/engineer_arm_config/config/kinematics.yaml +++ b/engineer_arm_config/config/kinematics.yaml @@ -1,3 +1,21 @@ -engineer_arm: +left_arm: kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin + kinematics_solver_search_resolution: 0.005 kinematics_solver_timeout: 0.005 + goal_joint_tolerance: 0.001 + goal_position_tolerance: 0.001 + goal_orientation_tolerance: 0.01 +right_arm: + kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.005 + goal_joint_tolerance: 0.001 + goal_position_tolerance: 0.001 + goal_orientation_tolerance: 0.01 +gimbal_arm: + kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.005 + goal_joint_tolerance: 0.0001 + goal_position_tolerance: 0.0001 + goal_orientation_tolerance: 0.001 \ No newline at end of file diff --git a/engineer_arm_config/config/ompl_planning.yaml b/engineer_arm_config/config/ompl_planning.yaml index 8e69e09..6af2b90 100644 --- a/engineer_arm_config/config/ompl_planning.yaml +++ b/engineer_arm_config/config/ompl_planning.yaml @@ -3,8 +3,8 @@ planner_configs: type: geometric::AnytimePathShortening shortcut: true # Attempt to shortcut all new solution paths hybridize: true # Compute hybrid solution trajectories - max_hybrid_paths: 40 # Number of hybrid paths generated per iteration - num_planners: 20 # The number of default planners to use for planning + max_hybrid_paths: 24 # Number of hybrid paths generated per iteration + num_planners: 4 # The number of default planners to use for planning 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]" SBL: type: geometric::SBL @@ -51,8 +51,8 @@ planner_configs: 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 + frontier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() + frontier_node_ratio: 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() PRM: type: geometric::PRM @@ -95,8 +95,8 @@ planner_configs: 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 + frontier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() + frontier_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 LBTRRT: type: geometric::LBTRRT @@ -127,7 +127,44 @@ planner_configs: 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 -engineer_arm: + AITstar: + type: geometric::AITstar + use_k_nearest: 1 # whether to use a k-nearest RGG connection model (1) or an r-disc model (0). Default: 1 + rewire_factor: 1.001 # rewire factor of the RGG. Valid values: [1.0:0.01:3.0]. Default: 1.001 + samples_per_batch: 100 # batch size. Valid values: [1:1:1000]. Default: 100 + use_graph_pruning: 1 # enable graph pruning (1) or not (0). Default: 1 + find_approximate_solutions: 0 # track approximate solutions (1) or not (0). Default: 0 + set_max_num_goals: 1 # maximum number of goals sampled from sampleable goal regions. Valid values: [1:1:1000]. Default: 1 + ABITstar: + type: geometric::ABITstar + use_k_nearest: 1 # whether to use a k-nearest RGG connection model (1) or an r-disc model (0). Default: 1 + rewire_factor: 1.001 # rewire factor of the RGG. Valid values: [1.0:0.01:3.0]. Default: 1.001 + samples_per_batch: 100 # batch size. Valid values: [1:1:1000]. Default: 100 + use_graph_pruning: 1 # enable graph pruning (1) or not (0). Default: 1 + prune_threshold_as_fractional_cost_change: 0.1 # fractional change in the solution cost AND problem measure necessary for pruning to occur. Default: 0.1 + delay_rewiring_to_first_solution: 0 # delay (1) or not (0) rewiring until a solution is found. Default: 0 + use_just_in_time_sampling: 0 # delay the generation of samples until they are * necessary. Only works with r-disc connection and path length minimization. Default: 0 + drop_unconnected_samples_on_prune: 0 # drop unconnected samples when pruning, regardless of their heuristic value. Default: 0 + stop_on_each_solution_improvement: 0 # stop the planner each time a solution improvement is found. Useful for debugging. Default: 0 + use_strict_queue_ordering: 0 # sort edges in the queue at the end of the batch (0) or after each rewiring (1). Default: 0 + find_approximate_solutions: 0 # track approximate solutions (1) or not (0). Default: 0 + initial_inflation_factor: 1000000 # inflation factor for the initial search. Valid values: [1.0:0.01:1000000.0]. Default: 1000000 + inflation_scaling_parameter: 10 # scaling parameter for the inflation factor update policy. Valid values: [1.0:0.01:1000000.0]. Default: 0 + truncation_scaling_parameter: 5.0 # scaling parameter for the truncation factor update policy. Valid values: [1.0:0.01:1000000.0]. Default: 0 + BITstar: + type: geometric::BITstar + use_k_nearest: 1 # whether to use a k-nearest RGG connection model (1) or an r-disc model (0). Default: 1 + rewire_factor: 1.001 # rewire factor of the RGG. Valid values: [1.0:0.01:3.0]. Default: 1.001 + samples_per_batch: 100 # batch size. Valid values: [1:1:1000]. Default: 100 + use_graph_pruning: 1 # enable graph pruning (1) or not (0). Default: 1 + prune_threshold_as_fractional_cost_change: 0.1 # fractional change in the solution cost AND problem measure necessary for pruning to occur. Default: 0.1 + delay_rewiring_to_first_solution: 0 # delay (1) or not (0) rewiring until a solution is found. Default: 0 + use_just_in_time_sampling: 0 # delay the generation of samples until they are * necessary. Only works with r-disc connection and path length minimization. Default: 0 + drop_unconnected_samples_on_prune: 0 # drop unconnected samples when pruning, regardless of their heuristic value. Default: 0 + stop_on_each_solution_improvement: 0 # stop the planner each time a solution improvement is found. Useful for debugging. Default: 0 + use_strict_queue_ordering: 0 # sort edges in the queue at the end of the batch (0) or after each rewiring (1). Default: 0 + find_approximate_solutions: 0 # track approximate solutions (1) or not (0). Default: 0 +left_arm: default_planner_config: RRTConnect planner_configs: - AnytimePathShortening @@ -154,5 +191,102 @@ engineer_arm: - LazyPRMstar - SPARS - SPARStwo - projection_evaluator: joints(joint1,joint2) + - AITstar + - ABITstar + - BITstar + projection_evaluator: joints(left_joint1,left_joint2) + longest_valid_segment_fraction: 0.005 +right_arm: + default_planner_config: RRTConnect + planner_configs: + - AnytimePathShortening + - SBL + - EST + - LBKPIECE + - BKPIECE + - KPIECE + - RRT + - RRTConnect + - RRTstar + - TRRT + - PRM + - PRMstar + - FMT + - BFMT + - PDST + - STRIDE + - BiTRRT + - LBTRRT + - BiEST + - ProjEST + - LazyPRM + - LazyPRMstar + - SPARS + - SPARStwo + - AITstar + - ABITstar + - BITstar + projection_evaluator: joints(right_joint1,right_joint2) + longest_valid_segment_fraction: 0.005 +gimbal_arm: + planner_configs: + - AnytimePathShortening + - SBL + - EST + - LBKPIECE + - BKPIECE + - KPIECE + - RRT + - RRTConnect + - RRTstar + - TRRT + - PRM + - PRMstar + - FMT + - BFMT + - PDST + - STRIDE + - BiTRRT + - LBTRRT + - BiEST + - ProjEST + - LazyPRM + - LazyPRMstar + - SPARS + - SPARStwo + - AITstar + - ABITstar + - BITstar + projection_evaluator: joints(main_pitch,gimbal_joint1) + longest_valid_segment_fraction: 0.005 +engineer_arms: + planner_configs: + - AnytimePathShortening + - SBL + - EST + - LBKPIECE + - BKPIECE + - KPIECE + - RRT + - RRTConnect + - RRTstar + - TRRT + - PRM + - PRMstar + - FMT + - BFMT + - PDST + - STRIDE + - BiTRRT + - LBTRRT + - BiEST + - ProjEST + - LazyPRM + - LazyPRMstar + - SPARS + - SPARStwo + - AITstar + - ABITstar + - BITstar + projection_evaluator: joints(main_pitch,gimbal_joint1) longest_valid_segment_fraction: 0.005 diff --git a/engineer_arm_config/config/ros_controllers.yaml b/engineer_arm_config/config/ros_controllers.yaml index 52ad421..6cd8482 100644 --- a/engineer_arm_config/config/ros_controllers.yaml +++ b/engineer_arm_config/config/ros_controllers.yaml @@ -1,33 +1,112 @@ -# Simulation settings for using moveit_sim_controllers -moveit_sim_hw_interface: - joint_model_group: engineer_arm - joint_model_group_pose: home -# Settings for ros_control_boilerplate control loop -generic_hw_control_loop: - loop_hz: 300 - cycle_time_error_threshold: 0.01 -# Settings for ros_control hardware interface -hardware_interface: +left_arm_controller: + type: effort_controllers/JointTrajectoryController joints: - - joint1 - - joint2 - - joint3 - - joint4 - - joint5 - - joint6 - - joint7 - sim_control_mode: 1 # 0: position, 1: velocity - -controller_list: - - name: controllers/arm_trajectory_controller - action_ns: follow_joint_trajectory - default: True - type: FollowJointTrajectory - joints: - - joint1 - - joint2 - - joint3 - - joint4 - - joint5 - - joint6 - - joint7 + - left_joint1 + - left_joint2 + - left_joint3 + - left_joint4 + - left_joint5 + - left_joint6 + - left_joint7 + constraints: + goal_time: 5.0 + stopped_velocity_tolerance: 0.5 + left_joint1: { trajectory: 0.08, goal: 0.05 } + left_joint2: { trajectory: 0.08, goal: 0.05 } + left_joint3: { trajectory: 0.08, goal: 0.05 } + left_joint4: { trajectory: 0.08, goal: 0.05 } + left_joint5: { trajectory: 0.08, goal: 0.05 } + left_joint6: { trajectory: 0.08, goal: 0.05 } + left_joint7: { trajectory: 0.08, goal: 0.05 } + gains: + left_joint1: + p: 1000 + d: 1 + i: 1 + i_clamp: 1 + left_joint2: + p: 1000 + d: 1 + i: 1 + i_clamp: 1 + left_joint3: + p: 100 + d: 1 + i: 1 + i_clamp: 1 + left_joint4: + p: 100 + d: 1 + i: 1 + i_clamp: 1 + left_joint5: + p: 100 + d: 1 + i: 0.5 + i_clamp: 1 + left_joint6: + p: 50 + d: 1 + i: 0.5 + i_clamp: 1 + left_joint7: + p: 50 + d: 1 + i: 0.5 + i_clamp: 1 +right_arm_controller: + type: effort_controllers/JointTrajectoryController + joints: + - right_joint1 + - right_joint2 + - right_joint3 + - right_joint4 + - right_joint5 + - right_joint6 + - right_joint7 + constraints: + goal_time: 5.0 + stopped_velocity_tolerance: 0.05 + right_joint1: { trajectory: 0.08, goal: 0.05 } + right_joint2: { trajectory: 0.08, goal: 0.05 } + right_joint3: { trajectory: 0.08, goal: 0.05 } + right_joint4: { trajectory: 0.08, goal: 0.05 } + right_joint5: { trajectory: 0.08, goal: 0.05 } + right_joint6: { trajectory: 0.08, goal: 0.05 } + right_joint7: { trajectory: 0.08, goal: 0.05 } + gains: + right_joint1: + p: 1000 + d: 1 + i: 1 + i_clamp: 1 + right_joint2: + p: 1000 + d: 1 + i: 1 + i_clamp: 1 + right_joint3: + p: 100 + d: 1 + i: 1 + i_clamp: 1 + right_joint4: + p: 100 + d: 1 + i: 1 + i_clamp: 1 + right_joint5: + p: 100 + d: 1 + i: 1 + i_clamp: 1 + right_joint6: + p: 100 + d: 1 + i: 1 + i_clamp: 1 + right_joint7: + p: 100 + d: 1 + i: 1 + i_clamp: 1 \ No newline at end of file diff --git a/engineer_arm_config/config/sensors_3d.yaml b/engineer_arm_config/config/sensors_3d.yaml index add9f47..51010a3 100644 --- a/engineer_arm_config/config/sensors_3d.yaml +++ b/engineer_arm_config/config/sensors_3d.yaml @@ -1,2 +1,2 @@ sensors: - - { } + [] \ No newline at end of file diff --git a/engineer_arm_config/config/simple_moveit_controllers.yaml b/engineer_arm_config/config/simple_moveit_controllers.yaml new file mode 100644 index 0000000..da7a627 --- /dev/null +++ b/engineer_arm_config/config/simple_moveit_controllers.yaml @@ -0,0 +1,25 @@ +controller_list: + - name: left_arm_controller + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + default: True + joints: + - left_joint1 + - left_joint2 + - left_joint3 + - left_joint4 + - left_joint5 + - left_joint6 + - left_joint7 + - name: right_arm_controller + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + default: True + joints: + - right_joint1 + - right_joint2 + - right_joint3 + - right_joint4 + - right_joint5 + - right_joint6 + - right_joint7 \ No newline at end of file diff --git a/engineer_arm_config/config/stomp_planning.yaml b/engineer_arm_config/config/stomp_planning.yaml new file mode 100644 index 0000000..1971dee --- /dev/null +++ b/engineer_arm_config/config/stomp_planning.yaml @@ -0,0 +1,156 @@ +stomp/left_arm: + group_name: left_arm + optimization: + num_timesteps: 60 + num_iterations: 40 + num_iterations_after_valid: 0 + num_rollouts: 30 + max_rollouts: 30 + initialization_method: 1 # [1 : LINEAR_INTERPOLATION, 2 : CUBIC_POLYNOMIAL, 3 : MININUM_CONTROL_COST] + control_cost_weight: 0.0 + task: + noise_generator: + - class: stomp_moveit/NormalDistributionSampling + stddev: [0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05] + cost_functions: + - class: stomp_moveit/CollisionCheck + collision_penalty: 1.0 + cost_weight: 1.0 + kernel_window_percentage: 0.2 + longest_valid_joint_move: 0.05 + noisy_filters: + - class: stomp_moveit/JointLimits + lock_start: True + lock_goal: True + - class: stomp_moveit/MultiTrajectoryVisualization + line_width: 0.02 + rgb: [255, 255, 0] + marker_array_topic: stomp_trajectories + marker_namespace: noisy + update_filters: + - class: stomp_moveit/PolynomialSmoother + poly_order: 6 + - class: stomp_moveit/TrajectoryVisualization + line_width: 0.05 + rgb: [0, 191, 255] + error_rgb: [255, 0, 0] + publish_intermediate: True + marker_topic: stomp_trajectory + marker_namespace: optimized +stomp/right_arm: + group_name: right_arm + optimization: + num_timesteps: 60 + num_iterations: 40 + num_iterations_after_valid: 0 + num_rollouts: 30 + max_rollouts: 30 + initialization_method: 1 # [1 : LINEAR_INTERPOLATION, 2 : CUBIC_POLYNOMIAL, 3 : MININUM_CONTROL_COST] + control_cost_weight: 0.0 + task: + noise_generator: + - class: stomp_moveit/NormalDistributionSampling + stddev: [0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05] + cost_functions: + - class: stomp_moveit/CollisionCheck + collision_penalty: 1.0 + cost_weight: 1.0 + kernel_window_percentage: 0.2 + longest_valid_joint_move: 0.05 + noisy_filters: + - class: stomp_moveit/JointLimits + lock_start: True + lock_goal: True + - class: stomp_moveit/MultiTrajectoryVisualization + line_width: 0.02 + rgb: [255, 255, 0] + marker_array_topic: stomp_trajectories + marker_namespace: noisy + update_filters: + - class: stomp_moveit/PolynomialSmoother + poly_order: 6 + - class: stomp_moveit/TrajectoryVisualization + line_width: 0.05 + rgb: [0, 191, 255] + error_rgb: [255, 0, 0] + publish_intermediate: True + marker_topic: stomp_trajectory + marker_namespace: optimized +stomp/gimbal_arm: + group_name: gimbal_arm + optimization: + num_timesteps: 60 + num_iterations: 40 + num_iterations_after_valid: 0 + num_rollouts: 30 + max_rollouts: 30 + initialization_method: 1 # [1 : LINEAR_INTERPOLATION, 2 : CUBIC_POLYNOMIAL, 3 : MININUM_CONTROL_COST] + control_cost_weight: 0.0 + task: + noise_generator: + - class: stomp_moveit/NormalDistributionSampling + stddev: [0.05, 0.05, 0.05, 0.05] + cost_functions: + - class: stomp_moveit/CollisionCheck + collision_penalty: 1.0 + cost_weight: 1.0 + kernel_window_percentage: 0.2 + longest_valid_joint_move: 0.05 + noisy_filters: + - class: stomp_moveit/JointLimits + lock_start: True + lock_goal: True + - class: stomp_moveit/MultiTrajectoryVisualization + line_width: 0.02 + rgb: [255, 255, 0] + marker_array_topic: stomp_trajectories + marker_namespace: noisy + update_filters: + - class: stomp_moveit/PolynomialSmoother + poly_order: 6 + - class: stomp_moveit/TrajectoryVisualization + line_width: 0.05 + rgb: [0, 191, 255] + error_rgb: [255, 0, 0] + publish_intermediate: True + marker_topic: stomp_trajectory + marker_namespace: optimized +stomp/engineer_arms: + group_name: engineer_arms + optimization: + num_timesteps: 60 + num_iterations: 40 + num_iterations_after_valid: 0 + num_rollouts: 30 + max_rollouts: 30 + initialization_method: 1 # [1 : LINEAR_INTERPOLATION, 2 : CUBIC_POLYNOMIAL, 3 : MININUM_CONTROL_COST] + control_cost_weight: 0.0 + task: + noise_generator: + - class: stomp_moveit/NormalDistributionSampling + stddev: [0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05] + cost_functions: + - class: stomp_moveit/CollisionCheck + collision_penalty: 1.0 + cost_weight: 1.0 + kernel_window_percentage: 0.2 + longest_valid_joint_move: 0.05 + noisy_filters: + - class: stomp_moveit/JointLimits + lock_start: True + lock_goal: True + - class: stomp_moveit/MultiTrajectoryVisualization + line_width: 0.02 + rgb: [255, 255, 0] + marker_array_topic: stomp_trajectories + marker_namespace: noisy + update_filters: + - class: stomp_moveit/PolynomialSmoother + poly_order: 6 + - class: stomp_moveit/TrajectoryVisualization + line_width: 0.05 + rgb: [0, 191, 255] + error_rgb: [255, 0, 0] + publish_intermediate: True + marker_topic: stomp_trajectory + marker_namespace: optimized \ No newline at end of file diff --git a/engineer_arm_config/launch/chomp_planning_pipeline.launch.xml b/engineer_arm_config/launch/chomp_planning_pipeline.launch.xml index 0863402..83f7dcb 100644 --- a/engineer_arm_config/launch/chomp_planning_pipeline.launch.xml +++ b/engineer_arm_config/launch/chomp_planning_pipeline.launch.xml @@ -1,23 +1,21 @@ - - - - + + + + /> - - - - + + + + - - - - + diff --git a/engineer_arm_config/launch/default_warehouse_db.launch b/engineer_arm_config/launch/default_warehouse_db.launch new file mode 100644 index 0000000..41bdc13 --- /dev/null +++ b/engineer_arm_config/launch/default_warehouse_db.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/engineer_arm_config/launch/demo.launch b/engineer_arm_config/launch/demo.launch new file mode 100644 index 0000000..1f3d99a --- /dev/null +++ b/engineer_arm_config/launch/demo.launch @@ -0,0 +1,66 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + [move_group/fake_controller_joint_states] + + + + [move_group/fake_controller_joint_states] + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/engineer_arm_config/launch/demo_gazebo.launch b/engineer_arm_config/launch/demo_gazebo.launch new file mode 100644 index 0000000..0ef8f95 --- /dev/null +++ b/engineer_arm_config/launch/demo_gazebo.launch @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/engineer_arm_config/launch/empty_world.launch b/engineer_arm_config/launch/empty_world.launch new file mode 100644 index 0000000..3b8b37b --- /dev/null +++ b/engineer_arm_config/launch/empty_world.launch @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/engineer_arm_config/launch/engineer_moveit_controller_manager.launch.xml b/engineer_arm_config/launch/engineer_moveit_controller_manager.launch.xml index bbb90dc..65af328 100644 --- a/engineer_arm_config/launch/engineer_moveit_controller_manager.launch.xml +++ b/engineer_arm_config/launch/engineer_moveit_controller_manager.launch.xml @@ -1,10 +1,7 @@ + + - - - - - - - + + + \ No newline at end of file diff --git a/engineer_arm_config/launch/fake_moveit_controller_manager.launch.xml b/engineer_arm_config/launch/fake_moveit_controller_manager.launch.xml index 62baeea..f160d13 100644 --- a/engineer_arm_config/launch/fake_moveit_controller_manager.launch.xml +++ b/engineer_arm_config/launch/fake_moveit_controller_manager.launch.xml @@ -1,9 +1,12 @@ + + + - + diff --git a/engineer_arm_config/launch/gazebo.launch b/engineer_arm_config/launch/gazebo.launch new file mode 100644 index 0000000..429bca7 --- /dev/null +++ b/engineer_arm_config/launch/gazebo.launch @@ -0,0 +1,34 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/engineer_arm_config/launch/joystick_control.launch b/engineer_arm_config/launch/joystick_control.launch new file mode 100644 index 0000000..9411f6e --- /dev/null +++ b/engineer_arm_config/launch/joystick_control.launch @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/engineer_arm_config/launch/load_controllers.launch b/engineer_arm_config/launch/load_controllers.launch index 39c6842..4a592bd 100644 --- a/engineer_arm_config/launch/load_controllers.launch +++ b/engineer_arm_config/launch/load_controllers.launch @@ -20,27 +20,21 @@ args="load controllers/robot_state_controller controllers/joint_state_controller - controllers/joint1_calibration_controller - controllers/joint2_calibration_controller - controllers/joint3_calibration_controller - controllers/joint4_calibration_controller - controllers/joint5_calibration_controller - controllers/joint6_calibration_controller - controllers/joint7_calibration_controller - controllers/arm_trajectory_controller + controllers/left_arm_controller + controllers/right_arm_controller + controllers/gimbal_arm_controller + controllers/chassis_controller + " + /> + + diff --git a/engineer_arm_config/launch/load_move_group.launch b/engineer_arm_config/launch/load_move_group.launch index 2f9b1f7..f1edc8c 100644 --- a/engineer_arm_config/launch/load_move_group.launch +++ b/engineer_arm_config/launch/load_move_group.launch @@ -9,7 +9,7 @@ - + diff --git a/engineer_arm_config/launch/move_group.launch b/engineer_arm_config/launch/move_group.launch index 07af4a0..19bace8 100644 --- a/engineer_arm_config/launch/move_group.launch +++ b/engineer_arm_config/launch/move_group.launch @@ -1,65 +1,105 @@ - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - + + + - - - - - + + + - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/engineer_arm_config/launch/moveit.rviz b/engineer_arm_config/launch/moveit.rviz index a0a4838..6a876dd 100644 --- a/engineer_arm_config/launch/moveit.rviz +++ b/engineer_arm_config/launch/moveit.rviz @@ -5,13 +5,8 @@ Panels: Property Tree Widget: Expanded: - /MotionPlanning1 - - /MotionPlanning1/Planning Request1 - - /MotionPlanning1/Planning Metrics1 - - /MotionPlanning1/Planned Path1 - - /TF1 - - /TF1/Frames1 Splitter Ratio: 0.5 - Tree Height: 315 + Tree Height: 226 - Class: rviz/Help Name: Help - Class: rviz/Views @@ -19,10 +14,6 @@ Panels: - /Current View1 Name: Views Splitter Ratio: 0.5 -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 Visualization Manager: Class: "" Displays: @@ -32,7 +23,7 @@ Visualization Manager: Color: 160; 160; 164 Enabled: true Line Style: - Line Width: 0.029999999329447746 + Line Width: 0.03 Value: Lines Name: Grid Normal Cell Count: 0 @@ -44,177 +35,30 @@ Visualization Manager: Plane Cell Count: 10 Reference Frame: Value: true - - Acceleration_Scaling_Factor: 0.1 - Class: moveit_rviz_plugin/MotionPlanning + - Class: moveit_rviz_plugin/MotionPlanning Enabled: true - Move Group Namespace: "" - MoveIt_Allow_Approximate_IK: false - MoveIt_Allow_External_Program: false - MoveIt_Allow_Replanning: false - MoveIt_Allow_Sensor_Positioning: false - MoveIt_Planning_Attempts: 10 - MoveIt_Planning_Time: 5 - MoveIt_Use_Cartesian_Path: false - MoveIt_Use_Constraint_Aware_IK: false - MoveIt_Workspace: - Center: - X: 0 - Y: 0 - Z: 0 - Size: - X: 2 - Y: 2 - Z: 2 Name: MotionPlanning Planned Path: - Color Enabled: false - Interrupt Display: false - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - camera_link: - Alpha: 1 - Show Axes: false - Show Trail: false - camera_optical_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - extend_arm_back: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - extend_arm_front: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - gimbal_lifter: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - left_back_wheel: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - left_front_wheel: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link1: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link2: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link3: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link4: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link5: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link6: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link7: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - mimic_link1: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - ore_bin_lifter: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - ore_bin_rotator: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - pitch: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - right_back_wheel: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - right_front_wheel: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - tools_link: - Alpha: 1 - Show Axes: false - Show Trail: false - tools_link_st: - Alpha: 1 - Show Axes: false - Show Trail: false - yaw: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - Loop Animation: false + Links: ~ + Loop Animation: true Robot Alpha: 0.5 - Robot Color: 150; 50; 150 Show Robot Collision: false Show Robot Visual: true Show Trail: false State Display Time: 0.05 s - Trail Step Size: 1 Trajectory Topic: move_group/display_planned_path - Use Sim Time: false Planning Metrics: Payload: 1 Show Joint Torques: false Show Manipulability: false Show Manipulability Index: false Show Weight Limit: false - TextHeight: 0.07999999821186066 Planning Request: Colliding Link Color: 255; 0; 0 Goal State Alpha: 1 Goal State Color: 250; 128; 0 Interactive Marker Size: 0 Joint Violation Color: 255; 0; 255 - Planning Group: engineer_arm Query Goal State: true Query Start State: false Show Workspace: false @@ -224,272 +68,19 @@ Visualization Manager: Robot Description: robot_description Scene Geometry: Scene Alpha: 1 - Scene Color: 50; 230; 50 - Scene Display Time: 0.009999999776482582 Show Scene Geometry: true Voxel Coloring: Z-Axis Voxel Rendering: Occupied Voxels Scene Robot: Attached Body Color: 150; 50; 150 - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - camera_link: - Alpha: 1 - Show Axes: false - Show Trail: false - camera_optical_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - extend_arm_back: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - extend_arm_front: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - gimbal_lifter: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - left_back_wheel: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - left_front_wheel: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link1: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link2: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link3: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link4: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link5: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link6: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link7: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - mimic_link1: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - ore_bin_lifter: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - ore_bin_rotator: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - pitch: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - right_back_wheel: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - right_front_wheel: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - tools_link: - Alpha: 1 - Show Axes: false - Show Trail: false - tools_link_st: - Alpha: 1 - Show Axes: false - Show Trail: false - yaw: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true + Links: ~ Robot Alpha: 0.5 - Show Robot Collision: false - Show Robot Visual: true - Value: true - Velocity_Scaling_Factor: 0.1 - - Class: rviz/TF - Enabled: true - Filter (blacklist): "" - Filter (whitelist): "" - Frame Timeout: 15 - Frames: - All Enabled: false - base_link: - Value: true - camera_link: - Value: false - camera_optical_frame: - Value: false - exchanger: - Value: true - extend_arm_back: - Value: false - extend_arm_front: - Value: false - gimbal_lifter: - Value: false - left_back_wheel: - Value: false - left_front_wheel: - Value: false - link1: - Value: false - link2: - Value: false - link3: - Value: false - link4: - Value: false - link5: - Value: false - link6: - Value: false - link7: - Value: true - map: - Value: false - mimic_link1: - Value: false - odom: - Value: false - ore_bin_lifter: - Value: false - ore_bin_rotator: - Value: false - pitch: - Value: false - real_world: - Value: false - right_back_wheel: - Value: false - right_front_wheel: - Value: false - tools_link: - Value: false - tools_link_st: - Value: false - yaw: - Value: false - Marker Alpha: 1 - Marker Scale: 1 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: true - Tree: - base_link: - {} - exchanger: - {} - extend_arm_back: - {} - extend_arm_front: - {} - gimbal_lifter: - camera_link: - {} - camera_optical_frame: - {} - left_back_wheel: - {} - left_front_wheel: - {} - link1: - {} - link2: - {} - link3: - {} - link4: - {} - link5: - {} - link6: - {} - link7: - tools_link: - {} - tools_link_st: - {} - map: - {} - mimic_link1: - {} - odom: - {} - ore_bin_lifter: - {} - ore_bin_rotator: - {} - pitch: - {} - real_world: - {} - right_back_wheel: - {} - right_front_wheel: - {} - yaw: - {} - Update Interval: 0 + Show Scene Robot: true Value: true Enabled: true Global Options: Background Color: 48; 48; 48 - Default Light: true Fixed Frame: base_link - Frame Rate: 30 Name: root Tools: - Class: rviz/Interact @@ -500,30 +91,30 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 1.4108070135116577 + Distance: 2.0 Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 + Stereo Eye Separation: 0.06 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Field of View: 0.75 Focal Point: - X: 0.23874804377555847 - Y: 0.04174364358186722 - Z: 0.6224369406700134 + X: -0.1 + Y: 0.25 + Z: 0.30 Focal Shape Fixed Size: true - Focal Shape Size: 0.05000000074505806 + Focal Shape Size: 0.05 Invert Z Axis: false Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.48979732394218445 + Near Clip Distance: 0.01 + Pitch: 0.5 Target Frame: base_link - Yaw: 3.2827558517456055 + Yaw: -0.6232355833053589 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 1016 + Height: 848 Help: collapsed: false Hide Left Dock: false @@ -532,9 +123,9 @@ Window Geometry: collapsed: false MotionPlanning - Trajectory Slider: collapsed: false - QMainWindow State: 000000ff00000000fd0000000100000000000001f30000039efc0200000007fb000000100044006900730070006c006100790073010000003d000001cc000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000020f000001cc0000017d00ffffff000004880000039e00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000100000000000001f0000002f6fc0200000007fb000000100044006900730070006c006100790073010000003d00000173000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000001b60000017d0000017d00ffffff00000315000002f600000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Views: collapsed: false - Width: 1665 - X: 757 - Y: 27 + Width: 1291 + X: 454 + Y: 25 diff --git a/engineer_arm_config/launch/moveit_rviz.launch b/engineer_arm_config/launch/moveit_rviz.launch new file mode 100644 index 0000000..a4605c0 --- /dev/null +++ b/engineer_arm_config/launch/moveit_rviz.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + diff --git a/engineer_arm_config/launch/ompl_planning_pipeline.launch.xml b/engineer_arm_config/launch/ompl_planning_pipeline.launch.xml index 7578dd5..e923462 100644 --- a/engineer_arm_config/launch/ompl_planning_pipeline.launch.xml +++ b/engineer_arm_config/launch/ompl_planning_pipeline.launch.xml @@ -1,25 +1,24 @@ - - + + - - + + - + + + + - - - - - + diff --git a/engineer_arm_config/launch/planning_context.launch b/engineer_arm_config/launch/planning_context.launch index 81bbf7d..cf9d2c8 100644 --- a/engineer_arm_config/launch/planning_context.launch +++ b/engineer_arm_config/launch/planning_context.launch @@ -1,25 +1,26 @@ - - + + - - + + - - + + - - + + - - - - + + + + + - - - - + + + + + diff --git a/engineer_arm_config/launch/planning_pipeline.launch.xml b/engineer_arm_config/launch/planning_pipeline.launch.xml index 3689d56..4b4d0d6 100644 --- a/engineer_arm_config/launch/planning_pipeline.launch.xml +++ b/engineer_arm_config/launch/planning_pipeline.launch.xml @@ -1,10 +1,10 @@ - + - + - + diff --git a/engineer_arm_config/launch/ros_control_moveit_controller_manager.launch.xml b/engineer_arm_config/launch/ros_control_moveit_controller_manager.launch.xml new file mode 100644 index 0000000..9ebc91c --- /dev/null +++ b/engineer_arm_config/launch/ros_control_moveit_controller_manager.launch.xml @@ -0,0 +1,4 @@ + + + + diff --git a/engineer_arm_config/launch/ros_controllers.launch b/engineer_arm_config/launch/ros_controllers.launch new file mode 100644 index 0000000..aad62da --- /dev/null +++ b/engineer_arm_config/launch/ros_controllers.launch @@ -0,0 +1,11 @@ + + + + + + + + + + diff --git a/engineer_arm_config/launch/run_benchmark_ompl.launch b/engineer_arm_config/launch/run_benchmark_ompl.launch new file mode 100644 index 0000000..0914c6c --- /dev/null +++ b/engineer_arm_config/launch/run_benchmark_ompl.launch @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/engineer_arm_config/launch/sensor_manager.launch.xml b/engineer_arm_config/launch/sensor_manager.launch.xml new file mode 100644 index 0000000..5f635ad --- /dev/null +++ b/engineer_arm_config/launch/sensor_manager.launch.xml @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/engineer_arm_config/launch/setup_assistant.launch b/engineer_arm_config/launch/setup_assistant.launch new file mode 100644 index 0000000..dae2d15 --- /dev/null +++ b/engineer_arm_config/launch/setup_assistant.launch @@ -0,0 +1,16 @@ + + + + + + + + + + + + diff --git a/engineer_arm_config/launch/simple_moveit_controller_manager.launch.xml b/engineer_arm_config/launch/simple_moveit_controller_manager.launch.xml new file mode 100644 index 0000000..321b88f --- /dev/null +++ b/engineer_arm_config/launch/simple_moveit_controller_manager.launch.xml @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/engineer_arm_config/launch/stomp_planning_pipeline.launch.xml b/engineer_arm_config/launch/stomp_planning_pipeline.launch.xml new file mode 100644 index 0000000..50fb33a --- /dev/null +++ b/engineer_arm_config/launch/stomp_planning_pipeline.launch.xml @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + diff --git a/engineer_arm_config/launch/trajectory_execution.launch.xml b/engineer_arm_config/launch/trajectory_execution.launch.xml index a8c0ac0..0fff921 100644 --- a/engineer_arm_config/launch/trajectory_execution.launch.xml +++ b/engineer_arm_config/launch/trajectory_execution.launch.xml @@ -1,21 +1,23 @@ + - + + + - - - + + + - - - - - - + + + + + + - - - + + diff --git a/engineer_arm_config/launch/warehouse.launch b/engineer_arm_config/launch/warehouse.launch new file mode 100644 index 0000000..0712e67 --- /dev/null +++ b/engineer_arm_config/launch/warehouse.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/engineer_arm_config/launch/warehouse_settings.launch.xml b/engineer_arm_config/launch/warehouse_settings.launch.xml new file mode 100644 index 0000000..e473b08 --- /dev/null +++ b/engineer_arm_config/launch/warehouse_settings.launch.xml @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/engineer_arm_config/package.xml b/engineer_arm_config/package.xml index eb4cd9c..dc51f38 100644 --- a/engineer_arm_config/package.xml +++ b/engineer_arm_config/package.xml @@ -1,27 +1,41 @@ - engineer_arm_config - 1.0.0 - - An automatically generated package with all the configuration and launch files for using the engineer with the - MoveIt Motion Planning Framework - - QiayuanLiao - QiayuanLiao - BSD + engineer_arm_config + 0.3.0 + + An automatically generated package with all the configuration and launch files for using the engineer with the MoveIt Motion Planning Framework + + CH + CH - http://moveit.ros.org/ - https://github.com/ros-planning/moveit/issues - https://github.com/ros-planning/moveit + BSD + + http://moveit.ros.org/ + https://github.com/moveit/moveit/issues + https://github.com/moveit/moveit + + catkin + + moveit_ros_move_group + moveit_fake_controller_manager + moveit_kinematics + moveit_planners + moveit_ros_visualization + moveit_setup_assistant + moveit_simple_controller_manager + joint_state_publisher + joint_state_publisher_gui + robot_state_publisher + rviz + tf2_ros + xacro + + + + + + rm_description - catkin - moveit_ros_move_group - moveit_fake_controller_manager - moveit_kinematics - moveit_planners_ompl - moveit_ros_visualization - moveit_setup_assistant - tf2_ros - xacro diff --git a/engineer_custom_controller/CMakeLists.txt b/engineer_custom_controller/CMakeLists.txt new file mode 100644 index 0000000..36816f1 --- /dev/null +++ b/engineer_custom_controller/CMakeLists.txt @@ -0,0 +1,118 @@ +cmake_minimum_required(VERSION 3.5.0) +project(engineer_custom_controller) + +## Use C++14 +set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +## By adding -Wall and -Werror, the compiler does not ignore warnings anymore, +## enforcing cleaner code. +add_definitions(-Wall -Werror -Wno-address-of-packed-member) + +## Find catkin macros and libraries +find_package(serial REQUIRED) + +find_package(catkin REQUIRED + COMPONENTS + sensor_msgs + roscpp + rm_msgs + rm_ecat_msgs + serial + rm_common + tf2_geometry_msgs + std_msgs + actionlib + nav_msgs +) + +## Find system libraries +#find_package(Eigen3 REQUIRED) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( + INCLUDE_DIRS + include + LIBRARIES + CATKIN_DEPENDS + roscpp + sensor_msgs + rm_msgs + rm_common + tf2_geometry_msgs + std_msgs + actionlib + nav_msgs + DEPENDS +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + +## Declare cpp executables +FILE(GLOB ALL_SOURCES "src/*.cpp") +add_executable(${PROJECT_NAME} ${ALL_SOURCES}) + +## Add dependencies to exported targets, like ROS msgs or srvs +add_dependencies(${PROJECT_NAME} + ${catkin_EXPORTED_TARGETS} +) + +## Specify libraries to link executable targets against +target_link_libraries(${PROJECT_NAME} + ${catkin_LIBRARIES} +) + +############# +## Install ## +############# + +# 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" +) + +## Mark other files for installation +install( + DIRECTORY launch + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +############# +## Testing ## +############# + +#if (${CATKIN_ENABLE_TESTING}) +# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread") +# ## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test +# test/test_ros_package_template.cpp +# test/AlgorithmTest.cpp) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}_core) +#endif () diff --git a/engineer_custom_controller/config/engineer_custom_controller.yaml b/engineer_custom_controller/config/engineer_custom_controller.yaml new file mode 100644 index 0000000..9923f1f --- /dev/null +++ b/engineer_custom_controller/config/engineer_custom_controller.yaml @@ -0,0 +1,18 @@ +engineer_custom_controller: + joint_names: + - left_joint1 + - left_joint2 + - left_joint3 + - left_joint4 + - left_joint5 + - left_joint6 + - left_joint7 + - right_joint1 + - right_joint2 + - right_joint3 + - right_joint4 + - right_joint5 + - right_joint6 + - right_joint7 + maxVal: 12.56 + minVal: -12.56 \ No newline at end of file diff --git a/engineer_custom_controller/include/engineer_custom_controller/common/data.h b/engineer_custom_controller/include/engineer_custom_controller/common/data.h new file mode 100644 index 0000000..e1932ba --- /dev/null +++ b/engineer_custom_controller/include/engineer_custom_controller/common/data.h @@ -0,0 +1,107 @@ +// +// Created by ch on 24-11-23. +// + +#pragma once + +#include +#include +#include +#include +#include "rm_msgs/RobotCustomData.h" +#include "rm_ecat_msgs/RmEcatMitSlaveReadings.h" + +#include "engineer_custom_controller/common/protocol.h" + +namespace engineer_custom_controller +{ +class Base +{ +public: + serial::Serial serial_; + bool engineer_custom_controller_is_online_ = false; + + void initSerial() + { + serial::Timeout timeout = serial::Timeout::simpleTimeout(50); + serial_.setPort("/dev/usbCustomController"); + serial_.setBaudrate(115200); + // serial_.setBaudrate(921600); + serial_.setTimeout(timeout); + if (serial_.isOpen()) + return; + try + { + serial_.open(); + } + catch (serial::IOException& e) + { + ROS_ERROR("Cannot open custom controller port"); + } + } + + // CRC check + uint8_t getCRC8CheckSum(unsigned char* pch_message, unsigned int dw_length, unsigned char uc_crc_8) + { + unsigned char uc_index; + while (dw_length--) + { + uc_index = uc_crc_8 ^ (*pch_message++); + uc_crc_8 = engineer_custom_controller::kCrc8Table[uc_index]; + } + return (uc_crc_8); + } + + uint32_t verifyCRC8CheckSum(unsigned char* pch_message, unsigned int dw_length) + { + unsigned char uc_expected; + if ((pch_message == nullptr) || (dw_length <= 2)) + return 0; + uc_expected = getCRC8CheckSum(pch_message, dw_length - 1, engineer_custom_controller::kCrc8Init); + return (uc_expected == pch_message[dw_length - 1]); + } + + void appendCRC8CheckSum(unsigned char* pch_message, unsigned int dw_length) + { + unsigned char uc_crc; + if ((pch_message == nullptr) || (dw_length <= 2)) + return; + uc_crc = getCRC8CheckSum((unsigned char*)pch_message, dw_length - 1, engineer_custom_controller::kCrc8Init); + pch_message[dw_length - 1] = uc_crc; + } + + uint16_t getCRC16CheckSum(uint8_t* pch_message, uint32_t dw_length, uint16_t w_crc) + { + uint8_t chData; + if (pch_message == nullptr) + return 0xFFFF; + while (dw_length--) + { + chData = *pch_message++; + (w_crc) = (static_cast(w_crc) >> 8) ^ + engineer_custom_controller::wCRC_table[(static_cast(w_crc) ^ static_cast(chData)) & 0x00ff]; + } + return w_crc; + } + + uint32_t verifyCRC16CheckSum(uint8_t* pch_message, uint32_t dw_length) + { + uint16_t w_expected; + if ((pch_message == nullptr) || (dw_length <= 2)) + return 0; + w_expected = getCRC16CheckSum(pch_message, dw_length - 2, engineer_custom_controller::kCrc16Init); + return ((w_expected & 0xff) == pch_message[dw_length - 2] && + ((w_expected >> 8) & 0xff) == pch_message[dw_length - 1]); + } + + void appendCRC16CheckSum(uint8_t* pch_message, uint32_t dw_length) + { + uint16_t wCRC; + if ((pch_message == nullptr) || (dw_length <= 2)) + return; + wCRC = getCRC16CheckSum(static_cast(pch_message), dw_length - 2, engineer_custom_controller::kCrc16Init); + pch_message[dw_length - 2] = static_cast((wCRC & 0x00ff)); + pch_message[dw_length - 1] = static_cast(((wCRC >> 8) & 0x00ff)); + } +}; +} // namespace engineer_custom_controller diff --git a/engineer_custom_controller/include/engineer_custom_controller/common/protocol.h b/engineer_custom_controller/include/engineer_custom_controller/common/protocol.h new file mode 100644 index 0000000..ed1f69b --- /dev/null +++ b/engineer_custom_controller/include/engineer_custom_controller/common/protocol.h @@ -0,0 +1,86 @@ +// +// Created by ch on 24-11-23. +// +#pragma once +#define __packed __attribute__((packed)) + +#include +namespace engineer_custom_controller +{ +typedef enum +{ + CUSTOM_CONTROLLER_CMD = 0x0302, // custom_controller + SIM_KEYBOARD_MOUSE_CMD = 0x0306, + ROBOT_TO_CUSTOM_CONTROLLER_CMD = 0x0309, +} VideoTransmissionCmdId; + +typedef struct +{ + uint8_t sof; + uint16_t data_length; + uint8_t seq; + uint8_t crc_8; +} __packed FrameHeader; + +typedef struct +{ + uint8_t data[30]; + +} __packed CustomControllerData; + +typedef struct +{ + uint16_t key_value; + uint16_t x_position : 12; + uint16_t mouse_left : 4; + uint16_t y_position : 12; + uint16_t mouse_right : 4; + uint16_t reserved; +} __packed SimKeyboardMouseData; + +typedef struct +{ + uint8_t data[30]; +} __packed RobotToCustomData; + +/***********************Frame tail(CRC8_CRC16)********************************************/ +const uint8_t kCrc8Init = 0xff; +const uint8_t kCrc8Table[256] = { + 0x00, 0x5e, 0xbc, 0xe2, 0x61, 0x3f, 0xdd, 0x83, 0xc2, 0x9c, 0x7e, 0x20, 0xa3, 0xfd, 0x1f, 0x41, 0x9d, 0xc3, 0x21, + 0x7f, 0xfc, 0xa2, 0x40, 0x1e, 0x5f, 0x01, 0xe3, 0xbd, 0x3e, 0x60, 0x82, 0xdc, 0x23, 0x7d, 0x9f, 0xc1, 0x42, 0x1c, + 0xfe, 0xa0, 0xe1, 0xbf, 0x5d, 0x03, 0x80, 0xde, 0x3c, 0x62, 0xbe, 0xe0, 0x02, 0x5c, 0xdf, 0x81, 0x63, 0x3d, 0x7c, + 0x22, 0xc0, 0x9e, 0x1d, 0x43, 0xa1, 0xff, 0x46, 0x18, 0xfa, 0xa4, 0x27, 0x79, 0x9b, 0xc5, 0x84, 0xda, 0x38, 0x66, + 0xe5, 0xbb, 0x59, 0x07, 0xdb, 0x85, 0x67, 0x39, 0xba, 0xe4, 0x06, 0x58, 0x19, 0x47, 0xa5, 0xfb, 0x78, 0x26, 0xc4, + 0x9a, 0x65, 0x3b, 0xd9, 0x87, 0x04, 0x5a, 0xb8, 0xe6, 0xa7, 0xf9, 0x1b, 0x45, 0xc6, 0x98, 0x7a, 0x24, 0xf8, 0xa6, + 0x44, 0x1a, 0x99, 0xc7, 0x25, 0x7b, 0x3a, 0x64, 0x86, 0xd8, 0x5b, 0x05, 0xe7, 0xb9, 0x8c, 0xd2, 0x30, 0x6e, 0xed, + 0xb3, 0x51, 0x0f, 0x4e, 0x10, 0xf2, 0xac, 0x2f, 0x71, 0x93, 0xcd, 0x11, 0x4f, 0xad, 0xf3, 0x70, 0x2e, 0xcc, 0x92, + 0xd3, 0x8d, 0x6f, 0x31, 0xb2, 0xec, 0x0e, 0x50, 0xaf, 0xf1, 0x13, 0x4d, 0xce, 0x90, 0x72, 0x2c, 0x6d, 0x33, 0xd1, + 0x8f, 0x0c, 0x52, 0xb0, 0xee, 0x32, 0x6c, 0x8e, 0xd0, 0x53, 0x0d, 0xef, 0xb1, 0xf0, 0xae, 0x4c, 0x12, 0x91, 0xcf, + 0x2d, 0x73, 0xca, 0x94, 0x76, 0x28, 0xab, 0xf5, 0x17, 0x49, 0x08, 0x56, 0xb4, 0xea, 0x69, 0x37, 0xd5, 0x8b, 0x57, + 0x09, 0xeb, 0xb5, 0x36, 0x68, 0x8a, 0xd4, 0x95, 0xcb, 0x29, 0x77, 0xf4, 0xaa, 0x48, 0x16, 0xe9, 0xb7, 0x55, 0x0b, + 0x88, 0xd6, 0x34, 0x6a, 0x2b, 0x75, 0x97, 0xc9, 0x4a, 0x14, 0xf6, 0xa8, 0x74, 0x2a, 0xc8, 0x96, 0x15, 0x4b, 0xa9, + 0xf7, 0xb6, 0xe8, 0x0a, 0x54, 0xd7, 0x89, 0x6b, 0x35, +}; +const uint16_t kCrc16Init = 0xffff; +const uint16_t wCRC_table[256] = { + 0x0000, 0x1189, 0x2312, 0x329b, 0x4624, 0x57ad, 0x6536, 0x74bf, 0x8c48, 0x9dc1, 0xaf5a, 0xbed3, 0xca6c, 0xdbe5, + 0xe97e, 0xf8f7, 0x1081, 0x0108, 0x3393, 0x221a, 0x56a5, 0x472c, 0x75b7, 0x643e, 0x9cc9, 0x8d40, 0xbfdb, 0xae52, + 0xdaed, 0xcb64, 0xf9ff, 0xe876, 0x2102, 0x308b, 0x0210, 0x1399, 0x6726, 0x76af, 0x4434, 0x55bd, 0xad4a, 0xbcc3, + 0x8e58, 0x9fd1, 0xeb6e, 0xfae7, 0xc87c, 0xd9f5, 0x3183, 0x200a, 0x1291, 0x0318, 0x77a7, 0x662e, 0x54b5, 0x453c, + 0xbdcb, 0xac42, 0x9ed9, 0x8f50, 0xfbef, 0xea66, 0xd8fd, 0xc974, 0x4204, 0x538d, 0x6116, 0x709f, 0x0420, 0x15a9, + 0x2732, 0x36bb, 0xce4c, 0xdfc5, 0xed5e, 0xfcd7, 0x8868, 0x99e1, 0xab7a, 0xbaf3, 0x5285, 0x430c, 0x7197, 0x601e, + 0x14a1, 0x0528, 0x37b3, 0x263a, 0xdecd, 0xcf44, 0xfddf, 0xec56, 0x98e9, 0x8960, 0xbbfb, 0xaa72, 0x6306, 0x728f, + 0x4014, 0x519d, 0x2522, 0x34ab, 0x0630, 0x17b9, 0xef4e, 0xfec7, 0xcc5c, 0xddd5, 0xa96a, 0xb8e3, 0x8a78, 0x9bf1, + 0x7387, 0x620e, 0x5095, 0x411c, 0x35a3, 0x242a, 0x16b1, 0x0738, 0xffcf, 0xee46, 0xdcdd, 0xcd54, 0xb9eb, 0xa862, + 0x9af9, 0x8b70, 0x8408, 0x9581, 0xa71a, 0xb693, 0xc22c, 0xd3a5, 0xe13e, 0xf0b7, 0x0840, 0x19c9, 0x2b52, 0x3adb, + 0x4e64, 0x5fed, 0x6d76, 0x7cff, 0x9489, 0x8500, 0xb79b, 0xa612, 0xd2ad, 0xc324, 0xf1bf, 0xe036, 0x18c1, 0x0948, + 0x3bd3, 0x2a5a, 0x5ee5, 0x4f6c, 0x7df7, 0x6c7e, 0xa50a, 0xb483, 0x8618, 0x9791, 0xe32e, 0xf2a7, 0xc03c, 0xd1b5, + 0x2942, 0x38cb, 0x0a50, 0x1bd9, 0x6f66, 0x7eef, 0x4c74, 0x5dfd, 0xb58b, 0xa402, 0x9699, 0x8710, 0xf3af, 0xe226, + 0xd0bd, 0xc134, 0x39c3, 0x284a, 0x1ad1, 0x0b58, 0x7fe7, 0x6e6e, 0x5cf5, 0x4d7c, 0xc60c, 0xd785, 0xe51e, 0xf497, + 0x8028, 0x91a1, 0xa33a, 0xb2b3, 0x4a44, 0x5bcd, 0x6956, 0x78df, 0x0c60, 0x1de9, 0x2f72, 0x3efb, 0xd68d, 0xc704, + 0xf59f, 0xe416, 0x90a9, 0x8120, 0xb3bb, 0xa232, 0x5ac5, 0x4b4c, 0x79d7, 0x685e, 0x1ce1, 0x0d68, 0x3ff3, 0x2e7a, + 0xe70e, 0xf687, 0xc41c, 0xd595, 0xa12a, 0xb0a3, 0x8238, 0x93b1, 0x6b46, 0x7acf, 0x4854, 0x59dd, 0x2d62, 0x3ceb, + 0x0e70, 0x1ff9, 0xf78f, 0xe606, 0xd49d, 0xc514, 0xb1ab, 0xa022, 0x92b9, 0x8330, 0x7bc7, 0x6a4e, 0x58d5, 0x495c, + 0x3de3, 0x2c6a, 0x1ef1, 0x0f78 +}; +} // namespace engineer_custom_controller diff --git a/engineer_custom_controller/include/engineer_custom_controller/custom_controller_data_sender.h b/engineer_custom_controller/include/engineer_custom_controller/custom_controller_data_sender.h new file mode 100644 index 0000000..2274140 --- /dev/null +++ b/engineer_custom_controller/include/engineer_custom_controller/custom_controller_data_sender.h @@ -0,0 +1,58 @@ +// +// Created by ljyi on 2026/4/6. +// + +#pragma once + +#include +#include +#include + +#include "common/data.h" +#include "common/protocol.h" + +namespace engineer_custom_controller +{ +class CustomControllerDataSender +{ +public: + explicit CustomControllerDataSender(ros::NodeHandle& nh, Base& base) : base_(base) + { + ROS_INFO("Custom data sender load."); + joint_names_ = nh.param("joint_names", std::vector()); + maxVal = nh.param("maxVal", 12.56); + minVal = nh.param("minVal", -12.56); + joint_state_sub_ = nh.subscribe("/joint_states", 1, &CustomControllerDataSender::jointStateCallback, this); + ecat_data_sub_ = nh.subscribe("/rm_ecat_hw/mit_readings", 1, &CustomControllerDataSender::ecatDataCallback, this); + } + virtual ~CustomControllerDataSender() = default; + +private: + void jointStateCallback(const sensor_msgs::JointState::ConstPtr& data); + void ecatDataCallback(const rm_ecat_msgs::RmEcatMitSlaveReadings::ConstPtr& data); + void sendCustomControllerData(const sensor_msgs::JointState::ConstPtr& data); + void pack(uint8_t* tx_buffer, uint8_t* data, int cmd_id, int len); + void clearTxBuffer() + { + for (int i = 0; i < k_frame_length_; i++) + tx_buffer_[i] = 0; + tx_len_ = 0; + } + + int16_t rawData2Int16(double value); + + double maxVal, minVal; + std::vector joint_names_{}; + std::map joint2position_{}; + ros::Subscriber joint_state_sub_, ecat_data_sub_; + ros::Time last_send_time_{}; + Base& base_; + uint8_t tx_buffer_[128]{}; + realtime_tools::RealtimeBuffer button_rt_buffer_{}; + int tx_len_{}; + bool r_pressed_ = false; + + const int k_frame_length_ = 128, k_header_length_ = 5, k_cmd_id_length_ = 2, k_tail_length_ = 2; +}; + +} // namespace engineer_custom_controller diff --git a/engineer_custom_controller/include/engineer_custom_controller/engineer_custom_controller.h b/engineer_custom_controller/include/engineer_custom_controller/engineer_custom_controller.h new file mode 100644 index 0000000..c3dd2f5 --- /dev/null +++ b/engineer_custom_controller/include/engineer_custom_controller/engineer_custom_controller.h @@ -0,0 +1,44 @@ +// +// Created by ch on 24-11-23. +// +#pragma once + +#include + +#include "engineer_custom_controller/common/data.h" +#include "engineer_custom_controller/custom_controller_data_sender.h" + +namespace engineer_custom_controller +{ +class EngineerCustomController +{ +public: + explicit EngineerCustomController(ros::NodeHandle& nh) : custom_controller_data_sender(nh, base_), last_get_data_time_(ros::Time::now()) + { + ROS_INFO("Engineer custom controller load."); + robot_custom_data_pub_ = nh.advertise("robot_custom_data", 1); + + base_.initSerial(); + } + void read(); + void clearRxBuffer() + { + rx_buffer_.clear(); + rx_len_ = 0; + } + + ros::Publisher robot_custom_data_pub_; + + Base base_; + CustomControllerDataSender custom_controller_data_sender; + std::vector rx_buffer_; + int rx_len_{}; + +private: + int unpack(uint8_t* rx_data); + ros::Time last_get_data_time_; + const int k_frame_length_ = 64, k_header_length_ = 5, k_cmd_id_length_ = 2, k_tail_length_ = 2; + const int k_unpack_buffer_length_ = 128; + uint8_t unpack_buffer_[128]{}; +}; +} // namespace engineer_custom_controller diff --git a/engineer_custom_controller/launch/load.launch b/engineer_custom_controller/launch/load.launch new file mode 100644 index 0000000..b013923 --- /dev/null +++ b/engineer_custom_controller/launch/load.launch @@ -0,0 +1,4 @@ + + + + diff --git a/engineer_custom_controller/package.xml b/engineer_custom_controller/package.xml new file mode 100644 index 0000000..135cbf2 --- /dev/null +++ b/engineer_custom_controller/package.xml @@ -0,0 +1,22 @@ + + + engineer_custom_controller + 0.0.0 + The engineer_custom_controller package + + BSD + ch + + catkin + + roscpp + sensor_msgs + rm_msgs + rm_ecat_msgs + std_msgs + nav_msgs + actionlib + rm_common + tf2_geometry_msgs + serial + diff --git a/engineer_custom_controller/src/custom_controller_data_sender.cpp b/engineer_custom_controller/src/custom_controller_data_sender.cpp new file mode 100644 index 0000000..56a4c43 --- /dev/null +++ b/engineer_custom_controller/src/custom_controller_data_sender.cpp @@ -0,0 +1,94 @@ +// +// Created by ljyi on 2026/4/6. +// + +#include "engineer_custom_controller/custom_controller_data_sender.h" + +namespace engineer_custom_controller +{ +void CustomControllerDataSender::pack(uint8_t* tx_buffer, uint8_t* data, int cmd_id, int data_len) +{ + memset(tx_buffer, 0, k_frame_length_); + auto* frame_header = reinterpret_cast(tx_buffer); + + frame_header->sof = 0xA5; + frame_header->data_length = data_len; + memcpy(&tx_buffer[k_header_length_], reinterpret_cast(&cmd_id), k_cmd_id_length_); + base_.appendCRC8CheckSum(tx_buffer, k_header_length_); + memcpy(&tx_buffer[k_header_length_ + k_cmd_id_length_], data, data_len); + base_.appendCRC16CheckSum(tx_buffer, k_header_length_ + k_cmd_id_length_ + data_len + k_tail_length_); +} + +void CustomControllerDataSender::sendCustomControllerData(const sensor_msgs::JointState::ConstPtr& data) { + uint8_t tx_data[sizeof(CustomControllerData)] = { 0 }; + for (size_t i = 0; i < data->name.size(); ++i) + { + joint2position_[data->name[i]] = data->position[i]; + } + std::vector positions; + for (size_t i = 0; i < joint2position_.size(); ++i) { + double position = joint2position_[joint_names_[i]]; + positions.push_back(rawData2Int16(position)); + } + positions.push_back(*button_rt_buffer_.readFromRT()); + memcpy(tx_data, positions.data(), sizeof(int16_t) * positions.size()); + pack(tx_buffer_, tx_data, CUSTOM_CONTROLLER_CMD, sizeof(CustomControllerData)); + tx_len_ = k_header_length_ + k_cmd_id_length_ + static_cast(sizeof(CustomControllerData) + k_tail_length_); + try { + base_.serial_.write(tx_buffer_, tx_len_); + } catch (serial::PortNotOpenedException& e) { + ROS_ERROR_STREAM(e.what()); + } + if ((*button_rt_buffer_.readFromRT() >> 9) & 0x01 || (*button_rt_buffer_.readFromRT() >> 1) & 0x01) { + clearTxBuffer(); + SimKeyboardMouseData keyboard_mouse_data; + int len = k_header_length_ + k_cmd_id_length_ + static_cast(sizeof(SimKeyboardMouseData) + k_tail_length_);; + keyboard_mouse_data.key_value = 0x0059; + pack(tx_buffer_, reinterpret_cast(&keyboard_mouse_data), SIM_KEYBOARD_MOUSE_CMD, sizeof(SimKeyboardMouseData)); + try { + base_.serial_.write(tx_buffer_, len); + r_pressed_ = true; + } catch (serial::PortNotOpenedException& e) { + ROS_ERROR_STREAM(e.what()); + } + } + else if (r_pressed_) { + clearTxBuffer(); + SimKeyboardMouseData keyboard_mouse_data; + int len = k_header_length_ + k_cmd_id_length_ + static_cast(sizeof(SimKeyboardMouseData) + k_tail_length_); + keyboard_mouse_data.key_value = 0x0000; + pack(tx_buffer_, reinterpret_cast(&keyboard_mouse_data), SIM_KEYBOARD_MOUSE_CMD, sizeof(SimKeyboardMouseData)); + try { + for (int i = 0; i < 5; ++i) { + base_.serial_.write(tx_buffer_, len); + } + r_pressed_ = false; + } catch (serial::PortNotOpenedException& e) { + ROS_ERROR_STREAM(e.what()); + } + } + clearTxBuffer(); +} + +int16_t CustomControllerDataSender::rawData2Int16(double value) { + if (value > maxVal) { + value = maxVal; + } else if (value < minVal) { + value = minVal; + } + double norm = (value - minVal) / (maxVal - minVal); + return static_cast(norm * 65535.0 - 32768.0); +} + +void CustomControllerDataSender::jointStateCallback(const sensor_msgs::JointState::ConstPtr& data) { + sendCustomControllerData(data); + last_send_time_ = ros::Time::now(); +} + +void CustomControllerDataSender::ecatDataCallback(const rm_ecat_msgs::RmEcatMitSlaveReadings::ConstPtr &data) { + double raw_button_l = (data->readings[1].velocity[0] + 45.0) / (45. * 2. / 4095) + 0.00001; + double raw_button_r = (data->readings[1].velocity[1] + 45.0) / (45. * 2. / 4095) + 0.00001; + int16_t button_data = (static_cast(raw_button_l) << 8) | static_cast(raw_button_r); + button_rt_buffer_.writeFromNonRT(button_data); +} +} // namespace engineer_custom_controller diff --git a/engineer_custom_controller/src/engineer_custom_controller.cpp b/engineer_custom_controller/src/engineer_custom_controller.cpp new file mode 100644 index 0000000..56d4431 --- /dev/null +++ b/engineer_custom_controller/src/engineer_custom_controller.cpp @@ -0,0 +1,87 @@ +// +// Created by ch on 24-11-23. +// +#include "engineer_custom_controller/engineer_custom_controller.h" + +namespace engineer_custom_controller +{ +void EngineerCustomController::read() +{ + if (base_.serial_.available()) + { + rx_len_ = static_cast(base_.serial_.available()); + base_.serial_.read(rx_buffer_, rx_len_); + } + else + return; + uint8_t temp_buffer[128] = { 0 }; + int frame_len; + if (ros::Time::now() - last_get_data_time_ > ros::Duration(0.1)) + base_.engineer_custom_controller_is_online_ = false; + // for (int k_i = 0; k_i < rx_len_; ++k_i) + // ROS_INFO("%02X", rx_buffer_[k_i]); + if (rx_len_ < k_unpack_buffer_length_) + { + for (int k_i = 0; k_i < k_unpack_buffer_length_ - rx_len_; ++k_i) + temp_buffer[k_i] = unpack_buffer_[k_i + rx_len_]; + for (int k_i = 0; k_i < rx_len_; ++k_i) + temp_buffer[k_i + k_unpack_buffer_length_ - rx_len_] = rx_buffer_[k_i]; + for (int k_i = 0; k_i < k_unpack_buffer_length_; ++k_i) + unpack_buffer_[k_i] = temp_buffer[k_i]; + } + for (int k_i = 0; k_i < k_unpack_buffer_length_ - k_frame_length_; ++k_i) + { + if (unpack_buffer_[k_i] == 0xA5) + { + frame_len = unpack(&unpack_buffer_[k_i]); + if (frame_len != -1) + k_i += frame_len; + } + } + clearRxBuffer(); +} + +int EngineerCustomController::unpack(uint8_t* rx_data) +{ + uint16_t cmd_id; + int frame_len; + engineer_custom_controller::FrameHeader frame_header; + + memcpy(&frame_header, rx_data, k_header_length_); + if (static_cast(base_.verifyCRC8CheckSum(rx_data, k_header_length_))) + { + if (frame_header.data_length > 128) // temporary and inaccurate value + { + ROS_INFO("discard possible wrong frames, data length: %d", frame_header.data_length); + return 0; + } + frame_len = frame_header.data_length + k_header_length_ + k_cmd_id_length_ + k_tail_length_; + if (base_.verifyCRC16CheckSum(rx_data, frame_len) == 1) + { + cmd_id = (rx_data[6] << 8 | rx_data[5]); + switch (cmd_id) + { + case engineer_custom_controller::ROBOT_TO_CUSTOM_CONTROLLER_CMD: + { + engineer_custom_controller::RobotToCustomData robot_custom_ref; + rm_msgs::RobotCustomData robot_custom_data; + memcpy(&robot_custom_ref, rx_data + 7, sizeof(engineer_custom_controller::RobotToCustomData)); + for (int i = 0; i < 30; ++i) + robot_custom_data.data[i] = robot_custom_ref.data[i]; + robot_custom_data.stamp = last_get_data_time_; + robot_custom_data_pub_.publish(robot_custom_data); + break; + } + default: + ROS_WARN("Referee command ID 0x%02X not found.", cmd_id); + break; + } + base_.engineer_custom_controller_is_online_ = true; + last_get_data_time_ = ros::Time::now(); + return frame_len; + } + } + return -1; +} + +} // namespace engineer_custom_controller diff --git a/engineer_custom_controller/src/main.cpp b/engineer_custom_controller/src/main.cpp new file mode 100644 index 0000000..b118582 --- /dev/null +++ b/engineer_custom_controller/src/main.cpp @@ -0,0 +1,21 @@ +// +// Created by ch on 24-11-23. +// +#include +#include "engineer_custom_controller/engineer_custom_controller.h" +#include "engineer_custom_controller/custom_controller_data_sender.h" +#include "engineer_custom_controller/common/data.h" + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "engineer_custom_controller"); + ros::NodeHandle nh("~"); + engineer_custom_controller::EngineerCustomController enngineer_custom_controller(nh); + ros::Rate loop_rate(100); + while (ros::ok()) + { + ros::spinOnce(); + // enngineer_custom_controller.read(); + loop_rate.sleep(); + } +} diff --git a/engineer_middleware/CMakeLists.txt b/engineer_middleware/CMakeLists.txt index b6764bb..c93c3c8 100644 --- a/engineer_middleware/CMakeLists.txt +++ b/engineer_middleware/CMakeLists.txt @@ -10,6 +10,8 @@ set(CMAKE_CXX_STANDARD_REQUIRED ON) add_definitions(-Wall -Werror) ## Find catkin macros and libraries +find_package(yaml-cpp REQUIRED) + find_package(catkin REQUIRED COMPONENTS roscpp @@ -79,6 +81,7 @@ add_dependencies(${PROJECT_NAME} ## Specify libraries to link executable targets against target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} + yaml-cpp ) ############# diff --git a/engineer_middleware/config/engineer.yaml b/engineer_middleware/config/engineer.yaml index 24d6f86..f38c687 100644 --- a/engineer_middleware/config/engineer.yaml +++ b/engineer_middleware/config/engineer.yaml @@ -6,4 +6,5 @@ chassis: yaw: pid: { p: 5., i: 0.0, d: 0.0, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true } yaw_start_threshold: 0.05 + linear_start_threshold: 0.05 max_vel: 10 diff --git a/engineer_middleware/config/engineer2_simulation_controllers.yaml b/engineer_middleware/config/engineer2_simulation_controllers.yaml index a43ad6f..98a6a44 100644 --- a/engineer_middleware/config/engineer2_simulation_controllers.yaml +++ b/engineer_middleware/config/engineer2_simulation_controllers.yaml @@ -15,20 +15,58 @@ controllers: - joint4 - joint5 - joint6 + - joint7 + constraints: + goal_time: 5.0 + stopped_velocity_tolerance: 0.05 + joint1: { trajectory: 0.08, goal: 0.05 } + joint2: { trajectory: 0.08, goal: 0.05 } + joint3: { trajectory: 0.08, goal: 0.05 } + joint4: { trajectory: 0.08, goal: 0.05 } + joint5: { trajectory: 0.08, goal: 0.05 } + joint6: { trajectory: 0.08, goal: 0.05 } + joint7: { trajectory: 0.08, goal: 0.05 } + gains: + joint1: { p: 16000, i: 100, d: 500.0, i_clamp_max: 10, i_clamp_min: -10, antiwindup: true, publish_state: true } + joint2: { p: 100, i: 5, d: 10, i_clamp_max: 5, i_clamp_min: -5, antiwindup: true, publish_state: true } + joint3: { p: 1000, i: 100, d: 20, i_clamp_max: 10, i_clamp_min: -10, antiwindup: true, publish_state: true } + joint4: { p: 500, i: 1, d: 5, i_clamp_max: 1, i_clamp_min: -1, antiwindup: true, publish_state: true } + joint5: { p: 200, i: 1, d: 2, i_clamp_max: 1, i_clamp_min: -1, antiwindup: true, publish_state: true } + joint6: { p: 100, i: 1, d: 1, i_clamp_max: 1, i_clamp_min: -1, antiwindup: true, publish_state: true } + joint7: { p: 100, i: 1, d: 1, i_clamp_max: 1, i_clamp_min: -1, antiwindup: true, publish_state: true } + + gravity_compensation_controller: + type: gravity_compensation_controller/GravityCompensationJointTrajectoryController + joints: + - joint1 + - joint2 + - joint3 + - joint4 + - joint5 + - joint6 + - joint7 gains: - joint1: { p: 20000.0, i: 0, d: 400.0, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true } - joint2: { p: 100000, i: 0, d: 0.5, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true } - joint3: { p: 20.0, i: 0, d: 0.1, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true } - joint4: { p: 20.0, i: 0.0, d: 0.1, i_clamp_max: 0, i_clamp_min: 0.0, antiwindup: true, publish_state: true } - joint5: { p: 20.0, i: 0, d: 0.1, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true } - joint6: { p: 5.0, i: 0, d: 0.1, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true } + joint1: { p: 16000.0, i: 10.0, d: 580.0, i_clamp_max: 20, i_clamp_min: -20, antiwindup: true, publish_state: true } + joint2: { p: 60.0, i: 0.015, d: 1.8, i_clamp_max: 7, i_clamp_min: -7, antiwindup: true, publish_state: true } + joint3: { p: 135.0, i: 35.0, d: 3.5, i_clamp_max: 0, i_clamp_min: -16, antiwindup: true, publish_state: true } + joint4: { p: 85.0, i: 9.0, d: 2.0, i_clamp_max: 0, i_clamp_min: -9, antiwindup: true, publish_state: true } + joint5: { p: 24.0, i: 1.5, d: 0.4, i_clamp_max: 2, i_clamp_min: -2, antiwindup: true, publish_state: true } + joint6: { p: 18.0, i: 4.0, d: 0.5, i_clamp_max: 1, i_clamp_min: -1, antiwindup: true, publish_state: true } + joint7: { p: 17.0, i: 1.2, d: 0.4, i_clamp_max: 2, i_clamp_min: -2, antiwindup: true, publish_state: true } constraints: - joint1: { trajectory: 0.05, goal: 0.05 } - joint2: { trajectory: 0.05, goal: 0.05 } - joint3: { trajectory: 0.05, goal: 0.05 } - joint4: { trajectory: 0.05, goal: 0.05 } - joint5: { trajectory: 0.05, goal: 0.05 } - joint6: { trajectory: 0.05, goal: 0.05 } + goal_time: 5.0 + stopped_velocity_tolerance: 0.05 + joint1: { trajectory: 0.08, goal: 0.05 } + joint2: { trajectory: 0.08, goal: 0.05 } + joint3: { trajectory: 0.08, goal: 0.05 } + joint4: { trajectory: 0.08, goal: 0.05 } + joint5: { trajectory: 0.08, goal: 0.05 } + joint6: { trajectory: 0.08, goal: 0.05 } + joint7: { trajectory: 0.08, goal: 0.05 } + offset_torques: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + tau_g_scale: [0.0, 1.0, 0.57, 0.625, 1.0, 1.0, 1.0] + chain_root: base_link + chain_tip: gripper_link chassis_controller: type: rm_chassis_controllers/OmniController @@ -42,7 +80,7 @@ controllers: power_offset: -9.8 twist_angular: 0.5233 timeout: 0.1 - pid_follow: { p: 4.0, i: 0, d: 0.02, i_max: 0.0, i_min: 0.0, antiwindup: true, publish_state: true } + pid_follow: { p: 4.0, i: 0, d: 0.02, i_clamp_max: 0.0, i_clamp_min: 0.0, antiwindup: true, publish_state: false } twist_covariance_diagonal: [ 0.001, 0.001, 0.001, 0.001, 0.001, 0.001 ] max_odom_vel: 10.0 # OmniController @@ -53,7 +91,7 @@ controllers: joint: left_front_wheel_joint <<: &wheel_setting radius: 0.07 - pid: { p: 0.4, i: 0, d: 0.0, i_max: 0.0, i_min: 0.0, antiwindup: true, publish_state: true } + pid: { p: 0.3, i: 0.2, d: 0.0, i_clamp_max: 1.0, i_clamp_min: -1.0, antiwindup: true, publish_state: false } right_front: pose: [ 0.213, -0.1945, 0. ] roller_angle: 0.785 @@ -70,63 +108,31 @@ controllers: joint: right_back_wheel_joint <<: *wheel_setting - gimbal_controller: - type: rm_gimbal_controllers/Controller - yaw: - joint: "yaw_joint" - pid: { p: 0.5, i: 0, d: 0.0, i_clamp_max: 0.0, i_clamp_min: -0.0, antiwindup: true, publish_state: false } - pid_pos: { p: 15.0, i: 0, d: 0.0, i_clamp_max: 0.0, i_clamp_min: 0.0, antiwindup: true, publish_state: true } - k_chassis_vel: 0.035 - k_v: 1.0 - pitch: - joint: "pitch_joint" - pid: { p: 0., i: 0.0, d: 0.0, i_clamp_max: 0.0, i_clamp_min: -0.0, antiwindup: true, publish_state: false } - pid_pos: { p: 0.0, i: 0, d: 0.0, i_clamp_max: 0.0, i_clamp_min: 0.0, antiwindup: true, publish_state: true } - k_v: 1.0 - feedforward: - gravity: 0.0 - enable_gravity_compensation: false - mass_origin: [ 0.0,0.0,0.0 ] - bullet_solver: - resistance_coff_qd_10: 0.45 - resistance_coff_qd_15: 1.0 - resistance_coff_qd_16: 0.7 - resistance_coff_qd_18: 0.55 - resistance_coff_qd_30: 3.0 - g: 9.81 - delay: 0.1 - dt: 0.001 - timeout: 0.001 - publish_rate: 50 - - middle_pitch_controller: - type: effort_controllers/JointPositionController - joint: middle_pitch_joint - pid: { p: 5., i: 0., d: 1, i_max: 0.01, i_min: 0.01, antiwindup: true, publish_state: true } - - silver_lifter_controller: - type: effort_controllers/JointPositionController - joint: silver_lifter_joint - pid: { p: 1000., i: 0., d: 100, i_max: 0.01, i_min: 0.01, antiwindup: true, publish_state: true } - - silver_pusher_controller: - type: effort_controllers/JointPositionController - joint: silver_pusher_joint - pid: { p: 1000., i: 0., d: 100, i_max: 0.01, i_min: 0.01, antiwindup: true, publish_state: true } - - silver_rotator_controller: - type: effort_controllers/JointPositionController - joint: silver_rotator_joint - pid: { p: 50., i: 0., d: 1, i_max: 0.01, i_min: 0.01, antiwindup: true, publish_state: true } - - ######################### Gold Ore Controllers ######################## - - gold_lifter_controller: - type: effort_controllers/JointPositionController - joint: gold_lifter_joint - pid: { p: 100., i: 0., d: 10, i_max: 0.01, i_min: 0.01, antiwindup: true, publish_state: true } - - gold_pusher_controller: - type: effort_controllers/JointPositionController - joint: gold_pusher_joint - pid: { p: 1000., i: 0., d: 100, i_max: 0.01, i_min: 0.01, antiwindup: true, publish_state: true } +# gimbal_controller: +# type: rm_gimbal_controllers/Controller +# yaw: +# joint: "yaw_joint" +# pid: { p: 0.5, i: 0, d: 0.0, i_clamp_max: 0.0, i_clamp_min: -0.0, antiwindup: true, publish_state: false } +# pid_pos: { p: 15.0, i: 0, d: 0.0, i_clamp_max: 0.0, i_clamp_min: 0.0, antiwindup: true, publish_state: true } +# k_chassis_vel: 0.035 +# k_v: 1.0 +# pitch: +# joint: "pitch_joint" +# pid: { p: 0., i: 0.0, d: 0.0, i_clamp_max: 0.0, i_clamp_min: -0.0, antiwindup: true, publish_state: false } +# pid_pos: { p: 0.0, i: 0, d: 0.0, i_clamp_max: 0.0, i_clamp_min: 0.0, antiwindup: true, publish_state: true } +# k_v: 1.0 +# feedforward: +# gravity: 0.0 +# enable_gravity_compensation: false +# mass_origin: [ 0.0,0.0,0.0 ] +# bullet_solver: +# resistance_coff_qd_10: 0.45 +# resistance_coff_qd_15: 1.0 +# resistance_coff_qd_16: 0.7 +# resistance_coff_qd_18: 0.55 +# resistance_coff_qd_30: 3.0 +# g: 9.81 +# delay: 0.1 +# dt: 0.001 +# timeout: 0.001 +# publish_rate: 50 diff --git a/engineer_middleware/config/engineer2_steps_list.yaml b/engineer_middleware/config/engineer2_steps_list.yaml index ce4df3c..90759bd 100644 --- a/engineer_middleware/config/engineer2_steps_list.yaml +++ b/engineer_middleware/config/engineer2_steps_list.yaml @@ -5,212 +5,69 @@ common: accel: 0.1 timeout: 15. normally: &NORMALLY - speed: 0.4 - accel: 0.3 - timeout: 6 - quickly: &QUICKLY - speed: 0.6 - accel: 0.5 + speed: 0.5 + accel: 0.4 timeout: 6. + quickly: &QUICKLY + speed: 0.8 + accel: 0.6 + timeout: 5. + maximum: &MAXIMUM + speed: 1.0 + accel: 1.0 + timeout: 4. tolerance: small_tolerance: &SMALL_TOLERANCE - tolerance_joints: [ 0.005, 0.008, 0.015, 0.1, 0.1, 0.1, 0.03 ] + tolerance_joints: [ 0.005, 0.02, 0.01, 0.02, 0.02, 0.02, 0.02 ] normal_tolerance: &NORMAL_TOLERANCE - tolerance_joints: [ 0.012, 0.02, 0.015, 0.1, 0.2, 0.15, 0.07 ] + tolerance_joints: [ 0.012, 0.03, 0.03, 0.04, 0.04, 0.03, 0.03 ] bigger_tolerance: &BIGGER_TOLERANCE - tolerance_joints: [ 0.015, 0.04, 0.03, 0.35, 0.2, 0.2, 0.1 ] + tolerance_joints: [ 0.015, 0.05, 0.06, 0.08, 0.06, 0.07, 0.05 ] max_tolerance: &MAX_TOLERANCE - tolerance_joints: [ 0.03, 0.03, 0.03, 0.3, 0.3, 0.3 , 0.3] + tolerance_joints: [ 0.02, 0.08, 0.07, 0.1, 0.07, 0.07, 0.06 ] joint1: home: &JOINT1_HOME_POS - 0.1 - ready_get_ground: &JOINT1_GET_GROUND_POS - 0.005 - exchange: &JOINT1_EXCHANGE_POS - 0.35 - ready_get_small: &JOINT1_READY_GET_SMALL_POS - 0.42 - get_small: &JOINT1_GET_SMALL_POS - 0.325 - ready_get_stored: &JOINT1_READY_GET_STORED_POS - 0.54 - get_stored: &JOINT1_GET_STORED_POS - 0.478 - get_stored_gold: &JOINT1_GET_STORED_GOLD_POS - 0.04 - ready_store: &JOINT1_READY_STORE_POS - 0.44 - store: &JOINT1_STORE_POS - 0.34 - + 0.121 joint2: home: &JOINT2_HOME_POS - 1.0 - avoid_controller: &JOINT2_AVOID_CONTROLLER_POS - 0.6 - exchange: &JOINT2_EXCHANGE_POS - -1.57 - l_exchange: &JOINT2_L_EXCHANGE_POS - -0.3 - r_exchange: &JOINT2_R_EXCHANGE_POS - -2.5 - get_small_island_l: &JOINT2_GET_SMALL_ISLAND_L_POS - 0.858 - get_small_island_m: &JOINT2_GET_SMALL_ISLAND_M_POS - -1.101 - get_small_island_r: &JOINT2_GET_SMALL_ISLAND_R_POS - -2.052 - store_to_s1: &JOINT2_STORE_TO_S1_POS - -3.22 - store_to_s2: &JOINT2_STORE_TO_S2_POS - -2.364 - store_to_s3: &JOINT2_STORE_TO_S3_POS - -1.763 - get_stored_silver3: &JOINT2_GET_STORED_S3_POS - -2.055 - get_stored_silver2: &JOINT2_GET_STORED_S2_POS - -2.802 - get_stored_silver1: &JOINT2_GET_STORED_S1_POS - -3.833 - get_stored_gold: &JOINT2_GET_STORED_GOLD_POS - -1.062 - zero_pos: &JOINTN2_ZERO_POS - 0.000001 + 1.553 joint3: home: &JOINT3_HOME_POS - -2.3 - zero: &JOINT3_ZERO_POS - 0.00000001 - l_exchange: &JOINT3_L_EXCHANGE_POS - -0.7 - r_exchange: &JOINT3_R_EXCHANGE_POS - 0.7 - get_small_island_l: &JOINT3_GET_SMALL_ISLAND_L_POS - -1.643 - get_small_island_m: &JOINT3_GET_SMALL_ISLAND_M_POS - -1.833 - get_small_island_r: &JOINT3_GET_SMALL_ISLAND_R_POS - -0.815 - store_to_s1: &JOINT3_STORE_TO_S1_POS - -1.024 - store_to_s2: &JOINT3_STORE_TO_S2_POS - -1.723 - store_to_s3: &JOINT3_STORE_TO_S3_POS - -1.467 - get_stored_silver3: &JOINT3_GET_STORED_SILVER3_POS - -1.741 - get_stored_silver2: &JOINT3_GET_STORED_SILVER2_POS - -1.57 - get_stored_silver1: &JOINT3_GET_STORED_SILVER1_POS - -0.307 - ready_get_stored_gold: &JOINT3_READY_GET_STORED_GOLD_POS - -1.67 - get_stored_gold: &JOINT3_GET_STORED_GOLD_POS - -1.868 + -1.06 joint4: home: &JOINT4_HOME_POS - 1.022 - zero: &JOINT4_ZERO_POS - 0.000001 - r90: &JOINT4_R90_POS - -1.57 - l90: &JOINT4_L90_POS - 1.57 - get_stored_gold: &JOINT4_GET_STORED_GOLD_POS - -1.627 - + 1.007 joint5: home: &JOINT5_HOME_POS - 0.00001 - down_45: &JOINT5_DOWN_45 - 0.77 - down_90: &JOINT5_DOWN_90 - 1.57 - get_stored_gold: &JOINT5_GET_STORED_GOLD_POS - 1.765 - store_to_s1: &JOINT5_STORE_TO_S1_POS - -0.215 - store_to_s2: &JOINT5_STORE_TO_S2_POS - -0.314 - store_to_s3: &JOINT5_STORE_TO_S3_POS - -1.153 + -0.033 joint6: home: &JOINT6_HOME_POS - 0.00001 - l_90: &JOINT6_L_90 - 1.57 - r_90: &JOINT6_R_90 - -1.57 - get_small3: &JOINT6_SMALL3 - 4.387 - get_small2: &JOINT6_SMALL2 - 4.387 - get_small1: &JOINT6_SMALL1 - -0.481 - get_own_s3: &JOINT6_GET_S3 - 0.6536 - get_own_s2: &JOINT6_GET_S2 - 1.22727 - get_own_s1: &JOINT6_GET_S1 - 0.9794 + 0.197 - middle_pitch: - ground_stone: &MIDDLE_PITCH_GROUND_STONE - 1. - normal: &MIDDLE_PITCH_NORMAL - 0.0 - highest: &MIDDLE_PITCH_HIGHEST - -0.1 + joint7: + home: &JOINT7_HOME_POS + 0.0001 arm: home: &HOME - joints: [ *JOINT1_HOME_POS, *JOINT2_HOME_POS, *JOINT3_HOME_POS, *JOINT4_ZERO_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS ] - common: - <<: *NORMALLY - tolerance: - <<: *BIGGER_TOLERANCE - - lv4_exchange: &LV4_EXCHANGE - joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_R_EXCHANGE_POS, *JOINT3_R_EXCHANGE_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_45, *JOINT6_HOME_POS ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - lv5_r_exchange: &LV5_L_EXCHANGE - joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_L_EXCHANGE_POS, *JOINT3_L_EXCHANGE_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_L_90] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - lv5_l_exchange: &LV5_R_EXCHANGE - joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_R_EXCHANGE_POS, *JOINT3_R_EXCHANGE_POS, *JOINT4_L90_POS, *JOINT5_DOWN_90, *JOINT6_R_90] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - lv5_r_straight_exchange: &LV5_R_STRAIGHT_EXCHANGE - joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_EXCHANGE_POS, *JOINT3_ZERO_POS, *JOINT4_L90_POS, *JOINT5_DOWN_90, *JOINT6_R_90 ] + joints: [ *JOINT1_HOME_POS, *JOINT2_HOME_POS, *JOINT3_HOME_POS, *JOINT4_HOME_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS, *JOINT7_HOME_POS] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - lv5_l_straight_exchange: &LV5_L_STRAIGHT_EXCHANGE - joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_EXCHANGE_POS, *JOINT3_ZERO_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_L_90 ] + ready_assemble: &READY_ASSEMBLE + joints: [ 0.43, 0.001, *JOINT3_HOME_POS, *JOINT4_HOME_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS, *JOINT7_HOME_POS ] common: <<: *NORMALLY tolerance: - <<: *NORMAL_TOLERANCE + <<: *BIGGER_TOLERANCE gimbal: right_pos: &RIGHT_POS @@ -226,50 +83,81 @@ common: frame: base_link position: [ -3., -0.3, 0. ] follow_ee: &FOLLOW_EE - frame: link6 - position: [ 0.15, 0.00, 0. ] - follow_gold_gripper: &FOLLOW_GOLD - frame: gold_lifter_link - position: [ 0.0, 0.0, 0.0 ] + frame: gripper_link + position: [ 0.20, 0.00, 0. ] gripper: - open_main_gripper: &OPEN_MAIN_GRIPPER - pin: 0 - state: true - close_main_gripper: &CLOSE_MAIN_GRIPPER - pin: 0 - state: false - open_s1: &OPEN_S1 - pin: 1 - state: true - close_s1: &CLOSE_S1 - pin: 1 - state: false - open_s2: &OPEN_S2 - pin: 2 - state: true - close_s2: &CLOSE_S2 - pin: 2 - state: false - open_s3: &OPEN_S3 - pin: 3 - state: true - close_s3: &CLOSE_S3 - pin: 3 - state: false - open_gold: &OPEN_GOLD - pin: 4 - state: true - close_gold: &CLOSE_GOLD - pin: 4 - state: false - open_silver_gripper: &OPEN_S_GRIPPER - pin: 5 - state: true - close_silver_gripper: &CLOSE_S_GRIPPER - pin: 5 - state: false + open_gripper: &OPEN_GRIPPER + command: false + clode_gripper: &CLOSE_GRIPPER + command: true + + chassis: + chassis_left: &CHASSIS_LEFT_30 + frame: base_link + position: [ 0.0, 0.30 ] + yaw: 0.0 + chassis_tolerance_position: 0.05 + chassis_tolerance_angular: 0.1 + common: + timeout: 6. +# chassis_left_60: &CHASSIS_LEFT_70 +# frame: base_link +# position: [ 0.0, 0.7 ] +# yaw: 0.0 +# chassis_tolerance_position: 0.1 +# chassis_tolerance_angular: 0.2 +# common: +# timeout: 6. +# chassis_left60: &CHASSIS_LEFT_60 +# frame: base_link +# position: [ 0.0, 0.6 ] +# yaw: 0.0 +# chassis_tolerance_position: 0.1 +# chassis_tolerance_angular: 0.2 +# common: +# timeout: 6. +# chassis_left50: &CHASSIS_LEFT_50 +# frame: base_link +# position: [ 0.0, 0.5 ] +# yaw: 0.0 +# chassis_tolerance_position: 0.1 +# chassis_tolerance_angular: 0.2 +# common: +# timeout: 6. +# chassis_back_60: &CHASSIS_BACK_60 +# frame: base_link +# position: [ -0.6, 0.0 ] +# yaw: 0.0 +# chassis_tolerance_position: 0.1 +# chassis_tolerance_angular: 0.2 +# common: +# timeout: 6. +# chassis_turn_left: &CHASSIS_TURN_LEFT +# frame: base_link +# position: [ 0.0, 0.0 ] +# yaw: 1.57 +# chassis_tolerance_position: 0.1 +# chassis_tolerance_angular: 0.2 +# common: +# timeout: 6. +# chassis_turn_right: &CHASSIS_TURN_RIGHT +# frame: base_link +# position: [ 0.0, 0.0 ] +# yaw: -1.07 +# chassis_tolerance_position: 0.1 +# chassis_tolerance_angular: 0.2 +# common: +# timeout: 6. + chassis_turn_back: &CHASSIS_TURN_BACK + frame: base_link + position: [ 0.0, 0.0 ] + yaw: 3.14 + chassis_tolerance_position: 0.1 + chassis_tolerance_angular: 0.2 + common: + timeout: 2. steps_list: GIMBAL_F: @@ -286,1973 +174,577 @@ steps_list: - step: "gimbal look back" gimbal: <<: *BACK_POS - GIMBAL_EE: - step: "gimbal follow ee" gimbal: <<: *FOLLOW_EE - MIDDLE_PITCH_UP: - - step: "middle pitch up" - middle_pitch: - target: *MIDDLE_PITCH_NORMAL - HOME: - - step: "main close" - gripper: - <<: *CLOSE_MAIN_GRIPPER - - step: "pusher back" - silver_pusher: - target: 0. - - step: "lifter down" - silver_lifter: - target: 0.0 - - step: "rotate up" - silver_rotator: - target: 1.64 - - step: "get gold" - gold_pusher: - target: 0. - - step: "lift gold" - gold_lifter: - target: 0. - - step: "gimbal look front" - gimbal: - <<: *RIGHT_POS - - step: "gimbal look front" - gimbal: - <<: *FRONT_POS - - step: "main close" - gripper: - <<: *CLOSE_MAIN_GRIPPER - - step: "arm home pos" - arm: - <<: *HOME - - step: "middle pitch down" - middle_pitch: - target: *MIDDLE_PITCH_GROUND_STONE - CALIBRATION: - - step: "middle pitch up" - middle_pitch: - target: *MIDDLE_PITCH_NORMAL - - step: "main close" - gripper: - <<: *CLOSE_MAIN_GRIPPER - - step: "s1 close" - gripper: - <<: *CLOSE_S1 - - step: "s2 close" - gripper: - <<: *CLOSE_S2 - - step: "s3 close" - gripper: - <<: *CLOSE_S3 - - step: "g1 close" - gripper: - <<: *CLOSE_GOLD - - step: "close silver gripper" - gripper: - <<: *CLOSE_S_GRIPPER - step: "gimbal look front" gimbal: <<: *FRONT_POS - - step: "main close" - gripper: - <<: *CLOSE_MAIN_GRIPPER - HOME_WITH_PITCH: - - step: "rotate up" - silver_rotator: - target: 1.64 - - step: "gimbal look front" - gimbal: - <<: *FRONT_POS + HOME: - step: "arm home pos" arm: <<: *HOME - - step: "main close" - gripper: - <<: *CLOSE_MAIN_GRIPPER - - step: "middle pitch down" - middle_pitch: - target: *MIDDLE_PITCH_GROUND_STONE +# - step: "gripper open" +# gripper: +# <<: *OPEN_GRIPPER +# - step: "gimbal look front" +# gimbal: +# <<: *FRONT_POS + + LIFTER_INIT: + - step: "lifter init" + lifter: + state: 0 + LIFTER_UP: + - step: "lifter up" + lifter: + state: 1 + LIFTER_UP0: + - step: "lifter init" + lifter: + state: 0 + LIFTER_DOWN: + - step: "lifter down" + lifter: + state: 2 + LIFTER_DOWN0: + - step: "lifter init" + lifter: + state: 0 - LV4_EXCHANGE: - - step: "middle pitch up" - middle_pitch: - target: *MIDDLE_PITCH_NORMAL - delay: 0.3 - - step: "arm enter exchange pos" - arm: - <<: *LV4_EXCHANGE - - step: "gimbal ready" - gimbal: - <<: *RIGHT_POS - LV5_L_EXCHANGE: - - step: "middle pitch up" - middle_pitch: - target: *MIDDLE_PITCH_NORMAL - delay: 1.0 - - step: "arm enter exchange pos" - arm: - <<: *LV5_L_EXCHANGE - - step: "gimbal ready" - gimbal: - <<: *RIGHT_POS + GRIPPER_OPEN: + - step: "gripper open" + gripper: + <<: *OPEN_GRIPPER - L2R_EXCHANGE: - - step: "arm move back" - arm: - joints: [*JOINT1_EXCHANGE_POS, 0.92, -2.38, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS] - common: - <<: *NORMALLY - tolerance: - <<: *BIGGER_TOLERANCE - - step: "reverse joint2 and joint3" - arm: - joints: [*JOINT1_EXCHANGE_POS, -0.846, 2.20, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS] - common: - <<: *NORMALLY - tolerance: - <<: *BIGGER_TOLERANCE - - step: "rotate joint2 to r exchange pos" + GRIPPER_CLOSE: + - step: "gripper close" + gripper: + <<: *CLOSE_GRIPPER +# TEST_PLAYBACK: +# - step: "test playback" +# trajectory_playback: +# filename: "/home/ch/rm_ws/src/rm_engineer/engineer_trajectory_teaching/trajectory/test.yaml" +# speed: 1.5 +# sync_time: 0.5 +# + GET_UNITS_UP: + - step: "gripper open" + gripper: + <<: *OPEN_GRIPPER + - step: "arm ready get second unit" arm: - joints: [*JOINT1_EXCHANGE_POS, *JOINT2_R_EXCHANGE_POS, 2.20, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS] + joints: [ 0.215895, -0.003815, -0.312477, 0.193771, -1.581238, -0.002813, 2.618987 ] common: <<: *NORMALLY tolerance: - <<: *BIGGER_TOLERANCE - - step: "arm enter exchange pos" - arm: - <<: *LV5_R_EXCHANGE - - R2L_EXCHANGE: - - step: "fold arm" + <<: *NORMAL_TOLERANCE + GET_UNITS_UP0: + - step: "gripper close" + gripper: + <<: *CLOSE_GRIPPER + - step: "arm up unit" arm: - joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_R_EXCHANGE_POS, 2.20, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ 0.421690, 0.390625, -0.198036, 0.487916, -0.722261, -0.511475, 1.735095 ] common: <<: *NORMALLY tolerance: - <<: *BIGGER_TOLERANCE - - step: "rotate joint2 to l exchange pos" + <<: *NORMAL_TOLERANCE + - step: "chassis back" + chassis: + <<: *CHASSIS_LEFT_30 + - step: "ready store" arm: - joints: [ *JOINT1_EXCHANGE_POS, -0.846, 2.20, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ 0.445447, -2.713721, 0.194878, 0.043058, 1.530397, 1.536000, -2.027769 ] common: <<: *NORMALLY tolerance: <<: *BIGGER_TOLERANCE - - step: "reverse joint2 and joint3" + - step: "arm down unit" arm: - joints: [ *JOINT1_EXCHANGE_POS, 0.92, -2.38, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ "KEEP", "KEEP", 0.768227, -0.149203, 1.362128, 1.536000, -2.259214 ] common: <<: *NORMALLY tolerance: <<: *BIGGER_TOLERANCE - - step: "arm enter exchange pos" - arm: - <<: *LV5_L_EXCHANGE - - - LV5_R_EXCHANGE: - - step: "middle pitch up" - middle_pitch: - target: *MIDDLE_PITCH_NORMAL - delay: 1.0 - - step: "arm enter exchange pos" - arm: - <<: *LV5_R_EXCHANGE - - step: "gimbal ready" - gimbal: - <<: *RIGHT_POS - - LV5_L_STRAIGHT_EXCHANGE: - - step: "middle pitch up" - middle_pitch: - target: *MIDDLE_PITCH_NORMAL - delay: 0.3 - - step: "arm enter exchange pos" - arm: - <<: *LV5_L_STRAIGHT_EXCHANGE - - step: "gimbal ready" - gimbal: - <<: *RIGHT_POS - - LV5_R_STRAIGHT_EXCHANGE: - - step: "middle pitch up" - middle_pitch: - target: *MIDDLE_PITCH_NORMAL - delay: 0.3 - - step: "arm enter exchange pos" - arm: - <<: *LV5_R_STRAIGHT_EXCHANGE - - step: "gimbal ready" - gimbal: - <<: *RIGHT_POS - - LV5_R_DROP_GOLD_EXCHANGE: - - step: "middle pitch up" - middle_pitch: - target: *MIDDLE_PITCH_NORMAL - delay: 0.3 - - step: "drop side gold" - gripper: - <<: *CLOSE_GOLD - - step: "add stone" - stone_num: - change: "-g" - - step: "arm enter exchange pos" - arm: - <<: *LV5_R_EXCHANGE - - step: "gimbal ready" - gimbal: - <<: *RIGHT_POS - - LV5_L_DROP_GOLD_EXCHANGE: - - step: "middle pitch up" - middle_pitch: - target: *MIDDLE_PITCH_NORMAL - delay: 0.3 - - step: "drop side gold" + - step: "gripper open" gripper: - <<: *CLOSE_GOLD - - step: "add stone" - stone_num: - change: "-g" - - step: "arm enter exchange pos" - arm: - <<: *LV5_L_EXCHANGE - - step: "gimbal ready" - gimbal: - <<: *RIGHT_POS - - GET_SMALL_ISLAND: - - step: "middle pitch up" - middle_pitch: - target: *MIDDLE_PITCH_NORMAL - delay: 0.3 - - step: "gimbal look front" - gimbal: - <<: *FRONT_POS - - step: "ready get small island silver" + <<: *OPEN_GRIPPER + - step: "arm leave unit" arm: - joints: [*JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_L_POS, *JOINT3_GET_SMALL_ISLAND_L_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_SMALL1] + joints: [ "KEEP", -2.3271, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - step: "open main gripper" - gripper: - <<: *OPEN_MAIN_GRIPPER - GET_SMALL_ISLAND0: - - step: "get small island silver" + - step: "arm ready get third unit" arm: - joints: [ *JOINT1_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_L_POS, *JOINT3_GET_SMALL_ISLAND_L_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_SMALL1 ] + joints: [ 0.214920, -0.003815, -0.312477, 0.172921, -1.596671, 0.0001, 0.583660 ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - step: "pull up silver" + GET_UNITS_UP00: + - step: "gripper close" + gripper: + <<: *CLOSE_GRIPPER + - step: "arm up unit" arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_SMALL_ISLAND_L_POS, *JOINT3_GET_SMALL_ISLAND_L_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_SMALL1 ] + joints: [ 0.314685, -0.379, -0.226646, 0.074980, -1.414886, 0.153958, 0.366711 ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - GET_SMALL_ISLAND00: - - step: "ready stuck stone" + - step: "chassis back" + chassis: + <<: *CHASSIS_LEFT_30 + - step: "ready store" arm: - joints: [ 0.24, 1.049, -2.4010, -0.225, 1.112, -0.11 ] + joints: [ 0.445447, -2.517585, -0.464470, 0.684690, 1.505160, 1.6, -1.960691 ] common: <<: *NORMALLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "stuck stone" + <<: *BIGGER_TOLERANCE + - step: "arm down unit" arm: - joints: [ 0.16, 1.049, -2.4010, -0.225, 1.112, -0.11 ] + joints: [ "KEEP", -2.517585, 0.194878, 0.788450, 1.525415, 1.6, -2.6 ] common: <<: *NORMALLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "get ground stone" - middle_pitch: - target: 0.49 - delay: 0.5 - - - STORE_SILVER_TO_GRIPPER1: - - step: "silver rotator up" - silver_rotator: - target: 1.57 - - step: "open silver gripper" - gripper: - <<: *OPEN_S_GRIPPER - - - step: "open gripper1" + <<: *BIGGER_TOLERANCE + - step: "gripper open" gripper: - <<: *OPEN_S1 - - step: "ready to store" + <<: *OPEN_GRIPPER + - step: "arm leave unit" arm: - joints: [*JOINT1_READY_STORE_POS, *JOINT2_STORE_TO_S1_POS, *JOINT3_STORE_TO_S1_POS, *JOINT4_L90_POS, *JOINT5_STORE_TO_S1_POS, *JOINT6_HOME_POS] + joints: [ "KEEP", -1.855548, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - step: "store" + - step: "ready pos" arm: - joints: [ *JOINT1_STORE_POS, *JOINT2_STORE_TO_S1_POS, *JOINT3_STORE_TO_S1_POS, *JOINT4_L90_POS, *JOINT5_STORE_TO_S1_POS, *JOINT6_HOME_POS ] + joints: [ 0.307, -0.013888, -0.188118, 0.060229, -0.018521, -0.020241, 0.008777 ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - step: "close main gripper" + GET_UNITS_UP000: + - step: "gripper close" gripper: - <<: *CLOSE_MAIN_GRIPPER - - step: "add stone" - stone_num: - change: "+s1" - - step: "lift up arm" + <<: *CLOSE_GRIPPER + - step: "arm up unit" arm: - joints: [ *JOINT1_READY_STORE_POS, *JOINT2_STORE_TO_S1_POS, *JOINT3_STORE_TO_S1_POS, *JOINT4_L90_POS, *JOINT5_STORE_TO_S1_POS, *JOINT6_HOME_POS ] + joints: [ 0.405447, -0.013888, -0.217, -0.01, -0.018521, -0.05, 0.008777 ] common: <<: *NORMALLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "left arm" + <<: *BIGGER_TOLERANCE + - step: "chassis back" + chassis: + <<: *CHASSIS_LEFT_30 + - step: "home" arm: - joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE + <<: *HOME + - step: "chassis back" + chassis: + <<: *CHASSIS_TURN_BACK - STORE_SILVER_TO_GRIPPER2: - - step: "silver rotator up" - silver_rotator: - target: 1.57 - - step: "open silver gripper" - gripper: - <<: *OPEN_S_GRIPPER - - step: "open gripper2" + GET_UNITS_DOWN: + - step: "gripper open" gripper: - <<: *OPEN_S2 - - step: "ready to store" + <<: *OPEN_GRIPPER + - step: "ready pos" arm: - joints: [ *JOINT1_READY_STORE_POS, *JOINT2_STORE_TO_S2_POS, *JOINT3_STORE_TO_S2_POS, *JOINT4_L90_POS, *JOINT5_STORE_TO_S2_POS, *JOINT6_HOME_POS ] + joints: [ 0.032178, -0.0028, -0.1699, 0.143689, -0.007022, -0.056858, 0.003342 ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - step: "store" + GET_UNITS_DOWN0: + - step: "gripper close" + gripper: + <<: *CLOSE_GRIPPER + - step: "arm down unit" arm: - joints: [ *JOINT1_STORE_POS, *JOINT2_STORE_TO_S2_POS, *JOINT3_STORE_TO_S2_POS, *JOINT4_L90_POS, *JOINT5_STORE_TO_S2_POS, *JOINT6_HOME_POS ] + joints: [ 0.003, "KEEP", 0.024193, 0.499239, -0.007022, -0.568923, 0.003342 ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - step: "close main gripper" - gripper: - <<: *CLOSE_MAIN_GRIPPER - - step: "add stone" - stone_num: - change: "+s2" - - step: "lift up arm" + - step: "chassis back" + chassis: + <<: *CHASSIS_LEFT_30 + - step: "ready store" arm: - joints: [ *JOINT1_READY_STORE_POS, *JOINT2_STORE_TO_S2_POS, *JOINT3_STORE_TO_S2_POS, *JOINT4_L90_POS, *JOINT5_STORE_TO_S2_POS, *JOINT6_HOME_POS ] + joints: [ 0.445447, -2.72, 0.194878, 0.043058, 1.530397, 1.536000, 1.153721 ] common: <<: *NORMALLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "left arm" + <<: *BIGGER_TOLERANCE + - step: "arm down unit" arm: - joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ "KEEP", "KEEP", 0.768227, -0.149203, 1.362128, 1.536000, 0.844977 ] common: <<: *NORMALLY tolerance: - <<: *NORMAL_TOLERANCE - - STORE_SILVER_TO_GRIPPER3: - - step: "silver rotator up" - silver_rotator: - target: 1.57 - - step: "open silver gripper" - gripper: - <<: *OPEN_S_GRIPPER - - step: "open gripper3" + <<: *BIGGER_TOLERANCE + - step: "gripper open" gripper: - <<: *OPEN_S3 - - step: "ready to store" + <<: *OPEN_GRIPPER + - step: "arm leave unit" arm: - joints: [ *JOINT1_READY_STORE_POS, *JOINT2_STORE_TO_S3_POS, *JOINT3_STORE_TO_S3_POS, *JOINT4_L90_POS, *JOINT5_STORE_TO_S3_POS, *JOINT6_HOME_POS ] + joints: [ "KEEP", -2.3271, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - step: "store" + - step: "arm ready get second unit" arm: - joints: [ *JOINT1_STORE_POS, *JOINT2_STORE_TO_S3_POS, *JOINT3_STORE_TO_S3_POS, *JOINT4_L90_POS, *JOINT5_STORE_TO_S3_POS, *JOINT6_HOME_POS ] + joints: [ 0.136601, -0.002062, -0.165398, 0.164752, 0.607200, -0.00001, 0.471600 ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - step: "close main gripper" + GET_UNITS_DOWN00: + - step: "gripper close" gripper: - <<: *CLOSE_MAIN_GRIPPER - - step: "add stone" - stone_num: - change: "+s3" - - step: "lift up arm" - arm: - joints: [ *JOINT1_READY_STORE_POS, *JOINT2_STORE_TO_S3_POS, *JOINT3_STORE_TO_S3_POS, *JOINT4_L90_POS, *JOINT5_STORE_TO_S3_POS, *JOINT6_HOME_POS ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "left arm" + <<: *CLOSE_GRIPPER + - step: "arm down unit" arm: - joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ 0.130791, -0.460589, 0.141, 0.121, 0.624274, -0.601120, 0.583819 ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - GROUND_STONE: - - step: "middle pitch up" - middle_pitch: - target: *MIDDLE_PITCH_NORMAL - delay: 0.3 - - step: "arm ready get ground stone" + - step: "ready store" arm: - joints: [*JOINT1_GET_GROUND_POS, 0.94, -1.266, *JOINT4_ZERO_POS, 1.112 , -0.397] + joints: [ 0.445447, -2.517585, -0.464470, 0.684690, 1.505160, 1.6, 1.2576 ] common: <<: *NORMALLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "gimbal front" - gimbal: - <<: *FRONT_POS - GROUND_STONE0: - - step: "move arm to get ground stone" + <<: *BIGGER_TOLERANCE + chassis: + <<: *CHASSIS_LEFT_30 + - step: "arm down unit" arm: - joints: [*JOINT1_GET_GROUND_POS, 0.943, -1.863, *JOINT4_ZERO_POS, 1.112 , -0.397] + joints: [ "KEEP", -2.517585, 0.194878, 0.764, 1.525415, 1.6, 0.539 ] common: <<: *NORMALLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "open main gripper" + <<: *BIGGER_TOLERANCE + - step: "gripper open" gripper: - <<: *OPEN_MAIN_GRIPPER - - step: "get ground stone" - middle_pitch: - target: 0.49 - delay: 0.5 - - step: "arm pull up stone" + <<: *OPEN_GRIPPER + - step: "arm leave unit" arm: - joints: [0.2, 0.943, -1.863, *JOINT4_ZERO_POS, 1.112 , -0.397] + joints: [ "KEEP", -1.855548, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - GROUND_STONE00: - - step: "ready stuck stone" + - step: "arm ready get third unit" arm: - joints: [ 0.24, 1.049, -2.4010, -0.225, 1.112, -0.11] + joints: [ 0.098978, -0.0028, -0.1699, 0.095101, 2.07, -0.0001, 0.025798 ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - step: "stuck stone" + GET_UNITS_DOWN000: + - step: "gripper close" + gripper: + <<: *CLOSE_GRIPPER + - step: "arm down unit" arm: - joints: [ 0.16, 1.049, -2.4010, -0.225, 1.112, -0.11] + joints: [ 0.098978, 0.475, 0.116, 0.097, 2.07, 0.575391, -0.202378 ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - - - - - GET_STORED_SILVER1: - - step: "middle pitch up" - middle_pitch: - target: *MIDDLE_PITCH_NORMAL - delay: 0.3 - - step: "ready to get silver from gripper1" + - step: "home" arm: - joints: [*JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE + <<: *HOME + - step: "chassis back" + chassis: + <<: *CHASSIS_TURN_BACK - - step: "open main gripper" - gripper: - <<: *OPEN_MAIN_GRIPPER - - step: "get silver" + READY_ASSEMBLE: + - step: "ready assemble" arm: - joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1 ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "close gripper1" + <<: *READY_ASSEMBLE + + GET_STORED_FRONT: + - step: "gripper open" gripper: - <<: *CLOSE_S1 - - step: "minus stone" - stone_num: - change: "-s1" - - step: "lift arm" - arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1 ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "left arm" + <<: *OPEN_GRIPPER + - step: "ready get stored front1" arm: - joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ 0.445447, -1.638026, 0.163229, 0.5, 1.418200, 1.599, -2.6199 ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - GET_STORED_SILVER2: &GET_STORED_S2 - - step: "middle pitch up" - middle_pitch: - target: *MIDDLE_PITCH_NORMAL - delay: 0.3 - - step: "ready to get silver from gripper2" + - step: "ready get stored front2" arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S2 ] + joints: [ 0.445447, -1.638026, 0.163229, 0.791120, 1.418200, 1.599, -2.6199 ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - step: "open main gripper" - gripper: - <<: *OPEN_MAIN_GRIPPER - - step: "store" + - step: "arm forward unit" arm: - joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S2 ] + joints: [ "KEEP", -2.459872, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - step: "close gripper2" + - step: "gripper close" gripper: - <<: *CLOSE_S2 - - step: "minus stone" - stone_num: - change: "-s2" - - step: "lift arm" + <<: *CLOSE_GRIPPER + - step: "arm up unit" arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S2 ] + joints: [ "KEEP", -2.517585, -0.464470, 0.684690, 1.505160, 1.6, -1.960691 ] common: <<: *NORMALLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "left arm" + <<: *BIGGER_TOLERANCE + - step: "ready assemble" arm: - joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S2 ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE + <<: *READY_ASSEMBLE - GET_STORED_SILVER3: &GET_STORED_S3 - - step: "middle pitch up" - middle_pitch: - target: *MIDDLE_PITCH_NORMAL - delay: 0.3 - - step: "lift arm" + GET_STORED_BACK: + - step: "gripper open" + gripper: + <<: *OPEN_GRIPPER + - step: "ready get stored back" arm: - joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S3 ] + joints: [ 0.445447, -2.263069, 0.654099, -0.073931, 1.362128, 1.59, -2.306812 ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - step: "ready to get silver from gripper3" + - step: "arm forward unit" arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S3 ] + joints: [ "KEEP", -2.724, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - step: "open main gripper" + - step: "gripper close" gripper: - <<: *OPEN_MAIN_GRIPPER - - step: "get ore" + <<: *CLOSE_GRIPPER + - step: "arm up unit" arm: - joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_S3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S3 ] + joints: [ "KEEP", -2.724, 0.194878, 0.043058, 1.530397, 1.566000, -2.027769 ] common: <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "close gripper3" - gripper: - <<: *CLOSE_S3 - - step: "minus stone" - stone_num: - change: "-s3" - - step: "left arm" - arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S3 ] - common: - <<: *SLOWLY tolerance: <<: *BIGGER_TOLERANCE - - step: "left arm" + - step: "ready assemble" arm: - joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S3 ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE + <<: *READY_ASSEMBLE - GET_STORED_GOLD: - - step: "middle pitch up" - middle_pitch: - target: *MIDDLE_PITCH_NORMAL - delay: 0.3 - - step: "open main gripper" - gripper: - <<: *OPEN_MAIN_GRIPPER - - step: "ready to get stored gold" + STORE_UNIT_BACK: + - step: "ready assemble" arm: - joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_L_90 ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "get stored gold" + <<: *READY_ASSEMBLE + - step: "ready store" arm: - joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_GET_STORED_GOLD_POS, *JOINT4_GET_STORED_GOLD_POS, *JOINT5_GET_STORED_GOLD_POS, *JOINT6_L_90] + joints: [ 0.445447, -2.733721, 0.194878, 0.043058, 1.530397, 1.536000, -2.027769 ] common: <<: *NORMALLY tolerance: <<: *BIGGER_TOLERANCE - - step: "close gold gripper" - gripper: - <<: *CLOSE_GOLD - - step: "move gold gripper back" - gold_pusher: - target: 0.00001 - - step: "add stone" - stone_num: - change: "+g" - - step: "move out gold" + STORE_UNIT_BACK0: + - step: "arm down unit" arm: - joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_L_90 ] + joints: [ 0.445447, -2.733721, 0.768227, -0.149203, 1.362128, 1.536000, -2.259214 ] common: <<: *NORMALLY tolerance: <<: *BIGGER_TOLERANCE - - step: "lift up gold" + - step: "gripper open" + gripper: + <<: *OPEN_GRIPPER + - step: "arm leave unit" arm: - joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_L_90 ] + joints: [ "KEEP", -2.3271, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE + - step: "ready assemble" + arm: + <<: *READY_ASSEMBLE - ARM_THREE_SILVER: - - step: "middle pitch up" - middle_pitch: - target: *MIDDLE_PITCH_NORMAL - - step: "gimbal look right" - gimbal: - <<: *RIGHT_POS - - step: "silver rotator up" - silver_rotator: - target: 1.57 - - step: "open silver gripper" - gripper: - <<: *OPEN_S_GRIPPER - - step: "ready get small island L silver" + STORE_UNIT_FRONT: + - step: "ready assemble" arm: - joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_R_POS, *JOINT3_GET_SMALL_ISLAND_R_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_SMALL3 ] + <<: *READY_ASSEMBLE + - step: "ready store" + arm: + joints: [ 0.445447, -2.517585, -0.464470, 0.684690, 1.505160, 1.6, -1.960691 ] common: <<: *NORMALLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "open s2" - gripper: - <<: *OPEN_S2 - - step: "open main gripper" - gripper: - <<: *OPEN_MAIN_GRIPPER - ARM_THREE_SILVER0: - - step: "get small island silver" + <<: *BIGGER_TOLERANCE + STORE_UNIT_FRONT0: + - step: "arm down unit" arm: - joints: [ *JOINT1_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_R_POS, *JOINT3_GET_SMALL_ISLAND_R_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_SMALL3 ] + joints: [ "KEEP", -2.517585, 0.194878, 0.788450, 1.525415, 1.6, -2.59 ] common: <<: *NORMALLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "pull up silver" + <<: *BIGGER_TOLERANCE + - step: "gripper open" + gripper: + <<: *OPEN_GRIPPER + - step: "arm leave unit" arm: - joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_R_POS, *JOINT3_GET_SMALL_ISLAND_R_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_SMALL3 ] + joints: [ "KEEP", -1.855548, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - step: "ready to store" + - step: "ready assemble" arm: - joints: [ *JOINT1_READY_STORE_POS, *JOINT2_STORE_TO_S2_POS, *JOINT3_STORE_TO_S2_POS, *JOINT4_L90_POS, *JOINT5_STORE_TO_S2_POS, *JOINT6_HOME_POS ] + <<: *READY_ASSEMBLE + + STORE_UNIT_BACK_R: + - step: "ready store" + arm: + joints: [ 0.445447, -2.733721, 0.194878, 0.043058, 1.530397, 1.536000, 1.153721 ] common: <<: *NORMALLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "store" + <<: *BIGGER_TOLERANCE + STORE_UNIT_BACK_R0: + - step: "arm down unit" arm: - joints: [ *JOINT1_STORE_POS, *JOINT2_STORE_TO_S2_POS, *JOINT3_STORE_TO_S2_POS, *JOINT4_L90_POS, *JOINT5_STORE_TO_S2_POS, *JOINT6_HOME_POS ] + joints: [ 0.445447, -2.733721, 0.768227, -0.149203, 1.362128, 1.536000, 0.844977 ] common: <<: *NORMALLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "close main gripper" + <<: *BIGGER_TOLERANCE + - step: "gripper open" gripper: - <<: *CLOSE_MAIN_GRIPPER - - step: "add stone" - stone_num: - change: "+s2" - - step: "lift up arm" + <<: *OPEN_GRIPPER + - step: "arm leave unit" arm: - joints: [ *JOINT1_READY_STORE_POS, *JOINT2_STORE_TO_S2_POS, *JOINT3_STORE_TO_S2_POS, *JOINT4_L90_POS, *JOINT5_STORE_TO_S2_POS, *JOINT6_HOME_POS ] + joints: [ "KEEP", -2.3271, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - ############################################# 2nd silver - - - step: "ready get small island M silver" + - step: "ready assemble" arm: - joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_M_POS, *JOINT3_GET_SMALL_ISLAND_M_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_SMALL2 ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "open main gripper" - gripper: - <<: *OPEN_MAIN_GRIPPER + <<: *READY_ASSEMBLE - - step: "get small island silver" + STORE_UNIT_FRONT_R: + - step: "ready store" arm: - joints: [ *JOINT1_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_M_POS, *JOINT3_GET_SMALL_ISLAND_M_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_SMALL2 ] + joints: [ 0.445447, -2.517585, -0.464470, 0.684690, 1.505160, 1.6, 1.2576 ] common: <<: *NORMALLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "pull up silver" + <<: *BIGGER_TOLERANCE + STORE_UNIT_FRONT_R0: + - step: "arm down unit" arm: - joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_M_POS, *JOINT3_GET_SMALL_ISLAND_M_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_SMALL2 ] + joints: [ "KEEP", -2.517585, 0.194878, 0.788450, 1.525415, 1.6, 0.539 ] common: <<: *NORMALLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "ready to store" + <<: *BIGGER_TOLERANCE + - step: "gripper open" + gripper: + <<: *OPEN_GRIPPER + - step: "arm leave unit" arm: - joints: [ *JOINT1_READY_STORE_POS, *JOINT2_STORE_TO_S3_POS, *JOINT3_STORE_TO_S3_POS, *JOINT4_L90_POS, *JOINT5_STORE_TO_S3_POS, *JOINT6_HOME_POS ] + joints: [ "KEEP", -1.855548, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - step: "store" + - step: "ready assemble" + arm: + <<: *READY_ASSEMBLE + + + + TEST_EE_MOTION0: + - step: "test ee motion" arm: - joints: [ *JOINT1_STORE_POS, *JOINT2_STORE_TO_S3_POS, *JOINT3_STORE_TO_S3_POS, *JOINT4_L90_POS, *JOINT5_STORE_TO_S3_POS, *JOINT6_HOME_POS ] + end_effector: + tolerance_position: 0.05 + tolerance_orientation: 0.1 + frame: "link7" + xyz: [ 0.00, 0.00, 0.10 ] + rpy: [ 0.00, 0.00, 0.00 ] + cartesian: true common: <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "open s3" - gripper: - <<: *OPEN_S3 - - step: "close main gripper" - gripper: - <<: *CLOSE_MAIN_GRIPPER - - step: "add stone" - stone_num: - change: "+s3" - - step: "lift up arm" + TEST_EE_MOTION1: + - step: "test ee motion" arm: - joints: [ *JOINT1_READY_STORE_POS, *JOINT2_STORE_TO_S3_POS, *JOINT3_STORE_TO_S3_POS, *JOINT4_L90_POS, *JOINT5_STORE_TO_S3_POS, *JOINT6_HOME_POS ] + end_effector: + tolerance_position: 0.05 + tolerance_orientation: 0.1 + frame: "link7" + xyz: [ 0.00, 0.10, 0.00 ] + rpy: [ 0.00, 0.00, 0.00 ] + cartesian: true common: <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - ####################################################### 3rd silver - - - step: "ready get small island silver" + TEST_EE_MOTION2: + - step: "test ee motion" arm: - joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_L_POS, *JOINT3_GET_SMALL_ISLAND_L_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_SMALL1 ] + end_effector: + tolerance_position: 0.05 + tolerance_orientation: 0.1 + frame: "link7" + xyz: [ 0.10, 0.00, 0.00 ] + rpy: [ 0.00, 0.00, 0.00 ] + cartesian: true common: <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "open main gripper" - gripper: - <<: *OPEN_MAIN_GRIPPER - - step: "get small island silver" + TEST_EE_MOTION3: + - step: "test ee motion" arm: - joints: [ *JOINT1_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_L_POS, *JOINT3_GET_SMALL_ISLAND_L_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_SMALL1 ] + end_effector: + tolerance_position: 0.05 + tolerance_orientation: 0.1 + frame: "link7" + xyz: [ 0.00, 0.00, -0.10 ] + rpy: [ 0.00, 0.00, 0.00 ] + cartesian: true common: <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "pull up silver" + TEST_EE_MOTION4: + - step: "test ee motion" arm: - joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_L_POS, *JOINT3_GET_SMALL_ISLAND_L_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_SMALL1 ] + end_effector: + tolerance_position: 0.05 + tolerance_orientation: 0.1 + frame: "link7" + xyz: [ 0.00, -0.10, 0.00 ] + rpy: [ 0.00, 0.00, 0.00 ] + cartesian: true common: <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - GRIPPER_THREE_SILVER: - - step: "open gripper" - gripper: - <<: *OPEN_S1 - - step: "open gripper" - gripper: - <<: *OPEN_S2 - - step: "open gripper" - gripper: - <<: *OPEN_S3 - - step: "open silver gripper" - gripper: - <<: *OPEN_S_GRIPPER - - step: "lift gimbal up" + TEST_EE_MOTION5: + - step: "test ee motion" arm: - joints: [ *JOINT1_GET_SMALL_POS, *JOINT2_AVOID_CONTROLLER_POS, *JOINT3_HOME_POS, *JOINT4_HOME_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS ] + end_effector: + tolerance_position: 0.05 + tolerance_orientation: 0.1 + frame: "link7" + xyz: [ -0.10, 0.00, 0.00 ] + rpy: [ 0.00, 0.00, 0.00 ] + cartesian: true common: <<: *NORMALLY - tolerance: - <<: *BIGGER_TOLERANCE - - step: "gimbal look right" - gimbal: - <<: *RIGHT_POS - - step: "rotate down" - silver_rotator: - target: 0.0001 - - step: "lifter ready" - silver_lifter: - target: 0.075 - - step: "pusher back" - silver_pusher: - target: -0.00001 - - step: "gimbal look back" - gimbal: - <<: *BACK_POS - GRIPPER_THREE_SILVER0: - - step: "get silver" - silver_pusher: - target: -0.042 - GRIPPER_THREE_SILVER00: - - step: "lifter ready" - silver_lifter: - target: 0.08 - - step: "pusher back little" - silver_pusher: - target: -0.02 - delay: 0.5 - - step: "rotate up" - silver_rotator: - target: 0.1 - - step: "lifter up" - silver_lifter: - target: 0.2 - delay: 0.5 - GRIPPER_THREE_SILVER000: - - step: "pusher back" - silver_pusher: - target: 0.0001 - - step: "rotate up" - silver_rotator: - target: 1.64 - delay: 0.4 - - step: "lifter down" - silver_lifter: - target: 0.05 - delay: 0.3 - - step: "lifter down" - silver_lifter: - target: 0.0 - - step: "add stone1" - stone_num: - change: "+s1" - - step: "add stone1" - stone_num: - change: "+s2" - - step: "add stone1" - stone_num: - change: "+s3" - - step: "gimbal look right" - gimbal: - <<: *RIGHT_POS - - step: "gimbal look front" - gimbal: - <<: *FRONT_POS - - step: "arm home pos" - arm: - joints: [ *JOINT1_HOME_POS, *JOINT2_HOME_POS, *JOINT3_HOME_POS, *JOINT4_HOME_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - MID_BIG_ISLAND: - - step: "gimbal look right" - gimbal: - <<: *RIGHT_POS - - step: "pusher back" - gold_pusher: - target: 0. - - step: "lifter ready" - gold_lifter: - target: 0.00001 - - step: "open gold gripper" - gripper: - <<: *OPEN_GOLD - MID_BIG_ISLAND0: - - step: "get gold" - gold_pusher: - target: -0.46 - MID_BIG_ISLAND00: - - step: "pull gold back" - gold_pusher: - target: -0.43 - - step: "lift gold" - gold_lifter: - target: 0.058 - MID_BIG_ISLAND000: - - step: "pull back gold" - gold_pusher: - target: -0.054 - - step: "add stone" - stone_num: - change: "+g" - - step: "gimbal look front" - gimbal: - <<: *FRONT_POS - - MID_BIG_ISLAND1: - - step: "get gold" - gold_pusher: - target: -0.44 - MID_BIG_ISLAND11: - - step: "pull gold back" - gold_pusher: - target: -0.42 - - step: "lift gold" - gold_lifter: - target: 0.062 - delay: 0.7 - - step: "pull back gold" - gold_pusher: - target: -0.054 - - step: "add stone" - stone_num: - change: "+g" - - step: "gimbal look front" - gimbal: - <<: *FRONT_POS - - - - SIDE_BIG_ISLAND: - - step: "gimbal look right" - gimbal: - <<: *RIGHT_POS - - step: "middle pitch up" - middle_pitch: - target: *MIDDLE_PITCH_NORMAL - delay: 0.3 - - step: "arm ready" - arm: - joints: [ 0.040,0.885,-2.107,*JOINT4_R90_POS,-0.00001, *JOINT6_L_90 ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "open main gripper" - gripper: - <<: *OPEN_MAIN_GRIPPER - SIDE_BIG_ISLAND1: - - step: "gimbal look ee" - gimbal: - <<: *FOLLOW_EE - - step: "arm get gold" - arm: - joints: [ 0.040,0.147,-1.429,*JOINT4_R90_POS, 0.08, *JOINT6_L_90 ] - common: - <<: *NORMALLY - tolerance: - <<: *BIGGER_TOLERANCE - SIDE_BIG_ISLAND11: - - step: "gimbal look right" - gimbal: - <<: *RIGHT_POS - SIDE_BIG_ISLAND111: - - step: "move stone towards down" - arm: - joints: [ 0.127, 0.997, -1.163, 0.0001, 1.57, -1.57 ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "gimbal look front" - gimbal: - <<: *FRONT_POS - - SIDE_BIG_ISLAND0: - - step: "arm get gold" - arm: - joints: [ 0.040,0.147,-1.429,*JOINT4_R90_POS, 0.08, *JOINT6_HOME_POS ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "arm lift gold" - arm: - joints: [ 0.114,0.147,-1.429,*JOINT4_R90_POS, 0.08, *JOINT6_HOME_POS ] - common: - <<: *SLOWLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "gimbal look front" - gimbal: - <<: *FRONT_POS - - step: "arm pull back gold" - arm: - joints: [ 0.114,0.988,-1.927, *JOINT4_R90_POS,0.344, *JOINT6_HOME_POS ] - common: - <<: *SLOWLY - tolerance: - <<: *BIGGER_TOLERANCE - SIDE_BIG_ISLAND00: - - step: "ready stuck stone" - arm: - joints: [ 0.24, 1.049, -2.4010, -0.225, 1.112, -0.11 ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "stuck stone" - arm: - joints: [ 0.16, 1.049, -2.4010, -0.225, 1.112, -0.11 ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "get ground stone" - middle_pitch: - target: 0.49 - delay: 0.5 - - BOTH_BIG_ISLAND: - - step: "middle pitch up" - middle_pitch: - target: *MIDDLE_PITCH_NORMAL - delay: 0.3 - - step: "gimbal look right" - gimbal: - <<: *RIGHT_POS - - step: "open main gripper" - gripper: - <<: *OPEN_MAIN_GRIPPER - - step: "gimbal look right" - gimbal: - <<: *RIGHT_POS - - step: "pusher back" - gold_pusher: - target: 0. - - step: "lifter ready" - gold_lifter: - target: 0.00001 - - step: "open gold gripper" - gripper: - <<: *OPEN_GOLD - - step: "middle pitch up" - middle_pitch: - target: *MIDDLE_PITCH_HIGHEST - delay: 0.3 - - step: "arm ready" - arm: - joints: [ 0.040,0.885,-2.107,*JOINT4_R90_POS,-0.00001, *JOINT6_L_90 ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "open main gripper" - gripper: - <<: *OPEN_MAIN_GRIPPER - - BOTH_BIG_ISLAND0: - - step: "get gold" - gold_pusher: - target: -0.46 - - step: "arm get gold" - arm: - joints: [ 0.040,0.147,-1.429,*JOINT4_R90_POS, 0.08, *JOINT6_HOME_POS ] - common: - <<: *NORMALLY - tolerance: - <<: *BIGGER_TOLERANCE - BOTH_BIG_ISLAND00: - - step: "gimbal look right" - gimbal: - <<: *RIGHT_POS - - step: "pull gold" - gold_pusher: - target: -0.43 - - step: "lift gold" - gold_lifter: - target: 0.058 - - step: "arm lift gold" - arm: - joints: [ 0.114,0.147,-1.429,*JOINT4_R90_POS, 0.08, *JOINT6_HOME_POS ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - BOTH_BIG_ISLAND000: - - step: "pull back gold" - gold_pusher: - target: -0.054 - - step: "add stone" - stone_num: - change: "+g" - - step: "arm pull back gold" - arm: - joints: [ 0.104,0.988,-1.927, *JOINT4_R90_POS,0.344, *JOINT6_HOME_POS ] - common: - <<: *SLOWLY - tolerance: - <<: *BIGGER_TOLERANCE - BOTH_BIG_ISLAND0000: - - step: "gimbal look right" - gimbal: - <<: *FRONT_POS - - step: "pull back gold" - gold_pusher: - target: -0.054 - BOTH_BIG_ISLAND00000: - - step: "ready stuck stone" - arm: - joints: [ 0.24, 1.049, -2.4010, -0.225, 1.112, -0.11 ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "stuck stone" - arm: - joints: [ 0.16, 1.049, -2.4010, -0.225, 1.112, -0.11 ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "get ground stone" - middle_pitch: - target: 0.49 - delay: 0.5 - - - BOTH_BIG_ISLAND1: - - step: "get gold" - gold_pusher: - target: -0.5 - - step: "arm get gold" - arm: - joints: [ 0.040,0.147,-1.429,*JOINT4_R90_POS, 0.08, *JOINT6_L_90 ] - common: - <<: *NORMALLY - tolerance: - <<: *BIGGER_TOLERANCE - - step: "pull gold" - gold_pusher: - target: -0.444 - delay: 0.2 - - step: "lift gold" - gold_lifter: - target: 0.062 - - step: "gimbal look ee" - gimbal: - <<: *FOLLOW_EE - BOTH_BIG_ISLAND11: - - step: "gimbal look right" - gimbal: - <<: *RIGHT_POS - BOTH_BIG_ISLAND111: - - step: "pull back gold" - gold_pusher: - target: -0.054 - - step: "add stone" - stone_num: - change: "+g" - - step: "gimbal look front" - gimbal: - <<: *FRONT_POS - - step: "move stone towards down" - arm: - joints: [ 0.127, 0.997, -1.163, 0.0001, 1.57, -1.57 ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - OM: - - step: "main open" - gripper: - <<: *OPEN_MAIN_GRIPPER - CM: - - step: "main close" - gripper: - <<: *CLOSE_MAIN_GRIPPER - OS1: - - step: "s1 open" - gripper: - <<: *OPEN_S1 - CS1: - - step: "s1 close" - gripper: - <<: *CLOSE_S1 - OS2: - - step: "s2 open" - gripper: - <<: *OPEN_S2 - CS2: - - step: "s2 close" - gripper: - <<: *CLOSE_S2 - OS3: - - step: "s3 open" - gripper: - <<: *OPEN_S3 - CS3: - - step: "s3 close" - gripper: - <<: *CLOSE_S3 - OG: - - step: "g1 open" - gripper: - <<: *OPEN_GOLD - CG: - - step: "g1 close" - gripper: - <<: *CLOSE_GOLD - OSG: - - step: "g1 close" - gripper: - <<: *OPEN_S_GRIPPER - CSG: - - step: "close silver gripper" - gripper: - <<: *CLOSE_S_GRIPPER - - CA: - - step: "main close" - gripper: - <<: *CLOSE_MAIN_GRIPPER - - step: "s1 close" - gripper: - <<: *CLOSE_S1 - - step: "s2 close" - gripper: - <<: *CLOSE_S2 - - step: "s3 close" - gripper: - <<: *CLOSE_S3 - - step: "g1 close" - gripper: - <<: *CLOSE_GOLD - - step: "close silver gripper" - gripper: - <<: *CLOSE_S_GRIPPER - - OA: - - step: "main OPEN" - gripper: - <<: *OPEN_MAIN_GRIPPER - - step: "s1 OPEN" - gripper: - <<: *OPEN_S1 - - step: "s2 OPEN" - gripper: - <<: *OPEN_S2 - - step: "s3 OPEN" - gripper: - <<: *OPEN_S3 - - step: "g1 OPEN" - gripper: - <<: *OPEN_GOLD - - step: "OPEN silver gripper" - gripper: - <<: *OPEN_S_GRIPPER - - LV5_L_S1: - - step: "middle pitch up" - middle_pitch: - target: *MIDDLE_PITCH_NORMAL - delay: 0.6 - - step: "lift arm" - arm: - joints: [ *JOINT1_READY_GET_STORED_POS, -1.2, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] - common: - <<: *NORMALLY - tolerance: - <<: *BIGGER_TOLERANCE - - step: "ready to get silver from gripper1" - arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1 ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - - step: "open main gripper" - gripper: - <<: *OPEN_MAIN_GRIPPER - - step: "get silver" - arm: - joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1 ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "close gripper1" - gripper: - <<: *CLOSE_S1 - - step: "minus stone" - stone_num: - change: "-s1" - - step: "lift arm" - arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1 ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "close silver gripper" - gripper: - <<: *CLOSE_S_GRIPPER - - step: "lift arm" - arm: - joints: [ *JOINT1_READY_GET_STORED_POS, -1.7, -1.263, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] - common: - <<: *NORMALLY - tolerance: - <<: *BIGGER_TOLERANCE - - step: "rotate down" - silver_rotator: - target: 0.0001 - - step: "arm move back" - arm: - joints: [ *JOINT1_EXCHANGE_POS, 0.92, -2.38, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "arm enter exchange pos" - arm: - <<: *LV5_L_EXCHANGE - - step: "gimbal ready" - gimbal: - <<: *RIGHT_POS - - - - LV5_L_S2: - - step: "middle pitch up" - middle_pitch: - target: *MIDDLE_PITCH_NORMAL - delay: 0.6 - - step: "lift arm" - arm: - joints: [ *JOINT1_READY_GET_STORED_POS, -1.2, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] - common: - <<: *NORMALLY - tolerance: - <<: *BIGGER_TOLERANCE - - step: "ready to get silver from gripper2" - arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S2 ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "open main gripper" - gripper: - <<: *OPEN_MAIN_GRIPPER - - step: "store" - arm: - joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S2 ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "close gripper2" - gripper: - <<: *CLOSE_S2 - - step: "minus stone" - stone_num: - change: "-s2" - - step: "lift arm" - arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S2 ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "lift arm" - arm: - joints: [ *JOINT1_READY_GET_STORED_POS, -1.7, -1.263, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] - common: - <<: *NORMALLY - tolerance: - <<: *BIGGER_TOLERANCE - - step: "arm move back" - arm: - joints: [ *JOINT1_EXCHANGE_POS, 0.92, -2.38, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "arm enter exchange pos" - arm: - <<: *LV5_L_EXCHANGE - - step: "gimbal ready" - gimbal: - <<: *RIGHT_POS - - - - LV5_L_S3: - - step: "middle pitch up" - middle_pitch: - target: *MIDDLE_PITCH_NORMAL - delay: 0.6 - - step: "lift arm" - arm: - joints: [ *JOINT1_READY_GET_STORED_POS, -1.2, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] - common: - <<: *NORMALLY - tolerance: - <<: *BIGGER_TOLERANCE - - step: "ready to get silver from gripper3" - arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S3 ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "open main gripper" - gripper: - <<: *OPEN_MAIN_GRIPPER - - step: "get ore" - arm: - joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_S3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S3 ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "close gripper3" - gripper: - <<: *CLOSE_S3 - - step: "minus stone" - stone_num: - change: "-s3" - - step: "left arm" - arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S3 ] - common: - <<: *NORMALLY - tolerance: - <<: *BIGGER_TOLERANCE - - step: "lift arm" - arm: - joints: [ *JOINT1_READY_GET_STORED_POS, -1.7, -1.263, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] - common: - <<: *NORMALLY - tolerance: - <<: *BIGGER_TOLERANCE - - step: "arm move back" - arm: - joints: [ *JOINT1_EXCHANGE_POS, 0.92, -2.38, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "arm enter exchange pos" - arm: - <<: *LV5_L_EXCHANGE - - step: "gimbal ready" - gimbal: - <<: *RIGHT_POS - LV5_L_G: - - step: "rotate down" - silver_rotator: - target: 0.000001 - - step: "middle pitch up" - middle_pitch: - target: *MIDDLE_PITCH_NORMAL - delay: 0.6 - - step: "ready to get stored gold" - arm: - joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_L_90 ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "open main gripper" - gripper: - <<: *OPEN_MAIN_GRIPPER - - step: "get stored gold" - arm: - joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_GET_STORED_GOLD_POS, *JOINT6_L_90 ] - common: - <<: *NORMALLY - tolerance: - <<: *BIGGER_TOLERANCE - - step: "close gold gripper" - gripper: - <<: *CLOSE_GOLD - - step: "move gold gripper back" - gold_pusher: - target: 0.00001 - delay: 0.3 - - step: "add stone" - stone_num: - change: "-g" - - step: "move out gold" - arm: - joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_GET_STORED_GOLD_POS, *JOINT6_L_90 ] - common: - <<: *NORMALLY - tolerance: - <<: *BIGGER_TOLERANCE - - step: "lift up gold" - arm: - joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_L_90 ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "arm enter exchange pos" - arm: - <<: *LV5_L_EXCHANGE - - step: "gimbal ready" - gimbal: - <<: *RIGHT_POS - LV5_R_S1: - - step: "middle pitch up" - middle_pitch: - target: *MIDDLE_PITCH_NORMAL - delay: 0.6 - - step: "lift arm" - arm: - joints: [ *JOINT1_READY_GET_STORED_POS, -1.2, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] - common: - <<: *NORMALLY - tolerance: - <<: *BIGGER_TOLERANCE - - step: "ready to get silver from gripper1" - arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1 ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - - step: "open main gripper" - gripper: - <<: *OPEN_MAIN_GRIPPER - - step: "get silver" - arm: - joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1 ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "close gripper1" - gripper: - <<: *CLOSE_S1 - - step: "minus stone" - stone_num: - change: "-s1" - - step: "lift arm" - arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1 ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "close silver gripper" - gripper: - <<: *CLOSE_S_GRIPPER - - step: "lift arm" - arm: - joints: [ *JOINT1_READY_GET_STORED_POS, -1.7, -1.263, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] - common: - <<: *NORMALLY - tolerance: - <<: *BIGGER_TOLERANCE - - step: "rotate down" - silver_rotator: - target: 0.0001 - - step: "arm enter exchange pos" - arm: - <<: *LV5_R_EXCHANGE - - step: "gimbal ready" - gimbal: - <<: *RIGHT_POS - LV5_R_S2: - - step: "middle pitch up" - middle_pitch: - target: *MIDDLE_PITCH_NORMAL - delay: 0.6 - - step: "lift arm" - arm: - joints: [ *JOINT1_READY_GET_STORED_POS, -1.2, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] - common: - <<: *NORMALLY - tolerance: - <<: *BIGGER_TOLERANCE - - step: "ready to get silver from gripper2" - arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S2 ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "open main gripper" - gripper: - <<: *OPEN_MAIN_GRIPPER - - step: "store" - arm: - joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S2 ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "close gripper2" - gripper: - <<: *CLOSE_S2 - - step: "minus stone" - stone_num: - change: "-s2" - - step: "lift arm" - arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S2 ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "lift arm" - arm: - joints: [ *JOINT1_READY_GET_STORED_POS, -1.7, -1.263, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] - common: - <<: *NORMALLY - tolerance: - <<: *BIGGER_TOLERANCE - - step: "arm enter exchange pos" - arm: - <<: *LV5_R_EXCHANGE - - step: "gimbal ready" - gimbal: - <<: *RIGHT_POS - LV5_R_S3: - - step: "middle pitch up" - middle_pitch: - target: *MIDDLE_PITCH_NORMAL - delay: 0.6 - - step: "lift arm" - arm: - joints: [ *JOINT1_READY_GET_STORED_POS, -1.2, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] - common: - <<: *NORMALLY - tolerance: - <<: *BIGGER_TOLERANCE - - step: "ready to get silver from gripper3" - arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S3 ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "open main gripper" - gripper: - <<: *OPEN_MAIN_GRIPPER - - step: "get ore" - arm: - joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_S3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S3 ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "close gripper3" - gripper: - <<: *CLOSE_S3 - - step: "minus stone" - stone_num: - change: "-s3" - - step: "left arm" - arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S3 ] - common: - <<: *NORMALLY - tolerance: - <<: *BIGGER_TOLERANCE - - step: "lift arm" - arm: - joints: [ *JOINT1_READY_GET_STORED_POS, -1.7, -1.263, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] - common: - <<: *NORMALLY - tolerance: - <<: *BIGGER_TOLERANCE - - step: "arm enter exchange pos" - arm: - <<: *LV5_R_EXCHANGE - - step: "gimbal ready" - gimbal: - <<: *RIGHT_POS - LV5_R_G: - - step: "rotate down" - silver_rotator: - target: 0.000001 - - step: "middle pitch up" - middle_pitch: - target: *MIDDLE_PITCH_NORMAL - delay: 0.6 - - step: "ready to get stored gold" - arm: - joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "open main gripper" - gripper: - <<: *OPEN_MAIN_GRIPPER - - step: "get stored gold" - arm: - joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_GET_STORED_GOLD_POS, *JOINT6_HOME_POS ] - common: - <<: *NORMALLY - tolerance: - <<: *BIGGER_TOLERANCE - - step: "close gold gripper" - gripper: - <<: *CLOSE_GOLD - - step: "move gold gripper back" - gold_pusher: - target: 0.00001 - delay: 0.3 - - step: "add stone" - stone_num: - change: "-g" - - step: "move out gold" - arm: - joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_GET_STORED_GOLD_POS, *JOINT6_HOME_POS ] - common: - <<: *NORMALLY - tolerance: - <<: *BIGGER_TOLERANCE - - step: "lift up gold" - arm: - joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "arm enter exchange pos" - arm: - <<: *LV5_R_EXCHANGE - - step: "gimbal ready" - gimbal: - <<: *RIGHT_POS - LV4_S1: - - step: "middle pitch up" - middle_pitch: - target: *MIDDLE_PITCH_NORMAL - delay: 0.6 - - step: "ready to get silver from gripper1" - arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1 ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - - step: "open main gripper" - gripper: - <<: *OPEN_MAIN_GRIPPER - - step: "get silver" - arm: - joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1 ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "close gripper1" - gripper: - <<: *CLOSE_S1 - - step: "minus stone" - stone_num: - change: "-s1" - - step: "lift arm" - arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1 ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "close silver gripper" - gripper: - <<: *CLOSE_S_GRIPPER - - step: "arm enter exchange pos" - arm: - <<: *LV4_EXCHANGE - - step: "gimbal ready" - gimbal: - <<: *RIGHT_POS - LV4_S2: - - step: "middle pitch up" - middle_pitch: - target: *MIDDLE_PITCH_NORMAL - delay: 0.6 - - step: "ready to get silver from gripper2" - arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S2 ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "open main gripper" - gripper: - <<: *OPEN_MAIN_GRIPPER - - step: "store" - arm: - joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S2 ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "close gripper2" - gripper: - <<: *CLOSE_S2 - - step: "minus stone" - stone_num: - change: "-s2" - - step: "lift arm" - arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S2 ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "arm enter exchange pos" - arm: - <<: *LV4_EXCHANGE - - step: "gimbal ready" - gimbal: - <<: *RIGHT_POS - LV4_S3: - - step: "middle pitch up" - middle_pitch: - target: *MIDDLE_PITCH_NORMAL - delay: 0.6 - - step: "lift arm" - arm: - joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "ready to get silver from gripper3" - arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S3 ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "open main gripper" - gripper: - <<: *OPEN_MAIN_GRIPPER - - step: "get ore" - arm: - joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_S3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S3 ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "close gripper3" - gripper: - <<: *CLOSE_S3 - - step: "minus stone" - stone_num: - change: "-s3" - - step: "left arm" - arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S3 ] - common: - <<: *NORMALLY - tolerance: - <<: *BIGGER_TOLERANCE - - step: "arm enter exchange pos" - arm: - <<: *LV4_EXCHANGE - - step: "gimbal ready" - gimbal: - <<: *RIGHT_POS - LV4_G: - - step: "middle pitch up" - middle_pitch: - target: *MIDDLE_PITCH_NORMAL - delay: 0.6 - - step: "ready to get stored gold" - arm: - joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "open main gripper" - gripper: - <<: *OPEN_MAIN_GRIPPER - - step: "get stored gold" - arm: - joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_GET_STORED_GOLD_POS, *JOINT6_HOME_POS ] - common: - <<: *NORMALLY - tolerance: - <<: *BIGGER_TOLERANCE - - step: "close gold gripper" - gripper: - <<: *CLOSE_GOLD - - step: "move gold gripper back" - gold_pusher: - target: 0.00001 - - step: "add stone" - stone_num: - change: "-g" - - step: "move out gold" - arm: - joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] - common: - <<: *NORMALLY - tolerance: - <<: *BIGGER_TOLERANCE - - step: "lift up gold" - arm: - joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "arm enter exchange pos" - arm: - <<: *LV4_EXCHANGE - - step: "gimbal ready" - gimbal: - <<: *RIGHT_POS diff --git a/engineer_middleware/config/engineer_simulation_controllers.yaml b/engineer_middleware/config/engineer_simulation_controllers.yaml index 6271b5b..79eb7ef 100644 --- a/engineer_middleware/config/engineer_simulation_controllers.yaml +++ b/engineer_middleware/config/engineer_simulation_controllers.yaml @@ -6,120 +6,148 @@ controllers: type: robot_state_controller/RobotStateController publish_rate: 100 - arm_trajectory_controller: - type: effort_controllers/JointTrajectoryController + left_arm_controller: + type: gravity_compensation_controller/GravityCompensationJointTrajectoryController joints: - - joint1 - - joint2 - - joint3 - - joint4 - - joint5 - - joint6 - - joint7 + - left_joint1 + - left_joint2 + - left_joint3 + - left_joint4 + - left_joint5 + - left_joint6 + - left_joint7 gains: - joint1: { p: 20000.0, i: 0, d: 400.0, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true } - joint2: { p: 30000, i: 0, d: 1000.0, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true } - joint3: { p: 10000.0, i: 0, d: 320.0, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true } - joint4: { p: 1000.0, i: 0.0, d: 20, i_clamp_max: 0, i_clamp_min: 0.0, antiwindup: true, publish_state: true } - joint5: { p: 350.0, i: 0, d: 50.0, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true } - joint6: { p: 1000.0, i: 0, d: 13., i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true } - joint7: { p: 5000., i: 0., d: 10000.0, i_max: 0.0, i_min: 0.0, antiwindup: true, publish_state: true } + left_joint1: { p: 100, i: 2.0, d: 2.0, i_clamp_max: 5, i_clamp_min: -5, antiwindup: true, publish_state: false } + left_joint2: { p: 150, i: 5.0, d: 5.0, i_clamp_max: 6, i_clamp_min: -6, antiwindup: true, publish_state: false } + left_joint3: { p: 100, i: 1.0, d: 1.0, i_clamp_max: 2, i_clamp_min: -2, antiwindup: true, publish_state: false } + left_joint4: { p: 120, i: 5.0, d: 2.0, i_clamp_max: 3, i_clamp_min: -3, antiwindup: true, publish_state: false } + left_joint5: { p: 30, i: 1.0, d: 0.5, i_clamp_max: 1, i_clamp_min: -1, antiwindup: true, publish_state: false } + left_joint6: { p: 30, i: 1.0, d: 0.5, i_clamp_max: 1, i_clamp_min: -1, antiwindup: true, publish_state： false } + left_joint7: { p: 30, i: 1.0, d: 0.5, i_clamp_max: 1, i_clamp_min: -1, antiwindup: true, publish_state: false } constraints: - joint1: { trajectory: 0.05, goal: 0.05 } - joint2: { trajectory: 0.05, goal: 0.05 } - joint3: { trajectory: 0.05, goal: 0.05 } - joint4: { trajectory: 0.05, goal: 0.05 } - joint5: { trajectory: 0.05, goal: 0.05 } - joint6: { trajectory: 0.05, goal: 0.05 } - joint7: { trajectory: 0.05, goal: 0.05 } + goal_time: 5.0 + stopped_velocity_tolerance: 0.05 + left_joint1: { trajectory: 0.08, goal: 0.05 } + left_joint2: { trajectory: 0.08, goal: 0.05 } + left_joint3: { trajectory: 0.08, goal: 0.05 } + left_joint4: { trajectory: 0.08, goal: 0.05 } + left_joint5: { trajectory: 0.08, goal: 0.05 } + left_joint6: { trajectory: 0.08, goal: 0.05 } + left_joint7: { trajectory: 0.08, goal: 0.05 } + tau_g_scale: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0] + + right_arm_controller: + type: gravity_compensation_controller/GravityCompensationJointTrajectoryController + joints: + - right_joint1 + - right_joint2 + - right_joint3 + - right_joint4 + - right_joint5 + - right_joint6 + - right_joint7 + gains: + right_joint1: { p: 100, i: 2.0, d: 2.0, i_clamp_max: 5, i_clamp_min: -5, antiwindup: true, publish_state: false } + right_joint2: { p: 150, i: 5.0, d: 5.0, i_clamp_max: 6, i_clamp_min: -6, antiwindup: true, publish_state: false } + right_joint3: { p: 100, i: 1.0, d: 1.0, i_clamp_max: 2, i_clamp_min: -2, antiwindup: true, publish_state: false } + right_joint4: { p: 120, i: 5.0, d: 2.0, i_clamp_max: 3, i_clamp_min: -3, antiwindup: true, publish_state: false } + right_joint5: { p: 30, i: 1.0, d: 0.5, i_clamp_max: 1, i_clamp_min: -1, antiwindup: true, publish_state: false } + right_joint6: { p: 30, i: 1.0, d: 0.5, i_clamp_max: 1, i_clamp_min: -1, antiwindup: true, publish_state: false } + right_joint7: { p: 30, i: 1.0, d: 0.5, i_clamp_max: 1, i_clamp_min: -1, antiwindup: true, publish_state: false } + constraints: + goal_time: 5.0 + stopped_velocity_tolerance: 0.05 + right_joint1: { trajectory: 0.08, goal: 0.05 } + right_joint2: { trajectory: 0.08, goal: 0.05 } + right_joint3: { trajectory: 0.08, goal: 0.05 } + right_joint4: { trajectory: 0.08, goal: 0.05 } + right_joint5: { trajectory: 0.08, goal: 0.05 } + right_joint6: { trajectory: 0.08, goal: 0.05 } + right_joint7: { trajectory: 0.08, goal: 0.05 } + tau_g_scale: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0] + + gimbal_arm_controller: + type: gravity_compensation_controller/GravityCompensationJointTrajectoryController + joints: + - main_pitch + - gimbal_joint1 + - gimbal_joint2 + - gimbal_joint3 + gains: + main_pitch: { p: 350, i: 25.0, d: 7.0, i_clamp_max: 10, i_clamp_min: -10, antiwindup: true, publish_state: false } + gimbal_joint1: { p: 100.0, i: 10.0, d: 5.0, i_clamp_max: 3, i_clamp_min: -3, antiwindup: true, publish_state: false } + gimbal_joint2: { p: 100.0, i: 10.0, d: 5.0, i_clamp_max: 3, i_clamp_min: -3, antiwindup: true, publish_state: false } + gimbal_joint3: { p: 10.0, i: 1.0, d: 0.5, i_clamp_max: 1, i_clamp_min: -1, antiwindup: true, publish_state: false } + constraints: + goal_time: 5.0 + stopped_velocity_tolerance: 0.05 + main_pitch: { trajectory: 0.08, goal: 0.05 } + gimbal_joint1: { trajectory: 0.08, goal: 0.05 } + gimbal_joint2: { trajectory: 0.08, goal: 0.05 } + gimbal_joint3: { trajectory: 0.08, goal: 0.05 } + tau_g_scale: [0.0, 1.0, 1.0, 1.0] chassis_controller: - type: rm_chassis_controllers/OmniController + type: rm_chassis_controllers/SwerveController # ChassisBase publish_rate: 100 - enable_odom_tf: true + publish_map_tf: false publish_odom_tf: true + slam_topic: "/Odometry" + localization_topic: "/hdl_global_localization/result" power: - effort_coeff: 2.0 - vel_coeff: 0.008 - power_offset: -9.8 + effort_coeff: 1.8 + vel_coeff: 0.01 + power_offset: 7.0 + pivot_max_power: 50 + pivot_effort_coeff: 50 + pivot_vel_coeff: 0.01 + pivot_power_ratio: 0.8 + power_factor: -0.2 + use_rls: false + use_K_angle: true twist_angular: 0.5233 + max_odom_vel: 10.0 timeout: 0.1 - pid_follow: { p: 4.0, i: 0, d: 0.02, i_max: 0.0, i_min: 0.0, antiwindup: true, publish_state: true } + pid_follow: { p: 6.5, i: 0, d: 0.0, i_max: 0.0, i_min: 0.0, antiwindup: true, publish_state: true } twist_covariance_diagonal: [ 0.001, 0.001, 0.001, 0.001, 0.001, 0.001 ] - max_odom_vel: 10.0 - # OmniController - wheels: + + # SwerveController + modules: left_front: - pose: [ 0.213, 0.1945, 0. ] - roller_angle: -0.785 - joint: left_front_wheel_joint - <<: &wheel_setting - radius: 0.07 - pid: { p: 0.4, i: 0, d: 0.0, i_max: 0.0, i_min: 0.0, antiwindup: true, publish_state: true } + position: [ 0.205, 0.200 ] + pivot: + joint: left_front_pivot_joint + <<: &pivot_setting + buffer_threshold: 10.0 + offset: 0. + pid: { p: 4.5, i: 0, d: 0.03, i_clamp_max: 0.0, i_clamp_min: -0.0, antiwindup: true, publish_state: false } + wheel: + joint: left_front_wheel_joint + <<: &wheel_setting + radius: 0.061 + pid: { p: 0.35, i: 0, d: 0.0, i_max: 0.0, i_min: 0.0, antiwindup: true, publish_state: false } right_front: - pose: [ 0.213, -0.1945, 0. ] - roller_angle: 0.785 - joint: right_front_wheel_joint - <<: *wheel_setting + position: [ 0.205, -0.200 ] + pivot: + joint: right_front_pivot_joint + <<: *pivot_setting + wheel: + joint: right_front_wheel_joint + <<: *wheel_setting left_back: - pose: [ -0.213, 0.1945, 0. ] - roller_angle: 0.785 - joint: left_back_wheel_joint - <<: *wheel_setting + position: [ -0.205, 0.200 ] + pivot: + joint: left_back_pivot_joint + <<: *pivot_setting + wheel: + joint: left_back_wheel_joint + <<: *wheel_setting right_back: - pose: [ -0.213, -0.1945, 0. ] - roller_angle: -0.785 - joint: right_back_wheel_joint - <<: *wheel_setting - - - gimbal_controller: - type: rm_gimbal_controllers/Controller - yaw: - joint: "yaw_joint" - pid: { p: 0.5, i: 0.0, d: 0.0, i_clamp_max: 0.0, i_clamp_min: -0.0, antiwindup: true, publish_state: true } - pitch: - joint: "pitch_joint" - pid: { p: 0., i: 0.0, d: 0.0, i_clamp_max: 0.0, i_clamp_min: -0.0, antiwindup: true, publish_state: true } - feedforward: - gravity: 0.0 - enable_gravity_compensation: false - mass_origin: [ 0.0,0.0,0.0 ] - bullet_solver: - resistance_coff_qd_10: 0.45 - resistance_coff_qd_15: 1.0 - resistance_coff_qd_16: 0.7 - resistance_coff_qd_18: 0.55 - resistance_coff_qd_30: 3.0 - g: 9.81 - delay: 0.1 - dt: 0.001 - timeout: 0.001 - publish_rate: 50 - - ####################### Position Controllers ###################### - ore_bin_lifter_controller: - type: effort_controllers/JointPositionController - joint: ore_bin_lifter_joint - pid: { p: 100000., i: 0., d: 5000, i_max: 0.01, i_min: 0.01, antiwindup: true, publish_state: true } - - ore_bin_rotate_controller: - type: effort_controllers/JointPositionController - joint: ore_bin_rotate_joint - pid: { p: 0.9, i: 0., d: 0.06, i_max: 0.0, i_min: 0.0, antiwindup: true, publish_state: true } - - gimbal_lifter_controller: - type: effort_controllers/JointPositionController - joint: gimbal_lifter_joint - pid: { p: 20000., i: 0., d: 1200, i_max: 0.01, i_min: 0.01, antiwindup: true, publish_state: true } - - extend_arm_front_controller: - type: effort_controllers/JointPositionController - joint: extend_arm_front_joint - pid: { p: 3., i: 0.1, d: 0.05, i_max: 0.01, i_min: 0.01, antiwindup: true, publish_state: true } - - extend_arm_back_controller: - type: effort_controllers/JointPositionController - joint: extend_arm_back_joint - pid: { p: 3., i: 0.1, d: 0.05, i_max: 0.01, i_min: 0.01, antiwindup: true, publish_state: true } + position: [ -0.205, -0.200 ] + pivot: + joint: right_back_pivot_joint + <<: *pivot_setting + wheel: + joint: right_back_wheel_joint + <<: *wheel_setting diff --git a/engineer_middleware/config/engineer_steps_list.yaml b/engineer_middleware/config/engineer_steps_list.yaml index 257570e..c648506 100644 --- a/engineer_middleware/config/engineer_steps_list.yaml +++ b/engineer_middleware/config/engineer_steps_list.yaml @@ -1,1987 +1,1527 @@ common: speed: slowly: &SLOWLY - speed: 0.15 + speed: 0.1 accel: 0.1 - timeout: 15. + timeout: 9. normally: &NORMALLY - speed: 0.4 - accel: 0.3 - timeout: 3 + speed: 0.5 + accel: 0.4 + timeout: 6. quickly: &QUICKLY - speed: 0.6 + speed: 0.8 accel: 0.6 - timeout: 7. + timeout: 5. + maximum: &MAXIMUM + speed: 1.0 + accel: 1.0 + timeout: 4. tolerance: + # gimbal_arm/left_arm/right_arm + # main_pitch/gimbal_joint1/2/3/left_joint1/2/3/4/5/6/7/right_joint1/2/3/4/5/6/7 small_tolerance: &SMALL_TOLERANCE - tolerance_joints: [ 0.005, 0.008, 0.015, 0.1, 0.1, 0.1, 0.03 ] + tolerance_joints: [ 0.05, 0.01, 0.01, 0.01, + 0.08, 0.05, 0.03, 0.04, 0.01, 0.01, 0.01, + 0.08, 0.05, 0.03, 0.04, 0.01, 0.01, 0.01 ] normal_tolerance: &NORMAL_TOLERANCE - tolerance_joints: [ 0.012, 0.02, 0.015, 0.1, 0.2, 0.15, 0.07 ] + tolerance_joints: [ 0.08, 0.03, 0.03, 0.03, + 0.1, 0.08, 0.06, 0.08, 0.03, 0.03, 0.03, + 0.1, 0.08, 0.06, 0.08, 0.03, 0.03, 0.03 ] bigger_tolerance: &BIGGER_TOLERANCE - tolerance_joints: [ 0.015, 0.015, 0.02, 0.35, 0.2, 0.2, 0.1 ] + tolerance_joints: [ 0.1, 0.05, 0.05, 0.05, + 0.15, 0.1, 0.08, 0.1, 0.06, 0.05, 0.06, + 0.15, 0.1, 0.08, 0.1, 0.06, 0.05, 0.06 ] max_tolerance: &MAX_TOLERANCE - tolerance_joints: [ 0.03, 0.03, 0.03, 0.3, 0.3, 0.3 , 0.3] + tolerance_joints: [ 0.2, 0.08, 0.08, 0.08, + 0.2, 0.15, 0.1, 0.12, 0.08, 0.07, 0.07, + 0.2, 0.15, 0.1, 0.12, 0.08, 0.07, 0.07 ] - joint1: - mechanical: - down_position: &JOINT1_DOWN_POSITION - 0.02 - small_ready: &JOINT1_SMALL_READY - 0.487 - small_down: &JOINT1_SMALL_DOWN - 0.34 - small_ready_store: &JOINT1_SMALL_READY_STORE - 0.254 - small_store_down: &JOINT1_SMALL_STORE_DOWN - 0.1962 - small_ready_store_third: &JOINT1_SMALL_READY_STORE_THIRD - 0.387 - up_pos: &JOINT1_UP_POS - 0.312 - bin_get_stone_left: &JOINT1_BIN_GET_STONE_LEFT - 0.2 - bin_get_stone_right: &JOINT1_BIN_GET_STONE_RIGHT - 0.1248 - big_ready: &JOINT1_BIG_READY - 0.005 - big_adjust_mid: &JOINT1_BIG_ADJUST_MID - 0.079 - big_up_pos: &JOINT1_BIG_UP_POS - 0.320 - big_ready_store: &JOINT1_BIG_READY_STORE - 0.133 - bin_get_stone: &JOINT1_BIN_GET_STONE - 0.1521 - exchange_pos: &JOINT1_EXCHANGE_POS - 0.1 - home: - zero_stone_position: &JOINT1_ZERO_STONE_POSITION - 0.01 - one_stone_position: &JOINT1_ONE_STONE_POSITION - 0.156 - two_stone_position: &JOINT1_TWO_STONE_POSITION - 0.22 - three_stone_position: &JOINT1_THREE_STONE_POSITION - 0.22 - - joint2: - mechanical: - back_position: &JOINT2_BACK_POSITION - 0.047 - get_ore_position: &JOINT2_GET_ORE_POSITION - 0.1 - furthest_position: &JOINT2_FURTHEST_POSITION - 0.232 - exchange_position: &JOINT2_EXCHANGE_POSITION - 0.105 - small_ready: &JOINT2_SMALL_READY - 0.029 - small_store: &JOINT2_SMALL_STORE - 0.060290 - bin_get_stone: &JOINT2_BIN_GET_STONE - 0.1 - back_bin_position: &JOINT2_BACK_BIN_POSITION - 0.04551 - avoid_collision: &JOINT2_AVOID_COLLISION - 0.1 - big_ready: &JOINT2_BIG_READY - 0.03 - big_arm_up: &JOINT2_BIG_ARM_UP - 0.142 - big_ready_store: &JOINT2_BIG_READY_STORE - 0.0739 - - joint3: - mechanical: - left_position: &JOINT3_L_POSITION - 0.308 - mid_position: &JOINT3_MID_POSITION - 0.04 - right_position: &JOINT3_R_POSITION - -0.278 - home: &JOINT3_HOME_POS - 0.08 - small_ready_mid: &JOINT3_SMALL_READY_MID - 0.0402 - take_one: &JOINT3_TAKE_ONE - -0.052850 - small_ready_left: &JOINT3_SMALL_READY_LEFT - 0.308 - small_ready_right: &JOINT3_SMALL_READY_RIGHT - -0.233 - small_turn: &JOINT3_SMALL_TURN - 0.257 - bin_get_stone: &JOINT3_BIN_GET_STONE - 0.2528 - bin_get_lv4_stone: &JOINT3_BIN_GET_LV4_STONE - 0.294414 - big_ready: &JOINT3_BIG_READY - 0.071 - get_big_mid: &JOINT3_GET_BIG_MID - -0.299 - get_big_side: &JOINT3_GET_BIG_SIDE - -0.199 - big_pull_out: &JOINT3_BIG_PULL_OUT - 0.188 - big_arm_up: &JOINT3_BIG_ARM_UP - 0.308 - exchange: &JOINT3_EXCHANGE - 0.081 - - joint4: - mechanical: - left_position: &JOINT4_L_POSITION - 1.00 - mid_position: &JOINT4_MID_POSITION - 0.021 - right_position: &JOINT4_R_POSITION - -1.554 - small_ready: &JOINT4_SMALL_READY - 0.054 - to: &JOINT4_TAKE_ONE - 0.828930 - small_turn: &JOINT4_SMALL_TURN - -1.497 - big_store: &JOINT4_BIG_STORE - -1.525 - exchange: &JOINT4_EXCHANGE - -0.086 - pre_home: &JOINT4_PRE_HOME - -0.91 - - - joint5: - mechanical: - mid_position: &JOINT5_MID_POSITION - 0.011 - left_90_position: &JOINT5_L90_POSITION - -1.54 - right_90_position: &JOINT5_R90_POSITION - 1.59 - bin_get_stone: &JOINT5_BIN_GET_STONE - 1.53912 - big_ready: &JOINT5_BIG_READY - -0.017 - big_ready_store: &JOINT5_BIG_READY_STORE - -0.001 - big_store: &JOINT5_BIG_STORE - 1.49 - small_ready: &JOINT5_SMALL_READY - -3.1267 - small_up_stone: &JOINT5_SMALL_UP_STONE - -0.001 - small_store: &JOINT5_SMALL_STORE - 1.4662 - exchange: &JOINT5_EXCHANGE - 1.805 - - joint6: - mechanical: - mid_position: &JOINT6_MID_POSITION - 0.001 - up_position: &JOINT6_UP_POSITION - -1.569 - down_position: &JOINT6_DOWN_POSITION - 1.569 - small_ready: &JOINT6_SMALL_READY - -1.607 - small_back: &JOINT6_SMALL_BACK - -1.987 - small_ready_store: &JOINT6_SMALL_READY_STORE - -1.633 - bin_get_stone: &JOINT6_BIN_GET_STONE - -1.6036 - big_ready: &JOINT6_BIG_READY - 0.014 - big_adjust_mid: &JOINT6_BIG_ADJUST_MID - -0.119 - big_arm_up: &JOINT6_BIG_ARM_UP - -1.519 - big_ready_store: &JOINT6_BIG_READY_STORE - -1.561 - exchange: &JOINT6_EXCHANGE - 0.139 - - joint7: - mechanical: - mid_position: &JOINT7_MID_POSITION - -0.098300 - left_90_position: &JOINT7_L90_POSITION - -1.56 - right_90_position: &JOINT7_R90_POSITION - 1.62 - big_pos: &JOINT7_BIG_POS - -0.013 - big_ready_store: &JOINT7_BIG_READY_STORE - -1.522 - small_ready: &JOINT7_SMALL_READY - -0.083 - small_ready_store: &JOINT7_SMALL_READY_STORE - 1.722 - exchange: &JOINT7_EXCHANGE - -1.771 - lv4_ore: &JOINT7_LV4_ORE - -0.098300 - bin_get_left: &JOINT7_BIN_GET_LEFT - -1.5793 - to: &JOINT7_TAKE_ONE - -0.844500 - bin_get_right: &JOINT7_BIN_GET_RIGHT - 1.554 gimbal: - right_pos: &RIGHT_POS - frame: gimbal_lifter - position: [ 0.001 , -3., 0. ] - left_pos: &LEFT_POS - frame: gimbal_lifter - position: [ 0.001, 3., 0. ] - front_pos: &FRONT_POS - frame: gimbal_lifter - position: [ 2., 0.001, 0. ] - small_island_pos: &BIG_ISLAND_POS - frame: gimbal_lifter - position: [ 1., -1., 0.] - test: &GIMBAL_TEST - frame: link4 - position: [1., 0., 0.] - test2: &GIMBAL_TEST2 - frame: link7 - position: [0., 0., -1.] - - gimbal_lifter: - button_pos: &BUTTON_POS - 0.05 - mid_pos: &MID_POS - 0.14 - two_stone_pos: &TWO_STONE_POS - 0.05 - tallest_pos: &TALLEST_POS - 0.28 - big_island: &LIFTER_BIG_ISLAND - 0.230 - small_island: &LIFTER_SMALL_ISLAND - 0.05 - gl_test1: &GL_TEST1 - 0.10 - gl_test2: &GL_TEST2 - 0.15 - gl_test3: &GL_TEST3 - 0.20 - gripper: + mid_pos: &GIMBAL_MID + groups: + - gimbal_arm: [ 0.0, 0.0, 0.0, 0.0 ] + pack_up: &GIMBAL_PACK_UP + groups: + - gimbal_arm: [ "KEEP", -0.048, 0.647, 1.184 ] + front_pos: &GIMBAL_FRONT + groups: + - gimbal_arm: [ 0.0, 0.0, 0.0, 0.0 ] + back_pos: &GIMBAL_BACK + groups: + - gimbal_arm: [ 0.175, -1.177, -0.667, 2.085 ] + look_down: &GIMBAL_LOOK_DOWN + groups: + - gimbal_arm: [ 0.175, -0.786, -0.366, 2.111 ] + + left_gripper: open_gripper: &OPEN_GRIPPER - pin: 0 - state: true + command: false close_gripper: &CLOSE_GRIPPER - pin: 0 - state: false - - arm: - pre_home: &PRE_HOME - joints: [ *JOINT1_ZERO_STONE_POSITION, *JOINT2_BACK_POSITION, *JOINT3_HOME_POS, *JOINT4_PRE_HOME, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - home: &HOME - joints: [ *JOINT1_ZERO_STONE_POSITION, *JOINT2_BACK_POSITION, *JOINT3_HOME_POS, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] - common: - <<: *NORMALLY - tolerance: - <<: *SMALL_TOLERANCE + command: true + right_gripper: + open_gripper: &OPEN_GRIPPER + command: false + close_gripper: &CLOSE_GRIPPER + command: true - chassis_target: - test: &TEST - frame: base_link - chassis_tolerance_position: 0.01 - chassis_tolerance_angular: 0.01 - offset: [0., 0.] - yaw_scale: 1 - target_frame: exchanger - common: - timeout: 2. chassis: - chassis_left_315: &CHASSIS_LEFT_15 + chassis_left: &CHASSIS_LEFT_30 frame: base_link - position: [ 0., 15. ] + position: [ 0.0, 0.30 ] yaw: 0.0 - chassis_tolerance_position: 0.1 - chassis_tolerance_angular: 0.3 - common: - timeout: 2. - - chassis_180: &CHASSIS_180 - frame: base_link - position: [ 0., 0. ] - yaw: 2.80 - chassis_tolerance_position: 0.1 - chassis_tolerance_angular: 0.2 + chassis_tolerance_position: 0.05 + chassis_tolerance_angular: 0.1 common: - timeout: 2. - - chassis_90: &CHASSIS_TURN - frame: base_link - position: [ 0., 0. ] - yaw: -1.40 - chassis_tolerance_position: 0.1 - chassis_tolerance_angular: 0.2 - common: - timeout: 2. - - chassis_back: &CHASSIS_BACK + timeout: 6. +# chassis_left_60: &CHASSIS_LEFT_70 +# frame: base_link +# position: [ 0.0, 0.7 ] +# yaw: 0.0 +# chassis_tolerance_position: 0.1 +# chassis_tolerance_angular: 0.2 +# common: +# timeout: 6. +# chassis_left60: &CHASSIS_LEFT_60 +# frame: base_link +# position: [ 0.0, 0.6 ] +# yaw: 0.0 +# chassis_tolerance_position: 0.1 +# chassis_tolerance_angular: 0.2 +# common: +# timeout: 6. +# chassis_left50: &CHASSIS_LEFT_50 +# frame: base_link +# position: [ 0.0, 0.5 ] +# yaw: 0.0 +# chassis_tolerance_position: 0.1 +# chassis_tolerance_angular: 0.2 +# common: +# timeout: 6. +# chassis_back_60: &CHASSIS_BACK_60 +# frame: base_link +# position: [ -0.6, 0.0 ] +# yaw: 0.0 +# chassis_tolerance_position: 0.1 +# chassis_tolerance_angular: 0.2 +# common: +# timeout: 6. +# chassis_turn_left: &CHASSIS_TURN_LEFT +# frame: base_link +# position: [ 0.0, 0.0 ] +# yaw: 1.57 +# chassis_tolerance_position: 0.1 +# chassis_tolerance_angular: 0.2 +# common: +# timeout: 6. +# chassis_turn_right: &CHASSIS_TURN_RIGHT +# frame: base_link +# position: [ 0.0, 0.0 ] +# yaw: -1.07 +# chassis_tolerance_position: 0.1 +# chassis_tolerance_angular: 0.2 +# common: +# timeout: 6. + chassis_turn_back: &CHASSIS_TURN_BACK frame: base_link - position: [ -0.4, 0. ] - yaw: 0.0 + position: [ 0.0, 0.0 ] + yaw: 3.14 chassis_tolerance_position: 0.1 chassis_tolerance_angular: 0.2 common: timeout: 2. - ore_rotator: - ready_pos: &READY_POS - 0.001 - exchange_pos: &EXCHANGE_POS - 1.6 - - ore_lifter: - bin_up_pos: &BIN_UP_POS - 0.22 - small_up_pos: &BIN_SMALL_UP_POS - 0.01 - bin_mid_pos: &BIN_MID_POS - 0.1 - bin_down_pos: &BIN_DOWN_POS - 0.001 - bin_get_up: &BIN_GET_UP - 0.033 - bin_get_down: &BIN_GET_DOWN - 0.225 - - extend_arm: - close: &EX_CLOSE - 0.0 - open: &EX_OPEN - 2.6 - steps_list: - TEST1: - - step: "gimbal test1" - gimbal: - <<: *GIMBAL_TEST - TEST2: - - step: "gimbal test2" - gimbal: - <<: *GIMBAL_TEST2 - - EXCHANGE_POS: - - step: "gimbal look front" - gimbal: - <<: *FRONT_POS - - step: "move arm" - arm: - joints: [*JOINT1_EXCHANGE_POS, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION] + GIMBAL_BACK: + - step: "gimbal move to back pos" + arms: + <<: *GIMBAL_BACK common: - <<: *NORMALLY + <<: *QUICKLY tolerance: <<: *NORMAL_TOLERANCE - - step: "gimbal lifter tallest" - gimbal_lifter: - target: *TALLEST_POS - EXCHANGE_R: - - step: "gimbal look front" - gimbal: - <<: *FRONT_POS - - step: "move arm to exchange pos" - arm: - joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_BIN_GET_LEFT ] - common: - <<: *NORMALLY + + GIMBAL_MID: + - step: "gimbal move to mid pos" + arms: + <<: *GIMBAL_MID + common: + <<: *QUICKLY tolerance: <<: *NORMAL_TOLERANCE - - step: "gimbal lifter tallest" - gimbal_lifter: - target: *TALLEST_POS - EXCHANGE_L: - - step: "gimbal look front" - gimbal: - <<: *FRONT_POS - - step: "move arm to exchange pos" - arm: - joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_BIN_GET_RIGHT ] - common: - <<: *NORMALLY + + GIMBAL_FRONT: + - step: "gimbal move to front pos" + arms: + <<: *GIMBAL_FRONT + common: + <<: *QUICKLY tolerance: <<: *NORMAL_TOLERANCE - - step: "gimbal lifter tallest" - gimbal_lifter: - target: *TALLEST_POS - LEFT_EXCHANGE: - - step: "gimbal look front" - gimbal: - <<: *FRONT_POS - - step: "move arm to exchange pos" - arm: - joints: [*JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_POSITION, *JOINT4_MID_POSITION, *JOINT5_L90_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION] - common: - <<: *NORMALLY + + GIMBAL_PACK_UP: + - step: "gimbal pack up" + arms: + <<: *GIMBAL_PACK_UP + common: + <<: *QUICKLY tolerance: <<: *NORMAL_TOLERANCE - - step: "gimbal lifter tallest" - gimbal_lifter: - target: *TALLEST_POS - RIGHT_EXCHANGE: - - step: "gimbal look front" - gimbal: - <<: *FRONT_POS - - step: "move arm to exchange pos" - arm: - joints: [*JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_POSITION, *JOINT4_MID_POSITION, *JOINT5_R90_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION] - common: - <<: *NORMALLY + + GIMBAL_LOOK_DOWN: + - step: "gimbal look down" + arms: + <<: *GIMBAL_LOOK_DOWN + common: + <<: *QUICKLY tolerance: <<: *NORMAL_TOLERANCE - - step: "gimbal lifter tallest" - gimbal_lifter: - target: *TALLEST_POS - ############ MID ############# - - ############################ HOME ########################## - HOME: - - step: "close gripper" - gripper: - <<: *CLOSE_GRIPPER - - step: "pre_home" - arm: - <<: *PRE_HOME - - step: "gimbal look front" - gimbal: - <<: *FRONT_POS - - step: "gimbal lifter down" - gimbal_lifter: - target: *BUTTON_POS - - step: "close ex arm" - extend_arm: - front: *EX_CLOSE - back: *EX_CLOSE - - step: "ORE_ROTATOR_READY" - ore_rotator: - target: *READY_POS - - step: "ORE_LIFTER_UP" - ore_lifter: - target: *BIN_DOWN_POS - - step: "home" - arm: - <<: *HOME - - OPEN_GRIPPER: - - step: "open gripper" - gripper: - <<: *OPEN_GRIPPER - CLOSE_GRIPPER: - - step: "close gripper" - gripper: - <<: *CLOSE_GRIPPER + _GIMBAL_BACK: + - step: "gimbal move to back pos" + arms: + <<: *GIMBAL_BACK + common: + <<: *QUICKLY + tolerance: + <<: *NORMAL_TOLERANCE - ################### SMALL_ISLAND_FROM_UP ################ + _GIMBAL_MID: + - step: "gimbal move to mid pos" + arms: + <<: *GIMBAL_MID + common: + <<: *QUICKLY + tolerance: + <<: *NORMAL_TOLERANCE - ##### TAKE ONE USE ONE ##### + _GIMBAL_FRONT: + - step: "gimbal move to front pos" + arms: + <<: *GIMBAL_FRONT + common: + <<: *QUICKLY + tolerance: + <<: *NORMAL_TOLERANCE - TAKE_ONE_USE_ONE: - - step: "GIMBAL_READY" - gimbal: - <<: *FRONT_POS - - step: "ORE_ROTATOR_READY" - ore_rotator: - target: *READY_POS - - step: "ORE_LIFTER_READY" - ore_lifter: - target: *BIN_DOWN_POS - - step: "ready to get ore" - arm: - joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_TAKE_ONE, *JOINT4_TAKE_ONE, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_TAKE_ONE ] + _GIMBAL_PACK_UP: + - step: "gimbal pack up" + arms: + <<: *GIMBAL_PACK_UP common: <<: *QUICKLY tolerance: <<: *NORMAL_TOLERANCE - - step: "OPEN_GRIPPER" - gripper: - <<: *OPEN_GRIPPER - - step: "gimbal lifter ready" - gimbal_lifter: - target: *LIFTER_SMALL_ISLAND - TAKE_ONE_USE_ONE0: - - step: "GET_STONE" - arm: - joints: [ *JOINT1_SMALL_DOWN, *JOINT2_SMALL_READY, *JOINT3_TAKE_ONE, *JOINT4_TAKE_ONE, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_TAKE_ONE ] + + _GIMBAL_LOOK_DOWN: + - step: "gimbal look down" + arms: + <<: *GIMBAL_LOOK_DOWN common: - <<: *NORMALLY + <<: *QUICKLY tolerance: <<: *NORMAL_TOLERANCE - - step: "JOINT1_UP" - arm: - joints: [ *JOINT1_SMALL_READY, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + + + GET_UP: + - step: "arms ready to get up" + arms: + groups: + - left_arm: [ 1.2, 0.868, -1.57, 0.163, 1.655, 0.0, 0.0 ] + - right_arm: [ 1.2, 0.868, -1.57, 0.163, 1.655, 0.0, 0.0 ] +# - left_arm: [ 1.2, 1.4, -1.564, 1.0, 1.567, 0.0, 0.0 ] +# - right_arm: [ 1.2, 1.4, -1.564, 1.0, 1.567, 0.0, 0.0] + - gimbal_arm: [ "KEEP", -0.048, 0.647, 1.184 ] common: - <<: *NORMALLY + <<: *MAXIMUM tolerance: <<: *BIGGER_TOLERANCE - - step: "CHASSIS_BACK" - chassis: - <<: *CHASSIS_BACK - - step: "CHASSIS_TURN" - chassis: - <<: *CHASSIS_TURN - - step: "gimbal lifter ready" - gimbal_lifter: - target: *TALLEST_POS - - step: "ROTATE_STONE" - arm: - joints: [ *JOINT1_SMALL_DOWN, *JOINT2_SMALL_READY, *JOINT3_TAKE_ONE, *JOINT4_TAKE_ONE, *JOINT5_SMALL_READY, *JOINT6_MID_POSITION, *JOINT7_TAKE_ONE ] + - step: "arms getting up" + arms: + groups: + - gimbal_arm: [ 0.175, -0.048, 0.647, 1.184 ] common: - <<: *NORMALLY + <<: *SLOWLY tolerance: <<: *BIGGER_TOLERANCE - - step: "move arm to exchange pos" - arm: - joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + - step: "arms ready" + arms: + groups: +# - left_arm: [ 0.627, 1.46, -1.587, 1.725, 1.411, 0.007, -0.4 ] +# - right_arm: [ 0.674, 1.46, -1.587, 1.725, 1.411, 0.007, -0.47 ] +# - left_arm: [ 1.488, 1.4, -1.564, 1.58, 1.567, 0.0, 0.0 ] +# - right_arm: [ 1.488, 1.4, -1.564, 1.58, 1.567, 0.0, 0.0 ] + - gimbal_arm: [ "KEEP", -1.177, -0.667, 2.085 ] common: - <<: *NORMALLY + <<: *MAXIMUM tolerance: - <<: *NORMAL_TOLERANCE - - - + <<: *BIGGER_TOLERANCE - ########ONE_STONE_ONCE######### - 0_TAKE_ONE_STONE_SMALL_ISLAND: - - step: "GIMBAL_READY" - gimbal: - <<: *FRONT_POS - - step: "ORE_ROTATOR_READY" - ore_rotator: - target: *READY_POS - - step: "ORE_LIFTER_READY" - ore_lifter: - target: *BIN_DOWN_POS - - step: "ARM_READY" - arm: - joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_MID, *JOINT4_SMALL_READY, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] + SPECIAL_GET_UP: + - step: "right arm first" + arms: + groups: + # - left_arm: [ 1.458, 1.4, 0.161, 0.202, 2.13, -0.489, -1.368 ] + - right_arm: [ 1.2, 1.4, -1.564, 1.0, 1.567, 0.0, 0.0 ] common: <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "OPEN_GRIPPER" - gripper: - <<: *OPEN_GRIPPER - - step: "gimbal lifter ready" - gimbal_lifter: - target: *LIFTER_SMALL_ISLAND - 0_TAKE_ONE_STONE_SMALL_ISLAND0: - - step: "GET_STONE" - arm: - joints: [ *JOINT1_SMALL_DOWN, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_MID, *JOINT4_SMALL_READY, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] + <<: *BIGGER_TOLERANCE + - step: "left arm last" + arms: + groups: + - left_arm: [ 1.2, 1.4, -1.564, 1.0, 1.567, 0.0, 0.0 ] + # - right_arm: [ 1.467, 1.495, -0.044, 0.077, -1.414, 1.009, 1.071 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "JOINT1_UP" - arm: - joints: [ *JOINT1_SMALL_READY, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + <<: *BIGGER_TOLERANCE + - step: "arms ready to get up" + arms: + groups: + - gimbal_arm: [ "KEEP", -0.048, 0.647, 1.184 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "ARM_UP_STONE" - arm: - joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_MID, *JOINT4_SMALL_READY, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] + <<: *BIGGER_TOLERANCE + - step: "arms getting up" + arms: + groups: + - gimbal_arm: [ 0.175, -0.048, 0.647, 1.184 ] common: - <<: *NORMALLY + <<: *SLOWLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "READY_STORE" - arm: - joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] + <<: *BIGGER_TOLERANCE + - step: "arms ready" + arms: + groups: + - left_arm: [ 1.488, 1.4, -1.564, 1.58, 1.567, 0.0, 0.0 ] + - right_arm: [ 1.488, 1.4, -1.564, 1.58, 1.567, 0.0, 0.0 ] + - gimbal_arm: [ "KEEP", -1.177, -0.667, 2.085 ] common: <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "STORE" - arm: - joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] + <<: *BIGGER_TOLERANCE + + + SET_DOWN: + - step: "arms ready to set down" + arms: + groups: + - left_arm: [ 1.2, 0.868, -1.57, 0.163, 1.655, 0.0, 0.0 ] + - right_arm: [ 1.2, 0.868, -1.57, 0.163, 1.655, 0.0, 0.0 ] +# - left_arm: [ 1.47, 1.567, -1.564, 1.58, 1.567, 0.0, 0.0 ] +# - right_arm: [ 1.47, 1.567, -1.564, 1.58, 1.567, 0.0, 0.0] + - gimbal_arm: [ "KEEP", -0.048, 0.647, 1.184 ] common: - <<: *NORMALLY + <<: *MAXIMUM tolerance: <<: *BIGGER_TOLERANCE - - step: "CLOSE_GRIPPER" - gripper: - <<: *CLOSE_GRIPPER - - step: "MOVE_ARM_OUT" - arm: - joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] - common: - <<: *NORMALLY - tolerance: - <<: *BIGGER_TOLERANCE - - step: "CHASSIS_BACK" - chassis: - <<: *CHASSIS_BACK - - step: "CHASSIS_TURN" - chassis: - <<: *CHASSIS_TURN - - step: "pre_home" - arm: - <<: *PRE_HOME - - step: "home" - arm: - <<: *HOME - - step: "ORE_LIFTER_DOWN" - ore_lifter: - target: *BIN_DOWN_POS - - step: "ADD_STONE" - stone_num: - change: "SILVER" - 1_TAKE_ONE_STONE_SMALL_ISLAND: - - step: "GIMBAL_READY" - gimbal: - <<: *FRONT_POS - - step: "ORE_ROTATOR_READY" - ore_rotator: - target: *READY_POS - - step: "ORE_LIFTER_READY" - ore_lifter: - target: *BIN_DOWN_POS - - step: "ARM_READY" - arm: - joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_MID, *JOINT4_SMALL_READY, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] + - step: "arms setting down" + arms: + groups: + - gimbal_arm: [ -1.4, -0.048, 0.647, 1.184 ] common: - <<: *QUICKLY + <<: *SLOWLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "OPEN_GRIPPER" - gripper: - <<: *OPEN_GRIPPER - - step: "gimbal lifter ready" - gimbal_lifter: - target: *LIFTER_SMALL_ISLAND - 1_TAKE_ONE_STONE_SMALL_ISLAND0: - - step: "GET_STONE" - arm: - joints: [ *JOINT1_SMALL_DOWN, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_MID, *JOINT4_SMALL_READY, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] + <<: *BIGGER_TOLERANCE + - step: "arms put down" + arms: + groups: +# - left_arm: [ 1.458, 1.4, 0.161, 0.202, 2.13, -0.489, -1.368 ] +# - right_arm: [ 1.467, 1.495, -0.044, 0.077, -1.414, 1.009, 1.071 ] + - left_arm: [ 1.4, 1.46, -1.587, 1.725, 1.411, 0.007, -1.4 ] + - right_arm: [ 1.4, 1.46, -1.587, 1.725, 1.411, 0.007, -1.4 ] + - gimbal_arm: [ "KEEP", 1.484, -0.478, 0.786 ] common: - <<: *NORMALLY + <<: *MAXIMUM tolerance: - <<: *NORMAL_TOLERANCE - - step: "JOINT1_UP" - arm: - joints: [ *JOINT1_SMALL_READY, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + <<: *BIGGER_TOLERANCE + SET_DOWN0: + - step: "arms ready" + arms: + groups: + - left_arm: [ 1.2, 1.4, -1.564, 1.0, 1.567, 0.0, 0.0 ] +# - right_arm: [ 1.2, 1.4, -1.564, 1.0, 1.567, 0.0, 0.0 ] common: - <<: *NORMALLY + <<: *MAXIMUM tolerance: - <<: *NORMAL_TOLERANCE - - step: "ARM_UP_STONE" - arm: - joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_MID, *JOINT4_SMALL_READY, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] + <<: *BIGGER_TOLERANCE + - step: "left arm first" + arms: + groups: + - left_arm: [ 1.47, 1.533, 0.161, 0.202, 2.13, -0.489, -1.208 ] +# - right_arm: [ 1.467, 1.495, -0.044, 0.077, -1.414, 1.009, 1.071 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "READY_STORE" - arm: - joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] + <<: *BIGGER_TOLERANCE +# - step: "right arm last" +# arms: +# groups: +## - left_arm: [ 1.458, 1.4, 0.161, 0.202, 2.13, -0.489, -1.368 ] +# - right_arm: [ 1.357, 1.495, -0.044, 0.077, -1.414, 1.009, 1.071 ] +# common: +# <<: *QUICKLY +# tolerance: +# <<: *BIGGER_TOLERANCE + + SPECIAL_SET_DOWN: + - step: "arms ready to set down" + arms: + groups: + - left_arm: [ 1.47, 1.567, -1.564, 1.58, 1.567, 0.0, 0.0 ] + - right_arm: [ 1.47, 1.567, -1.564, 1.58, 1.567, 0.0, 0.0 ] + - gimbal_arm: [ "KEEP", -0.048, 0.647, 1.184 ] common: <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "STORE" - arm: - joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] + <<: *BIGGER_TOLERANCE + - step: "arms setting down" + arms: + groups: + - gimbal_arm: [ -1.4, -0.048, 0.647, 1.184 ] common: - <<: *NORMALLY + <<: *SLOWLY tolerance: <<: *BIGGER_TOLERANCE - - step: "CLOSE_GRIPPER" - gripper: - <<: *CLOSE_GRIPPER - - step: "MOVE_ARM_OUT" - arm: - joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] - common: - <<: *NORMALLY - tolerance: - <<: *BIGGER_TOLERANCE - - step: "CHASSIS_BACK" - chassis: - <<: *CHASSIS_BACK - - step: "CHASSIS_TURN" - chassis: - <<: *CHASSIS_TURN - - step: "pre_home" - arm: - <<: *PRE_HOME - - step: "home" - arm: - <<: *HOME - - step: "ORE_LIFTER_DOWN" - ore_lifter: - target: *BIN_DOWN_POS - - step: "ADD_STONE" - stone_num: - change: "SILVER" - - ########################## TWO_STONE_ONCE ######################## - ######FIRST_STONE######## - TWO_STONE_SMALL_ISLAND: - - step: "GIMBAL_READY" - gimbal: - <<: *FRONT_POS - - step: "ORE_ROTATOR_READY" - ore_rotator: - target: *READY_POS - - step: "ORE_LIFTER_READY" - ore_lifter: - target: *BIN_DOWN_POS - - step: "OPEN_GRIPPER" - gripper: - <<: *OPEN_GRIPPER - - step: "gimbal lifter ready" - gimbal_lifter: - target: *LIFTER_SMALL_ISLAND - - step: "ARM_READY" - arm: - joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_LEFT, *JOINT4_SMALL_READY, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] + - step: "arms put down" + arms: + groups: +# - left_arm: [ 1.4, 1.46, -1.587, 1.725, 1.411, 0.007, -1.4 ] + - right_arm: [ 1.4, 1.46, -1.587, 1.725, 1.411, 0.007, -1.4 ] + - gimbal_arm: [ "KEEP", 1.484, -0.478, 0.786 ] common: <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - TWO_STONE_SMALL_ISLAND0: - - step: "GET_STONE" - arm: - joints: [ *JOINT1_SMALL_DOWN, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_LEFT, *JOINT4_SMALL_READY, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] + <<: *BIGGER_TOLERANCE + - step: "left arm ready" + arms: + groups: + - left_arm: [ 1.2, 1.4, -1.564, 1.0, 1.567, 0.0, 0.0 ] +# - right_arm: [ 1.2, 1.4, -1.564, 1.0, 1.567, 0.0, 0.0 ] common: <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "JOINT1_UP" - arm: - joints: [ *JOINT1_SMALL_READY, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + <<: *BIGGER_TOLERANCE + - step: "left arm first" + arms: + groups: + - left_arm: [ 1.487, 1.514, 0.161, 0.202, 2.13, -0.489, -1.261 ] + # - right_arm: [ 1.467, 1.495, -0.044, 0.077, -1.414, 1.009, 1.071 ] common: <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "ARM_UP_STONE" - arm: - joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_LEFT, *JOINT4_SMALL_READY, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] + <<: *BIGGER_TOLERANCE +# - step: "right arm last" +# arms: +# groups: +# # - left_arm: [ 1.458, 1.4, 0.161, 0.202, 2.13, -0.489, -1.368 ] +# - right_arm: [ 1.357, 1.495, -0.044, 0.077, -1.414, 1.009, 1.071 ] +# common: +# <<: *QUICKLY +# tolerance: +# <<: *BIGGER_TOLERANCE + + + GRIPPER_OPEN: + - step: "grippers open" + left_gripper: + <<: *OPEN_GRIPPER + right_gripper: + <<: *OPEN_GRIPPER + LEFT_GRIPPER_OPEN: + - step: "left gripper open" + left_gripper: + <<: *OPEN_GRIPPER + RIGHT_GRIPPER_OPEN: + - step: "right gripper open" + right_gripper: + <<: *OPEN_GRIPPER + + GRIPPER_CLOSE: + - step: "grippers close" + left_gripper: + <<: *CLOSE_GRIPPER + right_gripper: + <<: *CLOSE_GRIPPER + LEFT_GRIPPER_CLOSE: + - step: "left gripper close" + left_gripper: + <<: *CLOSE_GRIPPER + RIGHT_GRIPPER_CLOSE: + - step: "right gripper close" + right_gripper: + <<: *CLOSE_GRIPPER + + LEFT_GET_TOP_UNIT: + - step: "left gripper open" + left_gripper: + <<: *OPEN_GRIPPER + - step: "left ready to get top unit" + arms: + groups: + - left_arm: [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] common: <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "READY_STORE" - arm: - joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] + <<: *BIGGER_TOLERANCE + - step: "left gripper close" + left_gripper: + <<: *CLOSE_GRIPPER + - step: "left get up unit" + arms: + groups: + - left_arm: [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] common: <<: *QUICKLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "STORE" - arm: - joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] - common: - <<: *NORMALLY tolerance: <<: *BIGGER_TOLERANCE - - step: "ADD_STONE" - stone_num: - change: "SILVER" - - step: "CLOSE_GRIPPER" - gripper: - <<: *CLOSE_GRIPPER - - step: "MOVE_ARM_OUT" - arm: - joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + - step: "left back to ready pos" + arms: + groups: + - left_arm: [ 1.022, 1.567, -1.564, 1.0, 1.567, 0.0, 0.0 ] common: <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "ORE_LIFTER_DOWN" - ore_lifter: - target: *BIN_DOWN_POS - + <<: *BIGGER_TOLERANCE - #####SECOND_STONE######## - - step: "GIMBAL_READY" - gimbal: - <<: *FRONT_POS - - step: "ORE_ROTATOR_READY" - ore_rotator: - target: *READY_POS - - step: "ORE_LIFTER_READY" - ore_lifter: - target: *BIN_DOWN_POS - - step: "ARM_READY" - arm: - joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_MID, *JOINT4_SMALL_READY, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] + LEFT_GET_BOTTOM_UNIT: + - step: "arms back to ready pos" + arms: + groups: + - left_arm: [ 1.488, 1.4, -1.564, 1.58, 1.567, 0.0, 0.0 ] + - right_arm: [ 1.488, 1.4, -1.564, 1.58, 1.567, 0.0, 0.0 ] common: <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "OPEN_GRIPPER" - gripper: + <<: *BIGGER_TOLERANCE + - step: "left gripper open" + left_gripper: <<: *OPEN_GRIPPER - - step: "GET_STONE" - arm: - joints: [ *JOINT1_SMALL_DOWN, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_MID, *JOINT4_SMALL_READY, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] + - step: "left ready to get bottom unit" + arms: + groups: + - left_arm: [ 0.312, 1.644, -1.537, 1.018, -1.46, 0.059, -0.322 ] common: <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "JOINT1_UP" - arm: - joints: [ *JOINT1_SMALL_READY, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + <<: *BIGGER_TOLERANCE + LEFT_GET_BOTTOM_UNIT0: + - step: "left gripper close" + left_gripper: + <<: *CLOSE_GRIPPER + - step: "left pull down unit" + arms: + groups: + - left_arm: [ 0.557, 1.658, -1.537, 0.962, -1.411, 0.093, -0.104 ] common: <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "ARM_UP_STONE" - arm: - joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_MID, *JOINT4_SMALL_READY, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] + <<: *BIGGER_TOLERANCE + - step: "left back to ready pos" + arms: + groups: + - left_arm: [ 1.488, 1.4, -1.564, 1.58, 1.567, 0.0, 0.0 ] common: <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "READY_STORE" - arm: - joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] + <<: *BIGGER_TOLERANCE + + LEFT_STORE_MID: + - step: "right arm back" + arms: + groups: + #- left_arm: [ 0.812, 1.4, -1.147, 1.276, -0.548, 0.703, 1.631 ] + - right_arm: [ 1.488, 1.4, -1.564, 1.58, 1.567, 0.0, 0.0 ] common: <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "STORE" - arm: - joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] + <<: *BIGGER_TOLERANCE + - step: "left ready to store unit mid" + arms: + groups: + - left_arm: [ 0.812, 1.4, -1.147, 1.276, -0.548, 0.703, 1.631 ] + #- right_arm: [ 1.488, 1.4, -1.564, 1.58, 1.567, 0.0, 0.0 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: <<: *BIGGER_TOLERANCE - - step: "ADD_STONE" - stone_num: - change: "SILVER" - - step: "CLOSE_GRIPPER" - gripper: - <<: *CLOSE_GRIPPER - - step: "MOVE_ARM_OUT" - arm: - joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + LEFT_STORE_MID0: + - step: "left store unit mid" + arms: + groups: + - left_arm: [ 1.272, 1.442, -1.273, 1.375, -0.431, 1.019, 1.71 ] common: <<: *QUICKLY tolerance: <<: *BIGGER_TOLERANCE - - step: "CHASSIS_BACK" - chassis: - <<: *CHASSIS_BACK - - step: "CHASSIS_TURN" - chassis: - <<: *CHASSIS_TURN - - step: "pre_home" - arm: - <<: *PRE_HOME - - step: "home" - arm: - <<: *HOME - - - ######THREE_STONE_ONCE(yi jian san lian)######### - ######FIRST_STONE######## - THREE_STONE_SMALL_ISLAND: - - step: "GIMBAL_READY" - gimbal: - <<: *FRONT_POS - - step: "ORE_ROTATOR_READY" - ore_rotator: - target: *READY_POS - - step: "ORE_LIFTER_READY" - ore_lifter: - target: *BIN_MID_POS - - step: "ARM_READY" - arm: - joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_LEFT, *JOINT4_SMALL_READY, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "OPEN_GRIPPER" - gripper: + - step: "left gripper open" + left_gripper: <<: *OPEN_GRIPPER - THREE_STONE_SMALL_ISLAND0: - - step: "GET_STONE" - arm: - joints: [ *JOINT1_SMALL_DOWN, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_LEFT, *JOINT4_SMALL_READY, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] + - step: "left leave unit" + arms: + groups: + - left_arm: [ 1.356, 0.636, -1.329, 1.503, -0.8, 1.018, 1.714 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "JOINT1_UP" - arm: - joints: [ *JOINT1_SMALL_READY, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + <<: *BIGGER_TOLERANCE + - step: "left back to ready pos" + arms: + groups: + - left_arm: [ 1.488, 1.4, -1.564, 1.58, 1.567, 0.0, 0.0 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "ARM_UP_STONE" - arm: - joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_LEFT, *JOINT4_SMALL_READY, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] + <<: *BIGGER_TOLERANCE + + LEFT_GET_STORED_MID: + - step: "left gripper open" + left_gripper: + <<: *OPEN_GRIPPER + - step: "left ready to get stored unit mid" + arms: + groups: + - left_arm: [ 1.356, 0.636, -1.329, 1.503, -0.8, 1.018, 1.714 ] + - right_arm: [ 1.488, 1.4, -1.564, 1.58, 1.567, 0.0, 0.0 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "JOINT6_BACK" - arm: - joints: [ "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", *JOINT6_SMALL_BACK, "KEEP" ] + <<: *BIGGER_TOLERANCE + LEFT_GET_STORED_MID0: + - step: "left get stored unit mid" + arms: + groups: + - left_arm: [ 1.272, 1.442, -1.273, 1.375, -0.431, 1.019, 1.71 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "TURN_STONE" - arm: - joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] + <<: *BIGGER_TOLERANCE + - step: "left gripper close" + left_gripper: + <<: *CLOSE_GRIPPER + - step: "left get up unit mid" + arms: + groups: + - left_arm: [ 0.791, 1.44, -1.218, 1.264, -0.572, 0.658, 1.634 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "READY_STORE" - arm: - joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] + <<: *BIGGER_TOLERANCE + - step: "left back to ready pos" + arms: + groups: + - left_arm: [ 1.488, 1.4, -1.564, 1.58, 1.567, 0.0, 0.0 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "STORE" - arm: - joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] + <<: *BIGGER_TOLERANCE + + LEFT_STORE_SIDE: + - step: "left avoid side" + arms: + groups: + - left_arm: [ 0.958, 1.283, -1.463, 1.425, -0.693, 1.037, 0.79 ] common: - <<: *SLOWLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "CLOSE_GRIPPER" - gripper: - <<: *CLOSE_GRIPPER - - step: "MOVE_ARM_OUT" - arm: - joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + <<: *BIGGER_TOLERANCE + - step: "left ready to store unit side" + arms: + groups: + - left_arm: [ 0.827, 0.921, -1.063, 1.782, -0.878, 0.628, 1.395 ] common: - <<: *SLOWLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "ORE_LIFTER_DOWN" - ore_lifter: - target: *BIN_DOWN_POS - - step: "ADD_STONE" - stone_num: - change: "SILVER" - - #####SECOND_STONE######## -# THREE_STONE_SMALL_ISLAND00: - - step: "GIMBAL_READY" - gimbal: - <<: *FRONT_POS - - step: "ORE_ROTATOR_READY" - ore_rotator: - target: *READY_POS - - step: "ORE_LIFTER_READY" - ore_lifter: - target: *BIN_DOWN_POS - - step: "ARM_READY" - arm: - joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_MID, *JOINT4_SMALL_READY, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] - common: - <<: *NORMALLY + <<: *BIGGER_TOLERANCE + - step: "left store unit side" + arms: + groups: + - left_arm: [ 1.375, 1.165, -1.088, 1.736, -0.861, 1.009, 1.744 ] + common: + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "OPEN_GRIPPER" - gripper: + <<: *BIGGER_TOLERANCE + - step: "left gripper open" + left_gripper: <<: *OPEN_GRIPPER -# THREE_STONE_SMALL_ISLAND000: - - step: "GET_STONE" - arm: - joints: [ *JOINT1_SMALL_DOWN, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_MID, *JOINT4_SMALL_READY, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] + - step: "left leave unit" + arms: + groups: + - left_arm: [ 1.77, 0.686, -1.57, 1.689, -1.036, 1.011, 1.724 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "JOINT1_UP" - arm: - joints: [ *JOINT1_SMALL_READY, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + <<: *BIGGER_TOLERANCE + - step: "left back to ready pos" + arms: + groups: + - left_arm: [ 1.488, 1.4, -1.564, 1.58, 1.567, 0.0, 0.0 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "ARM_UP_STONE" - arm: - joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_MID, *JOINT4_SMALL_READY, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] + <<: *BIGGER_TOLERANCE + + LEFT_GET_STORED_SIDE: + - step: "left gripper open" + left_gripper: + <<: *OPEN_GRIPPER + - step: "left ready to get stored unit side" + arms: + groups: + - left_arm: [ 1.77, 0.686, -1.57, 1.689, -1.036, 1.011, 1.724 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "JOINT6_BACK" - arm: - joints: [ "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", *JOINT6_SMALL_BACK, "KEEP" ] + <<: *BIGGER_TOLERANCE + - step: "left get stored unit side" + arms: + groups: + - left_arm: [ 1.456, 1.132, -1.092, 1.76, -0.829, 1.01, 1.712 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "TURN_STONE" - arm: - joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] + <<: *BIGGER_TOLERANCE + - step: "left gripper close" + left_gripper: + <<: *CLOSE_GRIPPER + - step: "left take out unit side" + arms: + groups: + - left_arm: [ 0.827, 0.921, -1.063, 1.782, -0.878, 0.628, 1.395 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "READY_STORE" - arm: - joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] + <<: *BIGGER_TOLERANCE + - step: "left avoid mid" + arms: + groups: + - left_arm: [ 0.958, 1.283, -1.463, 1.425, -0.693, 1.037, 0.79 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "STORE" - arm: - joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] + <<: *BIGGER_TOLERANCE + - step: "left back to ready pos" + arms: + groups: + - left_arm: [ 1.2, 0.868, -1.57, 0.163, 1.655, 0.0, 0.0 ] +# - left_arm: [ 1.488, 1.4, -1.564, 1.58, 1.567, 0.0, 0.0 ] common: - <<: *SLOWLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "CLOSE_GRIPPER" - gripper: + <<: *BIGGER_TOLERANCE + + RIGHT_GET_TOP_UNIT: + - step: "right gripper open" + right_gripper: + <<: *OPEN_GRIPPER + - step: "right ready to get top unit" + arms: + groups: + - right_arm: [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] + common: + <<: *QUICKLY + tolerance: + <<: *BIGGER_TOLERANCE + - step: "right gripper close" + right_gripper: <<: *CLOSE_GRIPPER - - step: "MOVE_ARM_OUT" - arm: - joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + - step: "right get up unit" + arms: + groups: + - right_arm: [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] common: - <<: *SLOWLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "ADD_STONE" - stone_num: - change: "SILVER" - #######THIRD_STONE######### -# THREE_STONE_SMALL_ISLAND0000: - - step: "GIMBAL_READY" - gimbal: - <<: *FRONT_POS - - step: "ORE_ROTATOR_READY" - ore_rotator: - target: *READY_POS - - step: "ORE_LIFTER_READY" - ore_lifter: - target: *BIN_DOWN_POS - - step: "ARM_READY" - arm: - joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_RIGHT, *JOINT4_SMALL_READY, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] - common: - <<: *NORMALLY + <<: *BIGGER_TOLERANCE + - step: "right back to ready pos" + arms: + groups: + - right_arm: [ 1.488, 1.4, -1.564, 1.58, 1.567, 0.0, 0.0 ] + common: + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "OPEN_GRIPPER" - gripper: + <<: *BIGGER_TOLERANCE + + RIGHT_GET_BOTTOM_UNIT: + - step: "right gripper open" + right_gripper: <<: *OPEN_GRIPPER -# THREE_STONE_SMALL_ISLAND00000: - - step: "GET_STONE" - arm: - joints: [ *JOINT1_SMALL_DOWN, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_RIGHT, *JOINT4_SMALL_READY, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] + - step: "right ready to get bottom unit" + arms: + groups: + - right_arm: [ 0.322, 1.644, -1.537, 1.018, -1.46, 0.059, -0.37 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "JOINT1_UP" - arm: - joints: [ *JOINT1_SMALL_READY, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + <<: *BIGGER_TOLERANCE + RIGHT_GET_BOTTOM_UNIT0: + - step: "right gripper close" + right_gripper: + <<: *CLOSE_GRIPPER + - step: "right pull down unit" + arms: + groups: + - right_arm: [ 0.545, 1.658, -1.537, 0.962, -1.411, 0.093, -0.104 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "ARM_UP_STONE" - arm: - joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_RIGHT, *JOINT4_SMALL_READY, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] + <<: *BIGGER_TOLERANCE + - step: "right back to ready pos" + arms: + groups: + - right_arm: [ 1.488, 1.4, -1.564, 1.58, 1.567, 0.0, 0.0 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "JOINT6_BACK" - arm: - joints: [ "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", *JOINT6_SMALL_BACK, "KEEP" ] + <<: *BIGGER_TOLERANCE + + RIGHT_STORE_MID: + - step: "left arm back" + arms: + groups: + #- right_arm: [ 0.625, 1.227, -0.813, 1.166, -0.839, 0.942, 1.414 ] + - left_arm: [ 1.488, 1.4, -1.564, 1.58, 1.567, 0.0, 0.0 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "TURN_STONE" - arm: - joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] + <<: *BIGGER_TOLERANCE + - step: "right ready to store unit mid" + arms: + groups: + - right_arm: [ 0.625, 1.227, -0.813, 1.166, -0.839, 0.942, 1.414 ] + #- left_arm: [ 1.488, 1.4, -1.564, 1.58, 1.567, 0.0, 0.0 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "READY_STORE(THIRD_STONE)" - arm: - joints: [ *JOINT1_SMALL_READY_STORE_THIRD, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] + <<: *BIGGER_TOLERANCE + RIGHT_STORE_MID0: + - step: "right store unit mid" + arms: + groups: + - right_arm: [ 1.267, 1.398, -1.2, 1.308, -0.518, 1.051, 1.593 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "STORE" - arm: - joints: [ *JOINT1_SMALL_READY_STORE_THIRD, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] + <<: *BIGGER_TOLERANCE + - step: "right gripper open" + right_gripper: + <<: *OPEN_GRIPPER + - step: "right leave unit" + arms: + groups: + - right_arm: [ 1.09, 0.67, -1.101, 1.28, -0.744, 1.03, 1.595 ] common: - <<: *SLOWLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE -# - step: "CLOSE_GRIPPER" -# gripper: -# <<: *CLOSE_GRIPPER -# - step: "MOVE_ARM_OUT" -# arm: -# joints: [ *JOINT1_SMALL_READY_STORE_THIRD, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] -# common: -# <<: *SLOWLY -# tolerance: -# <<: *NORMAL_TOLERANCE - - step: "ADD_STONE" - stone_num: - change: "SILVER" - - ######GET STONE FROM BIN######### - - GET_DOWN_STONE_BIN: - - step: "GIMBAL_READY" - gimbal: - <<: *FRONT_POS - - step: "ORE_ROTATOR_READY" - ore_rotator: - target: *EXCHANGE_POS - - step: "ORE_LIFTER_UP" - ore_lifter: - target: *BIN_UP_POS - - step: "JOINT2 move out" - arm: - joints: [ "KEEP", *JOINT2_AVOID_COLLISION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + <<: *BIGGER_TOLERANCE + - step: "right back to ready pos" + arms: + groups: + - right_arm: [ 1.488, 1.4, -1.564, 1.58, 1.567, 0.0, 0.0 ] common: - <<: *SLOWLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "READY_TO_GET" - arm: - joints: [ *JOINT1_BIN_GET_STONE, *JOINT2_BIN_GET_STONE, *JOINT3_BIN_GET_LV4_STONE, *JOINT4_R_POSITION, *JOINT5_BIN_GET_STONE, *JOINT6_BIN_GET_STONE, *JOINT7_MID_POSITION ] + <<: *BIGGER_TOLERANCE + + RIGHT_GET_STORED_MID: + - step: "right gripper open" + right_gripper: + <<: *OPEN_GRIPPER + - step: "right ready to get stored unit mid" + arms: + groups: + - right_arm: [ 1.09, 0.67, -1.101, 1.28, -0.744, 1.03, 1.595 ] + - left_arm: [ 1.488, 1.4, -1.564, 1.58, 1.567, 0.0, 0.0 ] common: <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "OPEN_GRIPPER" - gripper: - <<: *OPEN_GRIPPER - - step: "JOINT2_BACK_GET_STONE" - arm: - joints: [ "KEEP", *JOINT2_BACK_BIN_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + <<: *BIGGER_TOLERANCE + RIGHT_GET_STORED_MID0: + - step: "right get stored unit mid" + arms: + groups: + - right_arm: [ 1.267, 1.398, -1.2, 1.308, -0.518, 1.051, 1.593 ] common: - <<: *SLOWLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "JOINT1_UP" - arm: - joints: [ *JOINT1_UP_POS, "KEEP", "KEEP", "KEEP", *JOINT5_MID_POSITION, "KEEP", "KEEP" ] + <<: *BIGGER_TOLERANCE + - step: "right gripper close" + right_gripper: + <<: *CLOSE_GRIPPER + - step: "right get up unit mid" + arms: + groups: + - right_arm: [ 0.625, 1.227, -0.813, 1.166, -0.839, 0.942, 1.414 ] common: <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "exchange pos" - arm: - joints: [ *JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + <<: *BIGGER_TOLERANCE + - step: "right back to ready pos" + arms: + groups: + - right_arm: [ 1.488, 1.4, -1.564, 1.58, 1.567, 0.0, 0.0 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "remove stone" - stone_num: - change: "-1" - - step: "ORE_LIFTER_DOWN" - ore_lifter: - target: *BIN_DOWN_POS - - step: "gimbal lifter tallest" - gimbal_lifter: - target: *TALLEST_POS - + <<: *BIGGER_TOLERANCE - GET_UP_STONE_BIN: - - step: "GIMBAL_READY" - gimbal: - <<: *FRONT_POS - - step: "ORE_ROTATOR_READY" - ore_rotator: - target: *EXCHANGE_POS - - step: "roe lifter ready" - ore_lifter: - target: *BIN_SMALL_UP_POS - - step: "JOINT2 move out" - arm: - joints: [ "KEEP", *JOINT2_AVOID_COLLISION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + RIGHT_STORE_SIDE: + - step: "right avoid mid" + arms: + groups: + - right_arm: [ 0.958, 1.283, -1.463, 1.425, -0.693, 1.037, 0.79 ] common: - <<: *SLOWLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "READY_TO_GET" - arm: - joints: [ *JOINT1_BIN_GET_STONE, *JOINT2_BIN_GET_STONE, *JOINT3_BIN_GET_LV4_STONE, *JOINT4_R_POSITION, *JOINT5_BIN_GET_STONE, *JOINT6_BIN_GET_STONE, *JOINT7_MID_POSITION ] + <<: *BIGGER_TOLERANCE + - step: "right ready to store unit side" + arms: + groups: + - right_arm: [ 0.575, 0.632, -0.626, 1.662, -0.943, 1.001, 0.962 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "OPEN_GRIPPER" - gripper: + <<: *BIGGER_TOLERANCE + - step: "right store unit side" + arms: + groups: + - right_arm: [ 1.422, 1.05, -1.164, 1.725, -0.734, 1.04, 1.475 ] + common: + <<: *QUICKLY + tolerance: + <<: *BIGGER_TOLERANCE + - step: "right gripper open" + right_gripper: <<: *OPEN_GRIPPER - - step: "JOINT2_BACK_GET_STONE" - arm: - joints: [ "KEEP", *JOINT2_BACK_BIN_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + - step: "right leave unit" + arms: + groups: + - right_arm: [ 1.766, 0.451, -1.599, 1.701, -1.126, 1.061, 1.591 ] common: - <<: *SLOWLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "JOINT1_UP" - arm: - joints: [ *JOINT1_UP_POS, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + <<: *BIGGER_TOLERANCE + - step: "right back to ready pos" + arms: + groups: + - right_arm: [ 1.488, 1.4, -1.564, 1.58, 1.567, 0.0, 0.0 ] common: <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "JOINT5_UP" - arm: - joints: [ "KEEP", "KEEP", "KEEP", "KEEP", *JOINT5_MID_POSITION, "KEEP", "KEEP" ] + <<: *BIGGER_TOLERANCE + + RIGHT_GET_STORED_SIDE: + - step: "right gripper open" + right_gripper: + <<: *OPEN_GRIPPER + - step: "right ready to get stored unit side" + arms: + groups: + - right_arm: [ 1.785, 0.452, -1.579, 1.681, -0.92, 0.982, 1.592 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "exchange pos" - arm: - joints: [ *JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + <<: *BIGGER_TOLERANCE + - step: "right get stored unit side" + arms: + groups: + - right_arm: [ 1.422, 1.05, -1.164, 1.725, -0.734, 1.04, 1.475 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "remove stone" - stone_num: - change: "-1" - - step: "ORE_LIFTER_DOWN" - ore_lifter: - target: *BIN_DOWN_POS - - step: "gimbal lifter tallest" - gimbal_lifter: - target: *TALLEST_POS - - - GET_DOWN_STONE_LEFT: - - step: "GIMBAL_READY" - gimbal: - <<: *FRONT_POS - - step: "ORE_ROTATOR_READY" - ore_rotator: - target: *EXCHANGE_POS - - step: "ORE_LIFTER_UP" - ore_lifter: - target: *BIN_GET_DOWN - - step: "JOINT2 move out" - arm: - joints: [ "KEEP", *JOINT2_AVOID_COLLISION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + <<: *BIGGER_TOLERANCE + - step: "right gripper close" + right_gripper: + <<: *CLOSE_GRIPPER + - step: "right take out unit side" + arms: + groups: + - right_arm: [ 0.575, 0.632, -0.626, 1.662, -0.943, 1.001, 0.962 ] common: - <<: *SLOWLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "READY_TO_GET" - arm: - joints: [ *JOINT1_BIN_GET_STONE_LEFT, *JOINT2_BIN_GET_STONE, *JOINT3_BIN_GET_STONE, *JOINT4_R_POSITION, *JOINT5_BIN_GET_STONE, *JOINT6_BIN_GET_STONE, *JOINT7_BIN_GET_LEFT ] + <<: *BIGGER_TOLERANCE + - step: "right avoid mid" + arms: + groups: + - right_arm: [ 0.958, 1.283, -1.463, 1.425, -0.693, 1.037, 0.79 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: <<: *BIGGER_TOLERANCE - - step: "OPEN_GRIPPER" - gripper: + - step: "right back to ready pos" + arms: + groups: + - right_arm: [ 1.2, 0.868, -1.57, 0.163, 1.655, 0.0, 0.0 ] +# - right_arm: [ 1.488, 1.4, -1.564, 1.58, 1.567, 0.0, 0.0 ] + common: + <<: *QUICKLY + tolerance: + <<: *BIGGER_TOLERANCE + + GET_STORED_SIDE_UNITS: + - step: "grippers open" + left_gripper: + <<: *OPEN_GRIPPER + right_gripper: <<: *OPEN_GRIPPER - - step: "JOINT2_BACK_GET_STONE" - arm: - joints: [ "KEEP", *JOINT2_BACK_BIN_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + - step: "left ready to get stored side units" + arms: + groups: + - left_arm: [ 1.77, 0.686, -1.57, 1.689, -1.036, 1.011, 1.724 ] +# - right_arm: [ 1.716, 0.651, -1.54, 1.671, -1.077, 1.055, 1.647 ] common: - <<: *SLOWLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "JOINT1_UP" - arm: - joints: [ *JOINT1_UP_POS, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + <<: *BIGGER_TOLERANCE + - step: "left get stored side units and right ready to get side unit" + arms: + groups: + - left_arm: [ 1.456, 1.132, -1.092, 1.76, -0.829, 1.01, 1.712 ] + - right_arm: [ 1.785, 0.452, -1.579, 1.681, -0.92, 0.982, 1.592 ] +# - right_arm: [ 1.422, 1.05, -1.164, 1.772, -0.734, 1.04, 1.475 ] common: <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "JOINT5_UP" - arm: - joints: [ "KEEP", "KEEP", "KEEP", "KEEP", *JOINT5_MID_POSITION, "KEEP", "KEEP" ] + <<: *BIGGER_TOLERANCE + - step: "left gripper close" + left_gripper: + <<: *CLOSE_GRIPPER +# right_gripper: +# <<: *CLOSE_GRIPPER + - step: "left take out stored side unit and right get stored side" + arms: + groups: + - left_arm: [ 0.827, 0.921, -1.063, 1.782, -0.878, 0.628, 1.395 ] + - right_arm: [ 1.422, 1.05, -1.164, 1.725, -0.734, 1.04, 1.475 ] +# - right_arm: [ 1.716, 0.651, -1.54, 1.671, -1.077, 1.055, 1.647 ] +# - right_arm: [ 0.529, 0.615, -0.552, 1.742, -0.905, 1.052, 0.948 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "remove stone" - stone_num: - change: "-1" - - step: "exchange pos" - arm: - joints: [*JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_POSITION, *JOINT4_MID_POSITION, *JOINT5_L90_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION] + <<: *BIGGER_TOLERANCE + - step: "left avoid mid and right gripper close" + arms: + groups: + - left_arm: [ 0.958, 1.283, -1.463, 1.425, -0.693, 1.037, 0.79 ] +# - right_arm: [ 1.422, 1.05, -1.164, 1.772, -0.734, 1.04, 1.475 ] +# - right_arm: [ 0.958, 1.283, -1.463, 1.425, -0.693, 1.037, 0.79 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "ORE_LIFTER_DOWN" - ore_lifter: - target: *BIN_DOWN_POS - - step: "gimbal lifter tallest" - gimbal_lifter: - target: *TALLEST_POS - + <<: *BIGGER_TOLERANCE + right_gripper: + <<: *CLOSE_GRIPPER + - step: "left back to ready pos and right take out stored side" + arms: + groups: + - left_arm: [ 1.2, 0.868, -1.57, 0.163, 1.655, 0.0, 0.0 ] +# - left_arm: [ 1.488, 1.4, -1.564, 1.58, 1.567, 0.0, 0.0 ] + - right_arm: [ 0.575, 0.632, -0.626, 1.662, -0.943, 1.001, 0.962 ] +# - right_arm: [ 1.488, 1.4, -1.564, 1.58, 1.567, 0.0, 0.0 ] + common: + <<: *QUICKLY + tolerance: + <<: *BIGGER_TOLERANCE +# - step: "right avoid mid" + - step: "right back to ready pos" + arms: + groups: + - right_arm: [ 1.2, 0.868, -1.57, 0.163, 1.655, 0.0, 0.0 ] +# - right_arm: [ 0.958, 1.283, -1.463, 1.425, -0.693, 1.037, 0.79 ] + common: + <<: *QUICKLY + tolerance: + <<: *BIGGER_TOLERANCE +# - step: "right back to ready pos" +# arms: +# groups: +# - right_arm: [ 1.488, 1.4, -1.564, 1.58, 1.567, 0.0, 0.0 ] +# common: +# <<: *QUICKLY +# tolerance: +# <<: *BIGGER_TOLERANCE - GET_DOWN_STONE_RIGHT: - - step: "GIMBAL_READY" - gimbal: - <<: *FRONT_POS - - step: "ORE_ROTATOR_READY" - ore_rotator: - target: *EXCHANGE_POS - - step: "ORE_LIFTER_UP" - ore_lifter: - target: *BIN_GET_DOWN - - step: "JOINT2 move out" - arm: - joints: [ "KEEP", *JOINT2_AVOID_COLLISION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + STORE_UNITS_SIDE: + - step: "right avoid mid" + arms: + groups: + - right_arm: [ 0.958, 1.283, -1.463, 1.425, -0.693, 1.037, 0.79 ] common: - <<: *SLOWLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "READY_TO_GET" - arm: - joints: [ *JOINT1_BIN_GET_STONE_RIGHT, *JOINT2_BIN_GET_STONE, *JOINT3_BIN_GET_STONE, *JOINT4_R_POSITION, *JOINT5_BIN_GET_STONE, *JOINT6_BIN_GET_STONE, *JOINT7_BIN_GET_RIGHT ] + <<: *BIGGER_TOLERANCE + - step: "left avoid mid and right ready to store side" + arms: + groups: + - left_arm: [ 0.958, 1.283, -1.463, 1.425, -0.693, 1.037, 0.79 ] + - right_arm: [ 0.575, 0.632, -0.626, 1.662, -0.943, 1.001, 0.962 ] + # - right_arm: [ 1.488, 1.4, -1.564, 1.58, 1.567, 0.0, 0.0 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: <<: *BIGGER_TOLERANCE - - step: "OPEN_GRIPPER" - gripper: - <<: *OPEN_GRIPPER - - step: "JOINT2_BACK_GET_STONE" - arm: - joints: [ "KEEP", *JOINT2_BACK_BIN_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + - step: "left ready to store side unit and right store side" + arms: + groups: + - left_arm: [ 0.827, 0.921, -1.063, 1.782, -0.878, 0.628, 1.395 ] + - right_arm: [ 1.422, 1.05, -1.164, 1.725, -0.734, 1.04, 1.475 ] + # - right_arm: [ 1.716, 0.651, -1.54, 1.671, -1.077, 1.055, 1.647 ] + # - right_arm: [ 0.529, 0.615, -0.552, 1.742, -0.905, 1.052, 0.948 ] common: - <<: *SLOWLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "JOINT1_UP" - arm: - joints: [ *JOINT1_UP_POS, "KEEP", "KEEP", "KEEP", *JOINT5_MID_POSITION, "KEEP", "KEEP" ] + <<: *BIGGER_TOLERANCE + - step: "right gripper open" + right_gripper: + <<: *OPEN_GRIPPER + - step: "left store unit side and right leave unit" + arms: + groups: + - left_arm: [ 1.375, 1.165, -1.088, 1.736, -0.861, 1.009, 1.744 ] + - right_arm: [ 1.771, 0.651, -1.567, 1.645, -1.01, 0.998, 1.583 ] + # - right_arm: [ 1.422, 1.05, -1.164, 1.772, -0.734, 1.04, 1.475 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "exchange pos" - arm: - joints: [*JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_POSITION, *JOINT4_MID_POSITION, *JOINT5_R90_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION] + <<: *BIGGER_TOLERANCE + - step: "left gripper open and right back to ready pos" + left_gripper: + <<: *OPEN_GRIPPER + arms: + groups: + - right_arm: [ 1.488, 1.4, -1.564, 1.58, 1.567, 0.0, 0.0 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "remove stone" - stone_num: - change: "-1" - - step: "ORE_LIFTER_DOWN" - ore_lifter: - target: *BIN_DOWN_POS - - step: "gimbal lifter tallest" - gimbal_lifter: - target: *TALLEST_POS - - - GET_UP_STONE_LEFT: - - step: "GIMBAL_READY" - gimbal: - <<: *FRONT_POS - - step: "ORE_ROTATOR_READY" - ore_rotator: - target: *EXCHANGE_POS - - step: "ORE_LIFTER_READY" - ore_lifter: - target: *BIN_GET_UP - - step: "JOINT2 move out" - arm: - joints: [ "KEEP", *JOINT2_AVOID_COLLISION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + <<: *BIGGER_TOLERANCE + - step: "left leave unit" + arms: + groups: + - left_arm: [ 1.77, 0.686, -1.57, 1.689, -1.036, 1.011, 1.724 ] common: - <<: *SLOWLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "READY_TO_GET" - arm: - joints: [ *JOINT1_BIN_GET_STONE_LEFT, *JOINT2_BIN_GET_STONE, *JOINT3_BIN_GET_STONE, *JOINT4_R_POSITION, *JOINT5_BIN_GET_STONE, *JOINT6_BIN_GET_STONE, *JOINT7_BIN_GET_LEFT ] + <<: *BIGGER_TOLERANCE + - step: "left back to ready pos" + arms: + groups: + - left_arm: [ 1.488, 1.4, -1.564, 1.58, 1.567, 0.0, 0.0 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: <<: *BIGGER_TOLERANCE - - step: "OPEN_GRIPPER" - gripper: + + GET_DOWN_UNITS: + - step: "grippers open" + left_gripper: <<: *OPEN_GRIPPER - - step: "JOINT2_BACK_GET_STONE" - arm: - joints: [ "KEEP", *JOINT2_BACK_BIN_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + right_gripper: + <<: *OPEN_GRIPPER + - step: "arms ready to get down two units" + arms: + groups: + - left_arm: [ 0.461, 1.688, -1.515, 1.665, -0.466, -0.525, -0.464 ] + - right_arm: [ 0.428, 1.655, -1.493, 1.654, -0.645, -0.57, -0.535 ] common: - <<: *SLOWLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "JOINT1_UP" - arm: - joints: [ *JOINT1_UP_POS, "KEEP", "KEEP", "KEEP", *JOINT5_MID_POSITION, "KEEP", "KEEP" ] + <<: *BIGGER_TOLERANCE + GET_DOWN_UNITS0: + - step: "grippers close" + left_gripper: + <<: *CLOSE_GRIPPER + right_gripper: + <<: *CLOSE_GRIPPER + - step: "arms pull down units" + arms: + groups: + - left_arm: [ 0.81, 1.101, -1.57, 1.602, -0.775, -0.262, -0.096 ] + - right_arm: [ 0.81, 1.111, -1.57, 1.602, -0.775, -0.262, -0.096 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "exchange pos" - arm: - joints: [*JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_POSITION, *JOINT4_MID_POSITION, *JOINT5_L90_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION] + <<: *BIGGER_TOLERANCE + - step: "arms avoid mid" + arms: + groups: + - left_arm: [ 0.958, 1.283, -1.463, 1.425, -0.693, 1.037, 0.79 ] + - right_arm: [ 0.958, 1.283, -1.463, 1.425, -0.693, 1.037, 0.79 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "remove stone" - stone_num: - change: "-1" - - step: "ORE_LIFTER_DOWN" - ore_lifter: - target: *BIN_DOWN_POS - - step: "gimbal lifter tallest" - gimbal_lifter: - target: *TALLEST_POS - + <<: *BIGGER_TOLERANCE +# GET_DOWN_UNITS00: +# store - GET_UP_STONE_RIGHT: - - step: "GIMBAL_READY" - gimbal: - <<: *FRONT_POS - - step: "ORE_ROTATOR_READY" - ore_rotator: - target: *EXCHANGE_POS - - step: "ORE_LIFTER_READY" - ore_lifter: - target: *BIN_GET_UP - - step: "JOINT2 move out" - arm: - joints: [ "KEEP", *JOINT2_AVOID_COLLISION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + GET_UP_UNITS: + - step: "arms back to ready pos" + arms: + groups: + - left_arm: [ 1.488, 1.4, -1.564, 1.58, 1.567, 0.0, 0.0 ] + - right_arm: [ 1.488, 1.4, -1.564, 1.58, 1.567, 0.0, 0.0 ] common: - <<: *SLOWLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "READY_TO_GET" - arm: - joints: [ *JOINT1_BIN_GET_STONE_RIGHT, *JOINT2_BIN_GET_STONE, *JOINT3_BIN_GET_STONE, *JOINT4_R_POSITION, *JOINT5_BIN_GET_STONE, *JOINT6_BIN_GET_STONE, *JOINT7_BIN_GET_RIGHT ] + <<: *BIGGER_TOLERANCE + - step: "arms avoid mid" + arms: + groups: + - left_arm: [ 1.192, 0.669, -1.568, 1.594, -0.541, 0.695, 0.041 ] + - right_arm: [ 1.192, 0.669, -1.568, 1.594, -0.541, 0.695, 0.041 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: <<: *BIGGER_TOLERANCE - - step: "OPEN_GRIPPER" - gripper: - <<: *OPEN_GRIPPER - - step: "JOINT2_BACK_GET_STONE" - arm: - joints: [ "KEEP", *JOINT2_BACK_BIN_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + - step: "arms get up two units" + arms: + groups: + - left_arm: [ 0.0, 1.464, -1.549, 1.209, 0.339, -0.353, 0.512 ] + - right_arm: [ -0.02, 1.474, -1.424, 1.017, 0.168, -0.074, 0.293 ] common: - <<: *SLOWLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "JOINT1_UP" - arm: - joints: [ *JOINT1_UP_POS, "KEEP", "KEEP", "KEEP", *JOINT5_MID_POSITION, "KEEP", "KEEP" ] + <<: *BIGGER_TOLERANCE + GET_UP_UNITS0: + - step: "grippers close" + left_gripper: + <<: *CLOSE_GRIPPER + right_gripper: + <<: *CLOSE_GRIPPER + - step: "arms pull up units" + arms: + groups: + # old +# - left_arm: [ -0.426, 1.258, -1.436, 0.459, 0.158, 0.135, 0.458 ] +# - right_arm: [ -0.426, 1.258, -1.436, 0.459, 0.158, 0.135, 0.458 ] + # old + - left_arm: [ -0.37, 1.121, -1.394, 1.013, 0.177, -0.584, 0.685 ] + - right_arm: [ -0.24, 1.184, -1.396, 0.876, 0.051, -0.073, 0.449 ] common: <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "exchange pos" - arm: - joints: [*JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_POSITION, *JOINT4_MID_POSITION, *JOINT5_R90_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION] + <<: *BIGGER_TOLERANCE + - step: "arms avoid mid" + arms: + groups: + - left_arm: [ 1.192, 0.669, -1.568, 1.594, -0.541, 0.695, 0.041 ] + - right_arm: [ 1.192, 0.669, -1.568, 1.594, -0.541, 0.695, 0.041 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "remove stone" - stone_num: - change: "-1" - - step: "ORE_LIFTER_DOWN" - ore_lifter: - target: *BIN_DOWN_POS - - step: "gimbal lifter tallest" - gimbal_lifter: - target: *TALLEST_POS - - - #################BIG_ISLAND############# - ######MID_STONE####### - MID_BIG_ISLAND: - - step: "gimbal ready" - gimbal: - <<: *BIG_ISLAND_POS - - step: "open ex arm" - extend_arm: - front: *EX_OPEN - back: *EX_OPEN - - step: "MID_ARM_READY" - arm: - joints: [ *JOINT1_BIG_READY, *JOINT2_BIG_READY, *JOINT3_BIG_READY, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_READY , *JOINT7_BIG_POS] - common: - <<: *QUICKLY + <<: *BIGGER_TOLERANCE + - step: "arms back to ready pos" + arms: + groups: + - left_arm: [ 1.488, 1.4, -1.564, 1.58, 1.567, 0.0, 0.0 ] + - right_arm: [ 1.488, 1.4, -1.564, 1.58, 1.567, 0.0, 0.0 ] + common: + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "ORE_ROTATOR_READY" - ore_rotator: - target: *EXCHANGE_POS - - step: "gimbal lifter" - gimbal_lifter: - target: *LIFTER_BIG_ISLAND - - step: "OPEN_GRIPPER" - gripper: + <<: *BIGGER_TOLERANCE + + GET_FIVE_UNITS: + - step: "grippers open" + left_gripper: + <<: *OPEN_GRIPPER + right_gripper: <<: *OPEN_GRIPPER - MID_BIG_ISLAND0: - - step: "GET_MID_STONE" - arm: - joints: [ *JOINT1_BIG_READY, *JOINT2_BIG_READY, *JOINT3_GET_BIG_MID, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_READY, *JOINT7_BIG_POS ] + - step: "arms ready to get down two units" + arms: + groups: + - left_arm: [ 0.461, 1.688, -1.515, 1.665, -0.466, -0.525, -0.464 ] + - right_arm: [ 0.478, 1.655, -1.493, 1.654, -0.645, -0.57, -0.535 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: <<: *BIGGER_TOLERANCE - MID_BIG_ISLAND00: - - step: "ADJUST_STONE" - arm: - joints: [ *JOINT1_BIG_ADJUST_MID, *JOINT2_BIG_READY, *JOINT3_GET_BIG_MID, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_ADJUST_MID, *JOINT7_BIG_POS ] + GET_FIVE_UNITS0: + - step: "grippers close" + left_gripper: + <<: *CLOSE_GRIPPER + right_gripper: + <<: *CLOSE_GRIPPER + - step: "arms pull down units" + arms: + groups: + - left_arm: [ 0.81, 1.101, -1.57, 1.602, -0.775, -0.262, -0.096 ] + - right_arm: [ 0.81, 1.111, -1.57, 1.602, -0.775, -0.262, -0.096 ] common: - <<: *SLOWLY + <<: *QUICKLY tolerance: <<: *BIGGER_TOLERANCE - - step: "PULL_OUT_STONE" - arm: - joints: [ "KEEP", "KEEP", *JOINT3_BIG_PULL_OUT, "KEEP", "KEEP", "KEEP", "KEEP" ] + - step: "arms avoid mid" + arms: + groups: + - left_arm: [ 0.958, 1.283, -1.463, 1.425, -0.693, 1.037, 0.79 ] + - right_arm: [ 0.958, 1.283, -1.463, 1.425, -0.693, 1.037, 0.79 ] common: - <<: *SLOWLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "gimbal front" - gimbal: - <<: *FRONT_POS - MID_BIG_ISLAND000: - - step: "JOINT1_UP" - arm: - joints: [ *JOINT1_BIG_UP_POS, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + <<: *BIGGER_TOLERANCE + - step: "left avoid mid and right ready to store side" + arms: + groups: + - left_arm: [ 0.958, 1.283, -1.463, 1.425, -0.693, 1.037, 0.79 ] + - right_arm: [ 0.575, 0.632, -0.626, 1.662, -0.943, 1.001, 0.962 ] + # - right_arm: [ 1.488, 1.4, -1.564, 1.58, 1.567, 0.0, 0.0 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "ARM_UP_STONE" - arm: - joints: [ *JOINT1_BIG_UP_POS, *JOINT2_BIG_ARM_UP, *JOINT3_BIG_ARM_UP, *JOINT4_BIG_STORE, *JOINT5_BIG_READY, *JOINT6_BIG_ARM_UP, *JOINT7_BIG_POS ] + <<: *BIGGER_TOLERANCE + - step: "left ready to store side unit and right store side" + arms: + groups: + - left_arm: [ 0.827, 0.921, -1.063, 1.782, -0.878, 0.628, 1.395 ] + - right_arm: [ 1.422, 1.05, -1.164, 1.725, -0.734, 1.04, 1.475 ] + # - right_arm: [ 1.716, 0.651, -1.54, 1.671, -1.077, 1.055, 1.647 ] + # - right_arm: [ 0.529, 0.615, -0.552, 1.742, -0.905, 1.052, 0.948 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "READY_TO_STORE" - arm: - joints: [ *JOINT1_BIG_READY_STORE, *JOINT2_BIG_READY_STORE, *JOINT3_BIG_ARM_UP, *JOINT4_BIG_STORE, *JOINT5_BIG_READY_STORE, *JOINT6_BIG_READY_STORE, *JOINT7_BIG_POS ] + <<: *BIGGER_TOLERANCE + - step: "right gripper open" + right_gripper: + <<: *OPEN_GRIPPER + - step: "left store unit side and right leave unit" + arms: + groups: + - left_arm: [ 1.375, 1.165, -1.088, 1.736, -0.861, 1.009, 1.744 ] + - right_arm: [ 1.771, 0.651, -1.567, 1.645, -1.01, 0.998, 1.583 ] + # - right_arm: [ 1.422, 1.05, -1.164, 1.772, -0.734, 1.04, 1.475 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "STORE_STONE" - arm: - joints: [ *JOINT1_BIG_READY_STORE, *JOINT2_BIG_READY_STORE, *JOINT3_BIG_ARM_UP, *JOINT4_BIG_STORE, *JOINT5_BIG_STORE, *JOINT6_BIG_READY_STORE, *JOINT7_BIG_POS ] + <<: *BIGGER_TOLERANCE + - step: "left gripper open and right back to ready pos" + left_gripper: + <<: *OPEN_GRIPPER + arms: + groups: + - right_arm: [ 1.488, 1.4, -1.564, 1.58, 1.567, 0.0, 0.0 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "CLOSE_GRIPPER" - gripper: - <<: *CLOSE_GRIPPER - - step: "add stone" - stone_num: - change: "GOLD" - - step: "move arm out" - arm: - joints: [ *JOINT1_DOWN_POSITION, *JOINT2_BACK_POSITION, "KEEP", *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + <<: *BIGGER_TOLERANCE + - step: "left leave unit" + arms: + groups: + - left_arm: [ 1.77, 0.686, -1.57, 1.689, -1.036, 1.011, 1.724 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "ORE_LIFTER_DOWN" - ore_lifter: - target: *BIN_DOWN_POS - - step: "ORE_ROTATOR_RESET" - ore_rotator: - target: *READY_POS - - step: "EX_ARM_RESET" - extend_arm: - front: *EX_CLOSE - back: *EX_CLOSE - - step: "pre_home" - arm: - <<: *PRE_HOME - - step: "home" - arm: - <<: *HOME - - step: "gimbal front" - gimbal: - <<: *FRONT_POS - - ########SIDE_STONE######### - SIDE_BIG_ISLAND: - - step: "gimbal ready" - gimbal: - <<: *BIG_ISLAND_POS - - step: "open ex arm" - extend_arm: - front: *EX_OPEN - back: *EX_CLOSE - - step: "MID_ARM_READY" - arm: - joints: [ *JOINT1_BIG_READY, *JOINT2_BIG_READY, *JOINT3_BIG_READY, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_READY, *JOINT7_BIG_POS ] + <<: *BIGGER_TOLERANCE +# - step: "left back to ready pos" +# arms: +# groups: +# - left_arm: [ 1.488, 1.4, -1.564, 1.58, 1.567, 0.0, 0.0 ] +# common: +# <<: *QUICKLY +# tolerance: +# <<: *BIGGER_TOLERANCE + # get bottom unit + # stay +# - step: "left arm ready to get bottom unit" +# arms: +# groups: +# - left_arm: [ 0.656, 1.365, -1.492, 1.724, 0.927, 0.276, 0.865 ] +# common: +# <<: *QUICKLY +# tolerance: +# <<: *BIGGER_TOLERANCE +# GET_FIVE_UNITS00: +# - step: "left arm get bottom unit" +# arms: +# groups: +# - left_arm: [ 0.458, 1.594, -1.478, 1.307, 1.2, 0.373, 0.714 ] +# common: +# <<: *QUICKLY +# tolerance: +# <<: *BIGGER_TOLERANCE +# - step: "left gripper close" +# left_gripper: +# <<: *CLOSE_GRIPPER +# - step: "left arm pull down bottom unit" +# arms: +# groups: +# - left_arm: [ 0.811, 1.539, -1.471, 1.25, 1.434, 0.446, 0.303 ] +# common: +# <<: *QUICKLY +# tolerance: +# <<: *BIGGER_TOLERANCE +# - step: "left arm get back" +# arms: +# groups: +# - left_arm: [ 1.026, 1.334, -1.57, 1.248, 2.881, -0.048, -0.987 ] +# common: +# <<: *QUICKLY +# tolerance: +# <<: *BIGGER_TOLERANCE +# - step: "left arm ready to store mid" +# arms: +# groups: +# - left_arm: [ 0.762, 1.392, -1.174, 1.278, 2.549, -0.726, -1.441 ] +# common: +# <<: *QUICKLY +# tolerance: +# <<: *BIGGER_TOLERANCE +# - step: "left arm store mid" +# arms: +# groups: +# - left_arm: [ 1.094, 1.243, -0.96, 1.29, 2.587, -1.18, -1.452 ] +# common: +# <<: *QUICKLY +# tolerance: +# <<: *BIGGER_TOLERANCE +# - step: "left gripper open" +# left_gripper: +# <<: *OPEN_GRIPPER +# - step: "left arm leave mid unit" +# arms: +# groups: +# - left_arm: [ 1.173, 0.52, -1.043, 1.433, 2.264, -1.063, -1.416 ] +# common: +# <<: *QUICKLY +# tolerance: +# <<: *BIGGER_TOLERANCE + # stay + - step: "left back to ready pos" + arms: + groups: + - left_arm: [ 1.488, 1.4, -1.564, 1.58, 1.567, 0.0, 0.0 ] common: <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "ORE_ROTATOR_READY" - ore_rotator: - target: *EXCHANGE_POS - - step: "gimbal lifter" - gimbal_lifter: - target: *LIFTER_BIG_ISLAND - - step: "OPEN_GRIPPER" - gripper: - <<: *OPEN_GRIPPER - - SIDE_BIG_ISLAND0: - - step: "GET_MID_STONE" - arm: - joints: [ *JOINT1_BIG_READY, *JOINT2_BIG_READY, *JOINT3_GET_BIG_SIDE, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_READY, *JOINT7_BIG_POS ] + <<: *BIGGER_TOLERANCE + - step: "arms avoid mid" + arms: + groups: + - left_arm: [ 1.192, 0.669, -1.568, 1.594, -0.541, 0.695, 0.041 ] + - right_arm: [ 1.192, 0.669, -1.568, 1.594, -0.541, 0.695, 0.041 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: <<: *BIGGER_TOLERANCE - - step: "ADJUST_STONE" - arm: - joints: [ *JOINT1_BIG_ADJUST_MID, *JOINT2_BIG_READY, *JOINT3_GET_BIG_SIDE, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_ADJUST_MID, *JOINT7_BIG_POS ] + - step: "arms ready to get up two units" + arms: + groups: + - left_arm: [ 0.111, 1.36, -1.493, 1.786, 0.386, -0.816, 0.75 ] + - right_arm: [ 0.145, 1.516, -1.495, 1.722, 0.442, -0.729, 0.548 ] common: - <<: *SLOWLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "PULL_OUT_STONE" - arm: - joints: [ "KEEP", "KEEP", *JOINT3_BIG_PULL_OUT, "KEEP", "KEEP", "KEEP", "KEEP" ] + <<: *BIGGER_TOLERANCE +# STOP: +# GET_FIVE_UNITS000: + GET_FIVE_UNITS00: + - step: "arms get up two units" + arms: + groups: + - left_arm: [ -0.038, 1.464, -1.549, 1.209, 0.339, -0.353, 0.512 ] + - right_arm: [ -0.041, 1.481, -1.409, 1.002, 0.16, -0.093, 0.293 ] common: - <<: *SLOWLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "gimbal front" - gimbal: - <<: *FRONT_POS - SIDE_BIG_ISLAND00: - - step: "JOINT1_UP" - arm: - joints: [ *JOINT1_BIG_UP_POS, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + <<: *BIGGER_TOLERANCE + ### + - step: "grippers close" + left_gripper: + <<: *CLOSE_GRIPPER + right_gripper: + <<: *CLOSE_GRIPPER + - step: "arms pull up units" + arms: + groups: + # old +# - left_arm: [ -0.426, 1.258, -1.436, 0.459, 0.158, 0.135, 0.458 ] +# - right_arm: [ -0.426, 1.258, -1.436, 0.459, 0.158, 0.135, 0.458 ] + # old + - left_arm: [ -0.37, 1.121, -1.394, 1.013, 0.177, -0.584, 0.685 ] + - right_arm: [ -0.24, 1.184, -1.396, 0.876, 0.051, -0.073, 0.449 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "ARM_UP_STONE" - arm: - joints: [ *JOINT1_BIG_UP_POS, *JOINT2_BIG_ARM_UP, *JOINT3_BIG_ARM_UP, *JOINT4_BIG_STORE, *JOINT5_BIG_READY, *JOINT6_BIG_ARM_UP, *JOINT7_BIG_POS ] + <<: *BIGGER_TOLERANCE + - step: "arms avoid mid" + arms: + groups: + - left_arm: [ 1.192, 0.669, -1.568, 1.594, -0.541, 0.695, 0.041 ] + - right_arm: [ 1.192, 0.669, -1.568, 1.594, -0.541, 0.695, 0.041 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "READY_TO_STORE" - arm: - joints: [ *JOINT1_BIG_READY_STORE, *JOINT2_BIG_READY_STORE, *JOINT3_BIG_ARM_UP, *JOINT4_BIG_STORE, *JOINT5_BIG_READY_STORE, *JOINT6_BIG_READY_STORE, *JOINT7_BIG_POS ] + <<: *BIGGER_TOLERANCE + - step: "arms back to ready pos" + arms: + groups: + - left_arm: [ 1.488, 1.4, -1.564, 1.58, 1.567, 0.0, 0.0 ] + - right_arm: [ 1.488, 1.4, -1.564, 1.58, 1.567, 0.0, 0.0 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "add stone" - stone_num: - change: "GOLD" - - step: "STORE_STONE" - arm: - joints: [ *JOINT1_BIG_READY_STORE, *JOINT2_BIG_READY_STORE, *JOINT3_BIG_ARM_UP, *JOINT4_BIG_STORE, *JOINT5_BIG_STORE, *JOINT6_BIG_READY_STORE, *JOINT7_BIG_POS ] + <<: *BIGGER_TOLERANCE + GET_FIVE_UNITS000: +# special set down + - step: "arms ready to set down" + arms: + groups: + - left_arm: [ 1.47, 1.567, -1.564, 1.58, 1.567, 0.0, 0.0 ] + - right_arm: [ 1.47, 1.567, -1.564, 1.58, 1.567, 0.0, 0.0 ] + - gimbal_arm: [ "KEEP", -0.048, 0.647, 1.184 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "CLOSE_GRIPPER" - gripper: - <<: *CLOSE_GRIPPER - - step: "move arm out" - arm: - joints: [ *JOINT1_DOWN_POSITION, *JOINT2_BACK_POSITION, "KEEP", *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] - common: - <<: *NORMALLY - tolerance: - <<: *BIGGER_TOLERANCE - - step: "gimbal front" - gimbal: - <<: *FRONT_POS - - step: "pre_home" - arm: - <<: *PRE_HOME - - step: "home" - arm: - <<: *HOME - - step: "ORE_LIFTER_DOWN" - ore_lifter: - target: *BIN_DOWN_POS - - step: "ORE_ROTATOR_RESET" - ore_rotator: - target: *READY_POS - - step: "EX_ARM_RESET" - extend_arm: - front: *EX_CLOSE - back: *EX_CLOSE - - - - - ################## STORE ################# - - - - ##########GIMBAL_LIFTER########## - ZERO_STONE_GIMBAL_LIFTER: - - step: "gimbal lifter zero stone pos" - gimbal_lifter: - target: *BUTTON_POS - ONE_STONE_GIMBAL_LIFTER: - - step: "gimbal lifter one stone pos" - gimbal_lifter: - target: *MID_POS - TWO_STONE_GIMBAL_LIFTER: - - step: "gimbal lifter zero stone pos" - gimbal_lifter: - target: *TWO_STONE_POS - TALLEST_GIMBAL_LIFTER: - - step: "gimbal lifter tallest pos" - gimbal_lifter: - target: *TALLEST_POS - - ##########ORE_LIFTER############### - ORE_LIFTER_DOWN: - - step: "ore lifter down" - ore_lifter: - target: *BIN_DOWN_POS - ORE_LIFTER_MID: - - step: "ore lifter mid" - ore_lifter: - target: *BIN_MID_POS - ORE_LIFTER_UP: - - step: "ore lifter up" - ore_lifter: - target: *BIN_UP_POS - - ################ GIMBAL LIFTER ############### - - GIMBAL_DOWN: - - step: "gimbal lowest" - gimbal_lifter: - target: *BUTTON_POS - - GIMBAL_MID: - - step: "gimbal mid" - gimbal_lifter: - target: *LIFTER_BIG_ISLAND - - GIMBAL_TALL: - - step: "gimbal tall" - gimbal_lifter: - target: *TALLEST_POS - - GIMBAL_TEST1: - - step: "gimbal 1" - gimbal_lifter: - target: *GL_TEST1 - GIMBAL_TEST2: - - step: "gimbal 2" - gimbal_lifter: - target: *GL_TEST2 - GIMBAL_TEST3: - - step: "gimbal 3" - gimbal_lifter: - target: *GL_TEST3 - - ############## ORE ROTATOR ############### - ORE_ROTATOR_INIT: - - step: "ore rotator init pos" - ore_rotator: - target: *READY_POS - - ORE_ROTATOR_EXCHANGE: - - step: "ore rotator exchange pos" - ore_rotator: - target: *EXCHANGE_POS - - ########### GIMBAL ########## - - GIMBAL_FRONT: - - step: "gimbal look front" - gimbal: - <<: *FRONT_POS - GIMBAL_LEFT: - - step: "gimbal look left" - gimbal: - <<: *LEFT_POS - GIMBAL_RIGHT: - - step: "gimbal look right" - gimbal: - <<: *RIGHT_POS - - - #################################################### TEST ########################################################## - ARM_BACK_TEST: - - step: "arm back test" - arm: - joints: [ "KEEP", *JOINT2_BACK_POSITION, *JOINT3_HOME_POS, "KEEP", "KEEP", "KEEP", "KEEP" ] - record_arm2base: true + <<: *BIGGER_TOLERANCE + - step: "arms setting down" + arms: + groups: + - gimbal_arm: [ -1.4, -0.048, 0.647, 1.184 ] common: - <<: *NORMALLY + <<: *SLOWLY tolerance: - <<: *NORMAL_TOLERANCE - ARM_BACK_TEST1: - - step: "test" - chassis_target: - frame: base_link - position: [ 0., 0. ] - yaw: 0. - offset: [ 0., 0. ] - yaw_scale: 0.9 - target_frame: arm - common: - timeout: 2. - - -# - step: "test2" -# chassis: -# frame: base_link -# position: [ 0., 0. ] -# yaw: -1.40 -# chassis_tolerance_position: 0.1 -# chassis_tolerance_angular: 0.2 -# common: -# timeout: 2. - - - CHASSIS_TARGET_TEST1: - - step: "test" - chassis_target: - frame: base_link - offset: [ 0., 0. ] - yaw_scale: 0.9 - target_frame: exchanger - common: - timeout: 2. - - ########## EXCHANGE ############ - EXCHANGE_DOWN: - - step: "test new exchange" - arm: - frame: exchanger - is_refer_planning_frame: true - xyz: [ -0.3, 0.2, -0.2 ] - rpy: [ 0., -1.5707963, 0. ] - drift_dimensions: [ true, true, true, true, true, true ] - tolerance_position: 0.01 - tolerance_orientation: 0.001 - spacial_shape: SPHERE - radius: 0.3 - point_resolution: 0.5 - max_planning_times: 1 - common: - <<: *NORMALLY - - EXCHANGE_STRAIGHT: - - step: "test new exchange" - arm: - frame: exchanger - is_refer_planning_frame: true - xyz: [ -0.3, 0., 0. ] - rpy: [ 0., 3.1415926, 0. ] - drift_dimensions: [ true, true, true, true, true, true ] - tolerance_position: 0.01 - tolerance_orientation: 0.001 - spacial_shape: SPHERE - radius: 0.5 - point_resolution: 0.5 - max_planning_times: 1 - common: - <<: *NORMALLY - - AUTO_TEST: - - step: "chassis approach" - chassis_target: - frame: base_link - offset: [ 0., 0. ] - yaw_scale: 0.9 - target_frame: exchanger - common: - timeout: 2. - - step: "auto sphere exchange" - arm: - frame: exchanger - is_refer_planning_frame: true - xyz: [ -0.3, 0., 0. ] - rpy: [ 0., 3.1415926, 0. ] - drift_dimensions: [ true, true, true, true, true, true ] - tolerance_position: 0.01 - tolerance_orientation: 0.001 - spacial_shape: SPHERE - radius: 0.5 - point_resolution: 0.5 - max_planning_times: 1 - common: - <<: *NORMALLY - - step: "arm back" - arm: - joints: [ "KEEP", *JOINT2_BACK_POSITION, *JOINT3_HOME_POS, "KEEP", "KEEP", "KEEP", "KEEP" ] - record_arm2base: true - common: - <<: *NORMALLY + <<: *BIGGER_TOLERANCE + - step: "arms put down" + arms: + groups: + # - left_arm: [ 1.4, 1.46, -1.587, 1.725, 1.411, 0.007, -1.4 ] + - right_arm: [ 1.4, 1.46, -1.587, 1.725, 1.411, 0.007, -1.4 ] + - gimbal_arm: [ "KEEP", 1.484, -0.478, 0.786 ] + common: + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - AUTO_TEST0: - - step: "chassis compensation" - chassis_target: - frame: base_link - position: [ 0., 0. ] - yaw: 0. - offset: [ 0., 0. ] - yaw_scale: 0.9 - target_frame: arm - common: - timeout: 2. - - JOINT2_TEST_F: - - step: "joint2 front" - arm: - joints: ["KEEP",*JOINT2_FURTHEST_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + <<: *BIGGER_TOLERANCE + - step: "left arm ready" + arms: + groups: + - left_arm: [ 1.2, 1.4, -1.564, 1.0, 1.567, 0.0, 0.0 ] + # - right_arm: [ 1.2, 1.4, -1.564, 1.0, 1.567, 0.0, 0.0 ] common: <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE + <<: *BIGGER_TOLERANCE + - step: "left arm first" + arms: + groups: + - left_arm: [ 1.47, 1.533, 0.161, 0.202, 2.13, -0.489, -1.208 ] + # - right_arm: [ 1.467, 1.495, -0.044, 0.077, -1.414, 1.009, 1.071 ] + common: + <<: *QUICKLY + tolerance: + <<: *BIGGER_TOLERANCE - JOINT2_TEST_B: - - step: "joint2 front" - arm: - joints: [ "KEEP",*JOINT2_BACK_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + CHASSIS_DEBUG: + - step: "run empty motion to debug" + arms: + groups: + - left_arm: [ "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + - right_arm: [ "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + - gimbal_arm: [ "KEEP", "KEEP", "KEEP", "KEEP" ] common: <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE + <<: *BIGGER_TOLERANCE diff --git a/engineer_middleware/include/engineer_middleware/chassis_interface.h b/engineer_middleware/include/engineer_middleware/chassis_interface.h index 95e437f..610f09f 100644 --- a/engineer_middleware/include/engineer_middleware/chassis_interface.h +++ b/engineer_middleware/include/engineer_middleware/chassis_interface.h @@ -62,6 +62,7 @@ class ChassisInterface pid_yaw_.init(ros::NodeHandle(nh_pid_w, "pid")); vel_pub_ = nh.advertise("/cmd_vel", 1); nh_base_motion.getParam("yaw_start_threshold", yaw_start_threshold_); + nh_base_motion.getParam("linear_start_threshold", linear_start_threshold_); nh_base_motion.getParam("max_vel", max_vel_); } @@ -136,10 +137,11 @@ class ChassisInterface quatToRPY(goal_.pose.orientation, roll, pitch, yaw_goal); error_yaw_ = angles::shortest_angular_distance(yaw_current, yaw_goal); geometry_msgs::Twist cmd_vel{}; - cmd_vel.linear.x = - abs(pid_x_.computeCommand(error.x, period)) <= max_vel_ ? pid_x_.computeCommand(error.x, period) : 0; - cmd_vel.linear.y = - abs(pid_y_.computeCommand(error.y, period)) <= max_vel_ ? pid_y_.computeCommand(error.y, period) : 0; + const double cmd_x = pid_x_.computeCommand(error.x, period), cmd_y = pid_y_.computeCommand(error.y, period); + cmd_vel.linear.x = abs(cmd_x) <= max_vel_ ? cmd_x : max_vel_ * (cmd_x > 0 ? 1 : -1); + cmd_vel.linear.y = abs(cmd_y) <= max_vel_ ? cmd_y : max_vel_ * (cmd_y > 0 ? 1 : -1); + cmd_vel.linear.x = abs(cmd_x) >= linear_start_threshold_ ? cmd_vel.linear.x : 0; + cmd_vel.linear.y = abs(cmd_y) >= linear_start_threshold_ ? cmd_vel.linear.y : 0; cmd_vel.angular.z = abs(pid_yaw_.computeCommand(error_yaw_, period)) >= yaw_start_threshold_ ? pid_yaw_.computeCommand(error_yaw_, period) : 0; @@ -154,6 +156,6 @@ class ChassisInterface geometry_msgs::PoseStamped goal_{}; ros::Publisher vel_pub_; double error_pos_{}, error_yaw_{}; - double yaw_start_threshold_{}, max_vel_{}; + double yaw_start_threshold_{}, linear_start_threshold_{}, max_vel_{}; }; } // namespace engineer_middleware diff --git a/engineer_middleware/include/engineer_middleware/middleware.h b/engineer_middleware/include/engineer_middleware/middleware.h index 199935f..5a12448 100644 --- a/engineer_middleware/include/engineer_middleware/middleware.h +++ b/engineer_middleware/include/engineer_middleware/middleware.h @@ -51,6 +51,8 @@ #include #include #include +#include +#include namespace engineer_middleware { @@ -61,20 +63,22 @@ class Middleware void executeCB(const actionlib::SimpleActionServer::GoalConstPtr& goal) { std::string name; + bool success = false; name = goal->step_queue_name; is_middleware_control_ = true; ROS_INFO("Start step queue id %s", name.c_str()); auto step_queue = step_queues_.find(name); if (step_queue != step_queues_.end()) - step_queue->second.run(as_); + success = step_queue->second.run(as_); ROS_INFO("Finish step queue id %s", name.c_str()); + if (!success) + as_.setAborted(); is_middleware_control_ = false; } void run(ros::Duration period) { - // TODO chassis run crazily in motion - // if (is_middleware_control_) - // chassis_interface_.run(period); + if (is_middleware_control_) + chassis_interface_.run(period); } private: @@ -82,14 +86,15 @@ class Middleware actionlib::SimpleActionServer as_; moveit::planning_interface::MoveGroupInterface arm_group_; ChassisInterface chassis_interface_; - ros::Publisher hand_pub_, end_effector_pub_, gimbal_pub_, gpio_pub_, reversal_pub_, planning_result_pub_, - stone_num_pub_, point_cloud_pub_, ore_rotate_pub_, ore_lift_pub_, gimbal_lift_pub_, extend_arm_f_pub_, - extend_arm_b_pub_, silver_lifter_pub_, silver_pusher_pub_, silver_rotator_pub_, gold_pusher_pub_, - gold_lifter_pub_, middle_pitch_pub_; + actionlib::SimpleActionClient teaching_client_; + ros::Publisher hand_pub_, end_effector_pub_, gimbal_pub_, gpio_pub_, reversal_pub_, + planning_result_pub_, point_cloud_pub_, left_gripper_pub_, right_gripper_pub_, lifter_pub_; std::unordered_map step_queues_; tf2_ros::Buffer tf_; tf2_ros::TransformListener tf_listener_; bool is_middleware_control_; + robot_model_loader::RobotModelLoader rml_; + robot_model::RobotModelConstPtr robot_model_; }; } // namespace engineer_middleware diff --git a/engineer_middleware/include/engineer_middleware/motion.h b/engineer_middleware/include/engineer_middleware/motion.h index eec089f..b0611e8 100644 --- a/engineer_middleware/include/engineer_middleware/motion.h +++ b/engineer_middleware/include/engineer_middleware/motion.h @@ -49,6 +49,11 @@ #include #include #include +#include +#include +#include +#include +#include namespace engineer_middleware { @@ -85,8 +90,8 @@ class MoveitMotionBase : public MotionBase(motion, interface) { - speed_ = xmlRpcGetDouble(motion["common"], "speed", 0.1); - accel_ = xmlRpcGetDouble(motion["common"], "accel", 0.1); + speed_ = xmlRpcGetDouble(motion["common"], "speed", 0.3); + accel_ = xmlRpcGetDouble(motion["common"], "accel", 0.3); } bool move() override { @@ -134,98 +139,111 @@ class EndEffectorMotion : public MoveitMotionBase : MoveitMotionBase(motion, interface), tf_(tf), has_pos_(false), has_ori_(false), is_cartesian_(false) { target_.pose.orientation.w = 1.; - tolerance_position_ = xmlRpcGetDouble(motion, "tolerance_position", 0.01); - tolerance_orientation_ = xmlRpcGetDouble(motion, "tolerance_orientation", 0.1); - ROS_ASSERT(motion.hasMember("frame")); - target_.header.frame_id = std::string(motion["frame"]); - if (motion.hasMember("xyz")) - { - ROS_ASSERT(motion["xyz"].getType() == XmlRpc::XmlRpcValue::TypeArray); - target_.pose.position.x = xmlRpcGetDouble(motion["xyz"], 0); - target_.pose.position.y = xmlRpcGetDouble(motion["xyz"], 1); - target_.pose.position.z = xmlRpcGetDouble(motion["xyz"], 2); - has_pos_ = true; - } - if (motion.hasMember("rpy")) - { - ROS_ASSERT(motion["rpy"].getType() == XmlRpc::XmlRpcValue::TypeArray); - tf2::Quaternion quat_tf; - quat_tf.setRPY(motion["rpy"][0], motion["rpy"][1], motion["rpy"][2]); - geometry_msgs::Quaternion quat_msg = tf2::toMsg(quat_tf); - target_.pose.orientation = quat_msg; - has_ori_ = true; + if (motion.hasMember("end_effector")) { + tolerance_position_ = xmlRpcGetDouble(motion["end_effector"], "tolerance_position", 0.01); + tolerance_orientation_ = xmlRpcGetDouble(motion["end_effector"], "tolerance_orientation", 0.1); + ROS_ASSERT(motion["end_effector"].hasMember("frame")); + target_.header.frame_id = std::string(motion["end_effector"]["frame"]); + if (motion["end_effector"].hasMember("xyz")) + { + ROS_ASSERT(motion["end_effector"]["xyz"].getType() == XmlRpc::XmlRpcValue::TypeArray); + target_.pose.position.x = xmlRpcGetDouble(motion["end_effector"]["xyz"], 0); + target_.pose.position.y = xmlRpcGetDouble(motion["end_effector"]["xyz"], 1); + target_.pose.position.z = xmlRpcGetDouble(motion["end_effector"]["xyz"], 2); + has_pos_ = true; + } + if (motion["end_effector"].hasMember("rpy")) + { + ROS_ASSERT(motion["end_effector"]["rpy"].getType() == XmlRpc::XmlRpcValue::TypeArray); + tf2::Quaternion quat_tf; + quat_tf.setRPY(motion["end_effector"]["rpy"][0], motion["end_effector"]["rpy"][1], motion["end_effector"]["rpy"][2]); + geometry_msgs::Quaternion quat_msg = tf2::toMsg(quat_tf); + target_.pose.orientation = quat_msg; + has_ori_ = true; + } + ROS_ASSERT(has_pos_ || has_ori_); + if (motion["end_effector"].hasMember("cartesian")) + is_cartesian_ = motion["end_effector"]["cartesian"]; } - ROS_ASSERT(has_pos_ || has_ori_); - if (motion.hasMember("cartesian")) - is_cartesian_ = motion["cartesian"]; + } bool move() override { - MoveitMotionBase::move(); - geometry_msgs::PoseStamped final_target; - if (!target_.header.frame_id.empty()) + if (!MoveitMotionBase::move()) + return false; + has_final_target_ = false; + interface_.setStartStateToCurrentState(); + try { - try - { - tf2::doTransform(target_.pose, final_target.pose, - tf_.lookupTransform(interface_.getPlanningFrame(), target_.header.frame_id, ros::Time(0))); - final_target.header.frame_id = interface_.getPlanningFrame(); - } - catch (tf2::TransformException& ex) - { - ROS_WARN("%s", ex.what()); - return false; - } + tf2::doTransform(target_.pose, final_target_.pose, + tf_.lookupTransform(interface_.getPlanningFrame(), target_.header.frame_id, ros::Time(0))); + final_target_.header.frame_id = interface_.getPlanningFrame(); } + catch (tf2::TransformException& ex) + { + ROS_WARN("%s", ex.what()); + return false; + } + has_final_target_ = true; if (is_cartesian_) { moveit_msgs::RobotTrajectory trajectory; std::vector waypoints; - waypoints.push_back(target_.pose); - if (interface_.computeCartesianPath(waypoints, 0.01, 0.0, trajectory) != 1) + waypoints.push_back(final_target_.pose); + double fraction = interface_.computeCartesianPath(waypoints, 0.01, trajectory); + if (fraction < 0.9999) { ROS_INFO_STREAM("Collisions will occur in the" - << interface_.computeCartesianPath(waypoints, 0.01, 0.0, trajectory) << "of the trajectory"); + << fraction << "of the trajectory"); return false; } - return interface_.asyncExecute(trajectory) == moveit::planning_interface::MoveItErrorCode::SUCCESS; - } - else - { - if (has_pos_ && has_ori_) - interface_.setPoseTarget(final_target); - else if (has_pos_ && !has_ori_) - interface_.setPositionTarget(final_target.pose.position.x, final_target.pose.position.y, - final_target.pose.position.z); - else if (!has_pos_ && has_ori_) - interface_.setOrientationTarget(final_target.pose.orientation.x, final_target.pose.orientation.y, - final_target.pose.orientation.z, final_target.pose.orientation.w); - moveit::planning_interface::MoveGroupInterface::Plan plan; - msg_.data = interface_.plan(plan).val; - return interface_.asyncExecute(plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS; + return interface_.asyncExecute(trajectory) == moveit::core::MoveItErrorCode::SUCCESS; } + + if (has_pos_ && has_ori_) + interface_.setPoseTarget(final_target_); + else if (has_pos_ && !has_ori_) + interface_.setPositionTarget(final_target_.pose.position.x, final_target_.pose.position.y, + final_target_.pose.position.z); + else if (!has_pos_ && has_ori_) + interface_.setOrientationTarget(final_target_.pose.orientation.x, final_target_.pose.orientation.y, + final_target_.pose.orientation.z, final_target_.pose.orientation.w); + moveit::planning_interface::MoveGroupInterface::Plan plan; + msg_.data = interface_.plan(plan).val; + return interface_.asyncExecute(plan) == moveit::core::MoveItErrorCode::SUCCESS; } protected: bool isReachGoal() override { - geometry_msgs::Pose pose = interface_.getCurrentPose().pose; + if (!has_final_target_) + return false; + + geometry_msgs::Pose current_pose = interface_.getCurrentPose().pose; + double roll_current, pitch_current, yaw_current, roll_goal, pitch_goal, yaw_goal; - quatToRPY(pose.orientation, roll_current, pitch_current, yaw_current); - quatToRPY(target_.pose.orientation, roll_goal, pitch_goal, yaw_goal); - // TODO: Add orientation error check - return (std::pow(pose.position.x - target_.pose.position.x, 2) + - std::pow(pose.position.y - target_.pose.position.y, 2) + - std::pow(pose.position.z - target_.pose.position.z, 2) < - tolerance_position_ && + quatToRPY(current_pose.orientation, roll_current, pitch_current, yaw_current); + quatToRPY(final_target_.pose.orientation, roll_goal, pitch_goal, yaw_goal); + + double dx = current_pose.position.x - final_target_.pose.position.x; + double dy = current_pose.position.y - final_target_.pose.position.y; + double dz = current_pose.position.z - final_target_.pose.position.z; + double pos_err = dx * dx + dy * dy + dz * dz; + + // ROS_INFO("Position error: %f, Orientation error: %f", std::sqrt(pos_err), + // std::max({ std::abs(angles::shortest_angular_distance(yaw_current, yaw_goal)), + // std::abs(angles::shortest_angular_distance(pitch_current, pitch_goal)), + // std::abs(angles::shortest_angular_distance(roll_current, roll_goal)) })); + return (pos_err < (tolerance_position_ * tolerance_position_) && std::abs(angles::shortest_angular_distance(yaw_current, yaw_goal)) < tolerance_orientation_ && std::abs(angles::shortest_angular_distance(pitch_current, pitch_goal)) < tolerance_orientation_ && - std::abs(angles::shortest_angular_distance(yaw_current, yaw_goal)) < tolerance_orientation_); + std::abs(angles::shortest_angular_distance(roll_current, roll_goal)) < tolerance_orientation_); } tf2_ros::Buffer& tf_; bool has_pos_, has_ori_, is_cartesian_; - geometry_msgs::PoseStamped target_; + geometry_msgs::PoseStamped target_, final_target_; + bool has_final_target_{false}; double tolerance_position_, tolerance_orientation_; }; @@ -350,7 +368,7 @@ class SpaceEeMotion : public EndEffectorMotion moveit::planning_interface::MoveGroupInterface::Plan plan; msg_.data = interface_.plan(plan).val; if (msg_.data == 1) - return interface_.asyncExecute(plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS; + return interface_.asyncExecute(plan) == moveit::core::MoveItErrorCode::SUCCESS; } return false; } @@ -447,7 +465,7 @@ class JointMotion : public MoveitMotionBase interface_.setJointValueTarget(final_target_); moveit::planning_interface::MoveGroupInterface::Plan plan; msg_.data = interface_.plan(plan).val; - return (interface_.asyncExecute(plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); + return (interface_.asyncExecute(plan) == moveit::core::MoveItErrorCode::SUCCESS); } private: @@ -460,8 +478,122 @@ class JointMotion : public MoveitMotionBase { error = std::abs(final_target_[i] - current[i]); joint_reached = (error < tolerance_joints_[i]); - // if (!joint_reached) - // ROS_INFO_STREAM("Joint" << i + 1 << " didn't reach configured tolerance range,error: " << error); + // if (!joint_reached) + // ROS_INFO_STREAM("Joint" << i + 1 << " didn't reach configured tolerance range,error: " << error); + flag &= joint_reached; + } + return flag; + } + std::vector target_, final_target_, tolerance_joints_; + bool record_arm2base_{ false }; + tf2_ros::Buffer& tf_buffer_; +}; + +class ArmsMotion : public MoveitMotionBase +{ +public: + ArmsMotion(XmlRpc::XmlRpcValue& motion, moveit::planning_interface::MoveGroupInterface& interface, + tf2_ros::Buffer& tf_buffer, robot_model::RobotModelConstPtr robot_model) + : MoveitMotionBase(motion, interface), tf_buffer_(tf_buffer) + { + if (motion.hasMember("groups")) + { + ROS_ASSERT(motion["groups"].getType() == XmlRpc::XmlRpcValue::TypeArray); + // test + auto names = interface_.getVariableNames(); + target_.resize(names.size(), NAN); + std::vector sub_names; + for (int i = 0; i < motion["groups"].size(); ++i) + { + auto group = motion["groups"][i]; + auto subgroup_name = group.begin()->first; + const robot_model::JointModelGroup* jmg = robot_model->getJointModelGroup(subgroup_name); + if (jmg) + sub_names = jmg->getVariableNames(); + // ROS_INFO("group num sides: %lu, sub names size: %lu", static_cast(group[subgroup_name].size()), sub_names.size()); + ROS_ASSERT(static_cast(group[subgroup_name].size()) == sub_names.size()); + for (size_t j = 0; j < sub_names.size(); ++j) + { + double val = NAN; + if (group[subgroup_name][j].getType() == XmlRpc::XmlRpcValue::TypeDouble) + val = group[subgroup_name][j]; + else if (group[subgroup_name][j] == "KEEP") + val = NAN; + else + ROS_ERROR("ERROR TYPE OR STRING!!!"); + + auto it = std::find(names.begin(), names.end(), sub_names[j]); + if(it == names.end()) + { + ROS_ERROR("Joint name %s not found in %s", sub_names[j].c_str(), subgroup_name.c_str()); + } + size_t idx = std::distance(names.begin(), it); + target_[idx] = val; + } + } + } + if (motion.hasMember("tolerance")) + { + ROS_ASSERT(motion["tolerance"]["tolerance_joints"].getType() == XmlRpc::XmlRpcValue::TypeArray); + for (int i = 0; i < motion["tolerance"]["tolerance_joints"].size(); ++i) + tolerance_joints_.push_back(xmlRpcGetDouble(motion["tolerance"]["tolerance_joints"], i)); + } + if (motion.hasMember("record_arm2base")) + record_arm2base_ = bool(motion["record_arm2base"]); + } + static geometry_msgs::TransformStamped arm2base; + bool move() override + { + if (record_arm2base_) + { + try + { + arm2base.header.frame_id = "base_link"; + arm2base.header.stamp = ros::Time::now(); + arm2base.child_frame_id = "chassis_target"; + + arm2base = tf_buffer_.lookupTransform("base_link", "gimbal_link3", ros::Time(0)); + ROS_INFO_STREAM("X is: " << arm2base.transform.translation.x << "Y is: " << arm2base.transform.translation.y); + } + catch (tf2::TransformException& ex) + { + ROS_WARN("%s", ex.what()); + return false; + } + } + final_target_.clear(); + if (target_.empty()) + return false; + MoveitMotionBase::move(); + for (int i = 0; i < (int)target_.size(); i++) + { + if (!isfinite(target_[i])) + { + final_target_.push_back(interface_.getCurrentJointValues()[i]); + } + else + { + final_target_.push_back(target_[i]); + } + } + interface_.setJointValueTarget(final_target_); + moveit::planning_interface::MoveGroupInterface::Plan plan; + msg_.data = interface_.plan(plan).val; + return (interface_.asyncExecute(plan) == moveit::core::MoveItErrorCode::SUCCESS); + } + +private: + bool isReachGoal() override + { + std::vector current = interface_.getCurrentJointValues(); + double error = 0.; + bool flag = 1, joint_reached = 0; + for (int i = 0; i < (int)final_target_.size(); ++i) + { + error = std::abs(final_target_[i] - current[i]); + joint_reached = (error < tolerance_joints_[i]); + if (!joint_reached) + ROS_INFO_STREAM("Joint[" << i << "] didn't reach configured tolerance range,error: " << error); flag &= joint_reached; } return flag; @@ -529,8 +661,8 @@ class GpioMotion : public PublishMotion : PublishMotion(motion, interface) { delay_ = xmlRpcGetDouble(motion, "delay", 0.01); - msg_.gpio_state.assign(6, false); - msg_.gpio_name.assign(6, "no_registered"); + msg_.gpio_state.assign(3, false); + msg_.gpio_name.assign(3, "no_registered"); pin_ = motion["pin"]; state_ = motion["state"]; switch (pin_) @@ -539,27 +671,10 @@ class GpioMotion : public PublishMotion msg_.gpio_name[0] = "main_gripper"; break; case 1: - msg_.gpio_name[1] = "silver_gripper1"; + msg_.gpio_name[1] = "small_gripper"; break; case 2: - msg_.gpio_name[2] = "silver_gripper2"; - break; - case 3: - msg_.gpio_name[3] = "silver_gripper3"; - break; - case 4: - msg_.gpio_name[4] = "gold_gripper"; - break; - case 5: - msg_.gpio_name[5] = "silver_pump"; - break; - case 6: - msg_.gpio_name[6] = "unused"; - ROS_WARN_STREAM("GPIO port 7 is unused now!"); - break; - case 7: - msg_.gpio_name[7] = "unused"; - ROS_WARN_STREAM("GPIO port 7 is unused now!"); + msg_.gpio_name[2] = "transfer_gripper"; break; } } @@ -884,4 +999,193 @@ class ChassisTargetMotion : public ChassisMotion tf2_ros::Buffer& tf_buffer_; }; +class TargetPoseMotion : public MoveitMotionBase +{ +public: + TargetPoseMotion( XmlRpc::XmlRpcValue& motion, moveit::planning_interface::MoveGroupInterface& interface, tf2_ros::Buffer& tf ) + : MoveitMotionBase( motion, interface ), tf_buffer_( tf ), is_cartesian( false ) + { + tolerance_position_ = xmlRpcGetDouble( motion, "tolerance_position", 0.01 ); + tolerance_orientation_ = xmlRpcGetDouble( motion, "tolerance_orientation", 0.03 ); + is_cartesian = motion["is_cartesian"]; + if (motion.hasMember("target_pose")) + { + XmlRpc::XmlRpcValue& pose = motion["target_pose"]; + ROS_ASSERT(pose.getType() == XmlRpc::XmlRpcValue::TypeStruct); + ROS_ASSERT( pose.hasMember("frame") ); + if ( pose.hasMember("frame") ) + { + target_pose_.header.frame_id = std::string( pose["frame"] ); + if (pose.hasMember("xyz")) + { + target_pose_.pose.position.x = xmlRpcGetDouble( pose["xyz"], 0 ); + target_pose_.pose.position.y = xmlRpcGetDouble( pose["xyz"], 1 ); + target_pose_.pose.position.z = xmlRpcGetDouble( pose["xyz"], 2 ); + } + else + target_pose_.pose.position.x = target_pose_.pose.position.y = target_pose_.pose.position.z = 0.0; + tf2::Quaternion quat_target; + if (pose.hasMember("rpy")) + { + quat_target.setRPY( xmlRpcGetDouble( pose["rpy"], 0 ), + xmlRpcGetDouble( pose["rpy"], 1 ), + xmlRpcGetDouble( pose["rpy"], 2 )); + } + else + quat_target.setRPY( 0.0,0.0,0.0 ); + target_pose_.pose.orientation = toMsg( quat_target ); + } + } + } + + bool move() override + { + MoveitMotionBase::move(); + if ( !target_pose_.header.frame_id.empty() ) + { + try + { + tf2::doTransform( target_pose_.pose, plan_target_pose_.pose, + tf_buffer_.lookupTransform(interface_.getPlanningFrame(), + target_pose_.header.frame_id, ros::Time(0)) ); + plan_target_pose_.header.frame_id = interface_.getPlanningFrame(); + static tf2_ros::StaticTransformBroadcaster static_broadcaster; + geometry_msgs::TransformStamped target_frame_transformStamped; + target_frame_transformStamped.header = plan_target_pose_.header; + target_frame_transformStamped.child_frame_id = "plan_target_frame"; + target_frame_transformStamped.transform.translation.x = plan_target_pose_.pose.position.x; + target_frame_transformStamped.transform.translation.y = plan_target_pose_.pose.position.y; + target_frame_transformStamped.transform.translation.z = plan_target_pose_.pose.position.z; + target_frame_transformStamped.transform.rotation = plan_target_pose_.pose.orientation; + static_broadcaster.sendTransform(target_frame_transformStamped); + } + catch ( tf2::TransformException& ex ) + { + ROS_WARN( "%s", ex.what() ); + return false; + } + } + else + { + ROS_ERROR("No frame specified for target pose"); + return false; + } + + std::vector target; + target.push_back( plan_target_pose_ ); + interface_.setPoseTargets( target ); + if ( is_cartesian ) + interface_.setPlanningPipelineId("cartesian_interpolation"); + else + interface_.setPlanningPipelineId("linear_interpolation"); + moveit::planning_interface::MoveGroupInterface::Plan plan; + msg_.data = interface_.plan( plan ).val; + return interface_.asyncExecute( plan ) == moveit::core::MoveItErrorCode::SUCCESS; + } + +protected: + bool isReachGoal() override + { + geometry_msgs::Pose pose = interface_.getCurrentPose().pose; + double roll_current, pitch_current, yaw_current, roll_goal, pitch_goal, yaw_goal; + quatToRPY(pose.orientation, roll_current, pitch_current, yaw_current); + quatToRPY(plan_target_pose_.pose.orientation, roll_goal, pitch_goal, yaw_goal); + + return std::pow( pose.position.x - plan_target_pose_.pose.position.x, 2 ) + + std::pow( pose.position.y - plan_target_pose_.pose.position.y, 2 ) + + std::pow( pose.position.z - plan_target_pose_.pose.position.z, 2 ) < std::pow( tolerance_position_, 2 ) && + std::abs(angles::shortest_angular_distance(yaw_current, yaw_goal)) < tolerance_orientation_ && + std::abs(angles::shortest_angular_distance(pitch_current, pitch_goal)) < tolerance_orientation_ && + std::abs(angles::shortest_angular_distance(roll_current, roll_goal)) < tolerance_orientation_; + } + tf2_ros::Buffer& tf_buffer_; + geometry_msgs::PoseStamped target_pose_, plan_target_pose_; + double tolerance_position_, tolerance_orientation_; + bool is_cartesian; +}; + +class TrajectoryPlaybackMotion : public MotionBase> +{ +public: + TrajectoryPlaybackMotion( XmlRpc::XmlRpcValue& motion, actionlib::SimpleActionClient& teaching_client ) + : MotionBase>( motion, teaching_client ) + { + ROS_ASSERT( motion.hasMember("filename") ); + filename_ = static_cast(motion["filename"]); + speed_ = xmlRpcGetDouble( motion, "speed", 1.0 ); + sync_time_ = xmlRpcGetDouble( motion, "sync_time", 0.1 ); + interface_.waitForServer(); + } + bool move() override + { + if (interface_.isServerConnected()) + { + is_finish_ = false; + rm_msgs::EngineerTrajectoryTeachingGoal goal; + goal.command = goal.PLAYBACK; + goal.filename = filename_; + goal.speed = speed_; + goal.sync_time = sync_time_; + interface_.sendGoal(goal, boost::bind(&TrajectoryPlaybackMotion::doneCb, this, _1, _2)); + return true; + } + return false; + } + void doneCb(const actionlib::SimpleClientGoalState& state, const rm_msgs::EngineerTrajectoryTeachingResultConstPtr& result) + { + is_finish_ = true; + } + bool isFinish() override + { + return is_finish_; + } + void stop() override + { + interface_.cancelAllGoals(); + } + +protected: + bool is_finish_{ false }; + std::string filename_; + double speed_, sync_time_; +}; + +class GripperMotion : public PublishMotion +{ +public: + GripperMotion(XmlRpc::XmlRpcValue& motion, ros::Publisher& interface) + : PublishMotion(motion, interface) + { + ROS_ASSERT(motion.hasMember("command")); + ROS_ASSERT(motion["command"].getType() == XmlRpc::XmlRpcValue::TypeBoolean); + command_ = motion["command"]; + } + bool move() override + { + msg_.data = command_; + return PublishMotion::move(); + } + +private: + bool command_; +}; + +class LifterMotion : public PublishMotion +{ +public: + LifterMotion(XmlRpc::XmlRpcValue& motion, ros::Publisher& interface) + : PublishMotion(motion, interface) + { + ROS_ASSERT(motion.hasMember("state")); + ROS_ASSERT(motion["state"].getType() == XmlRpc::XmlRpcValue::TypeInt); + command_ = motion["state"]; + } + bool move() override + { + msg_.data = command_; + return PublishMotion::move(); + } +private: + int command_; +}; }; // namespace engineer_middleware diff --git a/engineer_middleware/include/engineer_middleware/step.h b/engineer_middleware/include/engineer_middleware/step.h index 563c4e4..c8de702 100644 --- a/engineer_middleware/include/engineer_middleware/step.h +++ b/engineer_middleware/include/engineer_middleware/step.h @@ -47,13 +47,12 @@ class Step { public: Step(const XmlRpc::XmlRpcValue& step, const XmlRpc::XmlRpcValue& scenes, tf2_ros::Buffer& tf, - moveit::planning_interface::MoveGroupInterface& arm_group, ChassisInterface& chassis_interface, - ros::Publisher& hand_pub, ros::Publisher& end_effector_pub, ros::Publisher& gimbal_pub, ros::Publisher& gpio_pub, - ros::Publisher& reversal_pub, ros::Publisher& stone_num_pub, ros::Publisher& planning_result_pub, - ros::Publisher& point_cloud_pub, ros::Publisher& ore_rotate_pub, ros::Publisher& ore_lift_pub, - ros::Publisher& gimbal_lift_pub, ros::Publisher& extend_arm_f_pub, ros::Publisher& extend_arm_b_pub, - ros::Publisher& silver_lifter_pub, ros::Publisher& silver_pusher_pub, ros::Publisher& silver_rotator_pub, - ros::Publisher& gold_pusher_pub, ros::Publisher& gold_lifter_pub, ros::Publisher& middle_pitch_pub) + moveit::planning_interface::MoveGroupInterface& arm_group, + ChassisInterface& chassis_interface, actionlib::SimpleActionClient& teaching_client, + ros::Publisher& hand_pub,ros::Publisher& end_effector_pub,ros::Publisher& gimbal_pub, ros::Publisher& gpio_pub, + ros::Publisher& reversal_pub,ros::Publisher& planning_result_pub, ros::Publisher& point_cloud_pub, ros::Publisher& left_gripper_pub, ros::Publisher& right_gripper_pub, + ros::Publisher& lifter_pub_, + robot_model::RobotModelConstPtr robot_model) : planning_result_pub_(planning_result_pub), point_cloud_pub_(point_cloud_pub), arm_group_(arm_group) { ROS_ASSERT(step.hasMember("step")); @@ -62,58 +61,51 @@ class Step { if (step["arm"].hasMember("joints")) arm_motion_ = new JointMotion(step["arm"], arm_group, tf); - else if (step["arm"].hasMember("spacial_shape")) - arm_motion_ = new SpaceEeMotion(step["arm"], arm_group, tf); - else + else if (step["arm"].hasMember("target_pose")) + arm_motion_ = new TargetPoseMotion(step["arm"], arm_group, tf); + // else if (step["arm"].hasMember("spacial_shape")) + // arm_motion_ = new SpaceEeMotion(step["arm"], arm_group, tf); + else if (step["arm"].hasMember("end_effector")) arm_motion_ = new EndEffectorMotion(step["arm"], arm_group, tf); } + if (step.hasMember("arms")) + { + arms_motion_ = new ArmsMotion(step["arms"], arm_group, tf, robot_model); + // if (step["arms"].hasMember("joints")) + // arm_motion_ = new JointMotion(step["arms"], arm_group, tf); + // else if (step["arms"].hasMember("target_poses")) + // arm_motion_ = new TargetPoseMotion(step["arms"], arm_group, tf); + // else if (step["arm"].hasMember("spacial_shape")) + // arm_motion_ = new SpaceEeMotion(step["arm"], arm_group, tf); + // else if (step["arms"].hasMember("end_effectors")) + // arm_motion_ = new EndEffectorMotion(step["arms"], arm_group, tf); + } if (step.hasMember("chassis")) chassis_motion_ = new ChassisMotion(step["chassis"], chassis_interface); if (step.hasMember("hand")) hand_motion_ = new HandMotion(step["hand"], hand_pub); if (step.hasMember("end_effector")) end_effector_motion_ = new JointPositionMotion(step["end_effector"], end_effector_pub, tf); - if (step.hasMember("stone_num")) - stone_num_motion_ = new StoneNumMotion(step["stone_num"], stone_num_pub); if (step.hasMember("gimbal")) gimbal_motion_ = new GimbalMotion(step["gimbal"], gimbal_pub); - if (step.hasMember("gripper")) - gpio_motion_ = new GpioMotion(step["gripper"], gpio_pub); if (step.hasMember("reversal")) reversal_motion_ = new ReversalMotion(step["reversal"], reversal_pub); if (step.hasMember("scene_name")) { - for (XmlRpc::XmlRpcValue::ValueStruct::const_iterator it = scenes.begin(); it != scenes.end(); ++it) - if (step["scene_name"] == it->first) - planning_scene_ = new PlanningScene(it->second, arm_group); - } - if (step.hasMember("ore_rotator")) - ore_rotate_motion_ = new JointPointMotion(step["ore_rotator"], ore_rotate_pub); - if (step.hasMember("ore_lifter")) - ore_lift_motion_ = new JointPointMotion(step["ore_lifter"], ore_lift_pub); - if (step.hasMember("gimbal_lifter")) - gimbal_lift_motion_ = new JointPointMotion(step["gimbal_lifter"], gimbal_lift_pub); - if (step.hasMember("extend_arm")) - { - if (step["extend_arm"].hasMember("front")) - extend_arm_front_motion_ = new ExtendMotion(step["extend_arm"], extend_arm_f_pub, true); - if (step["extend_arm"].hasMember("back")) - extend_arm_back_motion_ = new ExtendMotion(step["extend_arm"], extend_arm_b_pub, false); + for (const auto & scene : scenes) + if (step["scene_name"] == scene.first) + planning_scene_ = new PlanningScene(scene.second, arm_group); } if (step.hasMember("chassis_target")) chassis_target_motion_ = new ChassisTargetMotion(step["chassis_target"], chassis_interface, tf); - if (step.hasMember("silver_lifter")) - silver_lifter_motion_ = new JointPointMotion(step["silver_lifter"], silver_lifter_pub); - if (step.hasMember("silver_pusher")) - silver_pusher_motion_ = new JointPointMotion(step["silver_pusher"], silver_pusher_pub); - if (step.hasMember("silver_rotator")) - silver_rotator_motion_ = new JointPointMotion(step["silver_rotator"], silver_rotator_pub); - if (step.hasMember("gold_pusher")) - gold_pusher_motion_ = new JointPointMotion(step["gold_pusher"], gold_pusher_pub); - if (step.hasMember("gold_lifter")) - gold_lifter_motion_ = new JointPointMotion(step["gold_lifter"], gold_lifter_pub); - if (step.hasMember("middle_pitch")) - middle_pitch_motion_ = new JointPointMotion(step["middle_pitch"], middle_pitch_pub); + if (step.hasMember("trajectory_playback")) + trajectory_playback_motion_ = new TrajectoryPlaybackMotion(step["trajectory_playback"], teaching_client); + if (step.hasMember("left_gripper")) + left_gripper_motion_ = new GripperMotion(step["left_gripper"], left_gripper_pub); + if (step.hasMember("right_gripper")) + right_gripper_motion_ = new GripperMotion(step["right_gripper"], right_gripper_pub); + if (step.hasMember("lifter")) + lifter_motion_ = new LifterMotion(step["lifter"], lifter_pub_); } bool move() { @@ -129,12 +121,12 @@ class Step std_msgs::Int32 msg = arm_motion_->getPlanningResult(); planning_result_pub_.publish(msg); } + if (arms_motion_) + success &= arms_motion_->move(); if (hand_motion_) success &= hand_motion_->move(); if (end_effector_motion_) success &= end_effector_motion_->move(); - if (stone_num_motion_) - success &= stone_num_motion_->move(); if (chassis_motion_) success &= chassis_motion_->move(); if (gimbal_motion_) @@ -145,42 +137,30 @@ class Step success &= reversal_motion_->move(); if (planning_scene_) planning_scene_->add(); - if (ore_lift_motion_) - success &= ore_lift_motion_->move(); - if (ore_rotate_motion_) - success &= ore_rotate_motion_->move(); - if (gimbal_lift_motion_) - success &= gimbal_lift_motion_->move(); - if (extend_arm_back_motion_) - success &= extend_arm_back_motion_->move(); - if (extend_arm_front_motion_) - success &= extend_arm_front_motion_->move(); - if (chassis_target_motion_) - success &= chassis_target_motion_->move(); - if (silver_lifter_motion_) - success &= silver_lifter_motion_->move(); - if (silver_pusher_motion_) - success &= silver_pusher_motion_->move(); - if (silver_rotator_motion_) - success &= silver_rotator_motion_->move(); - if (gold_pusher_motion_) - success &= gold_pusher_motion_->move(); - if (gold_lifter_motion_) - success &= gold_lifter_motion_->move(); - if (middle_pitch_motion_) - success &= middle_pitch_motion_->move(); + if (trajectory_playback_motion_) + success &= trajectory_playback_motion_->move(); + if (left_gripper_motion_) + success &= left_gripper_motion_->move(); + if (right_gripper_motion_) + success &= right_gripper_motion_->move(); + if (lifter_motion_) + success &= lifter_motion_->move(); return success; } void stop() { if (arm_motion_) arm_motion_->stop(); + if (arms_motion_) + arms_motion_->stop(); if (hand_motion_) hand_motion_->stop(); if (chassis_motion_) chassis_motion_->stop(); if (chassis_target_motion_) chassis_target_motion_->stop(); + if (trajectory_playback_motion_) + trajectory_playback_motion_->stop(); } void deleteScene() @@ -198,6 +178,8 @@ class Step bool success = true; if (arm_motion_) success &= arm_motion_->isFinish(); + if (arms_motion_) + success &= arms_motion_->isFinish(); if (hand_motion_) success &= hand_motion_->isFinish(); if (end_effector_motion_) @@ -210,20 +192,16 @@ class Step success &= reversal_motion_->isFinish(); if (chassis_target_motion_) success &= chassis_target_motion_->isFinish(); - if (silver_lifter_motion_) - success &= silver_lifter_motion_->isFinish(); - if (silver_pusher_motion_) - success &= silver_pusher_motion_->isFinish(); - if (silver_rotator_motion_) - success &= silver_rotator_motion_->isFinish(); if (gpio_motion_) success &= gpio_motion_->isFinish(); - if (gold_pusher_motion_) - success &= gold_pusher_motion_->isFinish(); - if (gold_lifter_motion_) - success &= gold_lifter_motion_->isFinish(); - if (middle_pitch_motion_) - success &= middle_pitch_motion_->isFinish(); + if (trajectory_playback_motion_) + success &= trajectory_playback_motion_->isFinish(); + if (left_gripper_motion_) + success &= left_gripper_motion_->isFinish(); + if (right_gripper_motion_) + success &= right_gripper_motion_->isFinish(); + if (lifter_motion_) + success &= lifter_motion_->isFinish(); return success; } bool checkTimeout(ros::Duration period) @@ -231,6 +209,8 @@ class Step bool success = true; if (arm_motion_) success &= arm_motion_->checkTimeout(period); + if (arms_motion_) + success &= arms_motion_->checkTimeout(period); if (hand_motion_) success &= hand_motion_->checkTimeout(period); if (end_effector_motion_) @@ -254,13 +234,9 @@ class Step ros::Publisher planning_result_pub_; ros::Publisher point_cloud_pub_; MoveitMotionBase* arm_motion_{}; + MoveitMotionBase* arms_motion_{}; HandMotion* hand_motion_{}; JointPositionMotion* end_effector_motion_{}; - JointPointMotion *ore_rotate_motion_{}, *ore_lift_motion_{}, *gimbal_lift_motion_{}; - JointPointMotion *silver_lifter_motion_{}, *silver_pusher_motion_{}, *silver_rotator_motion_{}, - *gold_pusher_motion_{}, *gold_lifter_motion_{}, *middle_pitch_motion_{}; - ExtendMotion *extend_arm_front_motion_{}, *extend_arm_back_motion_{}; - StoneNumMotion* stone_num_motion_{}; ChassisMotion* chassis_motion_{}; GimbalMotion* gimbal_motion_{}; GpioMotion* gpio_motion_{}; @@ -269,6 +245,10 @@ class Step moveit::planning_interface::PlanningSceneInterface planning_scene_interface_; moveit::planning_interface::MoveGroupInterface& arm_group_; ChassisTargetMotion* chassis_target_motion_{}; + TrajectoryPlaybackMotion* trajectory_playback_motion_{}; + GripperMotion* left_gripper_motion_{}; + GripperMotion* right_gripper_motion_{}; + LifterMotion* lifter_motion_{}; }; } // namespace engineer_middleware diff --git a/engineer_middleware/include/engineer_middleware/step_queue.h b/engineer_middleware/include/engineer_middleware/step_queue.h index 32424d7..d6fbb55 100644 --- a/engineer_middleware/include/engineer_middleware/step_queue.h +++ b/engineer_middleware/include/engineer_middleware/step_queue.h @@ -52,22 +52,20 @@ class StepQueue { public: StepQueue(const XmlRpc::XmlRpcValue& steps, const XmlRpc::XmlRpcValue& scenes, tf2_ros::Buffer& tf, - moveit::planning_interface::MoveGroupInterface& arm_group, ChassisInterface& chassis_interface, - ros::Publisher& hand_pub, ros::Publisher& end_effector_pub, ros::Publisher& stone_num_pub, + moveit::planning_interface::MoveGroupInterface& arm_group, + ChassisInterface& chassis_interface, actionlib::SimpleActionClient& teaching_client, + ros::Publisher& hand_pub, ros::Publisher& end_effector_pub, ros::Publisher& gimbal_pub, ros::Publisher& gpio_pub, ros::Publisher& reversal_pub, - ros::Publisher& planning_result_pub, ros::Publisher& point_cloud_pub, ros::Publisher& ore_rotate_pub, - ros::Publisher& ore_lift_pub, ros::Publisher& gimbal_lift_pub, ros::Publisher& extend_arm_f_pub, - ros::Publisher& extend_arm_b_pub, ros::Publisher& silver_lifter_pub, ros::Publisher& silver_pusher_pub, - ros::Publisher& silver_rotator_pub, ros::Publisher& gold_pusher_pub, ros::Publisher& gold_lifter_pub, - ros::Publisher& middle_pitch_pub) + ros::Publisher& planning_result_pub, ros::Publisher& point_cloud_pub, ros::Publisher& left_gripper_pub, ros::Publisher& right_gripper_pub, + ros::Publisher& lifter_pub, + robot_model::RobotModelConstPtr robot_model) : chassis_interface_(chassis_interface) { ROS_ASSERT(steps.getType() == XmlRpc::XmlRpcValue::TypeArray); for (int i = 0; i < steps.size(); ++i) - queue_.emplace_back(steps[i], scenes, tf, arm_group, chassis_interface, hand_pub, end_effector_pub, stone_num_pub, - gimbal_pub, gpio_pub, reversal_pub, planning_result_pub, point_cloud_pub, ore_rotate_pub, - ore_lift_pub, gimbal_lift_pub, extend_arm_f_pub, extend_arm_b_pub, silver_lifter_pub, - silver_pusher_pub, silver_rotator_pub, gold_pusher_pub, gold_lifter_pub, middle_pitch_pub); + queue_.emplace_back(steps[i], scenes, tf, arm_group, chassis_interface, teaching_client, hand_pub, end_effector_pub, + gimbal_pub, gpio_pub, reversal_pub, planning_result_pub, point_cloud_pub, left_gripper_pub, + right_gripper_pub, lifter_pub, robot_model); } bool run(actionlib::SimpleActionServer& as) { @@ -91,6 +89,7 @@ class StepQueue if (!queue_[i].checkTimeout(ros::Time::now() - start)) { queue_[i].stop(); + as.setAborted(); return false; } if (as.isPreemptRequested() || !ros::ok()) diff --git a/engineer_middleware/launch/load.launch b/engineer_middleware/launch/load.launch index 816a8c9..9ece14f 100644 --- a/engineer_middleware/launch/load.launch +++ b/engineer_middleware/launch/load.launch @@ -7,5 +7,5 @@ + clear_params="true" output="screen"/> diff --git a/engineer_middleware/launch/sim.launch b/engineer_middleware/launch/sim.launch index c8057e2..7017b03 100644 --- a/engineer_middleware/launch/sim.launch +++ b/engineer_middleware/launch/sim.launch @@ -1,11 +1,13 @@ - - - - - + + + + + + + diff --git a/engineer_middleware/mesh/units_studio.stl b/engineer_middleware/mesh/units_studio.stl new file mode 100644 index 0000000000000000000000000000000000000000..ddcff9f4c561d6764e64d17b0f759a1995adf25f GIT binary patch literal 428884 zcmbT92bdJa)BjJRz>;$YNe3Jt+!5~Jc5dfL0s=}BB?wCt6p$?GkRSpgIf*Da9q~Yc z<92SBC<=;55=1g8k^}=u-m0GIp6~3g{Ql3M=Y9Rnb5)n8C*6CyK`#TIw3u6N% zL4@aj6GSVm6Dhs@dH?((j180o5uX1|5UsT4uh9H)J-3Fjfs!D?^S=qAl~#0gZhy|F z$zg1uB#7|*Z-Qv0mH6D&^uWD!VQiozh)DR~1kpdGvt_H zuawu^o9jOQ(Y8UOiT@=TCF7sJ>l@|A=+jT394U_~C=?+5KK!UE7 z7I}}haPjRhHc%2o7!HDMB+>tA>=_)R^t;tX+?Cfdy7J#e+$C-Pm!zSDy%Go!Q59hV zL4=H=mObIeibygMvsCQiED0jSC{Tp5AwAr>uhNoU8A_mp>mlvY-^GYhgenyhs3pBJ zlt4)kAx4xUR6UTOYo#T;&*5-15GLp6XTIT#G)7ZJ#Gi1b(*PQ!eboIKB zKuI`)X+^}7i6kWCwUu8hKXwx6ui*&O#-I^{wfE{Sb+Zj5&|0f|KuUQMd((6J3CE*CuMw;8LRcKLF*g!&Fvle=#*M$U1!V#v8A|nfFX~kO! z8%W4&)|jxcSIw;R^@ zh(JPKvle+i%uz_7BphMdpiyg3cZ)tpLSC~LjbNB-kw8f}!n8rr$FSm61QPO^wI~v) zOjg7zMW7@cVcMWbZBT6IxXZC!-Ai7x7DZIZkdZ)1IKp8=%pXX|YgrFDuZY;rrIIs_ z%5lRHrVXpDe6k+$UR)}^W-V?7UO}3(8WJc8N0>IO9^`%({UPrqY1ZQ2Va!<6g+Py|ZC5vC3D z63!Wf4J70>Ymvu@eGTPTia<#?!n8r&&bg1UfrPwfE%JP^KV#ZJ0wv)H(*})Njk-|S zKtf)#7L8!B&t%#_0wv)H(}tBh93hOrQjb52oi8#Ou9}SwxQA zT_b}D)MDGb{$$&xjRFHlRg|TY5+v9j|D8){689c9SQAX3*6HLI5_w&_BL94gsQboJ zL7)VQC9mcaM3r4fE#jTib45LnK&_5xWfOT_+xV457Eyao5ka5?iQ;;GL3}XkYm0cg zd4(ty5~y`*beTk6*G^mWsYSGiK2?cpjuIq3eZPPp^a=+pBD&*oVFL-&YWtv6BCl(2 zUR`bxb8_Ss^*{*{WwI0!#D!~qi&%E&WC(#;Mq=f#Q`OgTJy3$gj?}_} z$oVMMBEG5JIfOv1*a5{8Id9(^x!EEj&ejlqg%Tv{&nzm4>??OzM6*Jlh}K09$Fah_ z$@9GJCA06<5Clq)VDDkV_QcYMi?8LHBY|4no18P)UQ&C{LP4Me3HBZ)Y)_Pag#>DG zZ;G5jd5QEZlpw+0!-VaL(yx#}E$&Uu8Eh|+euWYw*x#72JyH4<5~#(!$vK1VCDN}@ zf&}{;6SgNtEcv&waB3b&UGXvSv zbBXJH4UYu64k;s|XsbIPo1Umyao0cz690UZOAxjHIARf2+;tGB#kM)-vu$&q!HT=0 zR8oQj+vC4;2~ENp9}WVwUfNVzc>CC;vn^uwz0Dkb43r>|t8H#UWNv81M6JV!NbU~@ zfm(-srG&SaJvYy?VZ~hoB}jafm`4!ZvmCOBUuF#A5#=CItLfO1!rOb-T5J&|hSn4H zKnW7LC*~DI$t`;=!Wth25~#KRVR7N@hX#IT5!U!HP=dtltoa3zI>=`c*7$G`s8zF3 zG2!i>bXsK**7z_`f<(o%0)qH;(oTzbSYe51Dm2VpahAH zvkD2~YV~avA^i$H9LEaxCeP=#mq@=t2@>pYOxT_{<4%524^SSLM(yvg01p6BkwkJxz zLISn8H+ep{y+ryIN|0dhVZ!!AEAARdpcZ=x&*!$6?5|O@3i}mGkYImf!t}%^3;*@; zuEEXGogDEBXJ?GAyw6}0C_w`AU`52U4J1$tz0ygb1PQh$^I*>O@A-J&C*zN=I_3}5 zLa%fZC_#er2h5Ov;*|uga}q?{l_LkW&?|X1unCkPfmto@I{SRQ5BIIVyokGU?>e^@`=XsXZucR9TIiKd0wqXb z9_)1;x%NXE@9%#1W<4JgsO9u_Bv66`=E3q5lDSWf1Zv?(lh4mh0wqW|b80G8yj2f3 z5~zjaU({R?C_w^qc+tANGoev`xRF3DjAKp$B}m{}M0knY@+%Dq)WUe|Bv66`uBn8# zyRGq|A%R*ruQ&;mAc1Q`F>2k`IM%9aFy5+rbcMrC9?Y8~rZ)Z%QA$G=<^i5yN4C_#erIeY)g%0)T)IM(N= z#aXS~O;EY0AW(t?=GR=0&f0hNTJx@&L6W_EFWM{CDZ+_s+6C4rTi$E!Q|l&?xlm=X zRznFA6oUmp`_vYppKmNy^hlr<+qSZBi^%m;RHQ7Glpw+OtbJ;W=y|`Juz~kNEoz0m zRu*m%5$%SFnxg~>iot@Q9Y~9~aqa_ADkM;gdULO}Pi+y_exiXABq#<8f_5M+!rD)C z5U537x!2mKwur3Do`_PR1PO}4f}nkBizr>Qg6KgcP>V*_UTdG)BCh1^ENq|z35vmj zpdCnySUhK!uz>_>QIyzg?NeLC&p%8N1WJ&g7%T|dfwYJl9nXdks729kueDDt>rryK z*e^i|5)^|4K|7EZA^i$H9LEaxCXZU%OQc_+1PS&YYn;n^Sbl{BYH@FJyt2JS`V~r$ zV1HwR_Ngr!XBu7bC>NUiL3St0wqYWzcE4k)Rv7K9p4aMf&^-@mstDM z7GXXAFi?U7`Pzi+fN*0<~z?7T!+txkbq5I4D7aa&19S7H$#Nexim1YEkwfyq)qA zimgf%`iBv6a8DBAi>^a?Y3DqQofod>VX7mac^4t)D~gwQ}fD8 zNswUgv3A=mqSDMFqUK1T7WbyLPi+zM8~{p?VDGVZ+blx*6%we$y=m=JTZHs0lpw+W zX6;j3gcWyrPe$DfwYWE}eQJxa;;x1gB-nebeQJxa=5q&uTI?m(KD9-}lqt;n)EY{V zV1KjrsfoZ{1B`t*J7aWp5-33evsy)n{SqWl3%$}wpacoFC-Y#nm*M8U3?xtsz0ygb z1PRP)#TrNLUm<~7=#@?aB}iZ%toAZA-pfD&wa_b_1WJ&=tXA!1XuOw!1ZtsIIti2@ zfmyBE%MkmbNT3#arISDj5|{_8y$s%`b|ZmWPH)E@VU!?&d9d2c&}c*@A%R*r(i}tt zN|12o)Yi@f*TaVdYGK51?$DzI3C!WuUIzDv4++%5IOZf!f&{Kb)LsVrm4O6mVLWyc zC_w_(RBA7S$A^IgYT>-%Bv66`t_{VgRnMc4KrNgfodimdz%{Pgnc#S3Ac0yq&pQc} zAc1Rj5k>hKB*$F?3Dm+&)=8iQ3EXo~1n)W}BnYD9_MK_pO%$ELMUO*U{p5hY0A%2GuJvCD}BYH^I=6{MLV*2u5D|a({?!@i=#^Yf+1HJB}rCeQrOG zLJ1O_&)NG|_>>F@)WTd_<)R$n9J>ZcV16yL4B?IZ{<(awWUoA@O{cE+!W$}kodimP z2*35+k7p1>D=oIoSvcEs+CWJVA>N#PHo>Km_oY+i;@zcC0wqC&cmw#^1fN#t)h}z& z>3H!DRw#jzAVRzW{A_|x-Sg^~wdj<;c$X}cKuHiG-T;0!!EX=n>X)_XZ3Xd8TquE( zAVRzW{A_~Xg5cFJYw;T@yh;rvP!dFlH-Mi_@Y@{nzWjy?uTnz^lmrn*c!E8H^AElk zd!@YQ-sDlMY$Wk`qWG?%B#01iZ7IS8f(W9O7WXE{D@7!kNRozmP!dFlx3(0aY#>3`N{f4w=PpGk8z>1P z#9Ld6P&SaDYo!HmC7JO`*+5AUp>YNfVuQc8B;QdKahKy2y@R9TE?>(mg9|3UwCu_D^5-15GLb|UnUa4%vv&*AI+C##$f$z~HA+K2ry^_}k zVc)!0B|*ZpVZG_idzSdFzPc}Kp;z*%DC`#m!V;zp>rHQA1MelTSqr_A*J@$EgrG`- zglXeHzP6z5%UbA_yh;uGMTfA2Y2!b>8lmpXTIiL$b`JYxim-%f<3GNBq3+9C99`}4 zA&KiPhOIz}EC~{(4eo2sT=~C9$ZLxT8H0*Yl5m7+!+O(OyWZlja-@ZXxMnRXA;c0% zaH&utX(UV=*1O@N9!SV**22gq`a_(+W*aC8N0>IOcf*AZB;++~VFVX`CBHz!U-3{; zKOAA&u-^0*Hjt3ltVLr5<|rgk5{@u!SZ{g@8%W4&*1{Q8jC0l&1WLjYrVZ=eaDMK^ zu|&<=@|v|McBy$@#4Gt_BaVqm5{@u!P~7EjI>{)i2uZURX0jsgDgq_p2!{=x4aiH( z{R^Hy<@V_NwXIB zCa*b78%Ure9AVn9JW<#{LSC~LdnKqTyP`&8{y^D2Sm9887n=LTr`;3t%G7O+SeXhso zk>_5dQW+>gq6QIzh%f{p#|9A*TRXoIC_XqwfASzr=j$;|YU`Cd`^5GArzI2l@4P2t zjwVS+NjQROHFGjxkd_WMbV+Ycc|&J=!x{z?C}A22zBUQ(%k$keI=YS9M~Z4xTb~x)&kh8#8oocSzH9x$6p1WpWw^8G4Q4J+X6nW!$+Plh^u&(SVs>gp3sI_fP z7PhT5sxg9y!4nhRSE@EJI!>>sp#+JTQx80B=^`V?aX$MY$R&67IHl<#{C<= z_gVvE-Vc>Elpv9m=WowjS+hnqK0btq9QT{Mk33h~_$8&9gFvm9?`Pt^(JnR^O2o?V z+PnX)+rStVQ9(lq65l?`pg-A?Ir6Pu?-Eh`gSqbB7u?3GxkViWYGKb(F1mHT`@q|7 zBg>Ma8ul}KOw*w`sn2I8y!CiI*+2p%NaR14NBG;(TN8=sJ}}YUfckvajEWi(s5P{I zWsqh!oHL+_sb+5OY3zVe?QTElxlvQ+6pB|U_W_XeMcm^$5HEUqVa(QYGJE8M=kml zdIpY7uj{87iSB3`gX72sjwtkeypwYbV%f13a6Ecl!>1&=vqUv8ewpSR=V;GqV_;IE zJ3ux6nqmpY53D)b_PS0^PITX)*6mKMi`J2L5mZNRgdh7u&ws+SNWDzDDo&hy~)8@;v$6R72jsHATi+rsnL=QAy$TfM`A zKnW6kw!EO9x=_|L_sVpOXi~eKAW(ut{R*Y^57M3_-2MJTBKlq4>|S@JjB$TeEe*?_ z?T2P!?%UEYpd;;dO)R(CJv?7oWA)J(4JAmN{h+9*d8YGR^R?^8xm*8S#wfqIfrb(! z3TKKH_Bs#TPehqln!7txDr zY_x2|ZwVc~KP7%Zj-g(e%NQ-THr7xQOmNOH;S$?8 zNqxR-QN;EPuAz^dH`7xg=RzI5$Mllir_yRYCErB}5^uyt z>zzMc?&(+O6SBd6RrJ>{LI~8-`qkBMwfoXD;Pg_fR9U>KDO@U)py#7)vmR?7RNkJE zh=&<7Va??L>RwF#?sZ|td|zXp$$vEFAE%)N3BGo(_|1wX=eM1WA9iQ*?yn{}Yo5J%Y5hpjKK=2UgF2Vc?vJyF zPsKMpQ+H*JInLG%COBT%JvjaDI??BNFTR#q=sjK+_qilc5=<~{dt#9cu2sRF$h6dA zuarLRb+PYC0wuu&)8_an9Q#@PVQtjzPh!VuSlcBzO6pyNRg~*V9jMpGVhS(4fnw8CRa5 zKF52Z7XLlaX`^L>Nb;Z81sB+~ti#ITV5+w9%cPalGY?UhC@I{r|gdtFi?qt|D7FPdc z-RR0oqzxrOg6yTsI(c1L?i@;9(fdzP54;y@QA?-Gz6tIRTY?1jtY7x*e-o%hJ?sC^ zKKK8aZJ}tBUKyJBJNKGr^~bXac`qcW&;5=*SDuL0QHwk#J;blbQ<99bbB?E%JTq66 zWX{u}>90&OeJLzKc9IN~Ad!9K`1JPEXIg!36)A*3t!MU!8bw4T$orz_i~gWG@IO&1 zHFA)k5+=wxIeQTAg<8}i39?0;1dcW84-fR<|HC*BCZsPo>w#^8y@{=EW|mZK?Z%RM z>&}h)&0po_W@`7u#9P{Vf1dg~#I^nRa}?pMJxgszwJkuL(~($I_PoDVQ>G>H9N8!n z@j*3|gd^JjttWb2=hvqD%ABca1m;8$pJL1e0$Hy7py%;Dl;(NXR zZ}C9{N{~4J{!RauZhQ51g_!t>Y?QWaAc0yD5$*t=t>qRm30W$XAhEf~HGjFgyY;)N z3C|!<3tPcibCe(<+c31PP=drOS&sW3zx9RQVTmWGtx$r*nsI0Ru@_SHhFvux<`}WY z$;vMpUG7%j*tPL?f4gGq#J%J-_w)J1EB#mgSs6qeE}U;AO2QGQmmJ((Dw@Y%?it;y zb=aHcFW+#D&NYmP@QG{g1NnE8*c7Nzjfp8M-l&8UB+iXHY0>P7D~|nGn281R4^%-( zc!K+U-GHISFCWkH{W||}6t)$%kNmq?DoJoOK?xG_+9ZCfnsd##f11(zwMmA4uvFmB zftQ50pPqZe|NYQY;u=SRGnSwPiI1<`@_*q!Cg}erPz%S3*L5l5kC7i|F1nUWRqnYW zfu)6i7x&_5a5CR7;u=fqbv4PA9=U!>QF<%hSq~)G$9~H7lU1ra$y*}(m)uD2J39%~ z!a8|fV|yHr{JQz8A*K55O0Gae!k-}}^tz5MT97*-5{`M9BJ2`GxrAlo2 z!G`9!vxp1;B}mNk|Ku-x`=&)SUp00^wJVW=KnW59=4TA#Kk&pNyl*a!+In(E6qgDK z)M5`kmwLw{7Bt)(S?Z(tqUI<;;?0S_`z!9tKw~DBe0Th!%0+hH-WNik7LOa-#=~7x zBLjcDEeMn#vAEnO|IwGsmUj9T5~#&ve#6yURy}xJnE#Rtlpw(oqeRww7SVFe%au28 zpApGzg(ZoPJ?hUNlSR}PX=kZWg2a1cKJ~Y6{2)xJP=Z7!?==6dQ;)+CC_#ef-HLxb z4MU(Oa@0Qhr8(x&^PTlT2@)Jj4wgO_#s*4|;HZ>o-bITryo+lUZd_{{w=T{&Ttb-* z;A}@v0Sd;(L~Ljt!vso@xY)HpVDTb@Y;Y7EH0Rss^v#QC?={Il2@=aE*9}bSn_>~A z1K&q)=$K8+2q-}!>BAa<@~hTJ8=DVDAFiqk0wqYiH?4diB58?5%sT&N^r?pf1c4GH zcI+t>_`S+(i?~ksU(tJn%3c$U{O{kZ#5PcZ#H&l@`tMy@Vr8$~R+%nV6m5kBYH{?j z*9J^P*8XL+Sr1!+YXiI!t%Qf8>ezSky0Rx_H)gl! zVDxM_VGBya6P%H)>+_qh)IzWEZr*7Q!luvZxMKCXQh)o!m*=6^sB$OcE|ehg(VvO_ zm*eK?y+7w~x6N9U<|}acZDUgMy?RKX7Orub-{?K>7{^xi-h~n*Qcf21&w9#{ zfxo@ew9+D9%O*pNbnQevBv30)i){YDcu9ORv6L|?(@Vykq^rBIeXyl@CR)F1sc31h zYs#P~<5o<(QU334aVS9oJ;v+u=c{QfxLD7~QDR~>Bv4CUo4M$?iXZ#lJUrBx+38p; z)&tA#TtUX|oaS59beNHE_taRFAW^a9gY?I_WX-pC8tq$JY=ki_qkB0LsAd11Tlcxu zBaF%`2J;z~2%ZOdg~jES*Sy-|JhA%JbgQkp6?o4u{&?M2j@k+(NZ`87>+-%f#^_ck zzwa*f2NI~o*>W@Y0n5hGj^m8O#;mjwG(J#*gfpk^x_z9Hy0c;0{FeV#MFO?#YaSmK z5#P7nzqE%XSHC>&vE^`S&&9bvyUq;7!-owaQI)MuFS z=kclTSDKBhff6L_Ypw^cf@w9Zjmq1$yc{{~ec|RSb3{pE@h{m{N`fUw$ZL}*ms&#m zt8X*o6H3Anrj3|$e&32a{k53|>aN3G1G!@5`Bnbi zNwoPle|3}~A+Jq>*3E8S4dbZgw&E6*BUrA4&3y(WP!f(XZP0F%M)%i}b}vh&Qh9jS ziD!DsWIb~KO8(tRY#+6`5=xMue9mL$zSjPgu<_vaqE%56p5RP|J%syx&7I4q1+CU<>b<^E)Ulhj#jarY~3#D};d5+vkrTtw+pacn; zMLqUz?SB!dMZ37VV@FuoKnW5wKZ-YX&HWOzfdp#NZksN5qs4BmvVk?1afN#g>*OR* zf`lB8a*boR6%we0quJ}CyhQtk@{(edmtbvaKKID%1?wcT3~e{%B{lQBPh-(;lkkL< zmuP!-HjFP^_d^F^(<0;W*gKGvmuS5yFX>Nt3Ex*rkf2<`?}xpcds49y>3gJW=C3@i)p7v3;d^VPXIioM z#Ju8lJsP!V^Zj+%UgQ}8B}nXDa^3S_v)q}ue&OTKcIPiA2$UdU&qO>szd7aJ=5w

v(cZznl z$-1RRLKaafw1Hahx(n%Z&+t=yuE)-G30sc6lrC(b1PMD|VPewNJX`cF_d^KO>N2gQ zzWC~V%SQB~qFe57ye0^gAki$gyuLG5?yB9IQ)Y`l+ety71c}&sRrI{yEVFE+?yj(9 z&8~nTP=dtqku~)A*>bP1#Qw-Fu^(;}1WJ&&e>Ylh`u;}C#?_^jx6~TBKoBTFBFiUr z_1iP$4rHAkky{$%87~NwATfMkoW6JKF3U!V))lsFpU_?qC_y4=Km$GUwA>eM(y;%Q z(M{WmK1T@>I6rz_{rffDyrcj1P3(y%K>}wYuZwoi*TL?&M?N3mxn1t#)1I|Q?xZ_+ zol$}W?Iw7_@17%pTC~UEv3JkuX~tUlG=rXC=<>-3k0|*(g7*4VX}zvbD{d|*pJt#0 z3C@Eg?RC-9jJ5J<1`?=+b#m4mB}i~~7V|_d>#+qxdNqJ+)j5Pc0RJT3A}Am!Jd*&gbMa9YqK)K?xF^!-ap2 zg9K`E253LW;Z>dak0XOKhm*M$wxKE&K}medLglrXYhk-N2^`P7j*`#0aQurMNR4+O zD?OaT`z2zX!5NFZ=KTPU5{_L?jdwYD=ME)E;3~}PqMcNYcT#!h4hhua(#kz+ddFl) zZtW)RY!{~t86`;I+EDClYrL~PF?00|NT8N|%^C9gKEJtnXFKuVYBUbz%2e!_t5qtV zNtBDC1PR*TRi~`HF4{wO%ROWyP>Xhl6C8WU zZr($#)VyvoN|3Oxt(`lKcUc}bvOST#Px=*mrQBgLccW+rM-V)!P!f(XvvAr~bMvm+ z%LjMyuA177lGm(->o%`T5*e1oZ$wEr!nDDAKD?{8Z_4?dpZ9xr&U1-oTS;Q?!TKvu5{@u!$ep|9 z#}Cny{W1w1d-Ie0#KbS3CMEPr&nT|#zw?Na#O7?j(isy;oYRru6MH9iF^xpM;jbm5 zBphMxu>3%84`iYJ#NXEx+Dt?`--{(gq{~vtzbhLQZzHsWuWpD$2@-r3&#a$SDnS&@ zGLe3t!0;myYp#7**EO%?V~a#vQIk+lprBnMD8t#Gk3xU5~zjk zX4;?=bb>$$ue{E`kX!U1-btOg)A-3KN{|>6d(e|@efC7hxxSE6p;nP2H9ebtdM?pC z*N1I|5+s~+)PE7E#SvW2a9$UAyPLJONw5S7d2JGOKGDss z%WXwv_sce+w(>*kraSsv5bPz?x=Mls`k3fJ-oKKqs|eJRKCQe&5h$TLsVITA|Jw!< zRC9sGp-M5pam#rKmxViJGs)Iamn8nF_WIr=Rf?m_6Ox~*gn|Olt1_jbm7x& z+*Vr~ed1escd%BL@|h12J3-k zr)&Vu!!3KGmeF%%T`fE9s-XmlRv)eNKOXj%RrAfS)HPOasike0(Yyi@sAd11TQ{b4 zzvyjev!-y~jw2PvxjnD&{IT&|g_y%*yIUu=2m&Shhn9_wNHVpYrK&J$yccRwKdCIr z>sk;|VY~KfH`(V(;w0>u+UxqO@LMTwS2Fzy?}f87&huUu&w)I%{V_YCyc{{4e_%Y9 ze>X>zB!29Ys0fxIA+Jp$e%%;jN3ra_Lq)Q!#TiFlvlh-JUY8_VfBR$&O2QGQ4T=nI zjtmouRa`;k6?qg(cHMPXNq_k*e@Hl21jc1%LG~=KI?@Q4Y_7iEw(RjwWN;3`;s6}&$$1&q* zztD{HAQ=WuKX2@(`{)gGzWMX_Du*j|icI})fxQQKq3_ARf} z)mBh!zgp@2l_){NzUJ6YFqQ= zG#*i<)<3F5V?wN#IJ={coLAanj>FMw&wHdK%fMP5~a5JyRH712p)q>{6jYQ ze^G)2jZNz;CljwWd6+!B$(^b+*J|>#0N;y!iBBZ(wLBAG+VG9cw0&0H%lsrxLkSX{ zKmW$Fw9-7QR9%wZU9tNs7tbFK0=4YtUtFpy$E$815mR0eC_$pvfGeI--OaP3URT{0 z3dXdoG(LnttwBX@ds=?E!YbA0J@d!(ddDPCg2Y#G4?L$6%vNyhSvuNE?&t)!8-25; z#&0RFO}H=G3fqSxgWRXYI#JEPowxnBN8fNprlAB0>RFH7=gTwo*|4z09MOX)L4xC> zy=S?rX^RcU!KEPtYHF0F&`QSMn+tBM; z+~G)M)6+)W9v36W3T;%1g=?++L{C`0XL}0D|}7?Gsy<*B7nwUV%yUn|jWgqZZc5>#BLF z<@P#VSMggAnnMe0;NF?nm2pD(?bG@vhychDpT4S8s^z=I`j_8+v^Zs|WWzy# z7Vo#)CkGaFuCo2z@Avb26Bp+g9PTD&5&OLf1DcN^GH5+ul8 zf?cYohf1f!*!55ZYEhj$vfV^mZIf+<)=`UE*LwGz+iHH#VJRP4{ecoBsGmHx@7BH? zk@8C3iLyV`y-TTd5B`SYuLVnpGyB7COfB+3uVol@fBV`~iB_d)`n?|5A= z|CBi;?yo05o1ltE~(-Xxiucjr_>HdP93(;ClMl$iG5bJ9chje=saP}&Rr$v{oM~O`% z!EN>G;Bn$?NydyD9E42^!tzkIalZ5Wg7Bq%%7l`HC)fs+DxT}nV9KUoLV3R3B33;F zvGKtI$~2@`$0pkLL~K_cs=iWS45(F)pZuuV0Bdgloo!g(H8B4AW#D?0AYory1od2m zaePvL?hj=>e4isR;`ex(*(4F0h=hI31g!~ta&3Sk3cUoM<2nhHAb}a@e-o(X%q*>4 z9N$+1>z5-RRr4#(OZMNn9+JqA;^rL|i@+6~Shrd0L5tv(Fx%iKifVO-W5WJB67Dxg z0ud|L&gM5u#9V8QO(v`p@$PBg)LY4!g?awska$LGo~$?|&d}TEG9>Xz!PV7lA~w-J zi-9!Tuuk@CU*}y`%{~=$NIYvcPX!&~w>jWUkt7-%?6V3b_BkS?*+w;U1*!205>Kw; zxmWTxn$h?KHjhDT&8hLa79~hv251vZq67(Np2z{0{-6FOX`J<$=V1=&ZR(7>NT3#;Xca_~C=`_nd(H06Lpqk$X#*ulUiHJ2RQxoQi^#@9jz@GKGW@Mh5GV#gr5CXN5iWb!u$6vPkJbCPr zls%W02m&QY?7C4xpR-iHA9e19&r-7g+CUH}L1IW;Y5n2p&?;@W6mO5FFuLI~8d&lEDTOw1Oa(rn&)f9TOl|&el7?-C1h%@@_0~hL*7?_8+>dHCUvH17*hKsKkk~b{&x=Z;*NRo^?eXzn z1eZ#l-pL%Z+CkX#A>AGYJgZ5fM3zj+Q0OCVrkJe4#K86me?nZZK6${ z9aM8$5}ts181qVQsf@D$ww3*NsE5&eaNhMOLBhUfV)fJIlpiK(XHI_(r8*?`bj|b4 z!S!%&@+LDeYi6E`SP$%H`szQq?WLOr9zr>(dk`yeDH2m^-Il?y#;HE_Cg5~ z_`Jnlyd?vG%i#(k6Kz1Lngzi;G1z)e}3ZL4?wxzgtiH*~aDdi`Lf}w|TK;LrL(yd@cRW>*~0p zP)x2;QRS_AC`#{kE4xB$wQtytS<84{Xu54xLWO zc`3W<54+}{-I=?6W$r6<4u^ZtZqZ<3)mu3%Pb_+~Sxm|1XSd6GNC^??U_-T=*R^3o z$(Z?8sj#+4V4aljCYd(m`6Tlj8S=?-P|dm1#XsSRs!hm`)ww0|d^lM{BPt3;lp;`z zMwd7(<{0NmTq<4z%JIy5mNFVR35q4j5KGj(kf69C&R?1~XxBMe?mFYWP|N986ie32 zSb_v+lKq8{3s;T}Pt1PL7fCULD$rL}CsN!U@1 z2^m+sF6+dvmOJ{5a=7NOuVckqrslhSJV#05>4baYjIfl1CwMln&N>@81}>?9SqA10 z_TQ06jcx@*tT<0Ca}k?ng4VVC6+x}U^$y}Ig63N^V)d)O4@NeuFMn}8B*E^B63S%4 z5Nv}_MpNx+MZdE=)z6omaq=|JDN|2!SRlr_(U4Fh~%*A(03L7Xvg4S>W zdv(`jN`aVezP5ru2@va&3yOH+)6Ps_7@ZcN|2!SCw!A*UGBCq)!gsagg`CqC)%0FQ6pwmja!1i`(g=c{TZ;=wWseT#PmIKUJxijg0f}nYlz%dcVBrS z=Jy;K#XN`vYSH@BZ?9|V2WVnSZ2Mc-KnW6*)mmRFWE-h%3&c$D+>KV}yf1WJ&g3>j=B)HxEJr}i~bDkM;A&f~*=dtLk8a~Wa^{A;!qN|2zKXq^}3 zdR)nNGkRXm>%s;Ss70$+I4?@$Bgq^i;#`I*uN@P`dJt^jh{F3Kfzj3Ln!PP8deewU zA*I5S(2CV>ubtQ3x*6T@(S1Rn1PNLp!-?OOk(Z;36)^i83Dlxl6iyZUTC|M0{PqE1 z10{SdXJ@>Vm}`ykL-VGXyPWpDXx>#PqLDU<`XA(FxIV4?kvyyC<3+UEKWe498%|fyLd0uqBAoAdG^QantrKe%sA^T4oZ;VnOc5B%Ig|9p?u7}e&$&mBv6Y^ zzy$2GI8*wUkBKc|p2a~45_IAQz9^TYOO=?vtg|>spcb8<@yjo)h#eNSV!^U=#v#9s zj`qYknGN!s3`&sTjDx@FWO_TDlZle&WKecTd$f3(OLG0Q9`Z`kwkZ zou26g|`z^aZgGXYkJ5 z>2Hf~U~iXi@!Qv8cM!h0OnZHT_^RAW-s@A6@C47b))QW>(G9UXs0f=DyPWVPXi2<% zb2;x>DoJ>PZP31`!8@swpIhf3Y+5|cP+!otiGS8^;JsHR2~V(%--n&!C+^z1{<&+| znMJJlIy&qQsy3v(rDJ30_rj92U#>6U{X`|PX|bQEatWRX#dBx*L{Y74Xzd)3>m7RX z2VW7C1kao(K>|I-X#)w=LJw76-!pROpCi7dC)b&nz3|K+ziVL=JcFVH3HzE?kk;4t zv<^S~AilmQThwl=*hI{nY+_ZVX^wSmc!F(Mer4Qz{44aic_aM#RR|_JPB$$xDw4+v#N$kJmO9`EtzeAC%TjGe^nLG;`3=-Fu>a|JlH^Kb7 zlRya)pM9Rkzx%RD3xe*?|B^J6AW?N)XHTJ*h6{U68%Us58-HJa^-g?_tee?ZNx}r# zKnW7R%v|JoTA9CWWD%lBTq-0`Yjxsh{=cpPp=_W8iR%OXo+3s0w0f`&Bv7l|<^%pe zGk;(c$_7f1==a4{&$M)-X_d^flr zNTAl$86^VKe&$ot!Sz515~Doj^wj3tZ9>%p3Dhc4x_qF)sU0?<>VXm@j=mVFZ(YV; zHVUo>5~#KA^T@#BdVZTwHc)~@o_{Or%WHjY6RI9apcY*Rx*XwCFTrhv5+vwOdar{g zZGvskza$L_)S?mwT6N(s-3Hr02@=#I`o2j1;y<*Nuz>_>QA-CJZNF?2Nn9o_6-tnx zp4Eqz;a{i-wt)m{k;epHTy)DOl7tDeff6LhL-nZl_#NJ08%UrQd48be8GcVVxUEov z1dTMk$@_c)BG?8Js6`_McyfKkrxSv0Ac0yGiTrWxGTVe|E0iEX zk4h-2g(_KnW5w6M6pp zC1)5LNT3$YaQ@V4xokq&KnW5wqk8gvz+aFEt_KpRMKiVk$1Ol88z@17GJ}K)UGs#o zfdp#tbEC>{OXA9{Hrh-6y6*BhpXXogQQwWCU#rkZ&A&n(o|uSVRiY;iW7=tzeyZ%g zwkM0@drL^ruVc{fZF4N)_qG?#?x6kNe0TgOMV@yMsD)oDrPI+UmbXQ^`;A&o0=4KDIP{1lK260o){NA(e#8Cldt`UbJw{tK{jMk#+CVM(l@ERDZ+xDCOSSd0^4gd_IW?|1N|3-W&3Rq*XOz=w z56G=?>mq?#8#`vv`_0d4*{G48SGzx}jK+S25+v};B=r8rq}Z zUt?~Ob-t(HfWD%i`8lWjT|%$RQ(~_BgF`KhatpHtGbSNNoiKYT++ddR@~x z_pABE?v94BDvN;{>CLe z;f-kQgZ@yzk%2~re;9K9KmxTW&ckoG(fnb^`2!_LP|OcF<_|;8A4s4Ue(^}mABLPiP=W;i z=94{t)cWPAPtG4mpca0INz5NUIe(x83H*MI$YczUoyjO~r|cylza)=-OkW*Zb81hs zU&k5a3MOysyZ%GkBjxzoiRL^R2l1Nc!ThftO`E@MtkLr6dODjSwfMU@^14&vjB6Lu ztCrx|pc4^IZ(dF_`iwPRzwz>Px9_g|W-)s&KBU@2f1MjN}^!;!uM3jry^kf9v+H zmW>rV`uOg*7-@9=sc|Y2sD-hM#z%{}zWG_`i;LZI)kg^u{jZMoH+rt8W#dZ8O}^D> z1C6&zwogR@wJ>&hU0aNR@3g&Z;QK?9v zmNOE;_i8e;5BYBL5qaW=; z0=1@XEgRTbVzEU${IRXEgIah=q1aR;P|J*uONihxxVgxiM)gU5`}DW}j6(?$k3Oyv zc)2`((Sb|#_rjh=jlq|E2cssXB7s^wGuXckc=q47jP1p~_T_Dnu|7(WDDz{@z_2;I ztHw5V_I=lwIA^8rpzozrBv6ZIIQth1T{{OGKWtp$b9bJ&6D3GwA6qvtp(B4`oNdgD ze9zdC)X}&2^!xRYK&^$n>Id?6*kcj>3yd<7_BZhj+t?}<3Dn}4X#etJ7x!2r*O1$3 zFJ^vVIoQqq%XUCAcG-~bW zjKuoi;{#hZ9<^)?ARBk@PKw`LH-^TbO`w)Dwy$uHGcv_|8ee>W_T4B!V)^(6fq|F# z_j0*)^Sm(HsM@NgyX~#NW063u9Iaym>;Ac55nZWuhwX3T?!CRMsJYCkP>WmL&X%H9 z4>OjQ_|ToRcFy`JL1NgOF@YUtezj~&AisKZ&PsRs$O)nc<-Je~vl_4Khm-FZzqi=w zp8UbtIFumKFtu8s&9FZ#8%-AVG{y|Ru zJ++(vzAI)>6XEjgw8m9NrycGt=Fb*Y1e0-*W zXWU%by6q|E)}hwT*>qAW5~zh=M)bPc?;7FO+Ktcx`=4?Ts&6qOarU=2JO_tMFZqCC z?jG`z6E_pN2Ni)@}jlndnGSx(tn|!^b+Z?<(u4MPpNaP1)-z(IbO)`sGgAyT`Nf zs8tdqh8I8P>99^lh8Yxd({)zj?=WSZ%){a5+tTZWY$mJ;C)fH z@y6HnwJeXSX+!t5=SZ#Yg<9XflUFY`OU?!wm1q%_?CkIX9H|w7S~Rb~{;oT@xmLAs zajkkh{dOQokhnXwgx>xJKab*49io<nbOp17f-R?C+~JN-&B z&zwqvMBf?}^^-0+*LEZi`ItPVM4P{Pc2)#xQDy);^#0F!YKuBvaxX4?cQ;Cq*w(PR z{@Izk)_k5iaDX0nMR|jCylBc-;U)hLlLM&Igxr6 z;&ruf4c8jip6vd$Z63~+lmv-yr^V{G-jx|yYl;eQQFItkeIRE`ia;&O?cj;Yd!0vX z6Yo}aztXyE97>Sr^;rY`>02^S{HX0Xt$5bC@#9)f<;+su3$=FNjn}ueklE{KiiZy< zF6K!t!+D}2P>Zr!YFYVvcja!5(@x~xx93vZGn^qS2@=WUOT3 z!rw9Sx_*p!Ps`b^lP`Pm3vnnx;y|am`rYDkjkB=dNX@gMm+#c7<*`Vh7XG>teJLc@ zP_1a;xxU|5uiAwYB=8rUD6^b8SgW=FQ{Rxb8SA42iT1l|>TUYU)!p+82WXxS+kHtT zr|(7rweWYP=p5M(Z)y9_eeL_GUqpSBAW{F_Dth)Ta+R7rqOaDY*blyDMG|)*fm--$ zS6)}qf!#D?(I37{f0v9!2@?2=TJ)Cb&92(b(*O8A|K@reN{~4Jaan!nbBnEUzWle> zv|sLJHTt+F??wW(@OQpwm3r${Eg?@{Z1gSX4MMobBD-XgPrfR(w1Z?WXwMJ z$1WsL3xBW7>ni?lqPBfYd1Jq?avVyK*xNUU{@D<@`%&$wrgdps*~skfl!^pu;jgIC z^TC8@EoomJ;Qd7POc05fvtvDPMD(=U>T1bN zZr-zOShzj!Cn^H9XfG3<4+i4qy4~4^8hsv!eNiPrV(sR7p3K`|@72wFuYa{~!uz6% zKrPyn_1Mq6qLwvvr)C>vG#Yt`_ji>9iJ~80O!%-E>>jmb0J#>OVY9Rz9}E_Bwvcgrlx#()o_jl|hijRPqo+$cc;-@u?J zarI-2S`VritLhGS5UBOV(EI*DZ!WZKbkEY*_~UdzP5e4FE~?Tw8SB?=l} zRvPXgP%CHi%z^g}K5fWtHTL>&>(hy_Wz)HTjgQFb;YB^8#=O{PGXnk~$FZSIb zZj>OwZ=d!G@M%M~Q8wE<#^&17eBE!4aS*75?+AEZRSUdj#I0H5yLxJ{8zo5asp3E< z{&h#Tab#o{&pRJRYaD$JQ}jUswJ1u!H?S%CXdHdkQuILy5?>Cz?{P#Qjib*oiato7 zmh&6f{kAvOj$6?OB}h>8(e3DSD&?n&BPPAQhEJICn7IG6Wa5NN2g&QpBs{v2Q(SXD z$-mR{sD&Lib$WQMxx&Z|t`DK)FftWvTdGb}p5C-o6!mLcRyhG@bo85xcR| z#m6B8YRR{nO`_F{Wj78jmqFA6CGx%G&{BOkEa%4YJG01|tJXzAzHw~Y*qGsA^5q7u z5CXO2`@<&DXxvZ9UmeRQ2$aY-WJ61(UD=zQZ&R@l8%W5vWlbAr^RG$HUb12cfm-{@ zj`bJ&BD>Xt=^su`j=EG=5GbL3_RD_8exl#!jBJ`bXQA>E)mBLC>~SgG_N$zWI#jRu zxAH_qpcZE`(vNBWI6ABFbAf-udArg=KUVL5M6f5K1n|$z202;u96@@ek{I0XWA$;DQC4_2h{jb z1ZvS(5nskLiQ5Tft6h509D^vK@hHC07h0;5zr^hk|!WdjKs^WrOlrj1e!U$0he zpo%4mKrM))*ueS(&LF|i#D<*Gb0wqYu+{dhk z^js!T;v~%M_1T2Xaak*q9ufE~dYM%!w(WU?$3dNq|${6c|MDm89s2eQnvWT?54 zAVIAlR{CbCs%+es46UmO)S{LadjKXuJ(w(e5GB-`Vig}+D)N$z;3dii667&reQw$y zPuv8as0h@;NaUQ)F)q>Q3dqq#qd=?&O&dL37kxceja#|xZcnYz$ISwqy`CQ#(6@Ge zLtIzx?9=)0$6xtc=fBfA86tS(Nl0JE?gJ4=}{rt3aU|X$k^oF0ajbG*% zX(dJ--L>q(SPdmeWN%(Na3pD;{&)=&U;eQ)ZAYbHyPL<2(@=uMk{l%iUAFDfllCz2 zbKGrp48o=&bq4dZh#IbI0}H*B;clgzO7u>rT#d zDlK!%A>~)kbGB|U!9H#GAoY3q;2z}rN-gvrv(F`gl3;>qhbIc(MS`!TmUBc|zPmEm z+k*+F&GE6i$){;2ACHfF+f64epnW#oDG^va>#ROB&1@@KC;DAW>hpUQzl|GCeU4>E zg8PFBv(H^4yQUTCJ|nK<(s7QpGZHgO7Y)>kzN}|C$$g$Pa{iucRkyBqLVb?+LM{Hg z%o)9|J+%+`iqM<)W#5m|IA#Fb0&V@vrCgP)OU)Os2^r;&O)N0=)D$pi9$up(#Jj=#M zFaIoTprpW*%7GIjdwOOco2}!W=vOKa?f3CFAbKr}a+IpimKOr2E|m4my)r$hb)&QI z_Z1@I)G24pkw~jvLVOz{ul@m*DkjHfpZ{tZBkPZ~gtyE7KrNiF=yxqEtoGH(Q`Ttt zU5uCqncxV3#Muvu3LBZuvyH+%$NI8gC}TXoqk)4!EsiUECo^8ZkmPG-DfzlvI}(HCMOH0*==6-oy_NPCiS_xleW?M7qp z`WoNKX>OzLtpZwuf3pR~e4jD$M1}$Su435&u|+dQ7Mw6Z@4le0C{^{Q6R76tu2sGr zNp9oj!Gan}keJy!AHBykJK^dNNkklfvcNZ~aJ5jKwAnn=W;=V$p= zH*y=DBa3S&L1OCAoWe%>pZKGr6BC}>$@O@x;e@nGmFgI`%S1Z})cSD6A4Kd;C|`v=amMNiY0s6f zV`L~7t)T>o1_Ms{?-YGLGVA_fL=^nKLfXEUY8vgk*V9mf#QX1F^LII)H8N92CJOAm zlXm?3SR-+4H4P<5WX|)a-&-|nWKw~lL@X=Y$oFSzoY6Xaq=phCZs*EQ{gL>8e4PbY z6yU}Yapdv^boEaUwc3c(3u48x0HL<%pu3gv)w#>+k-HIL8?(V?&_S!RZ z_z(DgKJR;=l%(kN3wRcXr8rQ zA%Sh$nsZDE=G>Ypd_Pcv1h!+g_oH{1e6et-QhH$t5w)<+t;CHWhwsbtWfjHS+7k2h zx>DI}uUH}uTicv%oCHdc=(y@#<)rt6zEc!IXxv!rhENd2m5+rawAc;8c*W5y&mierwYC^?gO4Bw~ zluQqsg`mWIraa+{g~MhN{pvTj5U6E7>o(MBHpTl>Bjr`8<{>CCpUH+fccK&V22r|A zY%ofGN9g_e2DVSXKRW~Rx`vf7Y0|9yDZQ;V#lmierwYB$?;pSo*b zrQXHZ7?haLWCQ1%*l61zcBX2%-PEGrLM`)Ix1q9K_X<0cvvPCGvM4d1$p)MKg4-QU zqHnYsgAycgu9uwveK$64D?5#pf2$gR1Zv^EjY-7NATbstNLc6La=c?FotTYA<-|ZF zP|KXf({pYTg(gpnMal07z0O(A1(}uAv?(6~S(%xBVKH48ULn#B8VFJe*hJ3_a?+O9xtw@Qx}fk>Foq{UjObG4t{CTI`~ zziwgiCX*z;BXk?=jS}e{ER%Vwg)nK-Vs97UMW7SxJs0WAkY?6{?d)s7*ZWCgK2xpz zo|u-{zAQ?9N9c8KrfOi9_S=y#pGnJ1p-!-GqDbGw+zAiL;_sTzltT7>gq;H>(KWKv z4wU?k&~whd?IL~Km(MQckA(S5TI`z-I|oeS)}LL1QSv)Nw_)_o;^njt%bEm9Sbqhp z8EC^KGUQsz`tu|S61dO7*`JFOn#Yzw2@ZcZ_-AZ#V@fC zNohyIYJ=s2dS_)shpMbU*LSz*C7QI;h4~Ur+s@uuQ9DzAo+L48Y8kj^CW)A+6|)ei zg(-B{zIRv<>T)emxl|}fOt$;a*m=m$(K{nCva{jeXWGkk3smmy2(-j#r1fk)0!fQ7 z+^fh=_ov%s&-SCNGI(CFg+MKgzR1pVuFE8h9qzB3a}5+xf&}ilWG6;4Ruop0_E8qB z3APZZh0zz;nYbbigt}XOlrM7vMU)_cdq*9%(0&tz>cu>jf9nTX2-LzItq$9bGE;;F z%Zn>-$^=^o)WUd;4qG$Z1|j@(A*FKBKoKQK;LcpO&$9V$AzvB~W%IFM3xQe~uaT`< z7Ck0xIG0;_78od^1PR<>%;u@5-VrA4uqk!jgDnJVVZ26%t^fIZLSSE;@_tN^h!P}l zr?kW7Uj3&~bY&_fXkeg)KrP(A?XWGopGvGPewOp?47L!c#bUZaJ}uw79w?#&3EWTbu+48< zKpZoCt32+jl|U`r9PI6*{9(B7x^L*jKcOpLo7P zK6%RKAPa$7c;1A~4yN)Gzu5E2lb%=!)M9ZJAv$GH|3I<*ye)BcssxHCK?2Xsuo?QD z{^F8dt>W$<4z>`eg;6Bg9d7r6#0(LW5A4blNRcC*aVwF)^FI#Ts9phL*V5kh-qnLG z1ZrUvNr$bNBT(GyRo}kcCrCsI5_pb@&8i*s7f<~A-hN<9poKs!JS)XcHKz6x3pz3g zH|7Uh2-L#3lk8TK^}eF}bwOCbzSJl|0?&N0xaV`d#U&5A2xC_VTL{#`xRdN2lUQ%@ zT>B_NT@Wau1PMI5#(IMSMZ`sCmI?K02U!Tz!t-$~21K7i;)cE}h0FfI76P>}QYG7; zxY0xWF;o#;3I~cPK?2YDu{d`fvx|@4pA@FBFEtXVg^?=R`SbNz#I<#A3Xg+W4MJ>8 zB=Ec|B}m}8OSW^M|65jj?qczN zfffR_@NB2UHvRPjVP~53;@Dlm76P>}wk4Y*+k07f{3)9_n|-NKf&`vXb=dk1j1#7h z$s=AE5^Nz*3u9Y4Z1tz_5w<_gCuY1AD53-jJZsA$2+Uh3)JiNWhJ3aXsD&qh9k$HD zvxGt!i;26s2U~2Q7DmH#*z7O*3D*Xe5}VorDH60Z{w5N5F4Pp`9lOuSmr!yX_uI}&UmP>XhECPngW*X!4T zMe#vm6%X#cIweTpnRpM(*!A3Dm-f1uTY;M@6|=A_jjEHYGDLima6O*WuLRz z#r6fczw3;%iv&j7U=cPJ+?K24wTYeBml}TywJ?&e!{+GqTF!hqtvIC|_gWq7g1jaIP*m{l6ti+D~ zPuS9n`@2p8wJ^ep!zPZ*sx+K_UYNR@`@2p8wJ;VlTUUFRSIHf$3c-JH@6{Rc83~LO z!)z=spcD*TFL(^)-m8;9EsQBm@e>Ox*LSWIGAD5F)fu@O35;LEqDDOOQXYqo5}vuR zuMfn!MlFo0!)~aE^-|ut4H6o+;r_1E25Mn6ZPpJq_EiGjNWxwArA7%77#+xATjTAk z)Et#Z_z)j#Ay5mWg0s=xcYo!4T%vt$9qzq4V~-<&QI6Q^Yjl7z<8f>IvxFe>UY!JL zVRUoW52^vm>h3M=8{{BM+;b!__7YneT^FQyee8CiW)tr3I-{Ow zy7ztOr@X(KSAO@Ad#_Fj5*VS2y|qJ1E4Q?k@=oS6;BTQ8Mge1ciWG09aKWx}he+=4 zI&Gj9^J*c=|F&$NO7)I2~&D_Hft zfbwY3RyndO_gZ$&U<5mMwy$^^Y(8VqPuWH&N*EPx(PY zD#gw`0F)qsk@#5eJno@V$QN}=kidw7 zEEenGqjErp+)Dar?(aGY)WSH2Y&_T~PFA|)QEn{a{;rciE#}q2jUgkY4f5Cfg%mp* ztDyu5jK0X;sC2XBe{U33{$L&e5~#&|RJi%1T$)Mpoa3HKmVdeT>XaaXQ6$;TSv~8^ zh12*bZp;I~-$E^nZOK~V-0HHjrL^)SnESg<8>q#+TDbEi%l=GqnzsH*X66B)1PP4M z>9CbwdoJ!mcv+n)l{7Z=8}H8MCpww#>WQ!orfCZ%Rq` zxlxQp<5nxVQRG^FtCfC(-2WozW;vSGrrGVKSJzmGq~E0UtPYH4Ugwp~noS~J*5o&`IVHa*C?B-@f!ft6+Zw8KCz15b+iqpE zg(eZ+ahs?mIwij+$OdceBD=#)Ry!@T5J@z(t7nzyBLiUB+Tc=pCv{F-i#Mkz3HyB zT(4|ocK}vSGxsj5F@Br1@yu@=t9)iR-9$t8d6~rRxL|K~o0wCQL?b~b?~PQ6OQ~|+kcRgyC;c6XgYTC4pahAe zXCmy;!<3yK>4lZEb8n$L=JM99ZG&rP+2iTBpV?C z>AV??$RPG5_IrpCqDu7!pa(M6k10%017=>t1xq=TBc8^NcG! znl$g&%P%MI^`_Cu1^buo* zZW20{jXGdz(ON2MvHkQ?(ysfbbt1*8KO=?AISPcL1PO|%kaR2ijfRn8zG;y{?a+_| zNT3$mF zcN4ewJT3>^u@WXtT6p4=#cu1~Tx_xZs9fQ%O#(`gz!Rrz{J{tZBT_K}B}m}D1BdO% ztmpj3&Q{{Lf4&Q4n^tgs zOOg|KBAcza8$_0~l`RBnna{e7CF>fBZmwCxiFcc9LW%iIX~%QoEYht(?C4S3LZFuU ztlOyMTTR@2u8^3fS?f(GF`vnXd6L~qX#e)xgp%J8dgUA3?Q95w)CY1b+&~2QEC@HSXD~dfP&a@CFO5 zdd|l&!j%!hKf;4eE&46gGM{xDchWu*GVg3JhMWthy`22Fk`oxKfklF5L>5LozS1-V z3Dh#5bsPI0Z57^h?Jaiv*)#+t<};M)k+!fQEXiDmhzqM zB)pl`N2KUtNT3#$y2DnlN;lzAl|JH87DEUnNc`2zxAN_qrhnzWV}sDAP8X4)q#=P? zW^L;Ei1FVftbN^Cq)2QiL4xk!n$n8mo>M-GmA@@GUbhq}+8Yw6g)P)!lP^VWWV_+n zF1kowy5N608T?CrmB+ds3=A%im`4&RU8_Y^r$rX6PyD9dDK> zD?=U^$5|gT)d$6bMWBf)%MXM8~JQ!C_CcV|8|{uXNC*>`puRG$%w$o%#Q-EViEmq&ut zhnh5+U}FP?#s(+Z*Z_YEweUnf>pvEcP#R{hVLvzSY$!^QAbUxpqSw(QlqVbN*=@bv z?neT(@H{tL9}FF)&^YR-(uYPx&Xe#+;E8NLR#PI`SgrgTK2~#{2S#fJIRwIza#Yb+|0+d$r*Q;X(tJ$-F(&w zHa1XbG;wrbMt_u;&y;rN^{e-#PiD>yqR@n?J4`M5Ez~lfbsKDKpwMWd?vSbeC^4VO z2J_+7f*a{Z6e?Yl=w0*T4pWPM3$@H=-3A*?C^VV~s(8^KCFV2zF7x5zAGM*#^<={& zu8z$ZV`|ZFp_ciq+hAh@#gC09JcefsK#BQGzstP-_JKcs!T&xxzV`|ZFp_ciq+hAh@g+>z%gzx~An9uaP z%%6|1{J_n~he<@Q9~5J1(Ql!a`K;SuqX~sZ6U&wk3P6eZOg5Oe9Wy;OrwdXjXRb_!h9wz z^bD96ZV>Sak1RG&%Y4>T#YPh{jV3mZd{h=C<}=wq&w!6VI3eWn*qUFa21m6qF$0%g*rk zVQ2Ui*ItaE^t1oF(25+y7EuSKYr&%i%q}IXkiy>(lwOuw3qdsfooIHC$#|g@GZDr9 z$I9vkk6%JI>6x_Xd27rKZjU{dv=S&uP8c*Je2$b%Uffe@^5%Cwpw4%+``h*)D=|q-^CllpvA%(liziAX}wtwO=d$n?S9Rg{P8j zaeZ|n_H6P}9+V1IQXMHOqXdc4J*ILSUV|4LWNX4(yp*gZLzUH5ia;|645%3Dm-xW$~|KN-9Ts2uh{v<>IiOvBgY& zJ)6gX=-!l4g#=2Fn9*2oZ>QT5QJB?vD68}HH;T$gpw^^UbEyU^-55@#+nv?4pPR|pKBpd z3u}|zKPBsS@ zrJw`}{DM1d6`~977*#VBi#lW_P%FoTXdZ!xY_mD!6@U8GuKmlG2$UcZS7r%s?K(mK z)&E%u)RJA+@pwz5$NNDiff6J-Z(GTu9Hk^sf<(pQ%Xm~BomjTDsuKO6kTQBfgE&n4 zv>bZhCl;P}5NWnIsz6m`P`1KK-kr1KP=Z8uo1XJ>U&^zu&%>Hw%HOvODa}sglTm`i zKKnJ^*Y>)SBcAOq`6wz8EV5DAiGN!N)GD=MF)!W4`F1lKd!~dc`*`j2K& zt?wXe^*MZsM|;zWENS}(JYMn_Cs5L-z?v#@f!`PwjlK5> z+DGOR+Gczlw%6qE3dJ_0cO@lZN|2!EBtJ2^4J1r0vQ5vrjV76f@Kn{PT3sfINuU$zPd4;7iipCS(v=_rCCLe*bt1k@SLL$n zA0;whJYyk%5VHdFM+nes={U6N6PMD5)d`D~j@jzNqV`Jkgx zbm^E19))koC_y4a$VEQewy(|zMhuzPL78JeU!lo}dooIpp!b|osK4i#nzvUd;$qDn zk1YghrEikJXWM%AAIEIGa&5y)x5dTJGD?u3XPQ;hZ4CD8stkNHZr`;JXDm5SxBnua zBMm%1FrHGFR3CFcbWu*-O|v%s`dLfqCMPHoo23TVCa*#IT~iC&g2SfQ`NTEJTS;<) z=%kidZNaoP)OPW==-Jf5HqD}0>8(8~d23Hj5Usx-3llpkC&VH9W*@vMV{YGtUgUGu zOI{4;Wo70`uXEQnr}t5vW7?6R`XEBDk2k~HD->C^-|a^-N|3nUB7x7tul}3rJe_+R zW&HlNCB`5AY#~sKeu?&F=r(A4-fQ?uPxb*47gj5-4b2!Sh4){lejZp@t6O%mG;Cw6 z`gxb9wyV}~>1CQ_#%TLfo2gzzpahBPyNYU!*3Fd0-`ivm<5xUSL7-OmJjJyuVY4MU z_j-fKT)04rRMk0DL|a;=zjWAZn99&3qap}iqMD`lQKW+? zPrP6d*$!V~w?|N&=h3dGTOg6-{?EKx%Z_uU{FBeHR+5}(aOjwxDv%&CvQa)>=hBno zh7IqNqm5RQ^joO4W_NzxuKxM+utEGx_l*-9p5)j5yf#PL(C?7?xn=>4b{mGTSJae5 ziHMMB$_Gl2$X2$1M*9}sT@M;I`X4>RZ6JYKDeI%D>%sNBtl%xW5n6=*?$;mNRc!fs z+l|c|&269riBCm}@H}mb*kPn<=_WsJ0}0d$8(Wl@NaHD+4dQgx{=5e9T}$7q1h2sd z{a5|wJ#RZKHBS}3=gA3b3rSyws>(UNl|Z|~mVsqRIlsl$&1o*KlJYCWL!PR`3c{|NC;Dk0b&mNHkMds{Yk?N?Puv2EpbMxeb&c(e9InMnvAs z?->!`C5eTul~5|zZ&}fikU{&^=c-g<#V_@9a8~v;I3<<%^g^B1DwCE!@`6;x{)c8I zP=dtkCpoljZ8d4qs5gd8 z1}7s>3roS850oJBzsm|GNX#ALp?&YVOB!AKHKdA47bQr<7R{rTzq4PeGVm26W-EJz z%iey-AHVeH|BAd&i=0`j(%%o{oX>~>*Ei6OB#QTq z>^>+#V%DxzoH*2b2_r@&Ze-^SqLlgbs%}6D62AuJA5aqxm;6Q!AVHvG+ac=)oUq^subj zg#lM2G1Utd-vXUD`au7}p#+I7)w625;?GOR_Ar{;ke96I{W%h-h3|?^#MMd7zi=0V zGipn7zmV>o|E7MOmQ}l)?FsuzJXbNbI#Fx?Yi6+totGartu&0TX4Og@at+HLWY+m;;b{s2wc?xQ;5H8Ayl2?xSXko( zN|1;P fM@liSy^PfRPtE);-K9E4IneTFFbWY=%`yGRDP1wZyD3l;k<7G}Qzx#Xk zWw>b&1D3DzA{&?zN5kw|_CU9=Y-=B?Nb9Bhy7XDz5-k!S3bxLrHLm(q`jO{`VWX$; zl&8uf0U|ysxAyAFcWIN)6@v(#SD2^DA_0Q>-4dUEaZgQ8)y3_bc&e}^-U`pA(fJrH z_fr*HzE1StRgl*QN|5-zG@Ev`;7#@|d19n$&FYDqKnW5v-Lh+RPRD24BZF|Q>d(Ht zE+Y0g=nY_R$L5e(+jZSoD=;-9P=Z9N@T}URg^F~vI`y1HoZq_8pS%E+AmRNwt2VYv zjFdh!ok6T>nl69{lpt~XRW_~Insrj;k?y9AQW7Ung2c4CIW(83C6Y@a@)_tIy!fpb zCs2Y!$F#Y$7bRy&F-2Sr8~Im-a{?tu6kh0|(aEqg{XZC`JHPX!6a;D=&5=hdnJ__m zRQd-asH`eTgE)Z_Bp#&Br_l+wZ?5Dm5s~XtR0;yMti2lL+->9H6a;GR>FS}aT@fYC z&iT>EN5``p*sPk1h!P~Q7u8GGF>OZ*0=4LwZgHl*ot!cq@wqs6X(~p@FD^wNT4sF6CJ0=ZX^OFNMH&bwr(s{eadDI7|IBgBqx&c zK}5;gJN*Y6gegIy#03v+Ri{WPxXEoJ=QrYm)={cZf<%)+9@_hROQcd^w+!O%g6&ce zsD zB+RNEp=`MST1Fd4OdYpX)rs*eAJLt=3I!H86t`3-Ed*-eh?T{Sa_=H!`Q}j4Y&kBY z1PMp2Xthp-xzdb`LzxY~sdgd9i5|+xs;eyoYT+2yVSDUeUKo0yr}8lMa@it*-_3bt zl{)j+Ea`FY&di2?mb^mUYWGLz8e!~o@W@kmQPFzwb6r038K!ounUl`KX3$|yl1bJsQM_e`^-_utzw z8?i0E+n0JoD)px}vk<8DU(i~W=C9sXXv2t9YrhEItNO{`8$~FCv=u6iu4(rZS9bYzljarzwGyM3t2Koq z%%>(ByFdICPAD_u@>FiApaco4Pu)89m#}|NmAEE-tpsXK9k*Q7$GBv}AnZh-1PS!v z9kyS)e+eB6G_e0{ZzWJ`Sj>F&uhJK!LnCNZ6jZ_>%6`o4OrpPaX z*Q$O(R!4+_5+u+kVo|yBvZzKg1`8#BHMS6_)uG%BHQnGFl1m`jxYhZS@G)VMu%bgF z1tmyy?K4BwM=Wz0aoKyDFk2}v-kF|9!M6zeE9)DD1WJ&wj)aTuUn0}F?e+WJMeL(+ zjElV;J2wFYN|3;jH=V++Oy{=68u!=9IBS4oT%4=15-33e$GhwV==7{|bguzQVu7Io zzIXTr5t3%$NAwN!>RYlMyIE;%VLlK3~#!Zw^MVnF-mLnGjpw4CM1WC11=|BOYJn zvk_~Txv0?%1Nbb?U)3p9NT39XQM<13nF-g`i7e+Qq;~B5N|bVBs8%MiS}>m%37tMi z{hm=3J;p!7|*xsx*|oa6~L;*UJ-u5}H*t ziF@gNxPLX`sYx{1w><@cTDMP@<$kG8TAKGE8y_}j@+KQtYgqp&i7#s!a{?tuq|EuU zDz8eB4J1$t%Z|lNMISVzIL4^&}?m= zU7jMo6MnUV_+04DXVcUl%PR4i+j0Hdva&(~B}mj-=+9?s3vHPA{}8CPeO3iNBODY< zUIyAg2@;=!eR-;`ryKkK5U4e2Q6Qg<-Wk_E8G$*+FA?U+nktkafp7EwCQu9CX0{hc z{w1Gce#t(!mLlf1pyMX@UjEDfG#>NBy$pGu?w5!tL1OB-o7^wy==p@b=gcpW5A3Nz zeu;%ZEwQ*SpMfk_jY_xY;$Lzn=9k!ev=mW-L}J?n?w3p*b&=Va+u?^ye#u}*a|?l5 zRo;~0bENS<$;OGB-(~Vk{PQ*!QG&$tnJ2hka%$=!W@G%luksS+mmF!<#6qA}mi8yO zUo!mUVMgpI@k1s*aoK?8EM{?1iDF56eZ9ebQP;%hj3__wn@ql_`=BNwN|3;|!2Mm> z@OLc)YOShqgZsf5_rGB_!V|s_Av~-eVaW%kedK~NeC|~%LnE)FjlRg_EhW5cETRO7 z#eOHaxAe>RD6_%5CHa%?Em;WEn)mrQ_m(p5KgNgzHqTIF+vosy)1HVKUat3RY zJqAnPL&?9QnTc#0)6q_H5hX|*ZyKU~-5MpuU8i1+h_%^^asnktVBe(^$7khCL7-OK z)#bHUyC+Edo=q@pyuJUP^;0e)N|4BPAehg|xc!)A5T3Q4q##i1T8R)oQ{+E+ra@E| z-FT|d25OyLS5cdDZKhQ9?MQ>z((oX+ff6LFehHP;sr4UH5UAB}U}6N{{3O zN|3OXsibM!_eyUjQg1+|>)Nw1Cs2aK%N~_9ud9mG>MZq9M5L+_&g&c{NMQe{6Z5-2 zWB+g_i$!E_~Kc;aMcYhhJD^5Ig+h_&oKYXg|I(()5og z^}|vhz8(=eeUgf))oqL{;L2CYP=ZA1?4>zf<=J$@#?mcW_^#)Xf-Kx&xt7Ira;=_Ms_{!hj z3gZkSeS9LXL6jhoFC>7k8-}(p-&(d$hPNvuQ0sQ)K)(LjcR0Bj{S*QgRUR<^PHmu ziMMBb_&QFir{wQa&ex@>%u|H~YPIO(!`E>V>Prg0RlbI~poeE<0jq-@MUiY~P-@|ng#3$j?o9HGG%_$=oiXL7TA zv`yF~(=6wc3OPlT;8+AlVJr@PgQYUfq?T!%&O)Hpmv?9Rob|fZ1(}UMuT7U=a4g|_F*>I9CDm7hb*83iJ}3qd=5GDVP8hDc{tgaxwDL&u~p)@ z-AbSY3G18;n^mJIf;6ip;D`?AnQ+|3XVshpN|3<$89qlQ7_(|Jjt6iIXdN4{c{nG5 z5+tniJ`o3B*-yTW6c1~4HJqlngbcB-(~7f!I! zMFJ&Ae0aKBC8E(|-Rn!5hx<{mq()Iehi#PA@Ape;9S^1so0R5&I@K*+rN4h`+>qJ8 znLCsq(P);cUfP>F>|J7=WCTi(X!Bo5jflD*_cP)m+YjePzSS>LS#Y5Qm$dwPT>W{Y z1W(n##N+DW!TY$4Olfq_GHE{?N|2c9c7W%@En4>zlL(X`@ne}MFOkvVM_H$7me&zfy%-8*bDMrwYmr0<6R;^6}+n951 ziQ7N|weT&l5-7p11pbnhKnW7~#rWSutbYMs&#{fpsaPUb0wqY`ZXqjy5+rB^J8Ab$ zyHu@(M`B0u@ARsSTAgz3P%qR@6_$VUXucYrL@*D4*0x3F0r0hLQ-TDoTI+u&FFb?3 zelD8p=O%$#mc2%{71Cs( zddX+WzKv4#CcPkJKhZ<TDgXeIY%=SE~7kzrEmL*2eyrqQGx`v zH-{~%L}sDwi2>rGw}UJMYCRjjTcxk)+Y0@ejqVS(3G?oj6E}9uYsm+u-Lk*Tw!pqa z=&(9O%r?S9K?xF!})Duy+a@NX+|MiraV~?J;ZwEpD5FK&>Tz`0{*Q zh}~=u%j%9#L7>*bn%=yuN>7V7h?T?t<^)QRi2YQS*N5AfRR(b~)Q8sx5~zh<6uZHN z)kmcUZ3Dmm`X!*mLO@ULuw7+DA5Zs~pUDX9ow1*H*iOFvB1atTDJ-2IA>vzv-*f95 zg#=2Fu=DcxzW|vJ zhgrOre`*}#U!S4A6~&&7;{pOVT!aLy|oaiwX(=j-sAMzl7r>r)Xb$a^*Cv(q!Uqs1kMz)5q+QKGW9sAvt+Ok zs1+3$&wHHHyIq-$`*k3acpJWVarizyG-Bq5w-J*C_w_} z0NIHCpB*xN+xPeIun?$KvZKns?T@^Fu=S5^^>@pw*tfloT0lez67RpL{M-K8?<*tN z`ym)_l!9#uTP6C8RstnRSUpSje#pifC8Bqao-B@*_#5RUP=W+{wa)RNLLcbd8asnlIG7Qg{Iels(guLft_-}2@Q&z=uSHo@F zx$UM~>8&-qa895E2|U5&uvKrLU-?|Cs-iCS6mTye?j0nr=Jf7yhVNq%@r8A z2cVz?iG?0lSvKcNPuizusUjPDkK0&004sr7xWhxY5gAyFybJ{;NDO8t*$cE=EHy9n zj(Y2BHKjy-h(B}hEld`rDrX{A)k`wk;0AJJC} z(M+nPtZ?QAOF_>^v(X=TS)l}HBQZ}7+a+e>dE%*t1^X zuR)X`fishOJ{o>soPt0toM+UD`3u|9?76b|>3?dYHk+h-|6Ymzi`4<`P(NP!T|Abd zP8_fg(FrJbBn}GKRoZ**@j>5B)l#|pxCJ{^#cTk9T3E9*!_rduC!`Fm2`i6$?x?q) zZjcIGITIgu{jPfK+72mgs1}c@)$1dd=g3%US13UO_hsq%@K$S=OrCSp!aV6V{93Qs z!Ok=&sD(Cg?o215x>w?@1SLq=v)otL6xc7t-`K}mBJ;wPQ~3&~AW#eU2k1n<3y1wF z=O{q}_aNx+hfD0tz+dc*LJ1P}Puy20EYYMn7j_!&N7%@owDO}Mfm*mnLoeOhSM$*d zmZhw)6mb4hCvv}T#|e}mVV!}bw|4tVw-f|wS?5Lh_iDgJTK`b?ctxvpf;dx}#wzY4 zj*8u&(wTa<=)c)JNZ*pZsV)RCe^*9{g|K|viMSqFG%z`V-}!`x|ElkDjgn&AjP-NH z`^zZrmbGXxgUDx;6%wcw z6}y$U_HT_vgP{5#8z@1d%;*EW?bi94%^>P$9b?o-(r=*_`P$_B=w3MG{Oj@ebgoFj zwIW;vN=f9q`<+e+DJVe#*LU>N&9*qTo+_}8TGo{xN|kt}D6Lv5=qI8Fn3A~mF)Jrf zf<*O7+xa)6< z9^kuz5i6_&N|3;}fbH?R>r#ex{JKnUu>}d#!dM*+Th3moIWgR~?G}_Efi0A$>VT0C zI})gc5k#y6N{~oix_pg=#owu9=?yUF7(s&SBZ)u>5?Jb-pq=DoLqzLX=a^b6ff6Jz z<^)?mU+O8-*nq|e_(owX!IfVtff6Lp*&rH2@_#?Yw?}Hahr1_%n6hrf$PK09q|G?-7gMke@vxww63cY;%m-5s?rJBI^wl> z+;L$gP=bVYUxslOK)ijkkXSoTQ|To1;ydr-Zyt?T>5OyZy)G*57IK~*-~>vLuRtGMqqD)18P)lNyB}$G3je4`Qk?C1*YL$X_llGmM-^ zRhqkl2$Ud!HLIs;@sETQ1Zv$GoWP^}|9636Q&H*ezPE%EC_$oUuZ#Tdfk}Z;29f<~ zC?`;Y#Gk1z@jDdubd5BKMxNie2Y?bJtSym7MHPw$(Q1Yz=eLiZ<97kn7`HGU%TUip z&K3@zN&@;(n7JoInW@-r=WtJpBv3 zw;RO7KU$?AP%GxeF_of5czif(5C`Ut;si>Ns9E)-O7ZkPHz@{D@xs6q1ZoXUb4;bE z5pO~-8$?}o3MWv4#J4oZRf?zow3eBwM7PB;{EdR&Lapa74sjcqqiKITwJV`R2W|r; zNMxULL@n|nO<2nyb515>kX}|upq6!Blx#GycjPuuf&|X6>Zz(VcQ7YVf`s~6Qz-)f z>{>J%MK)Xt^iM&c7S0sw#EU?^K2U-L^}Dn(s(Z*Zf^5_OD6O%`C`nGxZ9Yjpb=~mP zd<2O-0QMzVB6_OA6RvRY6(vYusk5`;-|oxoHd>)kufIgx(THys?h3UMC_w_>8%kBu zejg-I3wO6#36vm#?+xAcds$|;1B+!cR}hgvE!=Z!B~XF{zBkTPajy@11I#(@*t8NT zK>|zNVJkE*E1Qpr5)*xAl}77WYnWPxZT`~xY-TV@yjS+GO(;PEcQI3{S}IrP=jAgj zxSI;+_i+tDCw|oY$WrE_paco)DgfCC_f5lFA`+-&U7H}nD=~oEz*Hekz9`*SrLSNS zQNz<;&j(15z!f?@AHSCU;vNA07HZ*on@-gDx_%?~WMGYsXmiC3e@UM&DZe-8&L8YO zM@iYC$5pyt>ucKJWV_p_AB>scZ8h@M%-IM3H~nb zdeh5lWw}P{X%zq^NIabp!^>?A^Od)5r2jPS7o5dVR1JccnnB8?lK-K1f2PSo7d+ zZezose#}Nvx#>Bd$5LhH+$?t_Fi(2vvQ|j{SUiBFy+gQX#+q}gj|^W*@cKXr61aax zx3PbJS>ugLDn-=7wR@eI+PNQZiMV=?tL!O>DeLra2}+Q_^?Kb#zybYxg#>D0ThNIm z`~3O01PRo_5@92+m5r3>tc(3OWz1+K3T;mV8vD};G^P3tLUvY;tp~GB}iCfg&FbsXhg3}PBV*- z=zry!tzs;!T?w=J=-y`=o!~(NB}iB!X3=||u2-8Fde5T*$MR8iK=o)9<67MBFqV&s zqTj7&Hi&Rr9JhnsbCe)4>+4uP^7`-HI!2J6c&G25F+`vQi9)Z&@zGt4T5AoWjU3Ji zlpx_>c><3%?c%c9NY$dlIe9)%f&@n1(DPAI=v($1OBE8RHTJ=DKBBL7*f<%c+--Zo zlfG9dK>{Oh=x^=$QZ@NZDiWw=jmbe@xVAZBd4G-sYSrmIlg~eBxoO3nh!5SW@^2za zkigcX6VtYIO+la*wjRB#8n-{nOZV8?8GLMcrvF^^wS6)lnf6Lps$yz&BBw_JCs2Y! z;T044sJQ1+iVj2VN;ez##tI@tNB|2@+%5&*EdwL%nHVIK3aE zD`(_3kU%Z>_A~jo*3HMP^LMlI#ZpT|2@*Mn&gC`^iHOT^#BFDlYHQMA*iVw7`~ zAc0?3Js(>m6H}xLQzG=4$H&h3*lkKWQ6=#MCs2X}?t#?_j~t&<5U53KlS#XZsa@Ug zaU_PmJ}9AQ>YZutG1_L4>$?oFj~I|OCT!9Owz`z`Tgi7Wn>4%Gff2O^@7!S$rUVJA z&k)N9Rj#{kiUo(${D9u$Fw61rcU6G>igf*$2eGSKD2t@H$5c5*QK8VY|+3 zOlCV$zOb@F0<}iITg~_TOsPXBa;YVjVY^tkIK*$ZqUgLK_(G7gMW$uMV@dZR*cag2eJKEBL-8 z?*YqLS3($z z-&upd+w9CrzW?T%>rxeIy>usSuEyIHN|3n3@}Wnkqk zJmh5%@lQf$J|08~5*QU+ZzXJx7ynWtfm$v77xTS8Km9f`8}yz>vAsC-o}&Z_j0mL@ zAG%fIEfERSy7he_-;0x_2JP7<8=?6Na{?tuVB9#Jc)|AKP%A+KwJ-{tURHg^dhl@+ zrX8bap*@`tJoJ4sC_y55{HvCVZKJ+V1|>*XqjFIVdiIFr^NAQy?zqHaIk8GuVP!^^KgPObEjZ*W9*$VPO_Ci z2@>c(Q0XR}yF&uCFy6D3KnW7)2l07P!I()^kU%Yr{c9yqf&_Yl?8YhINfcvC^ziZA ziZPeb8^AJj*dAT`!D38AiSJG~-;NR_u-qIr)-lk!vq&q8I3B?94#oqfx5GlR@tVPu*AvlptXpK^oEbM2fzL<%1=So|D6d1WJ&=5v#LZ z@!bhnw>TETwqPYtf&|8RcE-}9IRF92H5l6vM=w?aB}m|?n2pbCl~#)7E3Bk{f5dWI z8s3$&sAdsu76_?Zz5lC{|Y5Y zG&@#9%T;5LIiGuxWYrnjsq)`VI zgE&}gCm%Z_fm)XzmD4^wijtz^Qt1(`m1@OAo+^|eQSe<|El=KYQkMd|4I9HgEav`| zg#fJry@R!ne~g#%y1E%Qj($wbOBW^hyMu!3Ym*yImC{w&YUbm@WS$QsP-|&mp!T50 zH0kqgivCS~)IGltZUZGqGzo5~T{M$&K9E4IsSEtHi({8d!wY3GY&bfP<^)QRNK>V; zcCKTzltKH~u;Ep6Q3?XJ0&DnclW%X38h6NM*!U14Iqq)a=ZUZGqT<~b3t@J-B6>2}^~0H3gTsj5+w3@*VH~f$r$!y z`%%Nj;C^~5K?1dozO1P2_sJA?_2(CZNc)0E9a2z&M9wzVv~1%uhkc1SZxD@sl;LHC z1ZvIMCurBVWez)3>$O32xZ}=8!YDyv)W|AY@zzBCl=>EnlXCJ^iE+B1(|JxN$7L z&ySXN@9UF=wYLX|m=esB&v`+++3by!_^k&^m4CB-_Hp~B3!b3^M3f+bF}>K{!4pgD zS!*8@#`NuOAyBJ(A3Hn4ohbbpF^JiSIk(*2ZBV@MZ^`Z=N|3-vV=NxPrpxwO`Q8Y# zN4BvLsD%;J*gd1sm+Wh%ycJp(u@a~?dv`@`(lghvh5JUbRJ|*jQ5e=aqc~|`0}&-i zV5B*RZA}}S5Y#KPIIlo`3xQhx8_R3m=DCM?Bv3qn`X>W~4>Jpi|A~@_5+pD(iNm%m zCO|m1uBf;sTC@+y_YW(H1<5Ja1=`22>yh!P|)UW&t3@N$F@nKxKG zxVDsqKrK8c&PI2I<_N_yR#kHUoy8J!xlbj3ty`}y((ExSS-cFq*K&Qg<5#0CX4qvXt&U_K)7<$ep5gR z5}zKG*4iCxAuX`cms)s#NEom(TnX|zV)6KaxXb2c#mwN*n4dgP=W-V$nP4~ zjE!XIOZ{!*ZQ<0j2&H_!H}$n*ysSFCKgaShXJmSj%4+BrHw%GUc*mu~ z_OxO~k?P|>7R`pZ`gxC4iHrJVr!@YBhROfS7`dbLp!aGeIwh#02iqx*U46GT7 z5+o*!D9Br7_aXP0ja{xqL~6U41eZ7@P|JE(%EI==#PAMbN{`hu<4}SGMcXj%f??mp zn_~nbmPF>ZydU%ynUT4%<+HeIYuk#{8?>qMQpOt#=|&~$Gmg+*OK@AF!#1sFYms`K zH0htqC_%!D-Md8nm45G1s}rq7>eX_&KeiC4g*UaaTiO3>E>i!o{mLyFB}mYHOw@nq zH#NC`Z!S{*;q&0Sg+ML5an4~|GPAKr{YS&76EaGWn9A>VN;K|viksb7r2Zqk>2V8z zT6jAfyDR=`J(2p4O71&klpuk3t+D$*2GkR&|44UdyNnVf=$0tzKlJ;fHb1E*QvdP3 z$y^J8T9zBmY#r8C6{-K2(P)m05+vxBDC$4-`=e6F2qN_#Pfv8V5Ed=Cqs(EO(l=D3 z{^R=&hm5h1kf1xCsQ=J!gL-s1NTmM5p0SjLKrM{J#BO(7=_^wIk+xla86`+yG#VCX zB5ND5GV^5SfBtA`?ReuDwqrJvx-KHL=F<_%TXo-ospA9|*}a+M;&M}+TGeiR2k)>r z^!QE8)1a<>-j3nQiOoCWP=dt!pB1!H`CCY(D?DLehOP-+?Mmn2O2Lkrg+ML*cCmbv zI$}TWHblw3uZ)ZmBn~L$wC%C2rK7Gi8`U-UEBlAq|0vVGRkIMNh2JiR&7*ZTVV>In zrT_OR86`-xn;xwFm8ygEVE$)jV@c|=!rHjritC5D76P^K+vTvOb*U^|jqk3E^^TTN zg2a(Nfm*%K-K3l^eli>NPInT@UTd$^&3ME@pqBNUIA~Q*;ZxOi%72T`^1HUouO$-C zy83GiC-;~3eW4rKdW@JVtlH8@*>(TDg+Q&=BmK0ItAIc#ys+wom(ouxVIfcp@2GRwI=$0` zU!#jDN1u5sC_!S`l2TgQ$+M(LH50Q@uVI2v;B*#c`$VZ<|!# zLZBAjOX#o#EANGio0rIGGdEOFg2btLp4#Z9F_OF)51cami61dZ}2Z6P2j)wbcuSZOn(Q7rk_e^z3*R16U zg2eD|p4!f9Po)tVsm?$8eGuBrUM)Q7R^LLP7J4{ z)#A9&<#K-UaFj3qmY9AN5}|`jYrc2Vge{k7PJd94Doi`!CH5*)!a|@HdNnMrbL?Ir z>nvaK*x17SD{A^tNc784MvDnZ8@6LI-DNsxL$okBvXYqlWmXG;TIkg{Y@ZrN3x98{ zBzkCiZ(#aSNaQ-?tL+WW5H@@U-DO&M%N)Ve^>49Y`&7LDFbUMku%fJ%<$lJnwvE#; z;`^)V!inqQ;?8CtWwef3R{v^(+X$gxx0a&x=WX5(nh`>gxV|_*tKyp_Y{nB8W+QD} z2f?f zi8Xp_aXlhb zn^HSxScmB}<{b7jtG#Z&5#rPIZ7l?9Vbnr4x0GXh=)Hp@M5ROiIFumKaJit}ACWU` z_gb3eY_lfeK<2e0#JvkE*pWc3r%x(rv!tA1>*Ep_5!U$gfzB*dr(gB35U7Qb5!qKX zba7m@Qp3ax<6Q-mAkow(OdByLN7#pZCz*|j(+Q7Epr3gARXbM+apOi!6JH**N>6uzY~ElE*ctTL{#`$cQW&d&5$4U_c-7%9=R> zN{}cpwVJm5b=I(t`wuW1zgjhw^Iq;GZu`F9LZDX2vKpG-ugqaU{a@uDass8LpPOJ|yP$siU3zm_BS+16q9@w{4x=oqZE?j>u*qPz&cW9k%la zqvgLVh~lbo*)4OHNGvT;PrLHmJ#1=cT77NibwGAY^buE%FJd83t5fXXn#asEVe{N* z&Fi0Qvb>P>1_w)cS_ss_SzLAp=-N|qsoeR*^&Wnf*;*u?=Wd`ie(n;MK4by2F|Sa9 zJe&1u884T!5U7Ro*ACmkO4nt7*-d=hIMgyvjl|&E4YkX*Z&Jzhv~$@@dmv9|{rRXI z)hq;R;XFL+an3xD7dE{ojL1{XGT)9w(R$(9vTJXpqLXMw_@wI_d7$S`p{0L43xQhY z3N+F_)OsqNI6%9j1Gl}AJ1|e?T+ap;0<~~G!C|Z6_C>B5G){2Y*vPWdfQ0*=M%t^s zccjF1lbMaW{lCgxnMaoTX;TY|u=&W})-OH{FQmH!zNh?)UvLzYz}u(E@eJ+ zpftPt3?)ct!Hu;JUWcX6Z^*{Z_#bkoF{9&d9k3Fpg*OH{Y^8Jmk~iM_7&k1Txrh=Z z8oX(wP0DmY`u2cwK2-WH&&*IvzWS+|g+ML5$B5kst$vd;HfkX^uGvIH2@)mxHqzea zi;*%eqPv97PWviX-q=xo^C`kYpcdY)D2(58pf~k0?+} zLU9njABk#(}!gq2|NSKHcB=ANp zhi&z)JMzndU*rQZl|_^wF>y}=Z9u9;(z^6?9%u5vtMZfg?n=M!6)gm6;XPk$p88IL zTq#RdrFF+(5hX|jcdxI_=|4-lK9Tm9cvUznFLBGI42}t~5U7Rsl{svSs~wVKw-r+^ zt@RdBg2dtX^|WRECrfu%(9Y%9k+R$>+*_#;Tf#!17T%)fu&oV# z_))*Ewlw2dsZfnI%tq;7YvsX{Dk;AfX0s5eg*VQz@744ba?aA#lmXc?h$ul~)x_Fb z#reafZv$urEBBK*a?HzmN`wBXECg!dJ$Q6?%~Ux|ZltJ@?*)_~@nl&|?QB$ksW*#F z@356gGhW^q(Nt;J=b42-EnIbQ*#64jTbAp!Q?#^axzAv(MZ1IrpLNEaJj?0VPOu8dg2i>4lq)6EoMp)J z`4$4Ta2?NKOII?NETkKtXv;NPz%@b z9JZAaC*$^~8=@@!Tv|X0631isW@FEn7I8A0L;l?1s2vH^!gV}$7k2Kt zap5t;75k7Kc9bA7^@wP!C|+SUs;-MTKy%@V>b_7UPz&eCS+6!QXT>AGMkw{>ZmM7* zU?v^s$Q`ynnsl{SSzb@6m_8>zU1P?;#q%aJn$1=Hj!u?N*CU@n%zDaRHm#`qA75ty zR>ktaaRdb8SSSX1?EnP>uwfT>?QSfrV|U}d7Pi;!?#AwBM-F&xy^7t6-Cc74B2Zq&hqSS@p2{RQ|=(S8>)P@VQY%x1# zX+`_e3HBd%WK#6mC0Ow6N(>bwa0dn-ZS&c4|8K=v-9(wds8j2v+xoX(sD-|wUSIB| zfy(=9o~+`6+6)yW=$qV7+zjbSUa{rjo++OWYC<&29SRX*Ji zUb;!UOds-{rEbMyHQHH9nl3vN-yz z5f_!6ZV z-H$r?%BiRz(UBs1Pw6E?j5g2u!yFk(X zxGP+svi|a8bU#XVEvce{1bWz7Embc2FuEVNgM1|dqsX(0?nf%&W|dX(VRSzpN`mgL5a1iCl#cCcbB0A z=gNOsCPr_c`KP{01V*7(HUIvxHWQ<_Pi))o3MxpHk2x*gKI>~A;c2wFo0HMoCv_K# ziUdZ<9@uYhNW`=(HaH%i`^} zgclj~5fxP%t=Xr4cRx16PP}k`!o47!7%E8oQF4!MM%xVfxb~&^6Gdw|^SU;oGc6cxIHL13d|!%L7=iX)ZDil)^^JO$W>gNSBf-{ z_~K>mwQW6*&iSD<8fGk-g21So6_(k&_NCL`JQ-;x=AJGu2vqh?T_x^w#piwhN3I?$ zZI~hrBrdqG7xijvr}p+V_GB|_B9<;jeObIo)Z@uh8rq3ewx42cEfN@o_1L`fdLKK9 zm9nVF#3tLFaEqRl2CugfRCkjTjwt$f`kS6EGmTRTW?A8LkTvm$DAAm^Gyjig(6y3z zKkyku;)|E*Wzul5vpo%;)8;)#0;4M4FoEuqJl)f7bnDC{WqB%9(3#C%;y6MjEeR(o$*TccR1rsENMQ2sH~`Ddh}f{yYqkK zs%L>j@tk8CNKDFNx(L<|cen^nA7Q>rkie+s+f3KOhIJO%---Nw(|-&VqUqg5Z%+Ir zv#ic^y&`rfP)(#ZmELgl_lO#0#ouYiPO|OTxmo+s*s(uq#pt(NC=FDQ2#TL%yO(a0 z7Pf_YGW68;^WU=tZDXrh9^2!_P5Xz#cmE%B;thGd;PZ2vEN`w>+WLBHWGml zH6-0M+lqiy+S$JxZ_y|5b49MGOz3K;aT`ywRS1~! zv?iuUeBUz;kNAI5k4Zsb6xMvRtZG*?bA`3K)*17Jg7VLX|BokXz&P{D@rgo$+IA|d zq#i(-BWCF$fl+Q%%sGIJRcH=??%;`&<{d;O?*VfZ-?jam|B}A=QQPsoG-UE*@pLsjzru#^F+Ov5FC+U=clA2fvng|# z*;Dj{U%fwI3s2id{5_|HVWWSKYjj$pe>VyKY&baxhk^wCO|^t*lc-v^XtDoAIHD*G z`rAySYB?YFu0)|?0bU^@SB^BKsKtMZTs?@P*+C-q7R@3ED=5hcO2Z_w?+jt7j-@FU zHrbmYL4P}TRDE33BTWv8AlebN^8Ps6 zF8?!HjhvA@jYH85S)O6%4*uzvjiG|XIM4C6vk|e{<+JqFa!{@&Y;Un4hb$K}GgOc$ zynno{?XDx*%R99D%=txA=2!Rhp)>_EGE|Vj`ewDbRA|ojwV8b=xVS|kFzT&sg00M( zJ=%gQLnMlOu<8<@deD+mu^zTCaaL z+9-!q7tGR4m9_y(Q>y2>JF~M(rJJ0f(OuFVydU3cUGh6fzxz81YL9u(pA}y#2&^T^ z38Ir~;$5Ga>!|J`L4P}<JC1kvX6aj<+7cIC(DO83`gW_Y!GjPbU` zvu^kS6iP>|41 zOtAR}#A`1q&^_PQs5SQjaEdse&Q&5Xiqde5cdeGvJB}MU`Of?M*?if~u-3NR0|T|h zZ&9{hADh{Zc^1!4Ayl8d820LLSBQUD} zvAVXxZ&3AI5*v{idE}pAJoIs`f)ta^qyWF&lZ?-1O)h#>GDOnmwtgcqgwlXH6 z%I>S1cpB$+9XGsq8h<1sFzUODX4|v%MU}&;={euB$0TNF^<}73s~u!J*!g^wPMOyv z%T>1n$Bo=PjbR>gSs|hJEN^?=Y=4!$*J#$@a@29-4^BMEEE5>jx?+GWY}x86J+jK?0+ixAnC(IT}%AQuTTEG>qJHL>j1Mo8)7={AcGXvoFlH;V<#;wRuh$ zb(M0=02R7ymW7zFtT$Ztx-a%iAY41%p+VQvS<@|uJUVljOa)C z*_8@KBm$$P_R3N+_hX}6<^n9DWkJyn(sMv>03_F(ucWBYhY zgcJqs6`xPcpMmu$R)CfCwTSl>r9p2pB#tl7FVaZgVLVU6i1%dGcpB}zTqFXc=pEx| z;jEStg$l9imICb8(|7|FB+y60YU#412pjY`KYNzzwnShQwk2l0y3#L(JH$Atcn>NU z$!D7wl^F8w;@?uaSuH=Ewi#EZD6CnDtSsnfTHA<=F5Z`&``R+zO=qi^%hmhUh(5NC z3v!4nFIjUezw#pSX@s zmHKgF<5Lfos&F|Lx8}To3KDCs=d-=dmBxG5<`$f|#ZQ}#tyPBYYIe#%1&JDmbKBb9 zareIesVyhA2e`Aiixt_r<@XI#khuLlyUkj{-P_pEi4!A_y)!-?uEb_{Ni73oCwquo?LPb`C#sI!Z5$r2u%UlvkqC^!S}7B# zAc1vEzUO!c=k+Tp?(-V&zie20td^CV-x#BLy=uX0A}UB=4VAAPbAnec=L(+>RFJ@W z%va;?dShHF5X7GI+Ky3p&t;;=hsZb&4>kEhIlL9FyT#P5yR=1S&}2SHNUxfNL2NvV&O_&rn5c{nfb9pe4g7 z98LZ=fr>;(E`q6@A{2h*>Lq@b0*+tt%Q4d8Is`$dFQ9@1euMsR0;A9?&}z}5&nTxh zg5KU;5&X1kMNWcjA8# z7=`Nttd}M zv6jMWDLmaI8gK#?ByhIHY8fz~qcUhwXIATvSc$+WoYCPu{ec~oft+}sMIq}`HcA{W10;BNUM(!YR?07KCHjHI<_A^jH0?%#a>#6NT z22P-Y1kUyH9>BZQ!P^IpVgXLwqL9ESJh#zm33_i5nK*$867oD;@cYzJltyN!Zox=k z6rS|R*BZPtiMpIX1qqz@vs&&SJ04~5vT}F!Q;@(YJn7MDS!5^jZ~_%1a5j>!5uQ`n zpnINY;&h3?C_Ed|YFTC{igN-LByhgeYKa-x(V)8fG*7HVU=)7s=RPcU;u9xOK>}xP zc}<)?)u3na`@jzpfl)X+Y_&|Pyw;#MLu{ptD$Y9NH~Rl3P(cD$5%Ah>&($ZMDz4Nu4wlfC3+Xpo%qauM(^2&i}b|RD$ zs33t~1b(-*Dz)dR;I|3ums z^srtADoEg(OTJotdRey0*N=tNt0NH@h3<=dZ^njltp0XCw!372feI41c9gHcwyLaV z;bN?I!EaGWU=*%swOT4%3}Fjb7Gdf0e~<`_Lcc~njv7~;H9ec3jUPTX3Kb-9WiDS& zU9c|8?druAy;LOvqtLI>YRSkQRW|*bgL#LvIf4ojxWd?K8FsuGo4m`yx}`oIgak&R zUnB2jJZZ)P2Uu9`Ij@6IK>}Aw^Ob-%TC+l{Q?XvoD}s^0C|tj7wT$s?$LgwIj9FPC zB?6<+;gR<&=XPM3e4iUts(cDY1qob-&gW#}yRbT!E*LNOrdN=_D0Fz_^{Q%D*1Y*y zV@TiJ3Mxq8dU~s+`HP-x+{o?5uU(ZS0;6!1Ki|XKs5cv0ZKL7*u!2Njl&j=5*{gG3 z*6YPg!^N+af(jD2zk$2A_8q|HjT>SdJJ3=hFbX{(`Ck1M1K8Im?TpBWy%kiDzop(o(*B?L#G@m{&s?b3KF>ghqvc*!&whspWuKo z5fXt>=pxDcqPfD^LGMPvKee$6DoEfyCO+?TdoVk->_c$)w+M;ADBLT>`wWGKuzc3^ z%EYOIBm$$*caraMKQNHF-%yn1e+^YoK?3)D@vmfG2C$_MyDK3j+DZgQq3U$k>4w!Z!?W#LYXf(jD2 zUy<)=NNmTtC?6E(&MX)Sj6$bMt0h;jcI;8JPl{J57K{oKxbMp8^6|FT@~C} zA}|W~c3Lg}hKI7fuIbcE7rq8z8W@G%mR8G<1&!E~&mQWfy}?1KAc1>Sxr<foE#v>z zSpE!!)y8G-N(4rs8z%pP)uR%-KBR=&v za<+y)Ul(3l?eV&SfeI41znZ?p6=%L3{MDMxCQ1ZG;jVA)rMJE~D;40ct{Xo|A}|Ua zH~Hx9ZhrRn__FHuf7TeNAc4Er`L3;k`IuL5fT~>EEfE+cJ9!RH$iWszmsPjEiZ)O| z0{7JOJwF3ISd#|+>ZGH$B?6<+chqVLtC5)%@(55Xe7k3$A`z13K#adTyIQ}rI;_P{ zi9iPdbS$u1?ip^Za791$>NyvNXeuel}VL z7Eue{&LI&Pg+8!W%c`2Mj6xp@t1*Z88N=WX@OMd-yT+H$JZf}VABn&ybg8gf z$}hibOdIE=&b1Yn2#i7pT&tztyR(Mh`K;>chh-TmNTAyWfA5|;V^qy$QFE0Gk_e1K zCtv<*&;7H+3WeqtKC<@4PR#-`F(ei&A1@ zU4{x0=w)KHJo&lN*faKyvS3_uiNGjySm9p*zHTy_%($S;YTHsGFbchx`8VO4%Zwc5 zY|56V?Id4kB+w~_x6T8W8TkS?Dsjs?Nd!irr?k~Ft=alT%6X7$^}Kg|6UMOP~7VjSEo;!RNgPGgOd3mm{lXakYuY%qQ)F_l+JR z5g3K;=2lCkG!u=ry~2XSrVf#O&yhgyC7!E&V+`+4J&#l!5iSuJg^p1CE5P)>jex`6 zQM=p7Xwp$nMRGPv$+qd>kMhO5#?A}|WQ{H>Nc z#!RDTmzhT5?jDlMKN9F>#yud$E;XipS!HB?+*u+p3eP0qy^M%O#^c4?jnt2<3>75M zmCb7DW~?*zPOR!h5V8;n*$P(6!T2r4tjK4^bE7ds z1qpPpvs&`@Jz#tacyAoLR8Jx>3eSk(yG%zOFdoLGVm*7;qDaL2`f6UK;j7Ur2Yn4y9MItp4XQEr!wfX+GC(#xeK0;AC9&}wPD z?6$$W=VYa7`AGyu;VB=y{n+rp_!gg^tvXbQp@IauFLJ-|1|N)NHw&?a)4U`Cqwus6 zzBcOO2V?HZV(fC3>)+ia0;AB|l9$yXSH`yavS*XrBm$%G zWEZRDOObTUwSzx<`|_QE3KHniNn_3|>|)b0?DWTYiNGj4Z-&o!J@;TA;se>^cX0+P zNTAm!-#_S*oAnP4WT79UBm$#wPL{9t>G=oyM-O1`+YZVE%-rIfEdS#EJ)X{}GMe?R z#(tIeiYfkTyH?}aCmWtoR$!4=jCoGnN4{nP2~?246X0k~kvMBhoN$B$Mm==N9rG;n zChfa3`FJ6L3KDpd-R~2?SjkM47!nxuD?E2h;i?<7*naPL8tm#7I%P}{s33u7?O83G zieEgkC32Mdr*k>6^S)(QH=R7Y@x1k;wS8jfjCJ4X(L4<#P(cFEUgs|6`oYLpY@9yDXFZBp~=Ho6CIgMLP5KT=NIbGJnDNa6S6H-QQgN0l4myhE># zU&Q{8+XgM6cXA3*k-(_yMazr2>px`{PlLXcI0#gbsFv-fI2G}vFYR#SwcW5kA1V?U zl`yfqcy10>qLrCQpn}AtD>rTQq{Tmf$0u>~7e?1{iQvR@#cIw%4MW>~)7cXPQ z>A9IAUqsi9iV^2~FS++2+Bd6j%&o(3wW`l)Un&x)Akk)6iI~lgUTej^(jDa2q1dm( z!1K=Vyk+_9GbB(!LO#y=Kax3x9{k@@KGn+@5 zyfMwE&eYtU$&CaFRFF8}9cd#Xqib`MFzjilHA8j^<$Xd$+eph!kw(kym25YI3W_wo zCDM9op2lwi6(l|vEGTljC6sn^&>BwuzZ0#Gl6RRV?JSMhWA5Sp-kf%s5@9PEQ;G;w za4#yoyVmtcuYYge#7=Co?GXej$%#@Q)7pv5HJ%zD)T_z_?^-JEgCuVNTGQ^tT?1&x zm}5^QUuWs^&giv#i}K0;AHkh!d;s z`wW`E>u#QTKaA=>hboiLHd9eSq7V0Hn6vz*Hd**Hgth%(_lscz^JQzpQBgqxeKf3=uXAo2R~rv8p7v-f5f~NUCQj_f zS=*3mNnCn2R^sST<5K!YDk?~zkA~F}oBpy<|Jr8bRf`r9fl*Ya9eYWwma(p)wi_et zwcVj0fj%0%-`?g0clSSOjJQ}w)OH7fQMluj&j{DAz}oQL7XN;(9~*6F-!Z~A){qE{!gUT-3%(iXcO7p7d`sLtZtkhBHG%e2-`so0 z`2Ob)Wx(AK6%{0KNA`ac80AsI-059)7wz<3x*^VRbIz=GtXp111qs|I{@(;f`7|~6 zqF?N{lcy0<>bSA>-~8%X@(@h|(>^+{V zbj24NKJDwO3&x~YQ9%N|2>zSEDBBm)*I-T^>h(?hILSy=tA)C(+$#kYBxr>god|F4 z6Mn!6XHIlUMqm`J*dpTWesWt_l(vhJX>TVrWBKD^<(Wf40=*ize`5MBMrKZQDJK&c zwJdv-@PnB5Ju^>Z>$6hE+aCSYCruYBs2~wzGhJ2cd?iV>Z?BE0{NTBZv|MD|ya8%1;Bh{J9E(IfjQP^MMr*r*n z5_?x%3PuG9>=W@b6wXdEqSp*jYlN&hf&}(zFm0=4=UzgpO%&@T%fl+d8=PTk3t@TiqucC{>z9D&}I2=!?+>-i5J1VHSKo#d$ey9 z-H)!d;tW)fcs6slxaZ>%A~;d!qF+=$e$OYYyC4x5rK~qyb@sW_zM{WJ#Y9oPnr&HY zpn^p0qx(hO-4s5M6U{osMp4~u>$h4WFsgL{(|gBh40-Rof04_`$)69Ww_OZWkic@| zE6@J&63<{H>nITzh4+S^G1BsdLVceWadjle6ZCGQ9d1cGq^G5Nr|{8)5qhwp*k9{V zkU)1i{)Wrtrcxj7POq90fl;_S+iGc?=AlCU#9B++Nc*;tK$krJJ*r|_mHMLVi#3o4 zjKW>!yya{fuTXz?ch>R%CRu9**Id$G+OdvAJoh=IQ4#qZS}jGw zQ>jlkH3+^N-bNxY3iosKm6>;cDm0E-ex;SP(;Ergk`b+t#?GSdI(CgCK{em(Yp>;{%X{I9eS9tv7*$}E>9N*lIXMFP47DgU zn&_3YyYvjApAbG>R!bx&Xf)A<6R048d$YJ(=#<9_jV6w7tie!00{4IMv$p=@Ixz3( z{ARV>*n3B@k0ux@NW2L){o(q?(N1qZ{`j8+Mxj@n7#k?|(F8*UiEG)W2}ijzYiMuy zvJG+nlfWo+n&V|v>bPPbO)ylD*jLc>qx+nV_J&un6Z=Q`N(4rsA03~|=y*^mZH^`w zDoD6@GF|K3{K&N~kDYiqCBH;q6gt>hEhj@aDeL)YV(|E^3>73^Uow5}BHz<|;u$-^ z#~%`bQRsii`&Y#mD>Rx|I3_hi1qr3U>5`YbH~G+~w-bE)ArTmbE_r;7*T+c;jV6|t zd&Rw%lN|Supxs=Ko&i6j%1&%bMqm`};3C5G-r1X`i$bG`M&*wis33tJdwiFWov6wQ zRFJ^l2X|F@TuPzQM0B%-5`j_Jm-uf2qtLt0YI%A4buf)4I^C{spn?SUCHPl;J7G;m zU=+Ij@fE-02M5z=;`z!;QK%px_cEG{HHj6gWCEkG$7$}Pr@2@DpNgIw^f!5ckUz)v z-L1v${vlP#--qU0O~6S-B{@N~Nz8HY9#Hkp=-qrrtJ-nF0xcjQCWQaJxoy(lC4zqb ztIWMHTu@ut3~ z)}@O_h_-=x$p1xPRD&g1DQ(qhFZII9EqJ3mED^+Zl}#{EL4v*hL!>dQH+j&7EqtS7 zDIUZ!Rgwve@~xalq!Bi(8z*iTNvAfQRe}9I?~Z{A64QF*)+$|g_ny|i-Ty;i)WV5g zRBo!z-<>&8q_m3~^Ein0zkAO>1&JzJcCClIyLbA|L`2^GrkqVxmX&dK;cacwZ(-Dq zCs{-qhK+XjM`VAh{45{DMmeWqs338=Pi8G~M{4gSt9x+buECV9_Y`*CD}zK}6y8~$ zt9^#@Pj7{F%$Skkea0Gdw!0UM&{mQXC-q*(R~hyZP&+!x#g=Ymb_8U@hcx%K0(SA5_q5ZT=>#A$|zpC|CW&njKWfvpIWR} zSTpe14>oNY4{0w5=u@vxm`6%q_g9ch`MR^Z?A&v)_{y~@3!G>5wszO}h8 zlICzyvRaOFV&C{{W&hv=DoEfPT(k!2xW@O&P#Q>J6n7xf91cV#5p#T5DI!oof_wdl z+HQIX(RJ*|aYLkm1V(ZHB2DsJQkQsS*hOief&}-=(Ht&EX0GN`_}iZdRFL4VI-0{( z$7;Da?6wm5FhA=xyol7QWoy<`v~Ww)9whG_xQ;Bj9xLG<1(-{Vf(#WTxC4#Gt4W0}YB*Mb~6zTQ7Oka`t_5?MQI~!Hz&PGUJ6n7vJX_(GNWo(}6VV*{- zDlQBaB)Y#IBhqk-Ay2?Vg$k*C(-&a>Jc~Dwz$ornB+@unavUc@W*1SPa2Kd&lBXxSN&c@VVmtSDM2m3+sY;2lJ~fh5r>QNN~R`Ey>4= z>ebB7QM-QeG?2h3?gFMId4*A~X4Ste2vo4<<1YyxFSW?aSmEQv9n3U`w;A_5(;Uud zNSnlmsjoyDs35^T)-;DJn@RkW&L+}80;9O6o0jDEc7>j(Y{vqed2L69`@?At7diYT zGmWz)O6{hyLInwY$5<^1?J}yR77Zw}E4nGe`ijpv{X2OZ(z9X`{P}Zo5DoM-q*(w|te=A7v**4ALzQ{cw z?le{B24ASuIxr*6s{K}wpy!-&_}g(&bvP~}fl++6O_LlK#X1>sTto#4`r9#g##a^Z ztgjAvJK<29^ulHH_m%V6Q_bNiN;yor9~Beos~z(7Tz7t5cByoe6Evp6JJ@O+-E;gc z`r8qOwTJIY`HxzXoFMxDuZi@#jwtzwqPp8Ieuk*Ks3a$dHlL3n*_x=Q)M1AjUl-1* zzsrZuW@`>NRm>q@Gr{kig=9 z)UqaAfoyUA`Rc^`|ZT9876@W67^yWifcONy~9qN z8T&z82ND?dWk>;WZ$@n1V<)md?m*7GT*`|ZYG)R%q5 z#65rIy24KI->3ih9W2JbmoC&uN!Pci9@b@^_HYnCHzfrT?pfAeOOk#IiC#I2haIS%kp9Q@xmxq7TWx&3piV0qX}w@d zBCSt^$Q3F`@KuO9t!OmYCsM8k9X}^>g#<=nshihvY3CMiQC3hB`3g$$yR^0vYraWb z|1?XafeI3Q#iqC>TCr&oL!7)eP#Q>J)TXC-M2Tz~7;7h9mh}@?j?Y^B&s^dT=9zNt ze>~@O!})*GQ=6Qix{&l{m>YNByysA_ux4Nx(shV6^t_!4QTeK3omLOidgC84<@C&z z_i9`2&$jVZ#ahy;VkA&O;%My~nt#o`+J}|)_0$Ua$x<4qATjSyIh}~vcWKS`qzy4f z{_90q|9!>O$%_N^U;VFXd!stpzLYD)-v(#28)?VecFwM(=NWoYE4!y-G6EGO>MZit zcXWu+@^7NkPygm=eBkduWPk8hw1AQ5~v{YELWUHMDv5$I8kNway|-$IAm)|m5CBgNkZZ!e;i>l*z#tz74`IGZ-{6G5>}IR_wxoV>@Kr7P=Xe`F1tt-_KZ77pK|(E3 zO5YcKL90-MT!AT#MTbs^cOnuPh0lsf1bRe@G;)^@(3j_UsRgy^WBYigq<+QYDSt~$ zuwiaZ;>}-E=zIee6(mM4EUDklcw37pNuJA;tDF}fink~d7=_nl5_NLc5CrCGZdQNY zDddyp6f#80p-GgjIgi&AClzyoMBIpydY|+!H2*1%T)hm=A$kC)Adxw1Nj=A#SDIVm zB>Q!onNd^_s2~v=Sz6EfPomZ`HLa|nD<4{OnlI%F35@!<+)t-H4M8&|+ljM_e8v4h z1&QEaCH11Q&icbsj(g5dS4}}+6yF^o(lGZ;P_8z2nlA`ckSLw2gnq8pCoQNhy-SEF zVR-t`bs&LJe5Zw;w9|r!rFWbJfeI39BZ}*J+&^fW+c^Bfk4*RUrZg}olfC@(tbuO& z>y}e(NSnF3HQ7~M2P#OM%UE1*Uj3Wq@?^gKI>J5ouBS9mLBjWyxBmL-53T7%+ABlX z!K3Lv6%{0?-7Wh0m*}aPMA!VjqV8f%!-5JD{9JT> z@gk-@x<#!sdBmUJzQ~_E;!#0@pX08N@4j2Ro~f&ySkp3X01>Dl!Ox@D3$NRtC48sv zIFv?Q2~7~FAknLTuEyXBZLlyG<5pAc0Z-n=9(Y<0ffdar7=BqMQ~k(m(|XzAH_qU5(}rHzKlr zjz~dZl-#Nj;l6o^$Q7o6QChR|`ubH7TJPu71E6}<<@}}qs_m#Cfvu=Xbez5`1%XlY zx8vMrem42~>W1$iC+}2+hN;+RxO=*c?!Rxf=9gv(?N|HV`!I=`KX()*0u>~%FJTe` zx=brU1S&{i4*AY7o~yoPG6gts0+r-M(sfkggiqbQ{tm*SAmNp-q`s!>P%UKmQeL`r zTE#yF{Lr?TLF1vbBBE{VV>r#hE3db+|FX69)W8=XmMe`+wR5*0382j*QKLDv14 zURhLg2-|(Tzk&)9?|$yleq@}jWpMH1Y3yn9BY3&jP}XQ#D~Z6ULIHa;n!n0)j8^ui zTK`q?Su@c1(JWM5Q|qQiBdiRE-$vtz#rxY$t>omRnsdSn-lwiO?u#fP6QKy3KH0d=N>6bekon@HVzK%EfW}(&>>E% z=6g|_Hs%sfV_xS(rDOK2%3s#jDk@0Gz3@}Fekhfi{-OBik_n7@G3%5TTjQE`W?C#y zW8#Bv%JN$6lz&>aP*Fi5bEi{UVAPV*r?j+x-PC@SI?mI$-R-mTDQ>c|s&g|H6(r_=IjPZzCBxwpoVen% zLz%-Ws`qB(RPiao_DX)DkU#|qc_hqtFB^`1+t^0o7#CYR?oJ5=DoEhS`}cllmG*7p ztO1U3ajr%tP(cF6yL@~;Ba0E)cQ8xHJKVtM4xeBg70U!FNF*OSi=Fp0x5PSr_7ZbQ zxiWpQ{Y>x@^H)vtez3K&troLUF*#^$t$nY)AW%VqdrpWMuNuczCL>Tmf_q$uxij*( z5VIeKeZGXv?aF83`40QiYoFRu4ahC#NESQuy)lU!#N5escN*jH`I6rRDoB+15Gm$M znysN7>wIRyu+L1edBeQK{La|AZ)`C|){5B(za2|#dkWaHOj$TfoP+#j=zds&Ei5puam{FO)ieW&0{8~3shGc2+Fs9!RvdmBM?{%CAN z1&M;^)(hf@=K@aDZho4tu!vx{UY0723KG?SnCW?Bc*%(#jR%O4*ND994C(s^|4QPR z3oUxlOZ2bGwYY01()Qe6ln7Lab`UMXy+r@2gnrvjWEkn3g21Sr?s-JNbYI`ccH-EY z!Nn*IylZ&>Qxf~rWEW|mf<($I-#0zOCQ1VdjKXr`o@czQLS7~K59b6b^f&cO=?>yA znZ%M|EByaTmIe}7hP-F#&Alf1OlnA@u*&#McumSF=0ZzOdv8n3mqpB^-dUY4MkY`} z;?2`+Vzzejn76!iF%2XzYI@s@Vn(=RaOz}fpn}Aga%QfIex`bbX&@m*#ibXs(M4DN zN=D$7pD~POA1UQBJ1QtgZhbYTC`?ZK3LN4v#t_-Q8g8n z)udhD4C;%H-`;|uf&|tD(cd-f{auN`sPLFoq8}W5fxZuJ8Tyq7C5tswx(>{Hugh7) z+-vZK=bYG^>#IS%rQ*MuGgOf1Tw$i@Ejb0!mGj<`@!9MxNd!jq=s#2RmVz%(t9Fym z8r0e`HsC$Y6u}z@-$dShZ1u#4yUOwR;T{c z6a+?b&nhufbZ*c`dm5)yH<2q$1EWgi%&yP9K1&O8cjj+6y7K#VkBT%y$AUlwiAycA=?5w<)yih1_b#R3`t7nHP(kAJ!mN6h zGpn?Tt=#N{8f6m%DoDh<_0WT7Z_sXyaJLh?#%~n_DoC&n9(v6&TeaDP)7XhSLly`E z6(p9v%%Z=Uxmz0)lGaWPEjC&Zs37rrRu(;G=Rs}UZ3nTaeRDyef<%VKS#*Z}@9%8i&UuLM5wZQA*f6)|b-bNt5~v^%-71@)k9a^}zWmiV_QFog%eqlqIVwn8ESEuiH+*p6yPXKV`d)nhKmwyOwzY`wpU00n z-n%(Alo0DntkCnqwml_r!7Zam0~I8&wKK2$$EqUU^h671K%tMiunhD(0-W z_)X<$yv{hoc*SS7gLZv2P(cF6hGJ&hu+MBu1V(Wm1u?Tt&I^1`T(=$u&1}1;I?Ycw zN>Y%(l|1|$6Zh^0&1}1FJ1G%TRGmmMvpqAzaGpl#X~BjkpV=N=eXU^y1qobh#GSd_ zD;hMj{m5mdL|~NX)iq*f`$@lUJPrTMxeS`wE*?3~Km`f5Vy&3jK7Oz>C$65(XwV$; z>mox9RFJ?mQ+(EWczT28kTVw=AQ2dqsmD4ohy39OeNp6d$Wit=WCIl>?v!0G=8)f2 zYR?Hi4`6i6JT~C-aK8yukdWtnLXW-!ko6^>%(cdk% z*S964_0b2+Nu@7-v747J5~v_iw9qk)h|M{xQ7th&GsQd{_x{u!uAo0}7S+2PbP6()A0leRFF9K*CFllK^HxvYma1Upn}B0i$!%J!oHp41V1HBaihM~FI8=S zr?^n+e>!aY5no*7YO?DQTeI0Vk;eOkdS)65wH>s&4HYD~&y2VZ)7ubN(4m4vV!q;{ zM92sB_o^_J*28t|T~L(!n~SGx39N{?H=C-Sur=*bNN?`zqW8FdCK-VW67rSX*W8IT zkiaM`H#1j!1)U&Jp^?{}f|>aLFO%hJvbp9iMH)z?Nu5(qyD6RC!9p$Pf8`3J_W0-3 zx7|*q?`igLG6EGOu#YOIfdocLEgaSMDEsqaV2R?rnX&DVcvdzv?9OwA1S&{;e70Xa zAI+W&;6xywRin~<#AnsYzbq}vB>RckVm%y{ZUMd??sGT3hK1krKbP;}1kGO&;mrwD z=&QAZz&iGOZb_w?J0vg)p8}ac1>X|*OEQ5968OgWZ(>3NA90^QUx~C~iO2*hNZ`60 znLq^z`huObddHa8UU{r`QRmN#RxoP!=Tq9nx~cRN&q|B0;YkGV0noQ?mG=O|w{3@l z1bwxpe>cB|r#HW!tDEiL&m9Ct;aVf^MpAT&Qv6zTwOrwM3MxpfE_ht)v*?G`cUv}I zOU}$%t_*jrrKYKxR$8}&QMdw%f2r=jQt8vRhU$_zy|gw83GT|JZ9ec-Gd<|y>u*(_ z3<^>wq|Gi77}el>wC1w(i#9>5nk-c{Sjl>-w>qcwS_R7oOZv}RC$wyBo%P|RD)H;+ zm^P2H{M|tHs5(JG1qrNge5ZHOOiG7SgVn|F!X*NuN{>IL(G!*Fn9kG47P3>Bf4_n{ zs%|dnIxz3JzKr+!{@kT>URz#0+}le<1&QKaY}$_@pS8z6iTsP=_D1`awLARPy0*L$ zfl(j6+B8~o_onnWPSEO|{WsmkjzfG>@i~{<4_dKxa%9KAA2bgKEejHRK9|wkKggoH zja$R7oJJG-#`*bC%Z3C-aUXF#$&;LjfAz+ji9iL3rKwBl8}Db;FYeiDPs6KE*AxUs z&Hq+H|C+;7f2bX>6M^^o3j&qIyWV_VmnZ+i5M#r%&FdKawQq_vkm&SJ5s^losI&Go z%3kT1g21S@hYE`8C|T#4otP9mIt77I+z(xpmFcZcWtF(2p&(E}qWZ~v;(mM|=9tUK zThmwE4cP zF9>D$6yf_^exi^-1qr#I7;L&`8{9Qp>XBhz3)_<4uGxY>1qtkziZ_EnZ-zzh!j0z> z_lq~cyGnzk-pi_(J>u;%?%xsoo;!{4GU)AdWXA*p73?`leF@9kbc#W5IRAocB?6O);p&Igs_8L}1jF_iIFpQ>qq?gnNcBH>kx~S0^n)1qqxf zB(KqKF!G+qeO|dP?AB<<*yfyN_?6p7kb*!33EX4$yH8Do@npez@mtNFw6)>BEAEsp z(k5?xnLq^z+`+|H&@C^fe5q5N)$Ejq;aWai2}r$~v->C5`#v#1pn^oBQt|^F z?q=kLc*55PbLiP7)Z zYRikw)27z5uWwfmowNi}8c1LiuJ91+!qqtuCeqjm58o=Oac`o0^6<7u7#}D_I|SWWYomqd^CqFl@-q1 zU@4eo)oScVT2F1Dg0qo$P2Ay+=jvI)ncZJGfeI2QwTJQbYfS>m z9SMGpjb@&xW3HwOQ~d@nE-Faix-9cL!pn@|d%bwB;L0%yugR>tCEKmr^_{1I zQJ4nKotebwULoR%LIsK2KNe~0@*dVQ*P{L|?}e*zxeKHqFbdZPm_*l$$NcGuLInw2 zgJ52{)1Fy@ybmX>PC(+Mwn&?_G)8MZWt+Vwx?Su|`=cZRqi~IenZ~e?-1G&DVOe1* z;QXaYM9%0S2vm@eXCUeMxZc+-1%XlWyeJWmUy0qzjGuhM_aV>J=;RYzNzDDXHQG~e zo=QT*fm9dChlNfh`K?HV^llfq8d^w{E)w{jFFE#z_CEU zM2_COw4QDM)LuR-Yrh{O=eH1PAc0X4fm``Xr=k4+ns(xqb7%?zqY~O~5xLqJU)xTM zu&03v63-Jhh+JjM-ii}^L@%y`d)638V3hxj)uPCkiaPJA}Q*$8}}zS z)0o8X2jA^vpn^o)qh+G*?slUw4&|zGmT`fUDRkvo+dB`z!k#8$ZKMqaCt-J7(E%#f40u zf`q&-!@ja#<=zQw&*u3W?S#JWKE+me)k2N-I5%4}(T1yqe(wbk1S&|#Yl>{<2vVgH zB(7Y=b+6p_p0{Xcw1AqC{M}BYR9>$HfeI4X1N=Qo6$C0sr0geJ4$h#pIEhF$xJGpR!x?0Ac3n*O=8gd&Z2*X1V-VCR+H#5vYvRi z<6Xo1kGVC8Z@s+)feI3sL-XCR|=aw2zpYB{%t%-cn+Gb%{n zoi&O1iE$|ijN)$mT9V5@rE%!NQbC}C1V1A{OFDaih*Zyl1c3??{Nw^H=?n!Tnil&m z+H+Kp!1`uhM}csk)8hJpR8$>IYiFnYWH+^F8`&(g>> z;=41=0T5x#8za&{1qtqZuRU(>q?>;FL{z>wM5KWPMvd<@TO-$q*nZ@sLqvVsRFMWM zNO0eKEjM%4O+S5l1{2(taQAQi48m_=l;`kyB8?-S)9h&kckaAf^!h+Sg8SZU1z)=A zrk_6L%2Kwi$QAw;M#=M{MAQoIA_!EFz!_Ha%InViOXLa_B)C()R^_3aZaVN&8maOQ z5@{fTQ8-g7I!@nMq>c7vHRp_*4T+xV67p@AG2~?24r+{*mv{MfWjKbBeGJy&b_!Rs; z>sO`InvuXLTyrZEs33t)!S7sA&r)jD@XB$;rc9uM1eUth;@3Tp&&NcleF{$6f~n(O z!`$*y#-if+%wU9i-MgAEDoEfeX3ABVI{Jf`m|?-yR5-tn-w;gVL&z7NGbe@$67p97 zN+W;HTcRc+fl>0e2_g;*3lM2wu8^j_D4iK*e!(KbX;~TbIzT}JztEZ2v2o-%kt_Ty zjKc42Cb8zwhRt;4s1WV=VurtD5)}`6?xp7(6@CtuMrW$g`C52==DXxvQ1Oj4%ZW;I zBIyJ$y7K6bV%0rEg8p_y@sqSQ`bNJozAb-K6Va%0UNJ9<3KFy;E$Os0BL1#_+lQ`P zCX%A~iQpoQ7I|9Q)3|VIuITllg1?Ka-po1u2sUQ}eFZ=T34UgrC^z#gI(kz-I^emN z`V2^56n!yCI$4g0%{vS1rP9R`*A`LpL53k9*@_9U0jw^SRI}&(J<{jksg8t+6DrxNyu9=aqobKS) z3j4+VKm`e0KVzoRYf@?Z6O~ko7=_>NP2x$XrecN#zux0l_LRg?*RSF^M+FJ|UT>x` zexv!mLIR_(E||p0PW~y$YV|4g#?X@6K zK>}-!N$m9=wvpbuNMID!9Lbjs!;WlcL4A);x8Zh+MnAh%`_^Vz}!GG3I<5bj6;=*eV%B z8c1LiKPg&_Yt2)q>7KuzoqG@6K~#`<@iJDVktvcqgeX^;%03ojH6$>KpA;>|wdNVp zM0otXBT5$)B;>W6M9j(XBn5#{xDwQ?i4{sd6EzWk7vHEzn}lnaA!<7+NZ{MmB(@Dr z5Lb@ZfjQylSBtT;d9pQKho10JTn8#h;2K!7baQ9>l7he}`f8c9s+iJv(EA@zOHiS| zsdc8c$Cx&6KUOzWBL-*Ly(L2i6^Z04mmRc8RQqf1E(hUIkdXTflm>Ojh(IMdLG8J@ z%A0E9>Jod3QB5Sm;f97TV7Ly*ydOUwrV|7zNT4H_)pD7q@i$+Q;>y!N0;3jrT@&kl zo_2EKZ7r3qA7AxCPc14)pns7`j4v0Tg1{(rATqDx`g!54&QL)@_9>#Wn$$Q!v<9dk zA-g9Lk=4F#3DZEoU%Wo^I{x9`wv%5u5`Sg8Db_8uE0$(l3KHlFZkBF+ zzIumB7YU4tx^Z8u{YeuP!_%OeIFzr&p_UUBB+wDcB);|x5j7DBj0(;iFV^Bj-#lbb zqja7EfOhl-YTns<-T*lr2mZ8-$=wu@I zn21n!UTj?k6(q3Set#2IkMi$?I3B?94tfR31S&{ijY%Tt+cpvyg*BAdE7N_CMyV?H zWU%*wyKZCx6(r;lq}_c_-FVoYVfkQ5W6#NIK>`&daK!q%UWwHScyDoJfptM9P(cDc zo_~Ak(Hww*;~Mlf#LWSL< zguV7O7POlya)k>1Zse#yeM+-wT0)rJ7e_tVajeJ{5*RgRbR+#?@9A3B5XYL-_eGzH zT%m%*iBbOg2(A%Cpqvh}9NF%P&cu~5j zAd&EopWfy86|J_jBaNM>&F2FNj4FOVRBzDsns&1Uoq12!@jcDo>**Op1qr{C`Cn1l{GP(dO=@z-mNanj?eX0WHxKVgI@Dtl{Gdm7u9KM=J96(roIRn|W}&7ix~|86Jp{LCb-0||_3 zb)uGj*e|2*o$92W_}j;9IZ;6(eRDOQ?Q>mxRkk5|~KcC1!( zc_q9169avtyx(Nehs}Ser3>P92|TN;mg#-kv0}G<6xVbw3{;Rv)ijIVrOP$#pPJ+k zS9E;`_Oi~Q;P$m%Nd!irQz`cY$kLuwEZI4D;g?SaDoCJjoYivRPHX0KV~Vo#>TLsa zg4cBRHFsre^;WAgn%wu?f3#*34$V;f9^NoeK>|I!td^y-nzJl*k19Y1)!|A%TJfI*nN^O|CUyvva>yLfm&r1V*7_8uv*Z+<>i{ z`c6rEZ>L0HRF^c_^~ukj_1y)@adGR@8f-+j4C;p#^9)pwK&LsYrStmgtZd&*>daxV85E}|^b4LeDxco7Z+ETNuz2p5f#+I&oS%zLJW-dmA5uFA6(kzwFQ{j{+(En6 z{szB}WBqclsk!R2d8_?{k-#WCz0zuVRyhOfleZ~5oH-^K6(n4)7uGu-ZKHMlKz=fF zN2gT`8H4*riak?&38o_6?3nY35dcIE3K9sOaC?K zI)eJ{P*B0&{ruQhl+{8DjSZ9!u?Ce@S?_Zafl+wIB|jG+e7`~W<9P5b1r;PVttch# zd9f9nc^VqCKCNa{6(#tGmiQt z(fQ{ZRCjYU%q$TYC7+d2^VT?{X=j}+8Ja^n!37C&+i;u(W3?>mRG1MlaP2JV`JkuB z;mnOS-)gy2p+FS126duFs(4}{ov1`@2A#EpXS-P~Ei>keq88_AmEqDkib&AuO4MFm z6Xz~1+#ei8t=jgP!z2Qu@T4~G1f3WbMeWD>yMv`u-;kj5n5g|QPipdSH86_Wk77YG zfl+w28=omUwKR&_k8uNfsi+`fp5TT*U zdy7Hs$NapFq_fYEpc9;^{V>muYIHilp!VaAs!0S!q2rO&vSN3DLG8!7NL57z2|DwM z+7I(IsIpT+4QfAL-1L_Sj6xqKt0mu+P=nf!qw7nks33uEG*-*lYdelq;XRpjlSfIl z9Zwv?dTg~++YlO5``IY=!*(cW+5|;+a%!bHIo+~N`$x6E2d{D;dVD8lYFs~f{;rYi z?ABeus31`+qo$Y0(?+{=>^!emMcu=KSvO8}I4%(wh3_t_rP8$1!6)5@u^?YR1r;RP z)~l-T+|y30m`JU`?I!WTAM5_fT(Z}Z2#msa7w-X7%&yFL8_YVeQ3@(Z92ikuua>H_ zmZ#fMo`zqA63Y6hek^$90*SyVe0TA8$*8Kzwdh_fORabG;KQ)8)8CV3hn$^r+TP`CPptYgp*Kf(jC)SJ&1TO&O$3&A5xF5fDCAS+lJf z(}#bQ2#h*BrLI1D%}8y3A8J2-`b|^P-)zeAz5gr`7=vS|Bentc%DyVf!aTS#!#p{U2Xv0aeBJws9;JJr;<8EeMEWAm)YHGGlju-N)|k?(Xiu zzz%Gg5$wKpCw2$6==bhDbM)Epzu#T!uEp|se`oieo$p?s%SR&63SXR~Gs2-ql;5Mh zj7H}QFjSBj+@p?`YVs^K!(hJ7(DBJdCD+*u#=F>(5`k9w!MfVs9E()fpZwlLn?;wD zU1ic4Id_(p2(&W$N5?CCWnbS{3XlDwx4II-P(fnK^?F(juT|=MFMg+C_32opNbiUG z*d~=E00&ryhJbPR*#{AM9qF-+UQ0*)ism37upR)zAL*%G}8C4YA6wC)$(JQ zma)M>wav{*MC{r0O&M{tl791gBZ)vO-Y5QjdGWIMZzWxyw^0pWHeskB;a|GGwsOEx zHNz|Jfo9d(c;(dlu~Db>W)gu`*azEfhgvzY+hx}t+|ikCZ^4VnNaUlnrEQzfsjFjn z7*+rIrM!PXHn{X0nLsPKZ=aI%w~{5OkmzoV6ui}xEz1X|&!hEl%hi?S@oL?w4< zm?#g&I0}g+u3_4)8_(6GkNI60*H!P7mb1f^+r8>a1X}SL`ggSSVa{9SRrS3}(-k#E zO>`(od=3uP>h=4et~ok`!k9JskutRRL*;~bC5b>Q9HY=XKVgrQe77Gfy~|XzT9i4C zLgGj9dfNR1-__(T_}!(ucdjd0=cHl{I{hONXm#OR9WAYzM611=uZudVmz1St(y@VO zN=pP<;i$%Ln_m62((!5zw(@;p(U&;JQApgKRa+}^Ke={j5MQ%=wO?1Jo$_U)i}*+c zTH&aM%KZ6$CF86jEUZvo(Th69QAn&yQcK%eJe9WO8J`>Ef4NQ>G@u+S-OW`Z&W|g!7nO(Imt)eK5 zQ(e53!!LWX+qGv(1X|?|te|z6>!v-pbA*V61M(@?I`(2K^G%lsv_g-9c3b+CAA{4~ z9>50r)KXAEV%#@XJFq^JHe??6rmI983ND*zFw2v^h(w?j`XjX4embe9iJn%NqyNuN}_ZKa`F_0UpR| zT~iNB1X{_fYWBV(_3Jg7vb(Jwiub%6ewLBA+NYS-AR?{yaaD2(<9pH>`XTBWve)_~ z5ojf?0opp2pQnG@U6cLzmDE55iLr}|Xs17=)?%*m72%(cBJ?iQCwkt_C=qCdYnilK zyJmx4yDVdy%DWk;ATi{Lzjp0qN^SpXz82oJ+X3DAhd-+ql}{qjYG=bjn#auK+O8pd zuPAwz7=0mq4Jw!NmI$=MRa`oYv*VOrFiUp!ctcSG6(ov$BA?4IowUJ^_@3~$Ial@B z^i?}_wUk7l6|P^~ZIv_M&;xX5mVR@PfeI2U4i(U@+PvZTXG&>f8N%Pk7VaH+nyxUCQ=xwIl+q9=`F> z;;KDY7w6?Wqp|to^tLpTNz$W^M4%P!C)jN@T|es;ON>*({;6-Ef`nU`kM^e5eKlF3 z%oIl5$e(&A8j)T8+E60U3U@Z_wi7>p=wsd&RGvgMHc&xg#RqTgcc*LW_^UjO&@;dF z>?!92pZwWWBG9V-32$vj^>cKaBnuIb$~zg~X!LsFxlEvyyvOpY=x_Z>`XdLg-5|fO z@Dd&p4X%1?ZGDfbV^i`l?DOfS;h51;n~kOtfmZlp5P8`QjMq0l{1kPgc~b)wBu?b; z(I%xksE(e>Q@$WiygoCHm%e{@V~Id3e8-5^+m&y6+OVd2oBa(9RFH^U(#Ql_;p_>OzajPLZ229f%NCbc92t?*qbyRF0L=lapxr}dZ1su`#tQL%IZ zEw%G1HS<%xMJTyNuXSzi`YMIz7&U%R3g@|r);jfEfdOd*gMl!kA83S8~U@4DUGiC{*ef@!gs!?o&UU`m&=gRc=5iZ zfeI3jRut6c^qHl)9Y{xER9<~vU*eqE$UC>VM4%PES4Qh4k%#rjZC*zC3k3{RkO<4> zuPyI0SxsMrpZ8faBwBACTEIwiJ- z;O77~ADOG~d`8KzP&^16iKcbI%B98A`_82!_ z-_@Xzk=y%)M4%P!I@oRZCw9~ITCI&&L(YpagJVAeiBY`*wV839)d|mk(b#9z>Gpa6 zt@WKsdrTtGO5V*F@VT6xuomDeM5RFKFJU0O@AqlH@N*INoB(&J4OUwd8rt&&8b74GBFKIe<0 zQ3qTG8`CBf)=@#Cj?+KdM31Iwj}p%)j2jO+Md`GL{HfL9C?wDd_wgulz&7YvJ!>_8&w7t#FOpZu9Hse&E>e;YP=~o61TAtfb=_ zx!pEn_(!Guidx3B4qFs{yT;)I7w?<&Om|c*dVI2a?$8T5%elQ^3RWx?GkR{hte}E~ zd^e*)nG|f?48`bC`l>{rRpa?5)Wh3ms-3lq6h^?(^h}#o(3pSftAYv=cmspxAK~d) zz&AhRX!Gw9fmWpspH_ReT%fM2dXU0saxW|UaNXT-Zf9etAR*u8czZi5yOP7*Xi+q+ zM4(mq@#oZ0#&R`(-UtfgDhps|l4()L8kC~DOMh>f=AxUW&OS%f{hbCx<4I||ZEV4! ztoWCVdbQjo87fHdyQE!P9aJX=j-W7p{Vd8>_VCf~6bO_Ew31KuXCLOzYM<+=x2zbz zP(k7W-Aip36RF<2IF7<_`|Z!RmYA#;ZeLg;&E;IPsgTZb|{R?!h7bVRmncKr6g^K>Ijbz1gDZvwGgkUJMl^%BQ@bPUx^+&3b-5 zg;BfKWc?VGN2%2P=+^IFUs~u3>k;`v;$;)@fv@7UtvFfdiK0Bp+W0!$hX$RZc<)m-wL6u^UoVt18qXCy&e>X1>DDd!TrQpQOcd{Z z9uD_4P(cE{H`{G7MJ_~*rQXNujjw?U66-F^6upmk_t_My_uJCzy!TONm6ZszLSNN( zTdo$aI`4g|$CouwLE_Y!X`=UO@S3lrW?d4f^WLZI{HhXxR}t4?p8RvX=>C8dL(l#N|do4;ux5oon*%1X6$HJeuEMR$w18<%_y zCwQ!s)h|>YtNL5aThXM30vz#O>v`8Tg%Q9;+3VY>5c8 znmEY3i){aX&k?Izu7W_N%w6-|?!pWo{zt6NEb>Sc1`=O9%)7R=7bUgI!!?z6A_A@4 z;t#4GyHjgf?_{uu`u~&`1S(}B_KI>ocQbbk=7^c{%CpT{f(jBj3rC82HKt_|D~xPu z&6ii@i-+mUq{&?VZU(PcRkoOA zjs#lmYG8UvYu23al5wK#GqYZy()NJq;q7JY^#2j7_|azF#W0WPs9jET|s%9HRrRI+*!k0nXxr_fttnTEE7wsIwK%%Xe=|yn+5r?n) z;BjVOf&^ME7;kzl+&bNH|HCYDCj%8u^S+DsocK#-?)s-cCQhsKn#jMYyocj|53N)} z{QY>x1$Eo@jcQJ1metPd%zL+q*DF+z*irX_dO!6Bb^i4^7E!v$rp=r{1&RHy{}tcb zTyyx?hnM-muT#W%YE&emt2uft_+-6B+>Fa9+6NN*!D>Lkv+DBVE7j}E9KERD{W&65 zJWqIPdH#H%ms`0v?$Hk`iJrlc3udgAxQ-G8#srC}^-S;IJh7iAp@|5zs?gW; zPd+Dwmqnaue^uO2;IZQ6!DHnzdV(l}pLNX=bi~$-)$*xkd0;w_2#TE`%A?sPK9b?N z>r}xk4xxgDX_KEFJsU+SqMN6!vg9Fv&ne9}pj z2PzVA!W;p7TkjYFeBIewlsOWCV1=9I{%MY?R{g`M9eEBT0S&=#Lxfg@T->J$E@v0 zpcU49GgfQbo3X-L9p2G=q2S+JuK%M&wP<9f99t9;{M*j+>hB1k#8ESMkwB|waYw|6 z{L)yy2Eg;G^$D{CQTcbMIg9U^>Y~)!nZ!d6vzA~ONNkBP=jWT!^Z7XsV@W@=CL)1W z*b|w=u2-c+i^6_sz{c~Ub)BxhA{zTKI>XX#xbXY2pvaV{@btIDNZ7UefrxS78b+c>0>cZ;9rbR_uS4yzdtE~`5N&Zy5=9}1&XWCxaG@VR}p+g;I7 zLBi$0W%cZcBWmD9>FQzqfT%koXXES#7@Kpt|JMFd`02Y0LbppWc6e z?L!?EB(T2OZSRJ)VY{2o+TZ)$6Nx~p*ru1%Vs9hW@Y;O6#3f}*wsVECcKp#VIx0x; zza1k3yRDahC+75Sa@lh?&Pge+zUQ*4+`gnbW$GWzW60}*nY$TZc4TdCkI6qH#lKSS zCM5Xm?r#ZFndj$aj=#nKc35FuFv~pU3YSE+Bq70RtcmGF-NoPHe><$OPMdX?kFOHe z_Jjnd&GxbRemi#c=g6{IirvvMwP{LSRu|2@rS7jWT;!D_P2}N^%KW4E`Lg$@%rWjr z@bchuF`Hc73X&f<)?um(+mbv1+$Iyv*MuZON`rsJf=<gN_sc%UHTJ=17Ms3hOg9zIsvQex~EW5QIm5$d=sOxq(|BqP3QLH#| zy~k}S3?x=nII6BV{3FDS)kh*ch;W&BM5|N$4*q;266b2Hk^B(n0*d8m4*F@wb;$pQI5`k7t%Is8|EnQ_14~fW2 zM32Eq4`AqMr8})tw+yS!2|rq=j?j0!ZL=tVJb4(XAn|b4R(0Uq8CDpNOHM1kwq85( zEF`C*}Sa32sAdvrNg#W>b3iM z1%c_Ka{xqSCW1YkBBdOOsKQHx*9aaLIw`xhB8z^U$H?l|I0}CYt?+wgx8=xFk(JDl z*XXl*w)hJ3ZwBuHkZ`k^VU+$_#<1Ill+jq~`*{p^kDTJm=^)UG_bdG0&Hn0B9An*l z@)|iO))wDEhk``G4W{>t#o6~z7zYOju?iGM_t7IH0i^w9Um26 zj+X)Eo86Ws&W~N4tQgTf9vlezHBlXUF&V!B?DXj8M72z|dnz zlJ9;d?L{iGDZ-ol6u~~u*_daE#kU*)9m=6(k$zh(%$ET#xde=OOK&#`eXN$aAdg>h!gPM7=Bwi(r zVyouqs35W8#8~xRwp6|=1rdMSiyf&_%=po5t&R#3PMjf9o~kj=-4s^~ur&?- zF`DnsEfHu{cfmw;k(0A;$7z`;jB^hQuv}XeqrkPPQK%r1?b}3kNweg>YiIIW;uhh@ zqJn6J-sl>I3KEx&nz5?0k`o7B_^|_H6{GR_We1T!E3B0=feI2>$K*1{5}fvCt|;f$ z!xkLE+GDqsr+PJ#>eaqjxn3cGHB?SH#spI?#|qmADo9{Gw%h7c`?#Dp$hh^+Q%5T- zbD8KgG&g&Fp_pM5*elhIT_B?MmunGql3lRBcNU}hOxrg zHik#1MTj^}gfkJSAc6bpcH6}R`;;a{iW)Dj<&pN!k-+r;yX}%iJWoKN6`l{EoA<%t zN~4zLjRs}YF;tMi6$?6HY7zAl5NL(x9OxwZj44Wi&tXQ6-XEp&3`pQv3i2~C-6R$f zfeI41+Cn#x`m|R1ENpAc&2dB`&3g2*~8zZy5lw_TH8D8V3 zDX1WUYff~A#Uk=0AkYfmZM55Bjvfy-G7UB|B=uKNK?2`xr2A18;YI{1NZ?v8oznY| zB6w@>;YN0+4#7yE6~5bOx4n385(*KhAR({A5fR11U``#PkU%SZ>5}AJsnyQ*(s`Ntj-MtGXoYV>+HDss zB0CYNAc5LII{%*EtkuZCJr;aV8tDWC?8G1;hhcrs4gNjHJr+Fa zf?@ypI`grQf(jCNX6C;Mw89wD_-c>`bGi{|jBgpG;~7CbJBXeb$*W^CH&(iHpi!&l zWgQhH@T{WUHs5e%uJOf;xZigq01sEw$yw_1d0?%C1`S#P!Y-J&T zqw>XM1`=q6-WTn*Lb;Q&np^#i>L*hgs33u7M`^Yl`bnwmfeI3M!kAu=OmbS8w8Lf$WI+;vR_L#hUVe{1p#=7_8PmQ68K@wE zC#CJSm0kBKo~x1=qw80e2(-fU+jiTrnfsOM?2BIGS~ZD4EA;SaxAh&oNl9Pmg`Qzs zT>}*)@EkhLo!e|ss$IUI+d4Or2(&^Ek2D|ryjrPW|Ezv!cM}5@B=9^vtxUKrR>ln5 zs#jQRmk6}NQ~q|_P<5#?pu&3n!KyYAfmY}fl5SMJnxS-gIYV!{pp$_L5_rFX{GR*H zQRa^6r|&P=RU*&|eL~XRr6rS$7I`?{Pw9;nYmZ;#4{R~u)z~3E|E}Wy$X5Q764+0Lcfz#9(g7xDQ_x@ zf8JgODoEg+FIo{kJWE;fsG|~Aq?<&b75bf|aY?r6%D-(!C>6?gHBdnU@2-&-k0p1O8gK*$)2pGfeI3M&yQ|x z6^&4yemt%8Ow&Xn&(@;es z&fxxS(NSh6($jAg&qrNyi57yRmPuX@~tKjXodb$ z>7H;z8CE=LA@=%QHXRj-ko+@D$sNKP1?FeX3Vn`3Lh{cb`IJ3bL}i{Iz1X;P$D`0g zEE4Dsg8Y{>uFAeN$ibethDZcjp_d98`<$%CrjGGoCyJJm2(&^ExK!Jd)?xnV-I%`f z;XkM#f!;RAduy@!tb8^b3*Nms2nn=8Prl@j>}>;<{%R^VE_0(`RFFU)Aa+~$#pZ0u zKVOxs1Gh>9TA@c`n%zBb&NfW`qTJfOCm0nZ(3c6_=X7qvB1hj zXodc_>Gtx1-mLgLRVi1wzk&)9=pBe|vfk{=s*cL4>jD0f&_XwvfHLs@6TpDZ4tbniAfAY!n-sJZnt=(f(jDodx`G8ChgCB zKXpF%vC24!Kr8eJMPCEGKP!H~H|lKvVG@B>=+~Y6msIV?#+BU=wKUmS1r;RFvz6V} z&b1$#G(L@poxn0TwtK_t)${T$MWY+wy$bj)fT^Kg$s0&12jT8Wn{K z66k%A=I5JKw)B>#(P8y{i9joS--PaMzf{0Y=kRU365CKp&lU+qSTR>|$s! zBYN>fi9joSZ-$6}^0SYzfyUBdb97XYKwqOYkBZ67dIkp?UZXcl1X|&mEUmEQ$jy#v z#f|wFcFF|Y+r~B7KQB({{Eey~<4b9NueYlE%Fc+vH_C2!m(utr&r^%Hw!{lZNT5~bgC(`+88)cS@!YE;5~v`7FWLQh0nFfUh9QAgKPr{f zyvnargQoL)MaH$O{FO05pn?RxwMT0*eisjJiWtrYCM_v$-ZyIRtZ|>+{6%p7w{t|B zn6iC+HU7r><(}Lh84{=1dMt@~lq|a_^Y2NyS1u}#KLjdBG(NXVym$CLsd)$Mj!p;7c)c=^K&!k9Jw)9- z*C&p`;JZo=0u>~#d|fGCMLgGsdsd~|u3NR;Kmx6Pm+}zprqWUFPag?XkeF3+rOI1c zrK2l|pf|&m&mF58^vW2Ej$fySc|@o;QqGOZcW$l7tApgl?DdThO1k9rV@5TN5MivV9TwwSESCr)Wzs_l2vm^xQZ<)|ZRmRbf(zAjaVv$N z5|VG3{=Hc`BGSCWoic*oGUY_?qKAuc0u{WA%KNT0ozjTj&Lonaj1&Ya35gon~@aREB+dw+^o>hKm`fzg<ppxJQSvv%cy5n`KtMG-zs|fw;$KLX6}}N|2KhFZ-dR7-rXvUlZv z4aUb!(34bY$d;CTt)qejKOx3ngg5UA`w@|phzkh_wBjeWIN=e_y)F2rYNw~))rJi$ zbzGc0b0|oluLjx$NYPHuK!itWnLw*HbxnT|T~Z9EFpkF-(cg9I!4}tFpre9>vexvf zvO6U|PwgNU5rGO4=zqd)bH4s2D*3wsEX%!`5`k7t@|wP0oJMh9FR6e)1qt-~Lc3%G z`$jeL8OBhoJ7&`PfDbb?M%n+)Km;-heE$bF(qi;#6wDl0jNc$Y z1qnW{<>Os53_g3TUMTF93{NC%>AB$((x!JTH)p4{mx%O1&LxqPKq-BmZ~HXb-ElM&H0 zKreZ;>OA|7!pEY|J2jUGw8C5Fbmxa&pyA`)Vsq;;RFFV#e{_aE^0C6l)D3U6k_fcI zyYF^e-T^L*(YQT!eglRI66lYRPKC{kQ~2B<`NEbGfmV1UpWe;roPyPJs}k&*yD>ur ziJau;kk5+tlAl9b5pMfadA6Zea8%>w5`k8DpPOC=TJ%HV^Qh)0nlMz5z#G{#_UY!t z2GCs19^9Crf&|ZLJ|CPU{3kYP{!!uc!P-L`N(5Tr9d6o#ocmMZbIwU;!x$<^+$aBu zd|rE3_)kHH+`&C8Mly#e`}Uzd^U06ldIGQ(Vq~uE?RFVg3l(N6M+g6csI*#t1;z?!ebwAc6P4{yQBQcl7)w<_3y2n_#FQ;abr24_AH%_YZe-*WLd~pcVRxv)h)gyQEmN z35E(11wu`aau<7WUr03n_@4w?p{F@IdtT(YV$CKPDo9kVX!@h`*^}&!LVt90 z{;KsprKmZZV5lJRJ^4xDweEQ}?zPV2KM1r!4|aCjnFbq_wdQPsp@M|l9MjKTvl90y zjFT48cdVO4pcVSRqjP~g3*=-L}nM8Ra zP(cDmALRYwNfCw5CZfX@NCaBpSmM74v_ju~Q5Y4cM|VjOX&G{rd5w9msOhILM4;8_z*I5(Ho4nx9>%G8GxGB=#*c|# zg9;LNrY4EuH`Wh-?nT7Tly|*2aUmvX4Jt^4sF`CrrA+CYrfpjy-uBq;$%&uWL&K3k ztEru`$CSO2(sydh7DOybx^^`u7B*;}4;8v09m9P9(4A@(e~ISKCFY7)6+UxW3ImB0 z89qhx+x;Ul^`$V9bafU)`+{}DQ9)ug()Do8wP3I&a{#a%0el6s07%S7)5MlJ6 zv^Ntf_`B#K!EP%sGt+z?t4Sqq%|imMmRQ~va-M!yj=aNcoFd92KDxjnRFFWQ3e>lk z85qvPXjG%bDkRVfeJaq~XX$Iq+%e;zdt3ADm|6J;(^PG#9YE&y63ACy>XT0bcaf}&^K7MzRr{m`kt@mVbJ&73l${j z+pbz803vA2AYw&hhWw}?LE{J28p#kruf@M@1q6QwhH~G?F2L#y)E~L1Q0p zBxqFg$BIT#!dnxKuZU>&s+D6zwg$$AygVGE%ov%V5t#^sMr7WoAc0|vUR0!mdePPR zy+dC=)S5+knBQ9JMFl~a(Tke7 zo2NlI5@i<`wmDBHySNRjIFZj2TqX zi;CJ#z36IGkf5F~#Og(fpk7oE)Qhf01qtfygl{V9MTuzTl2+vIoHo6~kw7cz?SyYC z>Jy2mbz-(4uDYgPg9`OmqMcJesNyeCJO7$S5E<1{QW!{3Um0Tc)D#BwqJp4abPXy< zQ12tkoO)3rs23G=mwM4Ps31Z8qo}*oixNS-s3>#lMb{#MR@C!}R!hAo5!8!%@pjI? zoX~1jXoIRuP%kRNpk8z>Do9}1^tS9VKUSoXV${hI9Nhe`SGR)WCYXM;QrCG)b5X2` zhqn5$VjUIZSh0{`RFFt^Wg7Wab1iqH`dexr|3#qH$h=eeyq2x6%!$35eA%OdK}M2e zo(d{R6zV!vgyB1A0kzu4TYOnYzaS(0uuP!U&V7?b7^MetBBD=z7CN)E(ec}G1r;Q) z$M|mot;#x2qA)a8X#fvn)R%nh@RJ~;&+N7eDo9`-Lo@nUMcCOSC5&6g{3HUces$&P z(Aa?TJg+($1=z1rLB_i;=@nFvz&^%qi_hoB+TT}<(501vkw7afS$c_Pryo1kRWZul zEFFyHj5TJm<$bS9BOWUxP(fmb<>BvaD^BF4G7q9Mzi`V_K?1ENSw7Zo4dc1nh01(# zfgmHx6`4S*1(t`u??X6ou$Cw5?_bhz-ZoD`1&J8TUtbn6AN;<6yzf;nVT`LYS0c~~ zOOtNu`Q~FQP6rwh&sHj^Ac6g{T(6K=lgIQmm~HTaau{18DCOrU<;~A{O0hx$%Zd7S z%H83VyIC&D1X^LP%dHk`3Dyj3n{?k`t1l}~ZLqT8DYYo9`S?q6$}#Sk3)mj%iJl0i&uW*}=m=0(+65kRFtXKr8eEYqxFm4&Ofb^%y~*f&}^tz z(fCw$1r;Rr2b&&ZyI;!`Vz;II#8?CJ5L;|wZHYiD@Z?Hi?41(C4p0~k^NmnYL85Fa z(?hK0+LXemrv|d_Y4RFLm#mivv;yC+)WfBm9l)MG&SPvTxJ^L?3Gfh0VPq@tXy=od zy~TNXpjTk@BaF3&)?}`gE8ep}4(@qas#i#$-(tJ1dg^NdFY|2?br*jNtUJ$5Y&BtG|+cFJ%5!o{~O0+@r1dYC2*uQjr7~a(}4;S=&Rap8@F{& z*=Rhs0P(cDtEf6vCQ9I_j zY)n~?ymu5-kbqMQL=2wSmdy#iP`2UlhYBi4@OI8)Naq8H0MGRjfmU#8fe7xoe%^@N z-sHJnK?Mo^w{-@Ah(SJ`Sig7U_TPv*C#4+DnuPEe{welo_Rwu|vz9m|tR)EvPD?e>DPc|Izw5BVI!)g9%-X&tVQo)H zaN2Ai3xBj@r`X{Avk%=?Ftu>@N92_wO=g)VZFzP-FLR7L61+S(VU{_(P@$lL1e|B0 z667ybj6bl}n_j4p2(;qAWSzevf}fnbz4M?qwOnIirTp5Ev;)-8KI_yk{qkxxi%nLC zZj4mF?Do-iR~x3jPQIMxwVWu_A-8ya2o)qM@AlNf*3DGMKip&y<5#}qe#i|Z(5g$; z{Mt2bwyI}YZxJnYFg+OqO})T6%Jq!^m9dg}b8pa=uvj>Nu|d9=YPCaZ;} z?Xkib^hPN0Ismy&rjC#T+Y3ud21WG|DLK&!wrxkVV4d+v9nyvZUF1}aFbh{!3@ z^v|_ui}>IYC(?lgT7CJPOO(y%P`<;<%VXLVSF=1oK_b*UyC~UBlTTU1#rvJti@X8? zt-kEeD$3mL$3=^1HQmc9!K~U%mjxm{n>(*+b(*w)7u~DDJBod2Vof^t8uP}cO=MPtT?)qtrB-R^QIKio4-QdOWp=T7wsel%s;g_HG$9?;}aI93>oO z{(i*SL(MiPc{-3lE4Z<45g)_O=H~<| zNK|?4uH{Jik$Q&P7V)x;U4(%#vDb6c+yb37*R_vTq|GvqKH?^7yQBb-yG3TLe#LKU zeAZi57&&K-7b%w%AfiWP(cWD9sc!PWW)ZD#bTm_r3K0Ce>;3t+7^#`DO7^0Z2m@;( zcoR^u=9@&xVv0xyDoFfX=BgdfeVh7{XI88_ROl)QRFD9F2NqGFVgRiaIT`rH!O;MI z?Pv{|`l}=xs|6+_0u>~Zgl5zhEi}~QmHC&G6aQ{q7(k-{0~I6+yv?YM?YL7-9pqvW zD;v2K=L9N9oPFb}d97KehKx*U5tj?9fs4f%m(YzIPV{DoC_T zm05e`H%r}_FR2wquGOJ}Km`f#iecsMybhBR5omQhQ&!FI(ggKMp?D&AUKLaO3j!4+ zz`KV<n2(;SU$wOPaa)dhD{gaiBHs?3cDyNfy3KIAgHFLN1 zv>k~EwBmn{>(g81-*)m8Qn#1kqgE|2G}jsh#|)iQ=FkFmuTpn+f1={(!z2n6cRgYrniw_|Jm4w9Kba29_`mTUM7U58k@V@Awt!_U+ zE!ps%mGX~OT-Nbep@Kxi{vO)LhfCCg+8v9ybm5qHLISOL@8g&!nki3F`LYND6`UJj z*mQa)-E{p`%1|SC$5#b^Hy^z^?drRj`oSq;&iOZt)FUS8v#K{RHpISFFbpL4ok)`y zPw9y0&`Hm=z-~B`$8;pn3TLde7M`-Bp5eRQNWSHyf(jD$Y7uIUvUAlLX@^i4MW+Vq znND>zMpg`$2(-dEuHE)DWf^_Q!EVOmWGfU&f!}q%wpyL}dzSjNM+XWcAVW62W~Dxc zU&I)RKr3T;xEeEZmU?_2KkHm=_cDEO@+!u#GA<1B2Xp%F)eY)~eaq+@HG_z4={M2!L_Q+mRx5QeN zuQR+a+meVRYrpCRR`=F_hBaXQ53f}D3@iA;ausJRAKX@|g(n@N(eg?nChYj88y_0$ zW4AVBs33v!Ho8AJDPC`t*^O>BHjxOl`Vp~0t*RWOF*OfkPuws4lrb|ZYe+MO3KH^| zx<%w~{lMPxQ4M>^1X@iUw}Mu?_#BsqVG+TcKm`dL!_#X8dw%Qfa@7s~)kY@JYUs}S zYK1}<)g!}~P#A&L;`P><-IOeyn=n+6kVoO~)5Ys$Lvt#7Q_BQexnG;E9;tL)oqBs7 zh0*i$H+@-^7D|s+4H+s(yh}bq{paBgHSbhTY#Q)Y_g&pv$!KrDP(cF6M0VS*Y+v>B zGX^Pszw1i`TD2`bLvckQ-k(=#d&-N~1Jt|2~{cV7Ym6(qn{Ak`9DFHx-Z5~Jn20O6@@ z*qlXb(-cL;8p+!Oi`7QyuL#d(HQ%L8K%j!e^K6&IdP)6#nTVj33B_8OFk0LW6ze;F zuV$+ao?aKL5o?z_sS)*xi&dNomHGDr2~?06wflxxnMfM`gHnD`48BI{SPS*YU095-$_?Q?94G2suj+Fy*X zhCg=@)h>G{BGBsI>0)AB>Ys|Q`|vO>j`E7&VPL6Y`6nifVpRo!3KEG^{@gEJAsz-2 zXodMkt(IaH^7==B6A`HJzxlY7mmvO<-ByN(WrLRs0u>}M59wRG?y$~RYkThYF|bV( ztyV&;h3(h$z3JFw@sJ1Xbtk&k)F!BE(&}#dvvSLNJL?j<&U>K+% z@ug%D5v!XnWB(rlttKrB6sys@qWIZb32qrOM<&LGf+X|N2^O>TrzdkWeQ_%+jxbKO9t7SNCaAye^*$n zkw*XGVVt`CQ{m&1fNV_+RFHT%^OP8uoSAxr!WjSXo3ez)CC3^!lnAuS(B_mFmkc|7 zl!zVP@d_U&F7MmK!2H3S_WgECj75|Fcu7Q=e%}>77ERf|p@9k#SQo^2SFy&s5`k8$ ztK1Uf;Is$cQ5d0@zH&l&T)Bai4vhQA1%<`h>*2zD=5;*mtHMW1mtNO5P(fmG(Nkiy z^t;G$3WG*V%4c)5BoSye@5@OsT1tE11QC~LouTTs(Zy5pl_KbIuqV3cUQ(OvF-ZLx z#K%{BWun&x7k-D=Kn01DjY?_XwvJGvZt|}hCyu#!3IY`*@Y`h)*=M^aBG9T;cp2@@ zo(byy7Za>7wmth;gvSaMB*4qAMJ%cEED?cL;QQAiJejkI6^4OUr`P?X&ABmCt@wVV zMXalHNQ8k367sl&=at9ixI_e6b(|TbEqXjpO*Uhw6^5NIy7QEyf`oVbU~O-iC941N z{uc4^=M_Pqf<$s3May_*rTTC#zYD-)6{*Jv0u>}momaHrS?kp4srbo$PP7=aSrDio z@lSK6RUWlj-BX~GMKtU)Ul6Du(fktA-jeUAqQBZ%#Ex7e1%V0@w!-DK!`t_%?xAI><>ITHC8Q}t)N)P52`*}o#f#!R?^=znqtN3<-%cNWg8VF&ZYActLMp{ z@_7_b$DX=v#L6}jXjSO7uUI_~y|~CC7RQwr#F2=iVrQgLiV5j1_0im^4r zIKTadAW%V~kXs=^mwz$c3Zq!baft}D!Zev-G#lw6PA8y(MAN|qHST}c=PTc}594R|^ZkDViS$xuNe?rveR^S7_;IEyGVWWLB z+L>YWowh*im>_{x7+aIb`{Q<^bRYqqm92Et9^6vI3Kb;YpYs>{I7y!K@h(q!leeQ2 z5oiTonk}Ml^HK$P?&25@$6A<&W*Gh5cZqbMf&}K9NgUW)(U*sT?U~P`_&ygug@Wy0 ztUxN(3Z&sw?Xp;D?7tv`H0KB^c~Pu#rZ|^{($VVDCWWtZJ}c{Ppn`J|oQ07e$hylE zzLHwFzKcYl)z=T_#hUfH@Z1ze&KuK}*R--dD)_683KBRsB>!+DCMnj+wnU&6_eaO) zpX9-g!r0KElfqZFQzki0FU9^U>pA#~B^2GvT~V ztg3PU?SeoB30$8MYh=2$s;1z40Ox@6+ICDT6qsF8p`1&Nk-{WMP0 z{B(eb%XA*jna--k|7L8#MQ@=r`*u?Ob<11Cs^5>3>d`^_MHuOlnV zUc@kC#a~?z=iyL6B7V7#cyEH<(;&h+56Ae4tsgE0M7}S2xl{c%zkn#4WO4h|&mCw) zRw$V^a9Lym0u?0Wlw%l3pcUqu87n$L#|l!cP~kJLyZO>_VI^61qpA@qoRFm%vynnNwliQ^NO#krAm=irb8#My=Putf z5AEbB%DmH!Geq#!C5#*Q}d+S zn1A`ogwrSwecAio?BKR>3MxooeY4v}c&FD}o$AXLz3(p(X!TCO-cvGT{>T8TM)cMZyR2UxeoVWs337Q=>qjJy+8Q=tJ2_5H)YxU1_rhw?9b&E zg#;={$m2wMONhS=r1RH@@QZ_EE&P^<*N6TPs33vkQqeOgyk}VWzQ0~{>`~DJq(7=j zqnCkIPKe%TNRhHs9<5z+D7^PM#K!5U;HU=25_VgW^1%x4;bu1pmk6}l`T3OimYjHA zl)^CPwp92RXWras9TgmbIUNcTIMb)M$-O5l z{EJi4>%Byv)vA2Q#TTdh7QRz`X67=5e{oWkcQH^w0#^!YM&EOV!oN7lGNh3RwBmkQ z`4@+L$5R-wH8(5I=!-MfC9{DF61C1pi!aVN+8L+&&gHf%yl)>~J)40F61bvAGx~u$ z6yCQV=;|R6Xyw-~M)d7Z3dGa?$F@3ql-1O?*NDkwpn}B5uQ8%;e_Qk$5!60(t3@%a zOIRy$%qSD6AR&(|t=GADi!yL@k0V)}E&X}VOAx3afuq_#^Ff2pAmK$W>1No|`D;~v zV=U|`_iSX%AO(R6-VDP#W`D-ioY1pf*eQN%W7c9S-n)Vq$ILMJC|nSzAb~fy?6%5n zahwRXB%dBz-yRV51JD}})c zNdZDS-N?VrY|@6$1^FxzPmAEn1fHc3r;6FMQnAHp1i(;1Vxh-%b(zmx^;w%_6e}La zzLPe(c`p-ag(p1BFa`vA@lggt1&Kj)lRa1K#cGp+AE>s|7l+mG8Ogs<3KD3wxZ`cL z=APB++qZn*nG@~0nFK0GJlT9ly`+q$9S*_Rn&si{GcuCbD^!rcb6I9O z>hA1JcfDi+tuRey7)4vG*+F+27+PT%xOQd|W4e?RwFDI;f-}UbYjPb>qi^k}yrNM! z`!{>uL!PB+>U){* zM=^LoQpJf1SSbAV@HLIV5wOOKbTA2N?p zcRE}9=d5EO4dkwB~SL2^{lQOuONgO@1n%#cx$C(&nHehx9j)Y@A0Dd#lM0Fv0FD!J1eloE zzc7sN2{TlXfcI;yyo&62TeNc|&(Y51iOcndoOPh7|ZDoDuZGOW7* z?B4A>M)jz};^ptf_dkZ*J|3;|JI?j@IjMMB=+Er|L7;+!e5S~nL9#(teA*EtI{ziRqg}gWT8_Q5!8Dz^o$N3(Mc>UC& zBqXf2&#aNf=QeY{JM$v45%O>?*i~JLW?99Urr>@O5U5`#F0TIL>L$= zr1@BszrAekU~!`B#sD)Npdf)ebY?ovC}Y<1bl`8H74Em0M6An3%3&vl3a1@AX823y zdP$jm?z`ftoudNp2`4z+#=nEl&%a&A*ZMH!2?^^ZWvgd6n*EI!`yj#pc38PKHB@z| zv(_+2Ba2uWnoO*VqJjjxEo~7&)m{Gw@#k%7HP!Yg5ytvhzK_Gpqx{}5k-Mni@8YR9 zGp|;a4qMN60Z>5#-rlytc)Ztr7aubqfmVFS#CoBd2%ftew&&f&a~E?I-d+~v;m9|0 zO@D*!(Kf!OkNK03u-?L^Flfh}<*Ifo@`sfUt~f`uQQ>WJ3gd6SnJJ%3v2vuGhn|qY zG?}?ewQz)4cmJLp!ZR~+%6WODjm>V=_P=?81fHKU!&rFR(W3qmXoY+CCNZ@`Z_&M8?Gdo6@G0 zc9Hov-0?-m9c-Gb5izTOh#*iw0^7ey)HHn7{~upx0anHG{&7UbIx1p=9fX1?AS%Kx z?AqPfb?k1vCI)t5cN6NhTiMlfuv@TOyymq7732TT>^b@!{N4Y3p8MQ$`M#f>*_qji z-I@1gX&`}C?6c zb0UEX68N2)uWOkXLAI`ycW++ASHd?k%}~$_SgEy3_^P=1dbTUf=68@2s30Nw3bXm^ zV=H=jMh5b=!aZZnSI`&M-oz*+!@>vTuaBMJK>`&dM2}hQIsehG<1Y4`M+8LjwX=V% zI0gM$BzKME5xV2v#oBs?oXg{PvgaHXB%;1V^61%j-^CEY=85+Q72d@NRFEk4YBG<7 zUAu0yjcBhm;{+;5_=QciMYi5x%T?sDy!<{;K>|H*7(JYyG^oN?$`ul575QimU(we* zW;+=se@o9#*T|?Kfu1*vr?%*i>3mc}0I;6z}1XKr3uLMp-rQa)Ouc$>_O!ZFzpkLgjVH3?*6lr1Vc* zrQrP;L>|{fPN0HB@wL;G)=_Vieyf;w7}gTQPVD)EKr5^#gV^Z%iW7KU%;EJsvy`Hn zQ)Nl5taKUiB3t_1tnDI!R(ng#Q66`6P(S5aVk3&~ zOyOyuf<$DOD8A-Q7{K<0v*%-CSazNU5@?mF%RC-$+sVgX=WnCjcC$K11&O?)7xFX? z=bdFs!=>WmU2G1A1X{gHny-}jH-j4Yj-5DUxyt+FE`J75K|x}t+YFr?KDMpAfnpHJv;4$T|q*eGwddU+LQw$P)SR$@jQ1b8`W6v6ZExY9BYY; zuzN#8FJQtxAWrOfT9@~5s33tJ!OW(cl*V-0k@A_!3JJ6t|84`{?=!PLJCVy;qA%@w zVXYk%B+!46K}-lt_GiyI5@>}Uh>SZ=_uwtx!-5JDqMssGRvC&7;VD*6nS_O1i&G#+s`DokmE3XQj#&_1>bz7fbr#y4sM4o3?DM%Zo+qlmj-mXwV z;tJhI)Q2tP>6YCGE2|Z5YyKe6YFx=xeCN=V0&JGSh<+mn^YtJqNT646qm?wI-8-z6 zAc0ox{Fd{*KR?QErZm`dK7{t-u;&~VB+w(2L8PO-I4lh$(CW^&rF<`rb8WU~o25~; zP;pM6f&}`FGl*2$i^Ez85@>~9;Eb|*x6{}sgLy~qS(u(dNUojv8V405()zz@C!01K z`(#i-LiEbTYOv?u5qva2k8-CJ^5sOkg4)d3s{DB$?P*UDTWzC)1eSz~94Zwl{Y&dN;<=8T}-S1S&}2{DGD3?~{5+pcVRi770|4zL375e@a2~?24*`V1J>pPwK*pgj+JbcmTGR_9D49%v;H@2fOXgz{epw-pKmDNv=Bb2!K zbcRQ3IobL*&lM_26n)n~El_Z>(zA#y29F%j@(7<_2?SUb84##`%s53U=;&liBg@LY z6erg~Mg^}sJg~7kqxmc)OSNsb`^b}bDbE!WXtgRJKz-DAwvuv>dH-f(RK)CAJXfe7 z(K4{9dgbs!>mVL1=RLcXWZMQwCL($OXK~H$2?c4AQ7sDtB30DR{m+gX8NqGijRxp?FtFB@(C}e z24CBwOwXLhmd3_!CSD(?AW`N)xVmGluG}iiR`e{5((fn!L7-LYaUXSA<)ca+$Go;Q z+PHce^#KYJmt9+`>-^3r#kwrErI9>n^B)9SHCXPWwm)}Pc~jq>#?)!^L)pmo5@$^(o0mZP@<0;_}>HR$ntu<9Ch<5+B-!s|lVDm6xxWKTB3t zqhp%%xLFLryyOWjK*K^y->RFL}JXfe7@v=_~b!f4dO0$agc2zci@*f0R6&mEL zmS6fx8Ck^MuI~2k$a94X67vf;SG`B3C>eFO#$or-D1Doa?D;?ft)8AMPce}`D|smz zhC$TrE%Ew51&Pwbo2oS@IH;|MvG<}Zja&a<~Wr>%^kYcmd5&j7x6ns0K`lS5r%Oa8_5;x@9A*uf}?@wu=N>IWG@Wz4|(<6Px`*#NvG+@wJaulD5qyU##G@ z>e5A79ku9@ay@5v1-&Y<7>7mUJ??l(Ps2v*s37sBcc|K}+jZri+np(mJC%#a|6BK9 zNZt6c0)bZO+mxc8Uf+JWO1Z8fS*MTFQ9%Oz#*u%Y@9pBfZqAUR?+w*4CwNajmn8Le z>o-d3*S?f1zt)4}Cm)<6c?1vDQ9%NIdePp&(<|b0)jJ|h8q`N1(5lbC5IV!1s{9-` zoYL5JaZP;h;YX#FW%}r-Ac3C7$REI#tMO58Z>0I-I|>9^p~p0eGa7d#e&fuyQil>E zfmZYPRaK`ycT|@iWF8mamCUY<>5*NYKCFq33KHm>&TQJ)(WF)EpHp5`q_IGtmEYzn zYVSpvR9A`tVm38C~S4_SuD>^Djpl1?_@4U-jyBJea-WVqf1X?N0E2%!)vZ;Na zGoP6OGr~3591o}%cn~Giy*G3c!l#fK06$rG#bKpW3^BPi6k3b>uGt<66e%TCGhvSx>LiFTLQ`8Hr7+ebpRSIw>Bj*_)!!6PvV| z1smv1N~UUfEwsYumE`Yr_&%*)k!JddklPw6NPK$it#&@rR#{?VJ+<^cK^wZRnO@QJ zyg;B8MyxcO@~^n4d5v$WhxFgBp@IZEU*Ah@O)DAJQ-9rjPdocOT(9D`Mj+4%BUYMC ztt&m#_8x4d>l-I)s35`oHn21dA3`Z(zVb9u>YGIZtT1Aw+2lBjV(-v>v`en6p@P>< zf1?yHtM2bF(tRu#pH*jNHTtuYK%f=IxHOxdRn4xm`Z(-tjYkEEM#W0-I=|lWCZ%y~ zKz^Oo`NDO60)bW-)5L80u&AKU+EveCbq=F~#ME&`d8_O*Dw)#Q>sUf(Z8wMH5P}3+ ziLp|ab}6Md>#FK~H_Qt`1qtSD!yXHU`o!ClG)Al#kzaT|*i&Tp%#AId{HjHF(%ERx zvGz*|BNnoVN^HzH&SEV=v_!LMcE1if8*wsZeIcQOgeS#aV&jz&cd7m94mumv@@0A= z5NL(5-6$&i-8MQKKXzQZBcXx>i^s&q4rWNyA=xqFG z8gW`e1&LWawo|Gtwp0B47CIY0nzcG55NL(b*(g^0^@chdKSDF@lu$teW3AE2m!S=H zHhyHeze7R=2^J-ajUPt*sK1}q)Ys|wtjQ92gK15BGj1!hYJ}MO!TaQg9{a=sO&Y{6+BsG~{rAogRFHW8BS?*#&_I*c!` zj$c1k*;tFczZzeCj+X6KGyUD+j{<>K7~#xpnp|OxHn31l{q(GCGAc+cJWx(`$sMWW zZ_n0)Bj;_>%8dxs3vS9K5NL(5-OQ##U3O}Bwg&1C7rM!)AaOdMkGgo=3}u){ls%G85JZ}uPCc#o)M*tu;idL8a7STikx%S z$0q~|1X{IiR7TxjXql3dE*BAV+^%YSs^rj}zE%^!B{kJ*`}1idLYJBBLS@ z0bXkTQtOndFWH-kdflFAzJniX-i>Pr#B8v_2!IrQ$nCkd+x3j5C)AZuL85kDPj%S* zP0HNy>@AMRiTB#1`5UzD(;Eu}T47v5vnf!2uO6I<<4d=9d= zQ4z(zYI`DDX&0kg3ItjmEa;);3O}rzkIF|x@zLM3h+{Q1pDe8e0mo#w}YY&_jyE~ zHXvd$5}!}HtK0uRuWT#A(m37RL4W%;GQ{D2TY*3;v2Q;(@~4&|U}Z>_R3lgR`3@4T zKDny{lCLTGF0!)f{BNpOwqXUysaI=$=XL_EX14WE<38V4T#7Q{>f&$Ouwyl)BI#NQ z1X|&&hP>(S_^7QaG)-#upt+0+5@Wx5sC#ZaQ^sXyb^fvZ2d(4$4bs!zjRgX&a8^Te z(E_iuf9vg+isfj?TcUj)g+!?fo@&!!?-ZYv?Cs#Ut&g=agC9zRCf5)Mw8A+GecLYc zk5=OLKhlfT>M|-w6usf4-aqtJ8GDgE=ihyy$t|(vmWMpyPi`wT=NDR(aPTl37S=~8<#WEefDNY+X zB2>=*GM7N070znRrcX`dv__jlWml^)8rbJiNaRcKRSyJaQ^(F_u}q6^U7&e5Hj;~W zNyocrRle4RxT4W&N+p9U+>6^{uJ*_`Vm^xaC^Q);&?>L4QJ$Lh#X$M`{<#8yR;kx3sXZ3vR*RNkvEr9(b=R)-93&5TF-suO z3Ox!^ORW7eKF945a`c5p5-Lb6$Q`5}ip`@=>iL4w81mqJd{CZIa_tXg1OlzlA0f4? zHZ$U9j23}gYVx?e>b_|9mb2r=#NeFK;xk&r+t-OKrW z1p=+mGosnl*7sY8+;{JTA^n| z^2Xk@tQ6osP`vehoYW{tme)_tExfZtVwHPC_1cR}>Z~5@>uYPz!;({~kGyV534uVX z?z^tY8Vu{jx8r8C2tt=2|h3~J;rX!&@B|ptce$paXcu$SQ$a+oHtER6?nXGK* zvZwWtG>69X33+M?1X|&Hcp7oeKa!TVdMJ%6P*Zr{jzr0Z&D7O5-YO-hvp2$L9N$R8 zJoeD;xrPFPR+Wo1S3lH!rkp;^c1H(nePMrL^mxS_=ePVH_ih2yOW)WpCbAYf-1A zjtUa)gPN;v-F7M2m$6tv7iNEv!Zvr)-h2ud2(-fJN@ml`=O3hk`4?z4t2NV6K>{N< z(JHmy2g&c)O08CG6CD*KuE#f1oA%tKY{<&?>2ELcS_%u_tu0>LP$19><4T!L)pos* zj@>?^jVn@DM+J$bq^4?Cr*%qV1$IK=#q+097uO{1;`!PFfmRspiu~!1e<0~g-)R*q zsX8i1U_>pmX~W+8(#xWswZpr@bX1U-w!euwG~F^KCM!FSGh^6w>FN7S`rvO>1p=)w z&KJF>zMm+CI_J_mbPE(BfFTjsr?I+VNR)DO8rxsu8FWEf;gnAwxyxT5&1&LE8MV;o_R+)SH0;RG4bK4M&-XW)SJrR!tTH$v* ziiMrOK}fS*WA%_xJL6G7V%Bll_C@g;rBNd$Jea);PqiF8j09TYJ8~M;hUGnU{O35m z;leEk1p?lr<2!P*DPzlC@zvH)51KVEKV4(@fs5x&=C)p_ls_>;IoFWQ8RT4N<11vA z^@rm;WmJ&B6BuUGN#9HHljll$$yXwQRxy|6DONhm>E543u>Y_2?a?m`Zyk5iOz=q_Lzmfu!^L=!t3T>uMy-=CbDdKnbk+#qJcUpNc1OvORPR-1Vj+= z<6C)|)yEyL038)1(32~*#IZgytB;N~`~(869*j)n^>Od=WJ)9VPam1pN4f6hbX1T) zAGT&w*cC6C)yJJ+Z-GE7=Cg{`M>>bOEDgy^X7%yV&0R+Y3H1F--?q28%gd=giYJv4 z2(+q}=@PGxf)^H38gs|JmssyJv&UBQv;Er(<-i=rj2`F6^Dy*aN0EEWW|Ud)Ghk~H zc`*C!jTs5_-b~|Qm5g#E^*;S8U)4}SqV?jFy!Yv?UZ!06t;{L2-e=M2&jNu~=&PE( ze{9Gpv)(76{Z|bYBq~|Y@ZKl7&S6TU?Y#mr>wPkIGwDd6mFNTe&FunmBK1C<%4gS6 zL897}^St*dT|AD`VEZhq{dDBJ4cUJD4?hd5Y(IX=vTaK0i|p!{Dzd`%?6bdTIyT2f z_?CD=CsQ0`RFEiKZnrY0eRg$HM_+oPSQ--^HexYG1Olz9#O+r)*EOl zmMdxfbA{z<$rR&M^tL%`70j)X#^aGG{6rNhNOUb_oXFmhu*#N3$0y(ZAkeBo>P&t@ zI{QL)LYm!2SuaO^AE@|bG0r$wc3Sd3a^-o;i4&MBB*xS+PA}IiVn4lHI@t6FfmU4( z8E27SB(pUR%T@0OE}TGRPq=Yzw_SVkq6;-^5UX~&{*eX}DXzv@+cpbXoFA4(zxM8b z5NLI)@=C>ZZ&vm7ld(3UMO-CLpt5J?T3*jpp7;MBxf&g9v=YoO5|=VX^L90{OGjH8 zXNwsv5ec+PS+-R{ zefE!>mato~zq8-$=`zyT`^+dSd=7GdKg>(Cz#ZrR@eI}rGwK7MK_pUK4PPdWmO0we zNIGuRITC19<*t>V^j_-BeDl2KM?yI3Ei4UGkO)qms@%`INolu*&16^& zu9LU?%?MPG2&iSueMWz`@9dw@XDz=ER0QIrF?;!gnJ|Ly*+wD&tcGQsrL6W} ztDGBY??u=AyMX74l?l6BRz4}NhBxks4{HjgVBE*Bd=of=DC1`5?Wwt*mD6-NWyk<%-n@%hf+SFY!7kEJg7ewCC2y)y_pmec*i{v9rk~ zULT9EFR-=L}3KCo6jG4@q zoNOk;?&E3h41d%)T3sAt%&!J!X7ejXEIpo$6R14>kichsiILC$M}6!`$ooecNU+|H ztzLikiIFo){z0JChMtC}^ozIbp3-yoti%aast+}MwlBz7iucrFE7||bXo*->NX%+# z_=xX+*6t&I_WkjH0Y*zi0J z*csOhcc&=vdq~~i9Eg7{AzEY z8}&R9`e`rMXe7|;#HwPP$kr*6h;>WZ*b$)zPuU)W3Z1p(R_qimUWv{d41CLpF`cf* zpn`LNl{KTV851&LJy+&R6#b0p;|c$LHnzl${m0fZ- zY-Enmr@6;^l-*?HYPs7!c|nWCMy@)%mNBNx-PV^agHX0QqX1`j<>CR)9gqlrd zyB+1F>pa-c9SO8L(Rn^6R-S%C#O)LL`F*^v`rRF`i#hDP*vKK}zAWt>EZmgmYFw+X z8&N?bw`m1Wqw?n}ve~qQ2|27D+#$OjS3QwL)~l| zc3>{gRanEd)DnN+AzIC~)y=t1uZcJr!#s;c=!x@&#Grx%l==~~sgTcPgJ?Oy9SO9W zZmYptW0w#yI`KLuCMg{}P(i|CtHJzd-VpKqM<8$Qdrn>VKn00YwwC`fXn=)wu`aG0 z!|vlw=_}EwAOTMS5&b^@&D+(IBSQrOtrpszkM*7-i8%A{8E++J%e-5U3KAJ?&w0ZN z$gK zqTBj+N7>j_s>~E3*wfDLy!iMaPgIbg-wGp)T%~)JpHBQn=trKfib+HKvYNL3UD)q5 zp7eF&xk@US6padAcS@g(oUY98W6pp9{LZbhC*4s&qSS3?P8g-z?fDk|L~W@!KN=Mz zsH8bz)Sxn>3MX3CnCgxMT2Z^=1dZoZR@6%Po!eT;F9iu|G5qf|Ga!QI415Maa|YHD ze-UU!t&;zp<_|!kUCyGC{G-qHf@t1-GJzf0oG-seR z=&9wUOLGSH)czvSik@KpcbY#CLE|~U4;s%ssE^_bjps*bbmqTlJhzAhjpw{{X*~Bp z1qm9dIZfj^r9tC4Culqu2(+T{oD(!JA%ezpe&;lv$IuAC6&lZV8bys&uM1)n?NfXwQX0jjpufP+SP9b z2pO*=(0ESm>NkN_GzRl4(RfY-jpv-8@!SIyB%sto`FL&+ zEhj`HfmSqX^E7BYCxXUvPSAKBg9;KfYIB0db0TOw=dGQ_^B7c+pi!F>G@dJbJojLI zB8}(ns2~AP0TDEw^L9n!xj>*5joLg78qbNK@tn638qZ6jf&`7)oS^ZX2pZ3M8Z@5A zpn?QE|3pB~fc+JltMlu!)dchsM9_H7b7fnxa4WX@K`ZEqh=3k078U62*ox)Xm4sdt z`zyBc;#O?s1wAUI0X=mzUJI?D*Cv9-bKXj5JQoPGqEXu@g;&h85sl}Zpz%Bg6(nHv zA%ezpURE@o3j|uxsLj)$agYca&-sW$<9Q4!NYEH;A8Uz#@jMn4BnAz zj~ECX8(0J?KuoV~)j zpQUFnLkquiPSBnE(4BLIt>Wm;`ENG%(Vg3W)4G=KoIg=?=N`C5#&-a8=bT_~6U-*F z%M$WT86khI{ItyQEN%JM$@yh9CH>sq{P)Xu`;{H-+VJ1szr<4w?kjeV_3iSm$jCTU zkf1ikiHQ%H$CHd5MslJ|myfYXpw;?$?wn{)oe`Ug^HELCzjsRvUe~G@# zdbm}i*`3RNAKS(V3J~uibDde*qbo>mo4_j>kfNr<&z(a z3u&MgUCF`-5sG4F*r8kIDJJy%D08a!7-xc>)n6#fnb<*GC)kj7Cc1q+>( z8Fh}AF5NliO0a@@BBJo{sk{ca6uIk(3SPJ8E2GYxtec5=b10s-lE3Rjdmw>U?S>m| ztpAmrL|i+RojmnK$WBu3n4n5md6}p#ZNq9;y1{>cUX+*JxxK9DeS7IEt9TlZ7hh*( z^-Dowq@Qu6JAKvzfjnDl7RZj*y3&zr><~ z#DZN0v9RUwc(W;S@LHb6{4)`rn3LHQ^PD{sa`8o>Nb@_7U}S@0_=ni`7=66KOA^HMlE>=+U0KA-nw#Pi8Z<4{4O`kXqPxcird z2-STg@7pU}NZEu6687I-?KC1R8AEs)Th*H$c(>bjwc*ztR!y_m2^tNEpdOB50`MMA zcxtiSpjRS-dQsk&P%r9%3KI0&V(UeTpq`rNih63VG{i3}>b3cA>f4E+(SVmOjRqd5 z2-jWq7bj>eA%aF6PSA+sL8Hts1qm97_}^(vB!b34o+}y$y--1d%8C<4ShQmB@6B3?`^oEUW+{JI{WBH`w89(;tt18&B&fykG+2wFGYzw{@aO#d z_($&4UvY(c(WCSP^WW@Ar5=v|rZI|oIJeGuIgwbiqz5WUP_M)Z>Vu7bm)9WmyFyDu zE9y}>LH#Zf4ZppQWp}>zeCb%s74<~iih5BYhrGXvV+8eAG?V$AcO z1qmz#KH~7wW#c)P36wPZCd{v7)CY~Vyj{^)8w<~%LL(=)qVbi@$oOw#?n7fOuMZk) z*=*^Tf&}%){O>f@Ds(RO{3A|KPmL{6K`ZK&I6=KO5!7Gt8l?Uz23j1Ov2rUmV-*SN z+xdM^-yVkw5|~@wi}I(IdeJg=E9xy3>QQ+b)Yl5>(P$8so!`giWfrQB-#I}-Oe10L z6rQVcMQg{Q74`Ex4H^N`lWo@6?xw_2Gd8TESRCME?6Zb}f?F_>98VA_l?w3p-&~kYK;r&H=WPF^F&V)|L8Sgx!jz&3+qcR15Ty6Du7l z`KOp z1qne(LonJP=I(01({Rb0Uq*ucwp*=vFiF|zcUGxcAdc?i;L%2Mp;6}#{NB0=V9gFyD>yKzGx=<)H`!9bBP=> zDo9Wt!*gh?ane_AA@6M)bugriNg&Ybjb*Y@;q`81aac5^(Xf6SIYUZfw2Mmy85Jbh zZ~JaBItvg>W%Xu8P~H6bg*$)X8mUOP6O}&4V-=Re-}R9$OG8$=5qG2V38kBsU{9A& zgAuIG@mlQnFDqz0W)rLP2gx@}rEMi?2}b{JiT1XOEs_1UTd`Kj?&EiBzvnf#blTRQ zmSD6&9H`V(zWQx^(1VT6GT!ZOEmB!F@0Jpu7{klz*FBliQJueczYvs(>KyZq1gj55 z7xu(&*rtULeql^)dDlj=n*bEGgG8 z71w`0O_oqW0>9AE+wCXz@5eT%xvBYRsH7ho+z@{cx2USZKRi?y$4}E)i znow?ZcFAF@bajTLH!qu84*rov8FM+k=M~5PN{)M3l_~`@c)lFdFU`B~uqWH4JrR=r zwv7vYYyAC%2Z{4<@+#fBMTNSr9!D*)Xxk%FWd0D{y;e?vKr8&RMc;9%BuLTKCB1ql zlZ*-yp}kyr8cW8GrZj$Rdo4MZ2-Y`MO_h*9D=kY&PRy(}goun=UrWtO2J1_zq)MnD z@nl_5#ou2I9on%U5e_e1WDk!(eay@Y0)bZe-H>9wJav)Nl?v1o*I$rOL1O)lqRN|s znLT%IZb@m}9_K12)UKfKY<^lo1&Nx6+?4irGkHGv*q(@O{+Z;&%T@G^s~$+GAaUnw zKE+%%lcyHbg^01o-bx=21?f@UzDuYeF(y5$Sv53j*)Sqp99~Ns!>j5utNsuOv^uda zm$J;k$@9+NJ&EYPG#{2WK@s{ zpYFnQwd>GdL{yu&OF9%O=_97)5(u=yRw)vwAc1X6taGeE{C112$81`&`L#5j+Eq(x ziKrlfEmXX7%n9DPm@9lfP(cFQF}>^C`C3XU9;`p7){a(K=OWQ-Oi}ser3$+4wO?q7 zD^5o8_F9;}vSW#m2TUMPK?1+Tvp1;`5{-pAi}8f}baDT#c%BmpRFJ@vp=r{9NLEOQ zQLV!AhHFZj&)Us~tz@*q9mfAnpdt`LWVwtE5gMBXB<;E*;httZO^h_Z4^B{7NvI%! z=aT=MKr4(mM_*r$p4Cpr6x8=NtsrBra0fP~N0AqgoYl?{@qh?akifInXNZ>AZv&r-y1X|(QcRH&- zYo_M)skwf6$a@JDBv|xH`}umasnl$PXh;MqNZ<~9vuV(v&f1WrUG>_969fXSaF;*L z^app=1{3kFkVv3a;y=bof?ZBys24pvzm%4~*C4(4l-Uw0NZ>gHdh2W>N~9ss%I~Ie z9_RIKb{^;Gu@muno>989qmP6N63cVW}gi5ZGr6`T1Hlh#_s33tSRA?8{{8AFD z^FmW*3j|t~n`oSsssDwYm04*c$`FAH5_pEjY_bmSEU~uxv`~UTpcS3~r`Q}e;v*5L zAc3cOs3p#xDY0kp>)`hSfmV28$ZVQgb%VruhJ>ITI-W$rbKCz-pn?RRET`6P%hgB9 z6%uHL_eU{C4jzzLpBR3Cu`mp>9B~*|=|25RwBfMmn?-g_h zpE&}7Rv3|wVnH16kt4_Y>E%cDkx)Sb{qT?nv)L8pwcb8@X#KhZfmRrikNow;1j-Gz z`RKdK`AeuEf&PTZE0y-Esf&^Njmm{;O^5sx@$(qu7wqoxE0khnp6?$lXSyj#Ek=&eP(cFyfSOI| zQwGT^9`%$$%eEH?w8B_~*j|5s_^g^?#V4n{1Kg~Z% zuD*IaDoCI&UbCsf`j&F!hD`F>kgfuOR_O27Y`Qo+T;7u*tDJP{^I=Q_tuWppoiSP5 zSbp-!MNZliau^jP&~GtCmg`nuo-?U{tXIt)f&^M&yhB+dITjGQVEXoXQ2X?1t6m^>}AqP*?+dI=RI(4#qd@+?tQb`9~D zrR%!{0dHtR6+#_^s`Q9Wd^&*O&j{jQ;*yc2(-e;l4eu8n$B`b z7k|0(m-`Yb0wMUJxB6w0uQezyk81ftAkeEkda*Z~?vtnKQdNB9Yv`5XXoZmr%%)1K?n$#IxylPHWds7PFe<9qRR8Tc$>&0D`OKq= zGAc-51P1DNPoI^l6*S2O1A_$utuVSPeGTt_M9O(Jv;6T?n2ZV%7#D(ec_;0cR#g2g zy?b3(AkYe<)RH&$68of0Gg73oQ|ig6Ac65M=mgJ?SZVjfyVBxGEd&CsFzN<<1^B#4 zYC7kVG_QRtfj}#a=u6*(Z>^N_2U?^p%{s`aAc4_CXmlR5QYsn{D(gaT5k%V2>(D~v5o=LZ`^N|)kOLoRp@ zl~F+gBj1=!%c@V2=052VvUmJ2fj}#acx^TXWu78!=+iEw-OOP!Do9{F9?I3;36kf> z-iND=87>fLg;9d&E5Ph&lK&yk_+4$qG|)ZQ~V;z6)H$zv?JOPe`JO< zeM&aXa%QkVpcO_rCvWVdr%Lay7t~tQEEW|cFlrOM3qLhex?^ppwLafpAkYdULzzuO z>qSY$i}unoNc{u?tuR`<*;H4XE7j^YSNp!Jmy8M$7~zWI0!&;X%}iOVIX~_y5NL&Q z>S>k{u~d4zY@3$xAG3@K5*Vq=Z0ezHly(<6t6hm}BM@kX@x#d1OP(0%<>Rwj>mZRp zt5nHP9hACANvy`={2WN$BF&3?rajl1$fzKJQO(SzBK`MEAN}8H$CBy`1X`^M2v8sO zovozYV=>%Lj@>UkN=&Es?psSn1qqDKW;VTleo{*4kX5feNfHRODl#BY{g`ozQqYmb zqFdpAQW~?-r02~NBBO!?Mwz2ircPHR|E~G<6<5j&1X^M2JF}_H$~%(Yvw-ef+eaYK z>gwal>ZivMN?bgP0zNtBq4XuWn7;O4Nf{L+Fd`qtkZ$;1T6wFazGRlGK%kY=tg33c zwj-4E4ZhM>fGd~ZOAAhU=vQ*(lTkqeBMZ_#%L|Tj@eDrtjjx#m0R7x{g1fd2SxqJ#<(7=My{%BFXd2ZjXb;UD4!05Kc_X*DA*EHA?ZN zu4>eWEz0>JALu@iKm~~_pVuk|!F=|!clm5Tk%L31j09ReO7EsVbKazUb!4%fkU#~A z*5}tL2Eq2+>DR7mKLWDp<#GneNTAiv;cjZFYB5T}fVXJ~RFH_;yIwJf6+~<*b2)fR z+&K9!$3X6ly;XN7mBn$M<#beGzrVI!@}M!Dw<3X7K|?)x8c72aD2<8T4WhK(c`GVNWZn_a(}+!G`+ev>4%_Y{1PQca z9^ctL88M`hKn00ePKS9ZIK{CYLR24;tv>YBnt{CBgXb(zrnb1jOY}|fkFb1)19_c0 zeUC~*pn}8^=_app*AFT5Rfc`{u(c~43ADQ5UWvC|zZvr=4J1%OqB@=0V{P}8H}iT* ztzEJ`A373fl{%#oe{K#0v7MPnpn}BItGD>mmHhl)N~7eV722nsb@bRnndDYEuPDu3 z6y7qQ zI5@CfLZav4-`<8Jb{l@ZzqVkWhZ)hlq}7)ZsGxsh)^}~}m5ujy22twlZcd<*mhk zf7LBzI``S%uKjz-ZT1MM(U_J3fmYcnCGnF42kVccG+g^R$iv&^lguHlbyScjOpye> zUQbl^^GE`L%TlGSDS4%KC))@FTGiW^#7}lO!*R;-_n#%0pNh^FByf+=JuOE$yhIf<%@>N&Iw8z@=eClo*jk zuKKQ4{KLiJIx0wDqy#$KxBDMyzv7~W-{>R|X!R~QiJ!+=@$XnlBTuPpa=D`UwDJp^ z>Zl-ru@%S<%ltdiwI;)~r@h(>1X?AxP2}fsHZ)?bBr%(lT=vKaEh$@L9Tg-nwu0G| zknM`p;QD6mWy_WVfmW9ut8I61eYx&aupVEVYlitF?=*DG+Fd z`y9+B>>1c~?Y#l^68BCRezt2*W`4GB?YS#`{p*`F=w7If3KHn0`@adaaw%(g|E{)^ zdH-G!lPEbkI?J8wRnk#G0)2=7H-T1O%?$tKmj~>iG(vq(NPk}}Chs5Tt)qfOof?MU zbB`^|?>Q2vAc6kS*;(WRQiYl2<(D&x=}4f}*9@n)H}+?B*?5iwDoCJbb~>>hzDbHH zD9b}4bL&W;6?%XFZvw4yFEBjJx2t%ca+S5rGRdoBJ$dnjj5;bvpnv-RCeX@~V)(kB zUzg4LrhJ$xrK{aiUK#jOLj?)8L(FwUX4UW={g4PpBD$p^(2DKYV#K+9%me?@EZwA> zd%DOuDxKgv&+G~k=)a%NpJ(ePITO(>P$bZ5WxjYGBVgfIXG-JmXTH*#UIXMO&6a4W zAYrxa=Mfm{elAMH79dbT0%H7^*iHm0 zNMI}w`tmbxXh9xSjGQhN>^)2#7Ck!{6(la!JEmlRm{n~twHn>|+MgSwH&FxSGBuV7_kmVe zhU8CXLbUX_+CX{V=~eNlAW>oTDdpbnEb5ocfs{tWK1Za1^?Svj&I4dPj)>hxG>=SijVO){#^#{;uDmsG zKm|KtkH>)oTi3Gru8{^K3j37ak(NL!_M6>@LDa78e7G}}m7B*;z5|;*=dK$jE3+nV z<0YCqb&ArZi4iNMPeyjyY1(-25LO@EYbQ#mAn|POE?(!8QX`0{d)X&s0M+^Ajh6%h zt)yrp(#qaU16Ug4tRbvjMVU58s31}2$Ufe7Hw_<5MDs2QA*}7T_gN z70zuVfsxbbdsLMyI-83|do&aXw8B&7G;%ge*4TV^be+z^X>uem0vzq+D3euZbLt1? zW&(j$c=p|Fa(e$llWE>w`9^2qR6G(GqmJHww9BZowLyFT)&hZ6cp{(PQ+NEJHO*Z+ zq}K9|!ijYxk}38bTNSa`cjTLDcsl**riLN+hPM?6w8Hb;v@`SG4~?y(R$Xl^ob*Nl zPh`{H!Kg3V2wJOs4sR=?f&?pRwjMNM>b0r;U1RIP_G_981X|%4Zt|Jw^g&~5&YRU+ z%BUbw?A2txt~FxnU4E0Iv32c_Jq-i`t;Dn8w035#ovodD+qIt>M}oC{W3Ih{N|$Eg znr(e95NK6=tr2&t->S7lc#SY=Y&FrRKu_Tr#27R9beTB?c>E@ohnr$^9qk_ci5F=(-|3voInbsfwlRzts zAI8@Pnr$^9qk_csJhOQ0qOFM*KUYm_VQvMl~~=PK9sM zqG>fTG%~l03KFl9jM!&!@7Vjqvo?a(9|D0^7!!@=S7nxIY&EfDLPi-CB&2~x!nG_G_)LIsIil`$gQ0~=9|2vm^3*#~)5dF-pP)x^=}O9TR~a4zxR1X^J{ zH?!&AJFnu|YNE@X1`;Yr;9P>f>e~o&8Un2_0vzr5jT{=!Ruj+HB!!@YggDD+I?*6j zuN4Wj!Wk!hV`;l)PT#v8%;%Es`(v7AAf1Y*xuZic{bncR*>CiKO8coZ_1snM?)Y7d zKm`fWPbyu4zNzu|yE^lj#r%G-|7ZTTnAZU2ZwozX32%fs!Mu^ACD2^+-_74BJPyVsyCZgS)aD>u7nB_vwFE%g05upoYk?z|3jeFk}0mN++?q5 zU5O}NKD};z9IOw#cV9vUi7+LfrB|j*p4qxGBJSQ7?OeKwdIiVyG)Dct7Fun8l8dLI zS=g!ghQdq&UY>v|D!Pt&#T_a*&;Yj%M^E38?{)m}|I z-dEB)=g1*rJ!6YG*VC2iBPuj2_qKopDoBV?dz{z2)`F?dtN5l$NT3zQ{Gqe2t6ys~ zyn^-n0V07`BeIm_x$@>79gaV7(TDp4>V+aNNT?uzUWnbOk`Smqt0`w)%PDrRAvE@cl3-jo(o_ln_mWV`?bw&9zn9!l0g1M#L3Cmw==jqNz zR7w?cg#^|Uy+dB{S{p~Dd$EE@pcR(7_|#&%!j^&0CV9qN^jh<$XRtN-HNYnd+c92A zdC+VnMJ*=y|$m9DjN zs)VT{EfP2Ajl}FV%I+`ZNr!waS?pe!m|s1+wk`eW;H|1qtRW zkoEKAosynf_D14$|CQWx3leBWo_j2I&pifl=62>CGy>?TAVEHIsEt`enQwiASdeMe z9|T&F-ye(Mr$}$SDJv&XL4y3uSnS?`j5NI0$^1T0L4v%|SnS?t$Xn>BJ9MU~nBHe} zX&Lj*JR`F5VO|!IrnN!A$J%h0;(Gd)C1g~PAkRJg&W(s{RXe8F8{I3W&yL9}qk;tU z@W|3L{6Ol(vg_gG5vVG81VRF>$a4=*!-(Nl!ID=$L}|1ROE05>1oJz|(qNt{X?|6* zq~1SUasBwSWC;nhB40l|4HmP3z8gf9)}N5~q-TM5Bvg=~6(mo?@L)z>pvXH@w_QJ| z?UENLi`_pI`HiyJy-Q*1F>26MUiC}cu8<%3S$dgMI`^1`*Fr1ug=YEfCG9GuVV-cw-;vTlh5V{n?B3V#N=6!ua^BeW zjqU>#B(RS$n^HUE(0!K<+PL#*Ga1`!+GmCN(lb1gGJ?$u?Sx%Hg8gRmN2B*)#Lz=) zOaCvzZpG4Ozl}5|C41_LH}ujeokMx9>}jACYx(T&#uH@_@F+b1uaeS&d#C)RAVKf1EOvj5G}ihx)l06L6jb4F?y2yXf&{&#ve>;AlJA7O z&GZE!mx9^^=bYv6m#0GA?x_$7w4%3E7Qs^?ZzarAAu33)-}ZM-v>SIv z1AW+=$p_nJQ?2tT%w_Usz`)#*ktw$FMw2 z_0#gGgH3L5kEFlKhu#8P>|RJQhqTv#>fEv8xr1$~&QU>v)dx%Cmq$|F?vWG~B+fTa2gSO&(kf;^!PA4OEb* zpHPC|(=pHOHsb8W_xwJPK&zBt#d+O~*}U6E-=Sg)i#2x**{e09@d*#mMBcg+TY#UuG>Q8(UA4jO41Vf++W-3 z{I`Ncp8}FPt&E7e$M!l;jj>9_Zn*0M_j>5lmjzgV4!o}HiSMGM1o~2Ma8|jQB~sZD6=W?m z;<8e4cjq(&DoE5_>Sx{F$*L6FG>wR9ltxio8c3kk_tl|1y~#s5*$8{CP(h-6-HO(d zZx1Sap3nY&2(-dd5bqonB>uasP(h-@?EveyUVD`?|ISZSR;VBm*VNxy<^Cb1dab!c z%-0WSSG~&X_ih){X+O2qzSEj^V_WRIvD;=WZ+-N6gYxYcbohx8_v%l-c?-F;GvK`+R_II}9o8IDV+T_iMC!$shjy5=f#U^ltBdv+KtcCX1np}&0YVG+S57nw2QNv=~z}+KH*QUTUhBv zuXad7pn}ANy*DhZbR9ROQm$AJH+gS2-Y4G68DRZ5;Gr^TY!9W{n{w96L$4`Ee?%(y z6d1(Jqy*lVpn`;4+Sj`G=q06c%}B}>OQX;52;P??}a=oar{=n8adX(S3k|M99x_R`&!)_0eg2onN_@^ap`fbgF=-VVpN$xvC|t=L9N9 zlrLD;dcO8YCAc2zOBnHTqA_wJfmU=v!TS4z0wdlxXHmc$bX1Vo5K+cjDARjobK8Zs zGzzc#;>ifi$#hpAYwiFi^;N5x3erZdT50R~{0bE$&gUp&ZBgTklK#meTN(}LMe{UJ zLBji`r}fpfZ%VVv?2HM!506ok_!ET+5^UUc|MZj3)NHw$F^s2yEs;)-SncOZu;m*> zzqVO;eV~HG+w(rw6Gd+;>7KDsjpb_Hu<`tfLInvr-(t0&Z!w7UHT<}zbR2PTHh`lY z?QWwq95>euNToL{s37sBcc^vQQeFAyHXEIpr}T5%mijSI>8K!a>7s0n?72(1k+Zvv zSl=p(KO;~hl`fr&iW!b^;HX{G}W;_j4kf2jgR(3whI8nuNwW!B*o(2+V<+r(twM^nv#g+aw z?juMU&eK2z2|CSXWoP@0lVL24+@B);Aka!2)fnNlc{$G&rh!&U^Gep}wGm3+=WGVR z+Eup;oBY``hzb%oiW)@c**pIr(2D(@JY=xK#&)yGKe~qGIc~W}x{{-GoHN`zQ^D%D zcb(#sc{w}h^=tNF5V;qfiDd*TNZ?$;Al|%K>&*yMkiZ<$i7Luf{|Y(%9f&|BE%Ez4 zf{5^{x5v*;*cBvPi<5a9nDTovO5oWl*g0h3Kb+CX7#bYf4D*!_smXQ-yr`% zpcU(V?CV4$4XQl{mIf-gHo&wg^5Ts-+P|5a=@UXeYk1wy?=DzeJeMmm8;bHZ=kGMK zMoibD>V@l@l3#0>1`@Of*1`y5r)*r0URseQX5A^>8G%46T(O!>WzP4|oWGj&3EC+Q z6(sVWK5D5Sv`~@!J*afw9Sza)obIdFsI);K&A)%_j=H^tkwd7Rwey+TiE-noX6OX_;k^qHLvx9wQtSC_4T!HS=b6I`=K{s zxMK1Aa_j&2It%D1lCBMh5E5j7kPsxWxC9A-5IoZzdT_T8EZ6{x6P(4}-Q6v?1SefE zjoU8nvIKVt8Ulaa>KS;8eEaV?drl7TeWtpkt4r^76q|TRb5D5A$JFKeebw2!X1Z7Z zCJYrMaNfovR1`?m+hp?9m-THf5ondue#=oqIi}so{*<(ZXj4XB;%UrjpQyLZiLXQjw4mCX=kP! z;c1M!|3zO~y_IsTSrdi|65j1jIV#-0t_?kXloO3Ee%1q4^;RBMi(sfAfny@O<#_mK z{qN}mm5VPMO9WaiDt^k5YT!-nN6>Me#_f(D^$!;(Dl6KDGgOe6`{|^EW-MtBoZ!Tj zLfiCNtekppS{8{MN7Hy6=qND4gTs*N%I21OgQ#aOVB~FpF4{??zFa~w*BZY z){wHMf3JN{@E7Y>jdHx#nmJaA)hHt~ok?}xaTf$CNN`^bV#TZK(dEAps35_8Lx{CA z@(m$YKNRPBi8}jFz7o%GrWarRL`&8yn^+@R=pLsXPFyF}POf{=9EY!${3K98qQv`X zv0f6sif(%Il?lbUGNH~H)ri1ti?o=$S;Q(%rGBq|BTzwNe&bbQ zW#UJsC7jsNuMK%yVB7Z?#kda$u`WX1ArgON7k}T$^Frf3AjAsGktiCMjPKM!5bfjj zVyGaI=iFLB9Q2#Vi5iVhi>U0kUIcAJ1&J!(%=CQHzTiaX2EEi2)f%lEl4Fe`?SJrH z64zR2feZd(d{w&1T_^E(n#KV{phC2ZXwu(bjIWB=Zaay(W35RDwCduOU5rb2_n=$x z^i)Sz^({zgV69>OCnYL;jTZzeNF>eqp}6_y+K6~><`>}5jmU{0Vyf77^> zY7l?PB$f_Z7Ci8`G?2hD?n|M)CoEwtE_cUof33}Pkq&dSu> zlMWK7Ao2Q1MzLBuapW6by8lI>)zp^h#ENh+Yl`2}Kn00UrOl@*@R8aTrh$ZHbuo=t zjV`d_$8QAY9D5?nlbi-BNZ{N2-vnCW+syrP#(!7N@Nr3&ijgYjHX(Ys7`>d>yMnKz zW6t>~L)mYROH@>lm@;;`7?%|INOu~DIJmbGjY}i~t+rIlAXXqx?4{EEvFC^KCm)x5 zZx^Yeg2a%W^TfF1$S~?lLTV-|G%m^9rMX0)RrHVyVvSTyMrpiFm#EOVWN+%`Dk@0$ zU7aSzB~xzEx=+v3UzEjsToTo$iA12)8;|K?T#|bV5xx@>6&feLZqi)E^1+gR=et6T zMb%_fR^xYkQD`hWW@{4_6(q1Ni1DuC9Pdg5TJ<+ph;gv>Jnau|8t|D2C4)Ue$_GAs z*UK5i+N<^aGfwQv`dOjTQsEzsRaB5@Uv|0}ExCnI&iQCb`Dl)oBm%8E_na<9OV;!B zRlCVo4XSS&Rq`FJ6hV)JJyF1hY{n%2ftrUw?>VhA#BWGdoYomoL83$lf8)#6;hHT! zeQSw0;G0(vs33vgE|Z9x>6e5+s|DGz7_at>*Q)p>I@9?3@%tcpDpZi*UX{d}jLSn) zr23=olOzOMabHhjrO5OfMfs={<|&>E(?F}BESZei*Jo&vUhceyBVtR{Lm~}SkdVhE zL?qi7pM*fG8O8jJg%9UyKNcl-rqO=)BSD~o#HA(~jlJa-Yo*drze{OYe_R#>DoA{s z?`vc@vqBr!jJ&-OaW>W=2vm?T-uM{Snd`J$L%f`XZOmptpn?Qz<6~4CxmlapH>H!9 z6E#l|s35WUMF!(Fzc<<^jQrfuQ`IdpLJ+7R@oGi}!`Qx08-3eF6z|wr5U3!Lwm}9X z;2P5k)uQhprO~fiBT?t5Ac5aUli1Yd@fK={s33t~B9q7)m0IjnZ||N#>aJcxJJB zzA5mklL%WhP^@ehI+9-OjI8_mR{L_!N9;!&iF&Nz)0+8sGbdCKs338;Sw=x0^t3`6mB>35xYxY-Jd#%W;+Uq-6d3zXxc2Dx&L+VqmyVKoG}T3R@vTU5PM$d`q0;a zT4MbkbHpwgDoA{pnnCRRRXzILNm%pc5~Yg@5*JFR75j$w&wq6irF>f@AzX8JhWcQEIc;!9oQIEH{(rbFHHw@O`Fv6zy|~Qz%kj!dD;_=L)3i?^rKZ8jtLp zCCxeRT-hL2ITN!?=Q-b&ev?A0oE>`msi@#A1!rOWcK`0B3az9LEtg6n&?=ArX0c|y zDR44R<5jw8%1geo9lGPQf(jBiH{?E2hEG(SE87x*R^0!9SlK3z1^k9v$Ic3^YUv(awMxixi@4USv0>Rs338t)LOBI z{I+~+PVjX&#kq2)Vq3yiiSu@uKm`eTO~&aHkXF?coYCPr6VBVjsv7wn6a*?r;QEYM zBh#I$YC6sba1JQX4fs0TPXZMrwDo7k1xZiPkUvgVIkIuiPfeI4yFBC9{i2QPr6Fj<@?#a)p zCH_!Z>pO*oQs=_~?OR-7@l+E%4r<{u9U_f)3AN2M$a||e-G&Mh+&_!R2YEmhIsZwZ zf<$7@!lFdTf9lUuVJe-6Yuhc4DEHSFPH71&zo?rHl}>1lI_EVS7fo*KeErOC1S&|# zId`796KNoUR#1|rpRKX+K}4T#zJGC z|9UF4iV4nPY`LAx78Cx@Zv-kx;22d-0|~T}zHrpqt%^Z5 z2~?1Xf4WD!AK{OCaUz7Ts!{1a8O*v5XYEvd9}hXh*TTObptU@w8cBonA0fj!256JzTZ67~G?O0}^2m9CQ9eqw~{gK*1ojd2Sj#fKAo^o8MmCSbHX>qX|{)^xv0NUGD z`3OMlZMzgCXxEzl-P{dNWA2};8=d>-E&{FatP#JdS74G}_*!GNbiTJbDoCu%bKKE= z!8fhP7CL8nX2w!|ut#+@Wu;UM3ADl!P<*Gl=W@OKpH0Kbb-sV(ab>Z9o5`k95#~gLg8+jK5 zZwppyIdVt@TD|}5aL}2%*TuhZf==)3y6Giu9O9db@45W_pc7jshqeu&J8`HW5%aNx z(fWP{o9F0NJm)lU)zP4 z&CWC$FYJ(nK&!c5iWr|W``I37d!5A4PCW&IO5)uBey+>U=H_OQhwk~ENL zcPzh1BYW&wXBxYX+mjGz)pCCxk&j|EuQ`bufs>LDXvO`Ai>ESul2cjbK3Y!@s31}0 zWKK~ZUkAC?GL}c0^??Lh;V6oaUU?d7gEk>_a}pJafG@RK=iLi>?c`_mB?7sRtd&AvocgI93Mxp*;}YkblfF0-fmSpZrSFP4OLfj` zReoYi%sE{O5;)W6J{q-23Vm_*`o5J2w7T+cmH6TWRi~M7m;Or?`r@pqnMy?k30x`U zQCwFqSLll~K3N)xKr8NtM|^R}S0InbR%WyEgnw~@yfdk&AhCVHeDTGxZlkB_`f!^< zef!1^Syfb!z%@XAD$Ke=p}yT`n7>4z)fM;oqHiC&i_UUxtG`EC#ryW!jvOi~NTiLL zC;IjyJ1200zaP5ujbhlAuvOxiQ6^A9LLOQ2H;O!^)BB;~=pIM1I9vMZTU`*SAc3RW zpYuURGf3`%-9fj($Y(p=^U8XDr-N>cZOl@F=iE7i6a*?r;2pD{V`?Il$Me>T-wJ=+ zQp0;!+{3&`n|#vC1S&}24K99yZfQaNQ_U)@TD$Bjp5?=nfHbN(yJxI(>|+3d3KI2C zU2?QpJ57tan~pyfrSX}kQI)5G3KDp_QS{U-Ve0cDH1EXIBDgYvXDP&(fz2orSCWqa z6jYEH@ou$aX~8+#4GhUg=fV|KkXZhCg(FAXMOvZ0 zv};MX`|B1QLEk793A7rvYPqA%o>iL15!y>7qGNZHKn01AR?8gM!d7a#J~&4*EYX^i z)2$A%j{w-_ zhqFZuqJjjjOq#^4>PwOkXoc&HCQ+>LG?DXRd6zoETW`|FWm&87C@~IK^cgHelX!o3 zs7XM%Bf;apIDW;Ip?vhaH=Ez8a#QiQ&~ zCQ<)tTS1_LguDVt?P`YZm4rYmd0mucIH9TZ9Kj}0;b91ZaBwo5lBq7i$q2(s=RO{nvIEf+7G*Cg} zS;9K;R2efh;{>14i+phZJ_-_O6?|i*D7V+Q$X_3|!~)K;LISP0_eIg#J-J_RGYzT_ ze%nbw1&NCfmx#8z)05^n^i&Npj1HlvLISO5tW9Gda}-X**OTu{5`lX~xC@k&NcZnI zL7;*J?(djS<-I7m`BacPTFE;fIWDo=@NbqYH{{8s%u3??y)5-)YNTAONyM@mQ7|ji6j(~3z zwi4X=l?hakz!B$v6KExmES;-3I<5QQ7YDyfco*pBN|7K?K|-G0IcLH|FnnwAoxpk9 z&zZ0wP(cFshku@-SNQFIb&|(S2i>Fnk!co1OLNc-S&Ej1CoW_H6(r<1{=i^z$x&AW%UaX$FZWSMl5{_ZQE< zYG<^P)uMUdPP0@OfeI2h0{l5k6=|S?MAC8M{lBw_u@8?$=b$^z6pIeeh>m$M%|YG- zsve~irqqjGj`Jx*y(lV3;AvBn*!p~uNCOG9!V|3~v3FphwUh?d8rDBPtx0Sy7nmd; zNZ=Eiefyd}-xjACBt8{B3HR$RJb$^lkym;8a&8WrvxC0Qs33tgYZB8FFD4<-ihHql zP}DcmJ3gi1xW8Bss35_8>pOn=>?b1eNvI%DL4teycl?SyKt!X0U&Z$v6(q2|nfXxi zmfA^Wg*oS51os#3c+}d>X8NHgqSS?cA`K+aYD~MC4)PjtB#J!g5K+f5S)_pq65L`bn&GbW0PnEV* zOYv0rTWBS(ixN@RD!gnes33tWtY*$@&lxD53Kb-{hj~ZE2c9<5b3LWunWK+*DkRVf zSBlLvK8ExY1S&|-cb9fX%@Hz{ZmT}WX^%x(r~aMzkJp_}OSLD+s>KRZ$Lf!?SG>W<> z7vFOv&`RE$Afoc+V37ts71A^orC3|$4i*t>eT$2gB2bd;NCNm;nj2q8*c8 z5!r}{-ky6Gl`fVjkG18X_;VD)3roRV(;uGh!8TgcN9A|oSL`#|o5)e~W=wKtK3q|N zc`Q1S2F19;JefJ4!=K8Pb62?|fq6396)#!(kGHE|XNU02jGS|-!Lsj~nym!N1PMGp zW2TY!dyw;u`c;Z(g?slVF{NW~kp}MG<1TwrqEXC8@t&iC1n$?HX^dHK_E$)t6}APF z=L7pMaCU$?qQr08~Rri1S&}2`_E46Td3IN3Y|Y1LvRe{KI+TR}iQmfp-shOEgbMGw#U}GsUke^j zTFi>fIMqbNFO1zm1S&}I7|CMhWk$~?;(o6`L>j0d!DDZW*_|2Fn~0WrBSD~o1dkf* zoO4bmUlyDgf7DM9s33u!H%uZ>=~aSyI3&<&@aR)wMj!W#{C-gH^H7Nsvu#w6K+hW{ z@oLMs0P5k8Kr7iN2c@wo;)(d;Ac0mpKc5im5AT;#iei-JG}2_D5+%!?WHDfPR8ePxc5ow@;#LgEd#Jtu&o=TU9XK9kh zP-{m5t$2iGF|ReFH51{#dyuNF#kT`4FO~@-FpA%+-)UD;{B4%xlfq%S8BozavT)6(r=doJ7n@ z`#1@KR(KNBY>8!yJr&2P8CxcaovuIR)PxsOR?eV8)x`+*0!*6#txN6Bi1Mp!3uUDo8BqbW5CDdi|RGcu{@q8+9~< z>H`V1dbIhrIJZ=!5XEt)e2lCyT%4@I-#w(<5a)0F<3CkI+9bTbPY~@26(lP2e9+mS zlSL^Xl*Y->@kt1@TJ-deI5~8G$yF!OTdyf*kf-8W6nIaZ_sVhgTt#dP zVjWKZ9+HMu?j-NV_;p4<$ufZo5;%UK(*5P@9tpHUf6p?33KBRD66>OR}OMFM{^Akw7bKp}bw0-uGyh%5WruqZhn&BNM0~ASbw%FaXJC(7Uv?^7GweyB+$q6PhWbp2B6@)27Mdi>_sL} zK>}yRe6&=vD9e{EH%tBgm=rAyBi}qrvq;2O)32uplL=IisGRPCh{5A^bei*vQ@D+} zX6e1CqS2yecODbxp*DNJV$2KesU4~LS{pf{lF?;UcWrSsr#BL23@<2pI8>1EYhBgY zAJ#t5|^$7iZ3S;XqBx{4Wn3#0b1hr z<4$6*R!2M)DoD6%<%}PNhHETmGSfQ@%kisakfl+#-ed)vO7NJApP%DqZm<3oyZ+TRJgocTz#WVhH|LIr;}dRT}t zDSV2S5b5-b!`66f;;E28tC1t>8xOio)qKNTXHpZrlR0bfmx9EJVZp}bgLAYwmn_b* z8qqaGTt@8J8X!J^$t2OJl&Y6#{nMR4HLIsH@XNwzg1(s=kN76gfSXWoX$5D|$ zt66`98<)l`*HSP#txoOgMyai$tWZHBd#@nlpAOO5z#Kl#G={wROU%`fK&zKs8yl1E zZqOQ)TH;LO{q{$KKn00^dlfSd)Qr)lUh{RP@ia#n^1!bmfmZP?BaG0iyR?_D$TK(P zqwUR#qI6L~;^U=2WBUrG`8><$Ory<*&?E#}<$oPvEG&Ci>zY7*ASsQ(gT{)oLIsIB zivo?6!Kbx1wf&rF>{+?NtU>rKw7THm#Atc;jFzjND~(dOw+I3iBodAV8Xb;b(Q3H6 z(%2E`CY}ljv?}~>gi*KUHSJ~*iWN`!_?mKp$T=!V1V$7yjs)D-CZ=?i)$G`5NeHwG zvqcz{W8$=jbzFBdau1s+>Kqj$LaGNDeRI9k8n<_~tBDUECn3(59O5^p;G)i4F*ZNRFVrfW-k?y32?bBUX zf7PqwH<5EB(5ipw`bPQi6t+~AwmQ>TUwvUgdZSQ5B1cqdBgK%Ewyf?>pJ%LLHcycc zB+x47yE;an_bF|ia_n)YG4yPtC|y*Lxbmp1F=lcaTXd}J`|&MXut)<5w9471mXYjv z8r#T2%$Y{Dl{rMtQ9;6UN(JM?leD(vb-p@@ymNDio*D_XYIdT!aUd|AEg;!RCs8(g zfAQr+1&K6`6~kv-dRxl%PM?6>Wm|TU4wYy3qJl)dTzQOim)mI9THoOLI2x5% zot(7}o3k=F774V%n3{IW(+X+T?l~H<1Kvg~Do7;1p3i7|sD<|Dd-5kUdqirr@5)AO z@MljQ3ADn9m3E8!6u$2v_= z28HUpLEZ$3Kr4)CVz;c%uuP$LRik@y5&487sZ#7rBzlGgi&i;nG>uCR=bEih+s$0h zTO!a(j+IjF)@Y?sdmCFcAhQ&~1qt%D;fe)gw=8IvPbH%F>KW4eL2r@EGdH$;yX8*V z+*bM;)Qlan->YjP?$Z1{Rx5qg zwoV@;5om>x+PEj^#7HZBKi1yu%TPgr;xW6vJ&zYl}kPkGVPOOR>+8 zpa@R%{V?N4)jwTQq3_2XRg(y`LXSsw%d(v%75aXxiB=gZNKnit`hJ+vph``OQ0V*d z{ARF3pcVRIvRiUqiBRbKacFH3h6)nsjmB;nb!}T{MLv>AJ#m=S+A-o7wqv`c^16tF zHJ%P*-yHi7P8q8hZf?yqH@92blw+#1AH2%_&|{yNzCoSXxjTlkvzvFsqJl)hbed5l zdkgK-(Z6}SD&Q3v%Q|wR&2fo9E9|@Mmhw|h$DZ^Y#6pV(>Zl;mvUVk7drT{>Tq1o9 zZa0jJjj#0=OP;BwM4%P+U3>&kE|WgjvoCAIhUusvv3E!nqjIwLTK0~Icp8Cai|A`( zqnLI1Jc&Rn?7MhhGOUt*?QmC?q2PKQ6(kxhs%F&v)J1#lOVOI= zw37QopUP4C$0}`Ey}W?>l#H7LV#N`t2>HnP2K`3>75meXMKD8#YPvT|mAQwtmp{ zRi^^j`Wyu%06*dIH~N(5S&{i7=iU-_5!^kQQ^>1{4oWT+r9 z`C3DxcD|L`+kAAVVb$q4J*fA6eN6MJ5`k9qW%v~vu=4%qdW`>Ry?4c$3>75KA8usy zo3%mv_uEvSMs>?uee|r=dXGj8Bm%84E}`9Wf7^Tg(&oi_5#NRk6(s8P3pYkJ-Knja zL|$k&1bx+a4R5ONU)e+=(5m(Oa3e#+L9N}5iJaK8>5D%6XjT2jwWbn*R@5i{ioAHK zzz;oLpVzTXo;PQxAQ4!$v9V&nQO)}Wd7xRjHc>zIc1-MPJyIgj3j1KYdA8Hqf6ZE4%)bJ{8grBS28cm3_#G1juPWdg0_zJ0R$58WeViS@uA=2N-G zJ4hVP)YyoMyQ*DoMP(Hm@l7vMw}g_iW;2lw7lBq&w}l(gpYLh64o~95%Kcu@dJ}ckot!J%PZuM#) z5om>@8XmoF_G|q`t-VT%<#j|$bd95s_-JipH0<|ITXS?ePh;k&2l|lS_mvX`s!9Y} z;TVO-`3ZlZ=ehMz>0Pdh^NljcQAi||Y-rp&@Ky6@MR%9(+`gt~nVnLN=v+Y}(CYk` z`bJtUnXTS3S{HTGF6v9lrBnYpQ&u9-3P&||%d}dj^-fo^t1I3X6Mczm9EHT4ne~jI zdmgq!18L3j#eQ9%dMZF26;x0n&PDp?Z`-|MyLlQT3$@ZM3)-u5(;t-xv_fxxd<2lJv0mv= zM|FDpy*esLG>fci49MhT>)Ixkr*W!l0o{1sQ@vGhhD4xM&XCGR$2q>Xe{Ub*#DW2N z^sAkEsVnkKlL)jzkAikfdaw7f>23{B|0-BlM+J$oUo_*u`i!>0bIF^o5`8GPe8xd) zuJl0?fmY~`&~EwmU}o&3!9!H*z7MgeAQ84J%$Quu&$cM)5>I2osrLu#^d6?}j_)oJ zXoX%2`TV@;?t}La4O0i-^N&RZiQ`=rBW{?Vt$I^hvwYP0dT9E!!&JX_Wvxh{Rf&9| z#!Su6)2iGyXmtsVJOCC|`SBG3vwBib#4`p>pjEiyzcIoLx%1&PCV%NfIFXS7-V zc)`=i`|X^yG%wwmFA7TpTA^n|yQQA(gH;V5pbqcQL_r0K((B3?$NT%*#-Dr7(@@J7 zR1Wf1lE#=K5om>;5qY~hTSN&d*;5@ha<+mB65ocEGPb|YU`taefv550SraAel|R*z zISxt$TE%rMX%zpF-WFJvR_GtLZ?ELyZ`4)IkO;JrSJmu&M<~~7w@~l2xi8{*x%@06 zaivcQBO*Gj?fps*p2k=A>B=GAGi0m#K_bvfS_8Cnsx(*mvb&C&@ZDWS1&J|>f{fE2 zQrjF?XhrzjgJ`8I?-TRh${-PFg=?97wRX)0rCxbe-Big}MFok$j{=RW&%JE>Pt#g> zi|z*$&xAm=VQe0WK&zciiW>eiJZ!rL(_WEBW{0wXe+{aYDIgJOg{!#yEY6NoN|DUj z)Q202tEeCm^nv?ae(q))^nmt+zs$a(%;I0QLs!a31X|(xwcS!J({&|S_f*qw4pmV> zV)>y$#udv~?fKUXJdFuG9x2oK_x#%YDiVQKxDLbmx`Qui!3a``Ee?fmZil6*S^&KGhcGrk&BaJn>3+QdSG93hD2+yEekj?zW?N5wYatP6)%ZjKV@IuX+VPy3Iq{&9 zoBD;1Ue7<33AB>;SY8zWp6XiQ9ZP#ZOcay~y-qB0{*K7If0W)gu`7{`dO zw<}+iwBarEcKe&Cs2~x)u%Pia+fME44r(QBt9?-_ZtS2}P9qa&h0&GxYHjcbC2Qu{ z`plq4Dk?}|1ScMQdEGlD_~;USagl~9DoAK`1B`~9HfUQ0(vExU3~!W*5i$Ds=5-|k ztuU^X-O};nQ|0Kb)B5wJHC0rQs8Y6&k=k>mmgzC=xCi(>RoeMq(w91_O9WbBv@0GV zujRiATkuZLSkb1Uf&@m?;&C(b-&J1b{H(7msHvzRF{4poqi?c>+UBpcN58k_b>+!> zFV<~e1&Kf_jPu3c^Ka*sFz*cP+1t`8Do8w7Uc{K)XQt+RARSMm+N!^m#h#g1?l~nT z0^NmtL{pZtz%z+JE8KOkTkcKhuIP2!vKNE@7Gnn2egqOD zdxaP?;=5?$pMK|KpOvToRD$_h->J06Bm%AE-HZVr!<5WNy0VQqqZL$;*g37V(cG=Q zc5?0~p2oQ3B@|k-Oc_5%BG3x=@%UJ@rjMee>dP*_9Il{(g!kdHMv5J+w4&c%^E6`o zUs-AGb)0QP1X|%5x!qE@ zpI?PzKZdbRb2jal2v|wSHFCRU`mpzUrR8%N|p88a!E%83HfeD<#H+5*y#$3DtkpD(5l(I6ApuWYU*OUz|#m`lAhUS z7Gd*Feb!My0&igO`N!(?Eci=dcC_VJi9o9|#%V{-*7LP>H4gGLn%~XB-d*!!o*gU< z6(r=_9ItO>VVAS}u~x;?N(5R}8h6eyk}cEn=Z@xSTv3DBndG+EV-aQe-KEP8^4&zY zOmpy?rJecd;J%QkZld4O zDo*<$Z&<~<7#jxi9jpz+(h+Zdb4`+s;ffv z(REpS6%{1V_b(ssF6pK$;`I@Eq^(4tRcxCHqCU!7XdUidL}x4YKIK#U@%s4Xg>rhc zIikloTX%|v9_;LvvKh`;srR`*EI>sC3H07W~%TgUL;C*P|86%{1bou46kp8`E* z@uzybExkg$k215oM4%P=s;p)B>rw!>me*BL=Y;+#~)4~mo zgy(5(6DQTv=*&L-yY|-QXS++z$2|JGn~Dk&=e^cCrnO9K8`ZA0GY!AV^(e-OM4;8K z$txUfYg%mOp7n4Ncj7(7$z^&fWz{o{o~rQ{^Hy}dnhPr8)0+7h)bW$JAB74M<1?5y zvTO8T&EIp%x!;d(NeHxBR^GfJeZACnC-E%6UE~9m!$r+I&VfDI|9GlnD?9~(PlZI( z^AqCs@@sF`jrFL;mLvpPO&Dn2MYeyv>wKy#mwW_)O1V4cz1;=g@BYVA&7S3-Bn>1! z`I~ob>n(J5*2mP01(FbG<(qiW;lDezEz50hCo#@iRuHI^i{2~h`P_}18kUGzA6w>_ ztppV${wNkB+STaRLC!Qrr#D+75@_{l;a1U(hm=#DM9jQ3;)E#@Xoc&HX?$2&OM*Q^hG z2a)*XZ~8K6K8NCFP-{=TY}PpvXtgWC^pY0Yl6J|6Fdmuh3YGQ;Ob>6*oaF%2Zz=QF(sZa=crne+Bj&AtQ)w35r|YRzfY2zNys1(ceR=mJ)wI+;QHqZTm*;4`rtFJuf@>%_bsHL1IUP^NxF|H)!*& z&2|!T+NRA!pn}Bym;Z=wZI0PA_MtjI_<6E8-;RnzbTdbDh2bY{s|i?2z(MGrZhCY ze@C4u<*dP%7aAoY(5iA@(?9v_6#1OQnLn?H`ycdFR3G$IsYZ<#b?~i$S%a>pHJ|EO zFS9-{A4r78jTiM1xrs(HRJs{$W_=)mRw32QvCrT{SG%%iIxYIEeiJ7;*4&EL>ixY; z>z8_pDABV67imd}CD)pW`ane@PM9NrFY8?+fX_R7i#kUl1g!9B&3yPqd)3RzM`_qeIbn?Q1Ps2j{4G7oFd*Rxjy!u^h=Tk z64cw#?Dbcln7V!5Bm`PjKVy1IFY|P;^QnvpWdwoB`a`DA_Fk;Z|9Gm1>t;*DvO=Qu zebYz$yw6RYY0S@Nwss`Y3R}KeR%`w=p9)*`>P}{af`4xM{Es)PcN;V3_(mZ?-*zgi zUn79s$IQ}20v4cAc)Zydw5v{0vKnVeD%98Tsc2 zO}4I1e^ZQveR|9fo4wQgdrp&M;@>aUKdsTfn*{%THy7bjkf6V*?GkMgj(JTB{4c^~ zMQPLDW*RY{2ePl{!wdZNvARNvD-E=&Ugo;^_s?_vcv&HV%I^fFVG`}?^kQkE>J-RX zdX0hv{q3?E7<@&fal2a!o(2-A{7w*U5;yX9W*Y}rEzs)QIRy#&+hsNV-%E}i!DqB5 zY9F3PnV60&`=E3C>UFrRpn^oI1D70Uhab^G&iCWQ=}#Tlu7dsc5Bukqf(jDvLN7U5 z?l`C|ele622PU^?fwfNWzqj_jf(jDY-t3k)L))?4EoScTefN<>pjBLpi;fbnW3<)v zXuTwrS8KL&Ia@pK=qCjgBc_dbBWsu<~B{4OOAyzZfg5$ z4-;kO$`kkS$LsukfxpY&<#mqFjs(>Q5$28cbj{nbEN!QjuhH$Xf(jC;n_P4RmyFZ8 z_n|s}<=&cI8((8hi%Xv*0R|5EDOJRpMCg^^4lYCIzQ>uyOHTa3 zi3>HKNd#IoFSpYXxnv~~8~AxqPUPl9)FAf?m^xbNZtEOdhSqYXk@iZflHIjZCCQTq zDoEU)xz+L4oas)YZjrd1_78iwFK?MZtHwNbO~XUOoli9^#mSPlPOKDZpptQXwBzz$ z?Zxk6+T0Hd&jR0IB&Hm~`yvn*4%+3CoP*3z%E(M7q8%*yPi?Z$EX&f9B$}00T zdW;$_5okqy4E>VX@9wG?!m3zuvmT4qi{8hjAc1}~>=w(UVAkhRE|#*!HiPzUC z%>IhX*!*v}igi#S|1!t8*u*f>u-xpHH1UPml}QRa9QAL7(C-r*LoOtzyS8o*zD;nH z%jyv@XVyri?Yw!;8XR84Zb|mFFxxd;VU{1wLQz2?sOW4*hYmBt3Pii}viefAC>xX6 z%KZA2w<3X7vm2XfJZklt6Yw7!*&HQ_dKEOaC+)P2V9ztWdY%u|>QQlCtL7h*ZKDy-1eDOOaF z$ogf1V{xQMz}gwqN_?XWv)E9+LeIKcQ97a7=<VXoam( zCQv~F+n8MESc6kv%@Ot7cIf;tY&~{MCEl(^@OHH?PHtC7U<;LVj!%L)m!AsX4^)uA zc5Jsa=I_U)+@b8|o4g8IVV%oF=OH=S%kw1|E3{W?iJPwH6zz4@#wHq;2;Zp&0u>}K z^bQo|_NaUmp9%l?F-)Nu9z_(uIT_B?J04&YfeI4xIvgjgl!ofo z(TW6GVWdZX?)9BXq~HW9NZ`63zY}-#xK-z6mDN2^K?1EX(xct-+(~$F0u>~1HIl!z zv+^la=K5O?_PmL@=;f&{L;^Y_DXMxnmFZ^IHQ5@;o#954=> z#0*ZLf&}ge{QTCc^gTxf_a^X{WC9f=@SKB8pn?R(ujZ$a{2~T`8j#coC zAf6pWPmJ8FW27%D+a-k6t#e611qnQ>Xt&H`KFlYv1dIQ1TO!a3y)W_^q<3mIW@s?; zI`LLP1qnQJ$uY##nB?qp$!Kr8gu$m=}aC4Iv`nb|X=xQYrAc*2-RkW6-3pSZ)q{!&9F03LTsW1|{ZlL)lJ^V@dIu^IdITIwgg_SKpa zfmZ0@(QfHGXp^44=ri4WTLTppB=8(MpF6kPpx3-~Ubl2;JB7ClP3cJ|X#ysu$Ds zuFt3IE#`MtQ9%OlH*mk_0kicvqxgUU6(=)d2EfHvi_f71UyBT;I)~tH`Zv_kXxQfa>G*&P5AZCrs!k z5om>8B<+?GUB~GA0_t0H{@qVS1qr;zWVh@qGFCse{MJ_SX#Mf~(@M`Y%_Rb@&{HMvul`!6udQ=Ssoge0MFk1G zugLE%P1&t?P~Iyu-`180v_emncFQ8K-TK4u4@%jn+A1nY;Jr)kFXIB!$E@~Je;m^! z0eKr8fZX}A3G?Uug%X*RWV)50n$NZ?&tURG;f=rt4as^t>1O9Wcs4PbsF_trZ- zSK554WvstMpcQ(<Y>!Rph*I0Xr`!du^b%n;?xf=ULfU+T&PTA{~H zK3Zy)nN1i|O5K0xh=K|dczfM$$=dS|=5H;j_KVpk5ojfQ@_h3#C%+3&O1)8Kt%3>? zc&FZOsjC)Z4eJK0`A<%j2(&^kqTGYo)Iuz;Pf4|2rwIxwNT6>3zRqx<1iMsrXl&& z$f6&uNJ#z}B%iV;gEW@+eLi(;y5m;#5Q_x*gWn$=*RBC@NGd@4!=TA`N;KK40T zlT8`zubwDgMk3G(J>c@z?p~h-{_U&kJMSMr1qt-F!M(SZXv`{QwW!wJn-3y^R_Mu> z`y+cD!O~wzsgBLmG!_*k&t=q%_Rb@(Em2Sy*$4+EBQuK!m9PxQ9%N|1M!=z zH~O*~BeN)fRO%@aXoY^n?H2FVecAcg1Z$t&BXm@dKrcsj%amIE+4RS)tn-`81X`hY zbGxN`mHupX*GOxJMI&@nkU-x{{O+rJe-`ke%fa{6$4UfRp+_kGHPHLBk_Q4}&-NcG z5om>;;JN>j8vWSV@*858Bp;)rf&_ZDvRgX%^kWmprP0%k?<)~#g&ytgmWjptviH}r z>dR6M(osPIJ#^VEsqgh-w~a`BuA_@YpcQ%nL&JL>~8wUP+5LeKm>epJsM ztXhZZdit1VIx0w@H#57%zkGK#`O^x$bxf#4pcO_K;5+UIyRb(Kx9TC!tU4-4pjS4& zF1qng7L((QZVAgG5om?J;`rY7@;}+jM`!et-hL8+Rv24>pVB+fhRuk6st@vwk3|Iu z^k8SVq>F6LK9qc?XYaOGBG3wBMDTIqj^^ya#bm6RW|wx26( z_Q`_mR?~PZDoCIgNj~Z;T!!WL2xPm8mQs*FEA(y2*JSRMW^7AQmbgce2(-dDEqqu1 zQXuna6Ux!&Q?%C0)370 zd6Xk3>uC*P`9^J)2(-dAS-!%OJtsS6E6L`a-zgJtZyVQSe@2|rDMnSoxH3lJms_=c z<)>>HqwIQtG6uzo3!XHC@3?pV8b?vB1c3??7y*vXQ;S$zBEk_8XqD+;Y2&H)2F){( zygDL*3KAH}?q>urMlr*XK&ynRrHy=*)@h;B=w1=KdWE8l2?7-)FxDPllPP>5bW`*& z^)L6*;^uwRKRs>avzsD<)8C%a?X>cJ0}P6>eyJzfh>9_DG3-Cz=4J=+vR1qqD3E}rV3Gas=?pcUQjr#zYd!g)S^5~v`7 zk+enWD$deXfAr5L%Kb>xTW#jR`ZG|5+qjsetfmXQ}_=~oCu1`EqgLai%1S&{e{=7m&MLgGsJgf57t~gt}iUeBy zDB~~Qo2p02pFR?(AThJ_3J1MuRgW&`1dkb}f9zC)@hD^LaN;^gxPP>E-D{4P=iFLR zRtNp&X|w;#ZqUCMUrk;kkU#~AD&Mk;Quj$qe$_&5Ra92J>B_3dty5-r`WhZ{{?!7v zJ<@hxoGId82XdbYw;TBw=l0y!jLc7eBTzx2cGL_JN!iFuZ%bfBpx^!92vm@m@h+pW@!>08BIL`0=R~xg>82b@h+j9Nhr=V)V%@GLP_V*SR2_QTBNbAUbyB{B8N!B%l*)A1S&{; zs*yuHZKL&!6J|?y8&2n=HW5`k8>6Jy1x`{!lX@|^GXj903BAE4Z* z+(bnM3G%`)Yw1m=e}<;j6P0XJ2PmzUhf4%n#pq+iO@i2q`F6dv5gvPcm*-S+RiP3&z#O<2P zXX&Xt-al6=ysH*l?s{7l6(rCr2KQ%KIJLT0^U?3y8%YFO<=Znx+{bxwhf4Q&qd28l z&P@7{8f{fnkU&2g#93l$Y4 z(2s`Q@=sPzwa>`n|5zBm%9dwY%<;@)2?}(c1N<`^?tvQjkDD8hq5(^trOV`$@go z#hRkEy9l(x8>ifF?Es6?V*7EuTb52LDoEgsQ|^_E6LwDg$q7`Dz;g%OEAYfeO3Rsd z^tkm^B?7JRoC9CWfS!SV*VP-~ZRy-A%sbWHm(tzWo4f8PU;p~1^tv0SqJjk8$o_8v ztzKKro8CPx(kNr`x{HdZySLi5Rv8r)B=Da2e-mhRE#nk%7yZntMLdnLpySHse{!jN zhZR*(L1K3v^N#z+jC5NT2~?24yYKw8dfRd-NT8KhJM-qfXCQsgkw66r zyoJwCQ$=i0)@4=IK4W}UB+v?P`u{h9R$YS+N@2hlafFrLQoxFF?C z=O}eiqrDzHP3DOdT|>` zzFtxSfeI4n_l570{ngjnwBS&+&FV{5B+v@SE8IWz2$RUa@{$!5Bydc`TggCw{qU-O z>K4{kK>|lLn6};W`gvA8YnOiNkJlX(RFJ4~XpbZ9{nWM%{d@DA*KKzs_RY+mYGmL_ zi9jnXbsqU-m=XJ^Qcrb;WwlhgNIads({cA!D%#=k+I$nn@ieMe_YSqcj#RT0{2>u&CAW5df=<_( z4^a6jIxCJ1$w!LI;|Z19uW=$0sK~@&L40l8kU!OT9+UiPf#HRDjB@T(<(E&F;W6gl zUmL3$(qiMPi9oBhRZZVJ$%c~eot*RI^vF=_48P*jk>a^w3^XUB_oa8TGPi9jo?8-6yb z#dDp;KEp25Wa#k(eYepKw_i7;hj_fz`D{WTwznQb1qt*n$4?cfxvbL|?sbzU5`k8D zJKJtKpT?8XIPr3t`V18$&`Ta)b)I!wr?KeM&MhSZt?-sPzw^T*(9n3d#GHl<6(rEx zA3sAM^H8TTb(8CDBm%AQ?z`QRdq66t@^O3IyaTWR=4`(Y{pPQ;t%fUkY+`Dxt~M6BHaF){$xX4YizTY5`k8DpPNSpT9}~I zJgVi1<_r}i@J2Qt`*e3>1NdCcZf(X;L4rz}=7ST3|HS4k-|I9VtT&{IM4%Pk;pTgg zbH3>`=bU&poS}lmJ?=k|=CyZ(|3t<6nNIWC340ny1X{^=!};8qT0701Mcbt@HQn?? zf?B>g)+Yand=#!b=jRfER(Fq^KGv#_UBHQd>Xchc$M|ua*pAf!Xe7&6$G@E$J z2~?24yIFQi?a7aHnoaz*zA8fn3B3RH-}%62N6&9!ZlF776ATq3e2SR<;VSJQ|8O^U z-T9vcTA{BvyJg9`i@I|*!B9b>P$Sc$+=VFeg~aC{|C2x~^fbrMo(CP*owEss3KBJ{ znEvPrZelzQ^*;!-LVtAp{8ih1dU10$!B9cstH(*=Kr9|T&V2RpmvOvDC# ztvQ=us374x+w^l6S?V57_Cv<0RVsMxYgX`{U&b7$Gxr{jQ2saFkiX3TZaFWrRsk^4VxM~pYri<`R}pDqfj>IQXz&462rYe9q#Pq6;S13A5Nr+uc@C+R*F5@ z87L8GRXg5;>PG!Hv@a)C|I=9iUM7?ca?8(9LE`TOcSqv36ak$(^x{OeakKS5?y#3+?u~`mt^b^IV*jVLfAusn$2G@SV{4+X$Wp5~v{Id&^t2w{x#XbE0jO zO%LUDK4WSDh6Gym?V3UmWfza-#F>FMeNv%NwtJdPpw*rf?&7H)MbkUDSsfMIKd>~L z;a`%Wf<%W^@2Hi8*(+83W;~tL~2W6v-b>l;M4N^&LeI5IiB;2ysS_`0_%z2X&7MBhw;+g!ru=h z&%ztIM3|+_rv<2PaTLV(ZT4!gEW%O4L(R8XM{y zEJ0}?fmRvE7ZRR(W}ctvBw}-J+DZf}NW>P)F5ajN8nt#xqifh3`l?9;TIp^Dg`c92 z%cnbujZ>O$BLWp9I&RA&yaUaCFxg35i82I%3KA9a=M>&(-d>o%i7s(l_2>t=Sp1q` zhR>erQVmf)+qyU$NZT!=^RCtV`{ZV8&xJ5lkhnQ6tH`;>9m@Io4a4+$cXP3P3%Lgj zP>{&%QBI^68FiEsx$4){Be+MPadpBZ0L5ehy-b8vQp(V6HZgNKz5e1}4M5HRxi&#+p z?>oD*``skJf1XE~=ic|TQ+MW_*_n4rc@I+@Gg?OrlpwKTL1h_7t0pIjIQ?Q~^3dEB z#BFy|8-ZF4zAYrkOB+;A`q&zJWi=7p65E$)F5^H6wtRdNd9EiS?yqf$*(p_jXK_oPV2879^{;Qz zM0ylUxV$~hJIJZJKB8#Y8%h;E7m0zFn@ewIc^}79sxA@XKAI~rGsS@fY7IZ$QhG^y zul*h)AAfE>7|L;=WO&&RrT4X3CHJLC702=A@N)@i<3IxY80tkU-1KG{7P~PizOTUc zn)Y2`pMdn9TT6lan`62GF2)L z)Z&)Uzw2)l6U8$-HZg&cv;@bY6YFD|dFMXewsCRs@iu~KrNu3u*qJ+hF1; zaXpgRQ(>pob^^8Ll`A3lOk}^rapVbFdZfnbMHT;jFlmAbG0ZZcMH7Ua?;N!wCdy9>C4HxE44Bd|Bvq zz9DTZNlP$owZt*;4P@KJXK`3*ajR5q+T+k$yH21aEy1+@eiVyXlYCkmpY&zKWP!O| zmAj1G^$>eRFDsQNz0NcD{5B~k)j6ge39b(&^!hkGq)#$?B>iK>WH}lr2@-e1O2{1$ zDHplU!|E+NQuxp4Di^8F@mZ+Fzw> >BxRZD_I>mPnjX}4v8LEL=wUX=mggrp-- z>qyPgzRS^z41zkhSnlD{rfN;Q4JoS|k1|qarI5aJJ~#nT|j$dXi?v5z%Eu)v6X@N|2!V zqEwz}JYRSsc-FLMG9UOX)S{A(QgtK0lIcYy>%)>DK{e}AHT%B_)S{YoS#@qiAnSuA zKfbkGmPxVbO)CGFKPNlq`BP zI*NA!ay$Ot1WJ&|HEv22?*wGpURFq;*8f`{=37KWih3?R3EmGbRy{AV8cL9$6sD*= zrAw9hEYzYBNl_)@ar`r8MwPY8g5*1h?;6#I4{8wJbN_^GReF69nC`7jOQ^OWQ)T7@ z%LLmJmZ5GZQqHyM*OpXGoqXTLyKVylqK`h(uDkL#+$Y1<@2S+ov1Q3b?!mi(NcAJJ zqQZ4oXmh3&LAI`T^UaKSlw?G-f90k+zrHoaS^jEe;a?P_ttnm6{bu+T*%|@@o{*2! zGcr8y`w(~OnDg$2_aoB~C_&=-rw?7bdmVMRE6T)A6h}ED4kS=3Aizm+aeUDRp;Cns zBz6_Q>#F!P$^G%NCR%kvjR3L32wi z{o|EoOe|Y^VmnGQCb-VG4IQoh`o#k8@Fl0?u&l6r)bDz#6tSVCHxMOAsIX4lub!vM zkJ5#Ai?8qQy+}uU|f-i#DEJ`JenJgWG*gk=L} zrz21+u2Xh*wF@&1;(CQk3H9A$C4mwoxX0krfO;HBIZh@N{UbIVfm+sR$Mq4Jb526` z9bcv+Q0s%X_gtIT#Ts#x_0I|J``va)pacmVMLmvtC;Nx?p0_t0fm$5q`;zs1T;Cd{ zy)1euh^;=f8Tbtj-M?PiRLXu(mQ0k^r)@h)pacnJ87ONhN}HW!_;!v1B}iZj*-nho zhS$y)#so^z5>`H#2x_@6^r%572@-4iuXJtbA7fgtQQE$t!BwQa3lbz+uUhPSesh&! zy>eOA8XljHKrNgnvSmrBs`Kk+Js;K#66Xf?RAnEPPkWG~ttd8nU=lu;d%J=`UsA25 z`t5P#ip`}hY}rBdYdn>{fmm^5Ot3|^ZNPnJ+2tNFvcMc0VbQ66oUyWp@n42c2wB7s^s$ECBD7Y{lo92p|cZR#I^ z5+o8X6>%+i!#%?(ilcdz70y=8Mu`;d;z1-(D}T$kT>dGF_;OlVZG6^u#FN-N5m-K0 z(mWDvKeSqwH0_fa9;`hIj}(EgdL^L*32ZUsC$(U0ZP_pN#c!pi#Up`QDy-Y0lPiDW z?0I^$nBVo>A^i}@x@3&_rFY3d zBv8xxol7^<<`}K&hLK*UJAWk}2YH6Y=~ZE#ZLytLb9RbRR=o;;qKRjny%njfP=W-` z+h`|8$BA06qJ_LqsXmZEEw+~5b)GQdINNEmc3Lb*E=}(TN|3PI)IAa>YljauP7Z1D z-wq^D%L?=RVGxn$I{ru2rJDWnd#Az?N706-lX`RlyI}?ekHcFpM*D95Cr)mONTAl$8~0pq zeR9o+@Vngwe-8@~11@QUiq;ntVYc%{ElidgyUTa^^S z5+qbuCn_E;E%>~1+teJ}aEwx6*22-w;|Mf}kNOv=iIR*6J&y2HAD?&DTDPcI6;}<^ zjFrb%^}C&L{a0uwN{~=to#1&h&4$JOyFnYd6xF(j3bU4)3G3?&iun1@+Z#}l5uwLH zt5Jf?V{OKvb-|P>AFn#`NKclmPpx06-|a-ngv2cj&uFY&4M5~^pPS|d@v>#0)2=XHMAh!P}JSSPq-xx6aH=dVfS?o;bK zD$H6`hCXW*M-hkL@@+&(MuZ-RH}#jDWV4I79lviQOV7t^GHk1n9b|^B-?^-&j!Tt9 z@K=krq67(Ql?KhkhL9FC^2dq=_u{vsBqPGK;l!#rZ%f;UMtyU|l*4K*lq?ya&At*F zwr^3^k|{xgMp2*5z7p^KxGWukTJ$#ilwB(AE3vv{4`sckt7m?r(i2;<43Y#&RQ)rd zO2o?f>1v_X*@i;`wXoduRJk7h6KYtmX05Rn*rkvUvR=ii@oS{@ig6&3wscdxYF!lH z33_XN>Yb-~qTgCcrS)BtKnW5wiu$b8+W$qM7Omp?Z7afN94JA8#z&u8PnK)184{>P zt8IR@8f_;q=c-@fTEjfq36vnA-bXdZvC0Yw)WWx!ZU*W!S)3ueq$JrTm|Ggpead>l zJV`4-oYW=?K&+TQR%;(E+t4L!K5xR^NMGi&KRXBxtq8XRXOJ4}K@4 ze~Vw!5vWBgIzDTqX#IydLW)Oam#M-yP-}#rQ~~#?=#P%)ORlIG2{xQ;1$T-^{a2<_k-t4Y_X^?ubpr&VO!-C zyvoTdcPK#uXJO=3ep*?1+XvkD^2+w}%r$qQ#0p0o#hpBIW!uXu+r8w1^B5UoJ_ zY%AN|BDAtyhAd>1;B#pe7xwkh%C?tRw*RJ;J0wtx)~tQDm2EGtY!4+186`;2TCdMq z*`_sQFRvj_CtDOHNYMJOxo3)e_8*+%RcpvdpcbtTr`Xnzy}X9J;oW+{C_%ys8!LB$ zSMFjuR5M#5w?5UbxK*kZ7JW5}S8$m4@rUm=q9h|ix58;vjlJ(_4|W{lRW)-pN`+Yq z=WXn-PbX5>M%F+{MuZ**t@${4B`5jfp+N3G)C?Qvfa-TUk?HE4iYP%sg>|As$`I!_ zPh+$~RlFN`XI_WM%+Wj>RbkfR{eB}BWs26zN)b(ShXkS|BSMcut=#SY_Uo;@bCtbm zvR!-NO>9cPlq@oA{myTcB8oqLKs!bhk?Kc+ckEp}#55AoqdNwpBqKs!Vc}B{bOJ(q zv88A&BBJ9QX*+=uBvgs0wI#XdPR4-*YGJwQanKGrNuY#hUf18w zCu8()ZT(T+z0Ded$t#TE)Mu?c4cNrf7X%AC?tLkg$(Y|BFB^ z?zPnjN4E*44Ds^&k-Gk`KrGRW>PBxRif|rz#BY?WHI^Wu!a70w6TMuzTvn8Jmnst~ zD;Jb*l&#Js!L5W!*OVZEZA{i6uXw4_H3`&GZQ5)lCV>*lli5pP-2XccBq-<5ml$h( z=~6|t=~DHKC1Q^QB}h{*$haa7FkiSH|oFH<+UOGn`z&Z&43al0s_{$b{zbZY&#xn zJ4V;g?u}}s`N?KL0<}K5w9%Di)hmOTPv7VZo&H#RMm8Ll50*6953a)9d3PI^)t)9_ zI#)j(sZ}655hY02TVgDI+wOIGw3ew_t3V`BONI3~Qt12n(qHOppOYnn`M|W3H30kJ z)^!Qh`fjYJ<)T$Jlpyi`=i6Mb$NXXBeAkEdw2iw%wSEOpZA1dKtlznG!#)_4u=iSy z1h(|?O~v=z8drGyh)=DQ`1PdT#*QsD;>hnfW?wR5&;nP_n+0UAO*c->{5P&csTwu( z*@Q))7N3q_U*7s^)Jn&^?QI)=NVcv#&0$KAxRlV?we^=m1~G0)z3usq^h-yeR_A^v zqW&ya#2`k_ow#k>iQ$q!iFb5`gn(FG%bqIFzVTI?XB(C-4kWttJCS0=k?UkwwXV;5 zr6W*_Qs}etk$wEElKC@zmX1IzN|mscRW&FpQ-TDQo6lAs!Kyy+S*T^%L|o^~11g0_ zweF?r+_bOkgf&uo9Dfx1DD2}Zdb`4B;pmLxJl!|(G)A!HcXU?xCiUj9{ek_t`dxpc z6mj-OJ(FMw5-O|{k=rI}If!RJ!%3hRk9;s_pb?$C&X1ZvS(;a) z5+tY(_E{qi_3eWD_C?&cg9Hiccg;0Yx?#0TsNlZ6B=zk`pceJoKC5rv{b4<^f%^8h zTNmAk5+tlJ_w8&|@*A}{b}R3Vk}Z*2p9*uUr1!&|0a!$3@q%YwrX(XmZ|(Gc2!5lY z3kL9vMb4Mlx}$f+oK2|T?Zn$fD)BpL&ajYBVV&SS@VkEGvjOq^4(cbTPRTb_oucCR zTt6ShIafrEeK#ti#G)f)zha%JB7&`O(Ij8Njhu7!EGrx#^Q7W7?RK%W^eax4ah~gx zEK&7b>vvuqto7Gz@2eI~1^eZB`+vlt7{-1y2ee9H~5Ayg=UEM3g zm|BnHWrZT)MXF3mOqUNJcpcu_u|=ucPN}MvmOw4c6TRm*TZPx@v58MX2%8qffoo^v zvwvz}_=LexG9M_xe56z^=-*WKu+er)4Gs+du2hBe1ZeSkyR~y*Ww&bKBhMe_a}xq3 z_*~qBKxg$s-cJZfTp-^llpv8eUlISzRY#3fCG>qOc>UiW^Z6)&1Zv?P4Ugm9pQgth zUs#gQF4@WoO99s}=?;Yz(|4SY?j;G7AYosDEK}xYaHEph(-Ekp)=Tvsj@ID#FP2lq zmY#mn_Y~W3#);)qex4QS=eBvo`SZ!rgoByR%lbfxoxr~R=)a#OT#2bDOBV_3=dax# z<$6~1Qz~7KL#9do&z>r4RI{hhPGCxq!10lG(6tFq*x$UiOcfHipBhu>any0S5`x=Q zGxA~O5DDDBO*b#zc`xBenBKSJvrr4iM~`D^-!TcZV(Q2^uuSf6tmN7`a-u9BEJO0r z{Cl&#W!sHRSGq{>>`p)3&!zj{sd5QnKQxyFN|3lva7)yn?&FO5$W_6!cjD5H=?K({ z9aAVFARxvdHofk=;X&pO=?K)~8JU%;+!Z{b5Qiy2g5pcDQbqX)gM63-YEhnis@!B* zg{rc`*inm0*EsvmWkvN7rs@MFNKifbthO6^D|UtSyUp!F;y@cWL((ekHkVfnwzVx-C

bFI;c%^1-oF%yL~DNZ}L}6n39YMj)PK_%=u{l+0L|t+48LtG4dgaeSa>aF3;nh%rgcUXjs<{9WKXEYEhZ%dub&f=% zN0D(B5pfg=E6fDV37l$ffNvDG68w&9Cs2X}TAcrzKrOpvX{_RSH?$}&S8;TYAaWjM z&giJ^8sjeKLlKV??{7p2j$~HYAb2Lsaa6*$7T*c$cO>Gj}=)F_7jsjGg^jfr9JetzAK<jFg6!C50u7N1A_7Ne?an#UfkQ&b*apx-Td!_c~*LREYJ7~-~HJ;a^1PQbN zErLmuAYr!?x%;I5=r2~V)u-==ImM^8jD4u5{MP;%=&2=v5+rc9slDGt0<~~Qt0ZDM zO`IyMHLEsH`7yO-9IXlx66N_zw-d_-Yzdni zwM5Q8P=W;3ES(0-wm59{zZcRGs1;kHg!GQD#}PkiRao53Rgypn5{G^-Eq(UuMC#jX z!gBrAND?SPVpM~2^6mkh=$ZH3ur^ixmg6}}kiholam<)_G^}C8I%ILhN{?B(r~Om@ z3Ge0gcV5We57M@he-|AJ%hk2DB(TnrX!>Ig|1G!r#xiBBKdg6R({u!CS-al2CC(nY zGwepkk0pT;BsNS6katw+IX}`gC2V@O?&%2Bvi9O}9F2YJ!n{j9l>|zV_+Z`>-`$=8 z^!jNc7 z@VINOQwLw$lhAY2Nf`%9(h}^sUQZQwRP-PH#R?n&tdZfAT=&!W@^U_OZJoffLIO*j z?_~7|y2aJ`O~Dq`tT!s+s1-gXSB-vrjj-rbe(NpZ zQB4ufryHxGBx8c(5SQBvx>Z-}mu-uUuxRc}tR2P{5tL~k<=m2FOh7(_WBBfEI8I<$ zS-(R*M5DF^s-Xl4E6l{^H|xnhjMc7QSqrH;<;PmX)OsAwot|JO7R=AT86`+qVUD8( ztm3dOs`e+b{i$|Pv9-keOY{|%LX8T>wK??%r%cX@lmrQ$A@PnR+nS7QyZ9{BVjGV4 zGwDR)qZ`}Dej6&|z*Hg4wkY4Vr4u!8z8mZ*Qk!#6*0?1>0>8J}-wz~E3%{PxElP{} z$7Q>^We3NB5~kJnFMJZMg?BoV(BM#!eQ8tGq;oAF-$ll0$NYI5JNFe0znH)CMxC(g zBQ3!brrWwnw_ab?1{?WrK^155OyQ&&>>{y9}2R?b&HSscDG-|gM1bd@A6!FQkO z^-=ucyNTsnTuW5>P!b}dAP%$KJdPbZ$|No|QiZui0`ruv?Hc=&bRQY&%dtJ@T<7|q zd_T-Gp>}NUTcVZ^J8S5Tii0=GBv6aqF1cGw&$)_$bI!9k^*-~OrRojr1ob7s(3hCc zLW24gx&Km+gI1k`)v7Z-3$^U+iu#gjsxLtTwd`e8^RGT4zQC8EJjcP&nZs&C#_!QQ zE9xWGjde)^B}mwN1}1(!Q*kTDVJEDw(H?rwA+<;x>5_5c+ zEay=uL4xL2ZoVx|CvN2|lX!JR85svkkf1r7o9{l;iKa6PC-(HVl>|zVpqZsx&9CTw z&c#&{KR;eX5-34}=AYn`V{5*)iNVehbcceRtC`P2EwW?`|A1V&*fXF637V^ePmcdZ zpcb`hmwuA<|BC~)$nG+HtN{_r`9KL0R{aA(iL(=^WwjC}KCTj-*rtze>6683_!m6o zrx~PM%}lXG=+^0o28ks~UXb;H5+rDj>sGU3^0rXDaiVr|UpfM{ux4pxB6qFC^|c;J z0-uX1r1_`2+>U>Yc6H@>RN}yIuS)_YNRYK`cthlzKmG9S#0R;v$Z-$})S~&P%bM4I zQCUkYz4w)j10_h1Rcm-Ed@P=W;YiN=0W&d2Sy9wscwb5F*B1ZvUj z750mA6Quv>PwvYw(@V2EqfeCcL5M@wW-3bE7_F4YK{A zc5Kj24%4q1t-ZqzAtX?O1nnFFUtxv6Taq}_*pY$+YSB&=m$ly~pvL`#8pf^{lpw*a zN9`}7ogCw`CQhrX?#$FthAi*QG@&e~^44WF5_FKYi12t6f3%d{nFbeppXyPm!5J zB(Oy2HmLDM6Fbh1m35BwLHoVjYQGn@1-TBV@ygwctEH-QYgecEt*~4j1fR>a)+dQO zHRS4`Dan}NvDWy)t0lgatAi$iR)%HOu%)kv)K}|y&C>J{oiV|2(7LF`E2%EWHXC8l z@@t0aLE9oe_U_=dS5uNP!ErnolgeM*wQYm*?XoP32cGIID4@OT4#{n_w;Yu@_Vg8Bf)X7T@qNTg2?sUep?!U!BltU@ce`CgVgU=OGB8F zw1lGVGe~MnDP3C(p%&QdXiUDWzGaPu%ka(mZK4KE)ql4WC_!S)+WfAhTRJU?SWX%D zi6}v$`nGPqqVJ5A@!8`*0=3%s2D)l?<$Ywm^sD|~ONvS%ZM(1A1r zYHf~Qk;;s{}m)SnE2r~|pATenF9p9XGpId~P z4XD6?T_bKgc%1)kZ}H(*?;i}?^aKn4Rg8X~dv1g;S`9K1-wyh0vudMI02r~|p zAd&ySs{Zw%-&lm14V|1-6WCM`qJhDB}h=s`bU@N8!FPqfdpz%i*bjn;CmX<#(@$f zsD=82KjAaHY2!cwwW#I0%Uu1_BFwTv2@>?C`I~*pJ0Q}=fdp#N8|+?MfxjrG%?C=5 zpdQ1Q_iNsrkTwn^P>XsZSA%xhEW*qON|2zQ&zEf?f3-{-2NI}7J*sQeb0Ew(P=W;Y z;J)c|`Fm8_IFLXs8W~)feh0#g10_h%*yVfqYn}|^KmxUBgmWFPk=G*3I8cHFji|nY zpRostwD~{+wP>Vv{kR(lGY*s>L6$*Er5^b+hyw}K;_pUPKUT!;M{UGAzIx8UJURK+ z9@B3W-B#frzw|b>@aUuHRf)bdOl&8r{8ZI>_egfW)yniEiUi$`LHBKQUn0-&F6)Z=(>c4rY$vu~txURDhyNTA<1^2+t!d!p)(bDU?NXV)+#m?yeV z!=Kuj_hNCX`kZJfW+Z*#EO0u9h7u&ur;w%az5 zsQYt(uXjpUBaR=+mG_Qb-&RDN&uJr23vaCCQ*^%Ms#7|O(ZlFHhx;;-;F-)v(VdMr z?tRfd`ON(eqU0}BRzRQ@-dO2z4DU8*&;F!NLTt*ep#%x;uWWHdsyNaRrWW2fEX79G`_+ysB<%S59bG zrUVIUV=mPyW!t5GU2nViEYz~!m2&^VQD@t}(IRi-Y~0$-yIqiA{~XHaw#V`NgQH#` zz$dxonHyU^d30FVK~tjv_3h^EhkT=w8Z&5IF>gzhBaQ|mjwwNcZ&y;|6@8C-)J9XI z8a@lPsGoORJ{oEK(A4;W5+wLOCfoR-sqq5|)S`agym3yBADS9JP=bVhgA>+?LNw8b35OexL*i>gVAOHyS@QHGZH33BD!DHhySo{6GS=sGo;B+-UsJ)cAoC zB&g4K+r|$~jUPy$7J59A*sYWzS666n81 zS~8l;vSiHGPSy+cnmnV6K8o#_ybf)--00okCX2~M;zAc3nni1*+`o1`YV}tb=5er4 z>$8zd_f8V6-fR!GwAe3>>N&d}op<+_sOqJ8H0Vl1^M|)0#ehko^Y8yPu(YCWTG)5d zSN+uH&abnN7jeIzNJ0rdH~7c;u04r8j5s#z8{mA=a-8V)Q`3e>pceLB^nSEl>|C0i zJTCUidl)5148AkT)g)72BaYi;b~-mF4-+4iY2OeD)WW{Yly`-u6&gBv1&TJ}D%$*9N9KbrOy)k_UeLJ1PVNj}$#IeY^( z*T)C{IJ6Br+lhbYE~omi`d8Gt)%u2O%7#e>AtLf>xv#Yq2bPv>hy-eJtF-PuyKt|( zHoSB*kq~=62_;C3`|h=?V4>+o9J5acX*FY_L`d7I4Us@CyxWa@F1HWUe%e`AeE4nd z!ze+b_@vzKWs~{#Y|i<+F;Uuv(?McGc#noipceNM*6nqhDmK>MObrxWhu=Dc5+sJd zU(B6z4&T(oaio@Rqm}(Ozi2$+p9mySYtEhu?t`UQ8pPi}w$=7UPpM)eEP0&>~RF-nVaX zPS&!9e-&Bkc&-L0L1O)sM($y^_`X~&-TZG)(5k;*+u8Qft3ybjR_+fX+}r-VVGup2 zbjKWT=Io!?L*`uBRH((JZdpsgo5yIYOV4uVsgvh0N{|@SGu*xJ>TgCIQ>k6`T(r@d zGH$A@LG>)uLaWB(_~FtJ?Lo_f&KaLwOF{_}jStsww;A)yh@;txzS_i*H=R4$m6vT- zSzf4xHj2m5G^LOBcIqAHzvad>KnW5f2UT+4IQ+K}$HVCzwc?9&YHy0RFy30-@0jW3;fMKO~_9iE=&ix(;tGY{b!tdW8nmJ6!8LwILFyh5iUVjx6Q#Ip<86sFiD; z`!Gt7I1@J~YHmyRTFCkMh5Ex*)GuCI)x9ASsD)k&>E!+Xu*jP!leLBG{D)A2L_k<8 z@2$xyj*3&;C+FKdSzB`MLIe`1_2<&%DGS|-cuXU~07})DsgVtlKrQr)=y6p2v6#16 z^@&<=$lpmQLE?+6g?*D3tI};xBU)W5-8{{wH$(!p&@&?KVm%b&)!M~q{^M`B22Ed# zNL;($#dmV7Y9*i1Sbl_B$;F4!T!SWoTGV>L<6`gmJG|jx!?iv=KaW5O5=95j_w9Mi zJ~O!np9fy`mblbkTj!W@2np2MG<%OPSoj%uz+pZZqYSFuCdQ+ww z?E7RESKjNYjkxd@zqO_WiLs^5`8sSY$dTnD}dd zD{W_&RPKFD2@(M`A3m}}2UDN;@6)%r_c007qP`MVISYnW6CX6tv~M0x zZGaLaW(Q>RU;drfMLCWx-!v52|EZykKGvRlYV%pB_1%yH{*nvSXrPe=zJhw|Yh#CU zPi+#YMdJ#r?>ggJi0Z{kX*DDB^N3?gka#+~w7>oD{5^_Ob?V2qViEPd=UPtY5yvD@ zi$+vf*`BtelL+-@*RFgR$0MgHL1JL7%Kl3ZHP&`o(N}yyeQ)VDuXuDe3DhFX09NRI zYx;^6oo+f;7JJ$NB}nXTT+_ei>QiGpKRj%x7*l1Rv&q)2Y-O0wLah$T;rk&|1^ zX3Nri7HTCujr8wrsjSxtBPWZ$sJG4^SDx)elRz!9YW-@Jf_4Z!oGdQpJ9gw|+pBCL zn-V1ArbPPt1}Q5%GKOwTn>IE1ms%6)t+fc$!W)A;jx&`ei5)jzCZFt*`!Gt7sMM;V ze|BCqo0z?4f>@9{(0jRIYy%`v3-2-VIDQQHMC57L)tjr-jU<#HaiVKI|I<=xjS09%|JD~%ej20aCJ!Z#+I4e014E>d!=X}*$*FyNt!NBwp`-?OUBkt>PTo7bHHJ9VMoPPHcz- zYT=D@9>?e(%8Mew&BVc!%Sk9fqGRnFzJ|-xYEwQ#HUyD97YKe2bcJK-^HpmviF4Z?U}f z0A7bPB}nXwTIXBv0@nJxp;!Be!!N5#D?>dCwd`LrjwSlN#f}XYUtbL1^+fZVAQIu% zCi%Jq^flVwIgp81R=|7d0hFY~5Agmu)Ys zy?S}=^^f+=cwN*aP>a@NebzUx;C0Qthu<16nv6Te>$|1|i4tG@k}|6#tRW|RX$`q$ zYyhwCngnXm+A@3}+*9mqa@&MSqC}3ZyiRROGA7tAp|$WMycX^#w#`OZbl0P}M(%NF z!ykm4cr#gaT@p++XvKkA^t<`3kk-Qc=F}RmeP5jIFqTitnm%yxyvb+Zd0inV`K{$$ z#aH9YYPEA}V&nQ4FG`TW6By*zphr1v#(bxUZ#LFOpw{W4*IY+;FEHX5Ix9?zURYh6 zNQm*G1PMHWL0{q;hHIgJRTG=)jkOV|wSV*r*YKXpjW~K|Z>l}JQbe3R7UM+;686&^ zA0;-`E|xAL4pbRyBTy?(i)`*s1@AWGvYK>ntac_#bn>yUKVc6|^sBz>n$M4BuX0Rk z;ZAA#UVC=57WS%uH!1rV_RwVR=0}3hl78HaEdY)qeEKMD-LNX&1}7)j2-LFg?9W|s zxc0%>;ogW5qr50Vf={3J^Yd;)j-$d`L$qCW=6HKQo@gUb3(p969Mubdq&3*G#e3)S zNH0o|;9bT3u6(;A$MNmBKH8RFQoL32PO=fGg=Y`QqeIc&+DhNI-bX28y(mEOaZr7T6IOlrReexNo8DBZr2VRHWPJ!#A1FbB%FS=9525M)D@FJ4(6Le+O}&qp zPQA}=>U~gx1obgKtM?f)D@<_jbDDY|Bv6Zb3GjhUy^rADXDjtSC_&=D=oda)?<2VP zIY+$@5~yYOfjua(skmVDJ}5zgdLO^l`&>@=X>-i*U_2jhFydgpGpr7Bf?mYq|j&O39M;&!Vi(XF91I@j3I{+09&gAydvIcTP7 z@5jZ68eM;RosK{)b*fn>-VdozV|2yLG9M^W=aSQ>YSx%MHKrWQu5xas7YTLZSdSw< z^WVX@8adJtsHM&i>qL{uKLvk%uAn4PqE5)BPnCB2XmG)uCDX-$ggPy&$8oLDmf&1v zDyJh*>sW0d} zm1jkVowZ+?Ezu-Ui!B+|j%oZjyI@%+|9=^^c2f)6vHko9O7OWHzCKp9-2nZM_NDT< zJL@e%E&kmi)-Ib77x+>yD|`>A+7y@wsxJK+5kLGN^F24?K!V|R$p zE}jXLAfap@Js+`2@mrEsHm|}2O6-Jgz3PO~+UXxVp=?~%%IXequL)acq>71G*A~lE zaha&JkI~+vbEGlpO2Ol}^1;mN!(LBOgqd3;&Kusp^;E^Qs~TMXLfdo%YAN60I&orb zr`?=1{F_p@ z$?~BR^{W!ikWlp@?I@!TfY2=fJr1h#Dp2Ppq3i_K!qn1S?bWfJKaZ|dgHwePB$UOW z$B{2AcYHBNR#hKnouihrgLJ}i{8d4JdAxcb4&9Yd|w zZW5@4EuUt0g~kP|RAH;86v`V4j8vKL4Xr?yUl$BHHzi0=Dae_=o~ml`$AY1BO#-#3 zq~#ibPEZX7s~SWJ)ux=qr%x5Nk{ZxT%s7ys79;29dK}aeYeGvj3Dm-#$UdH9zeI1B zTfJS_^Uia@QJKh&40@{Ku8aWLU;S-c7p zC`n6j96C{VbW`W)k;SSEt~B08Fs-!mj;k!=_`NCH)J!A{`z4SGl%yq?)`=DiuSISf zJTA~ZW0H+vT50|Krks0k=oxq8wH(K?i1c_WN%DKOdJ?4JBg^2^t zR!8ouGA5}-gUJFVNUX|T#@&7I5qInnCceotJ+em2Z<1n0PZlUaf_grgLcJxP9?~ar zf7gXcf2^<*sC752guC|RM0d(Xjw7tzvPJ_!vIJgyFX&mnQ!lt5Y(44c6mnb8OLs>8%aPeyjSAen#9q2-32xK>e+}|;l@_)hz0TK` z%e^ISD@jW*ZMDQh|76*cwk7g&m6rXD`d_U*Ey1+@er#^`Rph1DQyP5ioGdW6JD-$x zuUv4=Jvv!0E0rgY<22Rz^UB{f7)y1IX-9(Vg9)RD8`mSUc<*@)I<20}bQ1`F;pN{tM6|NB?6 zfAEWT?&C$OxZi*GknD;0UD5lJ#Rd^<5K4l?@UkDe>jsu`*DAS>QWb3w(P;?O8h*T` zyF>r{?)Td7A;KSi!dZe+HGl||47}Xjy=8B9cizYGX;S4dh*fFgKw^E9ChoOI|8XT; z-cE584?E#3Npbv=hCr>qv$)-f@xQwcXWL3d{n|P)Xlk%PNt1UQxexXI&ebRXhBT>) zsd2(tisG0wO)p(lR!C@r>bW1aJK!36Wi`c-E$D=^3=w%|*a_6?5)tO^_SJgVpt@fY z@k{;_&I&~AoNFgg%NtO|y>n7a*B@)=6T$wFbM^X}ya)yelpt|)VJ-K#CG%aC@+>Ff z3K3s@+c50EH*dU1pjP|t!R|IGv96g-ml$yj%bgJVh6t1to>|p>aa>>5!gCA#_$0ce z^3-uB`+(@TF4&f;0lVLJU%pYnwfOejG^HDs>$tNh5tlF9bB;uE&C=30L;?3_l&bLD zyPUo|Zl;uev;l_ zal2Ynf0k@lSO*O&m2-cV{BP9L^RsN_M(^OgEzV1GoTA>N!lKcCZ@DL)&k}So^HBex zl5e>qN@NWxGIglG_p)L#RW+MWrJScYHaYjjI>p12MFdKanBTu3onu-Ub?1jzA};*9 z%sIUn9kzH}(ng?GU}}CD$Js~Ih!~!8fpc>cr|1?`N}vRZ*`xEwI8t6uAtKY5XlLfP z8;OO>E87Uvx|c1pBua1ReDoO=?QC1IkqBE>S)c@oFP#6n!os4XG9Bc6bZk5|vPzY@ z;&J&f8-ZH0HasKZXjEV|Zi(|YPmRnJSXX4O7$#7HM5CdXT~A8n49anQ3=u`nSBgCL zPHoY?cYT2pBt9K+*VX-cj-afam?(VoN#up|5h8k04S^CQvgLp2@>I_e6kB*S5$h^6 zalSml|zFv%%S9d#%4}h96sx$MOBRXy+7aS1Xs= z+Z7VnrtLY$lwi*7slxXIB}ia9roA7-qMbifXe5GHR2Ha(b#5oF)$#0lc&@e(RqZXY z_|VHf)n2hgJdR#PdYS}Ekm$GJna_GZc>Lo&Yxl>c_Tro@qq&;?eNYi+rzG!G1WS;> zuYhtsDC79`*8ZI+LBj6A%vGd==I-*R*WD$?i&{9F{NDshkU+0|GF2M;Bs<)xNjT15 zRanncnpHJP%x|+i1fPopeuK8+`1)8S8-ZH(R0a84YNxl95Z|@PdO$swN0y9c+j@Oe zA>tb%ek#z#Bv^t3epe@-%SjPhms%mhxu)|0lpulY0T%IfNEaJ{S}LrkYG%Vgty9m& zBJZ8f2T-EIoF`ndpgVXBV&sQiYy@hlupWnhoPPYqiE0Q<9Fpq2{jsX9PAKeK!^ zLcIS&Vi-zPnB%}TCz@>=L`bG?wUrh>3$;|(C@b3eDQE}ii}-GPP@=*d2d#d=?T&tf z7{1LPh7u%jt(Q)KKBH4ew4bx|E`I_NsD<}7TEv7pS~yCOu&=`vel|Us<7ii3OGE;- z)GD5yb48S&GcOz^84-G&Q_j6!Dyw;O=OrSc!bW|-4U&rZe0*{^m5(XOh|uGp8uW6V z-pMJ?3)+E2F$8pM}> z?IiE2mLwxWkAvPQ!S7&5zFjs#(X2&pm)u356ZD=7?isQ!yj`1m1Npt5B`VCdmN60a zy3ZbzWJKt7u2OYmKMLWNn2`h3_qpopK!4BU^Bj0imrV|3Oo<-D^;5iFtcD&4mUJ?Dyu zEV`M-b1QZvaG!%Yo@+^65_h5m2^!Vlrg0k21&rtBHx?SH&6~85CjTWi!b&?5_BbdX zJUWZ1eVfvFuJ3NqOVrxwLbik%w>dhCQv-NBw>Q-t@#sXTx7FNUlF9T-@9nF28LnAC3GyTlYZH^(gLDq|^Ox z?N2WFZ*B3((z-SRwb1(_o#)(=*E@YusQ5l}xIhUKxaX2ijO1zPT^C$KEZTxt0Im_ETu!{KV!#XwswQxtP$8oLZT<`L=mBf>p zb!`M{p}$6tqqAeXx9zv(g{xw?KnW7KGne*Rb~)fJm9?aZKULR8pceXTq_370PkFb0 zUrgK&4;Lsw0(ThGI`!u_ytDT@M5`QiZ3JqezebN^)VZ79@DUF2d|DlW5+rb^w8xR7 z`G4Mu>obWuW5R6&YT^EEk7LcPOroXs%3Eq*T^oT~JdlXiJ$ z{ADLl3wQZ@9AEA(D`J{#_1+j**A@qAp-)KqUi-MR82tNuZ>c=t0wqY``38@pN|zw9 zWZD?-tmL{j0=3X5B-P-#VDaa@9^Rh4!Ual@z;hbZi@qHq&NV3Iof}`rMxYj+H=)(R zOd;aW$P(V!zuF1ZBEO2@owClTaM5S!&gAzRhYOS-f#+sut#4nbShc@<@~z`_Z3Jqe z7fHIq?PeX3yThC#`-_JQlpulUe>{#)hlYudf~!UjYhKqzpcZ2XZoT8ma{ zozCUdQ=S_lb4-mxl|KAc1GsXf!DM zwpe*~jr0AMb!-G`;rTf70WrM1*gj&t^HOMC8-ZHrsgm|5?kFk#ogkb66~YBdkic_( zwa;kic`7v~yt8Q}6VRIkZY6!)*j=;n_})t#eOuBR*`Z6`dw&$v7Ew=)5+v}9s>d;MOtSZjX~nhk<4Y z9Q5w_t(2DMr*MH1B=D>)c@S8-!rS7Xidwx_b^^8V1hB`EukJ!``8YZb!`M{p*KvAW8Z`j@2)x_ zS`O+{P=W-Wucr0(*B6t6dxdJtx7D!`sD-D#$)C)QbIG;BLbdPi+X>V{kDDGxxetmb z&zfFGON=J#5J>(wMs3%!Wa%_sFvM^-EtrZuizhiy^QS1A(c8^GgO8CcDE zxmAev`wnUEngnX0#{%*pRI;HnFrtQ*x?I}3CV^V$Kh@*tUv7vqEORh7^A3 z^t?bV@xmNur|?Qz{Ca7S$HkC8xMCLYJo&+=)BQ?EOB4wd$v9_#ME_|!A z?P>P0&h|Z}y=%sSTIfxi#=-Wrw6G^`=P%S#qXY@`4&-rcs#;5X_tWCe7rweS z0=3X9IL+=}hid1N|A}1kp0r*~-{VN2mm~W6x-Cqbf3HX6{R?&2dNm2uLht4@4*J8i zjf1;IZuizS#eP2OW90qqKKT0=3X16zyAj6{>|D4oXh(N_*EN zP>ZZu@Dbn88?Mc0us!)?E@{1*5+u;G72R=IJ4BoPS#GbFW<~fc)FK-deBIZ27NR}B zQo{S}rLd`i{9Gt!P-wpyLtDK&4ABBE%X9Ldx}I=twOnvynSP&y=%sST4dFN zm;c=bt7y&p&G-J5UD~^*1PSzJMkD8+<+Qo4)_MOtD6LnMKrOO3;kJSeughw8SMKu0 zd?c+`Q-TC~Wg{#6Okpjt>>2O*6?NEpH3`&0UvYFwsAysB;oURdpg-&cYLSf!_cT0u zC%3j>+i%|SjikM6N{~Pgc67F{QdaF{*fVd1lhWQb3DhF1)@9u{QU2b4&f6Cd7W8%g^GM+0iXm4^wH^YL~Q;( z@_gIcqA6JbNT3$ksBjZaZn6MSf&|;5YK_$6xO0>4w22HC$%pIM2-Lzg zS&!rRf-4PAM2Cs*gtT|f*innuVbz+fdE=CKXpw?+*Ot1gE&8YxPWAKcQ~Et|>?hT@ z)ykW1>SlSXm6Bvk@Xd1km6})S^W55GBdq79`mKBEI1WWTJh8nRN-`!m4&w$e5q4;u zjj-rcKi_tx-}=XKC}McRN_h{Kl4MM99NVk4PLA6)S@UW+s&gsox0Xu}u|_!6Z{4KM zaVVnrqYc$iV$n!&90falx|7aK(Cu7XY=lKi!nk9de1~(II)um`{~1)qxR~b?(z| zFEr{y5{Y%?jcleQV}j%0uO-~B9=(#czL^Bopb?gB*QZ*jMNEy4JQ*yCY!=n&4nSYlCBL`>0_1JhMp)iB z<~w_UZ=&J*ycF?c_IlgsHZfCT(Ma&gdm~la?C*0_<=EA2X`DiJzqj=}$DxRvsndf| zf`k<|Qsv})?2kRfsWNX!!~2%4-;vn#Y)w2$kg&o`L~fg??JJqfd3;f^ntTWShtn6y zGO@y^{Cv}V`?>RtvQosQ9GQYpk}<($MTFouMnvV_N<@nOtW$m>QuO#_9LDcPsw9zm z=XJbAL>xuJ3Nyj|$f>H&wmj%xiuQBbPMMk~OV#h{TWYNKu-4I9?Xn-HUPvcc!eMn5 zmBX7pV(*mPb~%n8P9@d~3D_qIB}q%vdhwP)6-SNCyTc#H%cy>c%~6bkf1X(e(QA2#^IAi z!)MKt2NVfUMhOz=6~p8BBbS4 zpjQmqv6Ur8Ty8zan?HP1Boe4ar*8b#d7QuV#fa)DG2VeWB9c*p1bW5rIQFg?CR$zD z;k}YFDG~|P;udP1;4xae_p3Es51=F?LVs&>bQvV}4Efp{cEe66nziu6Dfw<2+(mTV zbJE+ONt_oYNZ^T6ntu@CAtDnIC_w`E9e5lk=JXTY7hd;Pevs_|5~!uZ`rZ%KGgN(E zA()@bCtNt~*h{Fep7U$_x{JTZy>`~_)WGaZ(h_(go4&XkME>twHUhO&SdU}XmNp`L z=KR{MUpfY(M1?u+cut%=-5SKVecRXw)KXzRj;LDAMEv*VwZ^6T1*1fTISzG_-A)|+ zYGg1MzmJ*uw@vH?l zLeZ>+C;e&l!yrCuyUa$QmI~{s%2lnTsM@ooR=E7CV3epZ$AP{G==_HwzFKc1P)mjN zI6ixkQ)GIlqn0^gQ!q+YnB(C6S9~M9p7R+*WG15Szisy`Eq)eisjwc$jhuJA`S$hE z>V4mq_j1Z-r6tf;19^fbB0mxLE_XVB1Zt_U9!JvMUEU`j4b%Gl*XaOCRG8CV(eSEb zCpI7JbO0qtp#KSvbXd4T3wh^eMytwFbJTQokdpaFJNk)WTx~HbUOzu== zl6En$vyD)+ULTgd;&CJy#LS<+q|&t{84-FMlcFCb`!>HP@rBdJa_dZ!TTx0KINT3#$y2tT;<3ZlLjfZO|$%hb1 zkZ97mmhb0OWnbmkyWKndy@48gNkam)RBh_{NC=JdZhkyKV^3@-L4xn#n%kZIo^w6| zBYyUJ9(U8&+Zz(7g)P+M@Lr6qPP^f0SA2{tU9cuizbIU8mYqm5y8tAVgtU-|Qol=E zRCK6&y=wE+&uQnf<S zE&km~Rh5XJoxQ274Bu2P?lbh~4s)pzr81O$am@N~a(%FGDwOc;Q;MLvaoOtJIgIL@ zeN!QUTJ&~FuR3}h)UKS|uGlvfN|2y77G-O@PHwyGn+gfkQeJiRIPMnwBeFQXAMBe7 zB}i~tsdDo;hDAS$G-6PxOGqIsel z39=c%`ySb%8rz}~WQ*dnPzz6)dmLo%YHaWB)9qdJWI7UL`&$G*RPu)JZX%XGP{ocqH&dww$YJ zF*H|;*d*s_=6P@=xTJ0KL5=5wGig4E&q6Ic!|idH`CgQ$FvmeQyt~{Ez7d5>R}uB!{UKCo@v~4%h4naSHlgusVo<{$ z_M$|E`MG4nyYBX6&-EOKB2G`w6Q;EIS*WGLdK@%2&_ZZ7QF&6H1eB;SKbNe2S1G~w zrE(mK*l{-`Olk46P)mjNIB0I5ZKm0T=Xyv2N>rGitGwCiaVX;JW^Kci7C#HMR9KIL z<^~$iCb~M?CZI%x`MG4zyIi-k8~ISgwyk5slome=wNzM-gJu&N&nDKa9-Dv?73Mg| zS~4vIk3$iGHQw8&wD?)5rNVj~zvR5@;CIk8zmVLdg;<^ z!pXCVr1t&xAfdvng_Z$X;Rf;Zg}b&mP)mjNRMBk0$+L--Q||6Ti3)QZXc@@)ha{Xu zYlJE-eimw}upY<$=`SOBHsSj%crQv+nB%Zp8HWwx*B!O@D=mH&YN@au2Y;JbKD=KL zf1gzfH6ZyG(uu0|ZKtcsQb> zBu4gKWDs8^_1>D6KrN1Y=DcGvZat0ynN!JwnVu>oL4xDs-#LXkvE!4qWS%Vn=`3{*Xlpt{>?rlkID74fdT2IOQe`TD1td?aN$KPsrHg>=ak`mEF z21+^&r)4zW$GrzzZIR?$?QFDuFthrju;rHG7%`m9JV7iP;XuI%umaO?H9GH$?<*xW zGnbi4Hfi%%F;hCZ%0_j4ujjh&=X0n3cy_+m=eeKfxxe>yU-$I@uOS#Uw=-+>j8`7) z+_vVILCoHLp5GM}5@Qqdop|4eza7Nnv_HLuVAS^YvqzU)_rp$S!OzokoV;iBaRPFnJf>9%}7^T_#*|y0}~}?lg#0EVJF$wqTTMSa(=*w_%O344?~M4*hqrI) z{BwM!XVJ_5_Gs2BW zuqI-C6a=GYUVFN`ecH0fiB+tRiVBGj>_69uwL3P)7_mMIf>Gn6Z*;eR>a<6Ki1krX zA@PguIZn*~{gxok-?UXODF{ZLzTpga`@H)GLHzck)8*ZY3W@ixo#({$mu?H<+{Fug z3=)jmJT%MQe&~j7x~uVBPEa9n7iTY z_KFIL#aEr>#GNa52XSKTnBNr%M*ZL$XS&;;zxe(j?t5y96I4jtyZS9o-1Wfsf=Iri z!?{*8o7&GCmn2_NAtAqsXq=dQMS@Y9P3`B6OOmgskdWU*G)|1XTajRtW>fol!89ubWb`|FRZA}a_+X*RW=H!exOqC!IM z5z#m?`HBRiYnRBo1iV6w&O+@X)Q(OPjo;K^ri5G8K*<!ZNyj0cN-(&gmRQu+4?`Xf_nQQzxq$r0oimogPDkNA3E4M$} z)|+j$uI!Ou6kS;oR7kK6w!aI|?zR_v@WSe`hu+>J!6>@2B&d*J9c=&d*G=DQPt3i# zs#d(dM}kpwWl2yW!K!xE@X=~Td)^;DTdf@XQI`ax=*p6yLV{JT{fqCDPb~f8^*2|) z{=^ks5{xR{PJ#*v*1`I2&|0TID`Wp}kzf>8niGPYphBXosqL;N;yKzR7{&GPPo4=X zBv`}y+wI1Dv`H|EnXe?MklqphALY zLtnM_t;^SWu}3kA`(sH^A)%9u_S*VXWcjKa`KltpDDLx~uQEY}1kdW8MZ1>Um3J!= zjAA8Q5>!aA=a32Q9~B8kY3!y#w*5nQB-tZN_J4YX>VphAKt%bXc{mfPDT7^NJe6J%Zc*lwWg z;{+8FJXz)p*R$O2`6^wDQOcD%LDscTWKk!mkl1p7?6vQ+l*Ix$^~QLME~f(i-N*LseRRXc|^`nlGWfpVhtsr#1U z{AcR|_pMqUM4!4}6RBT9g@ol`C#+8$#M;+=-A}0`7^ShJ3J)UnOQ?|0cp`L%^&RArS0or^vpI-9br2u<&A1a(NLUVb z!a9&aJiY9=zbg`qGFJ|wPaVYBH(ue-L4}0nU?;2t8N~jh|JY+Gs3O5ATU~?bQzv4^ zBEKstBrFFzVI9aI_ALLqk3oV_mL&$!rw$_ei4_$RmV=$J4rCBd+&STQMS@Y5?FP}O z4x)AJlJDxtsgST7?1Xh7gZRnvd;B@*aIO{2W>f${9P1A|L4}0eBf|RBK@441uMZN8 z(rhZnG%mUPr*&723JJMKg!QRoj8hA@`CXAr)4jz7J9%VU@EJ z)~604@~-+N`C5$9*vk1D+p}mdeIKMkLgPi{97N<@e~xr5M%ixXZnu3fh|N=%Xg;*7 zWrc**+Wvd1!h`tyQ{U8_7X+hh*LJttejbEH>p$(zSs`Jyw*TI$@F2$a-YzE=1f#6_ zaJO4s5=5F0DkQAdcEYOgAQo>p;LkyVQC1VV+pWF|;^dn?=XXVggjLQ?SQQ>bfBh%V z*K?3ylvRW7cB>PEi1pEzaNM1lD@%e330Ae4@O}vi zM$wfeL4}0IYbs>+eMm5huFTyJb+#)iBv{pIH}K9xrCtUJM$wfeL4^dX+T6>i)XN~j zD7vyFsE}Y)n|m3RdKn}bMOT&t6%wp!b1$P(FM|Z5=*p6yLV{Io?q#S?-6p}P((UXB zQz5}RIQKGa&b?2aR}Q1N(n^8~iL$1S&V-($OM+2c|7C}s3JKQmxtF2&=#pR*Ghazi zA;Ggq?q$eVMNVWC^KnT~A;B|M?qz6w6uF&I+*eA13JIPKebwggQ6w0}{jnsdkkCmc z`qY+p`CW|!qqxtP1QilIt9usJH%R5(9tlRVk}U};B-nGvgu2c>5{%N=O@++woK#3? zJP~y-Bi@zweMm4$?@%YmxEr_omhuHb*|`1l;_&mar7K*UW-xM zyP~&TKZm|SIzfd5&(V43)O`3pn6AYr?ZMHfwlVnKfC>rr1KcI~YX%8Msg{V|a_y^3 zP$9v-My_PEKD;hT*J6}vqUbH#82pYyg#`OExgt~7+3TxxEk>!fQ)Wo#^X7XL6%y<- X<;v2(l96B(YwePtLW1@6s^R|u5cBqs literal 0 HcmV?d00001 diff --git a/engineer_middleware/package.xml b/engineer_middleware/package.xml index 8fa2577..0d5ac75 100644 --- a/engineer_middleware/package.xml +++ b/engineer_middleware/package.xml @@ -20,5 +20,6 @@ control_toolbox moveit_core angles + yaml-cpp moveit_ros_planning_interface diff --git a/engineer_middleware/src/middleware.cpp b/engineer_middleware/src/middleware.cpp index 34b0105..acd2907 100644 --- a/engineer_middleware/src/middleware.cpp +++ b/engineer_middleware/src/middleware.cpp @@ -37,33 +37,26 @@ #include "engineer_middleware/middleware.h" #include +#include "geometry_msgs/TransformStamped.h" namespace engineer_middleware { Middleware::Middleware(ros::NodeHandle& nh) : nh_(nh) , as_( nh_, "move_steps", [this](auto&& PH1) { executeCB(std::forward(PH1)); }, false) - , arm_group_(moveit::planning_interface::MoveGroupInterface("engineer_arm")) + , arm_group_(moveit::planning_interface::MoveGroupInterface("engineer_arms")) , chassis_interface_(nh, tf_) + , teaching_client_("/engineer_trajectory_teaching/trajectory_server", true) , hand_pub_(nh.advertise("/controllers/hand_controller/command", 10)) , end_effector_pub_(nh.advertise("/controllers/joint7_controller/command", 10)) , gimbal_pub_(nh.advertise("/controllers/gimbal_controller/command", 10)) , gpio_pub_(nh.advertise("/controllers/gpio_controller/command", 10)) , reversal_pub_(nh.advertise("/controllers/multi_dof_controller/command", 10)) , planning_result_pub_(nh.advertise("/planning_result", 10)) - , stone_num_pub_(nh.advertise("/stone_num", 10)) , point_cloud_pub_(nh.advertise("/cloud", 100)) - , ore_rotate_pub_(nh.advertise("/controllers/ore_bin_rotate_controller/command", 10)) - , ore_lift_pub_(nh.advertise("/controllers/ore_bin_lifter_controller/command", 10)) - , gimbal_lift_pub_(nh.advertise("/controllers/gimbal_lifter_controller/command", 10)) - , extend_arm_f_pub_(nh.advertise("/controllers/extend_arm_front_controller/command", 10)) - , extend_arm_b_pub_(nh.advertise("/controllers/extend_arm_back_controller/command", 10)) - , silver_lifter_pub_(nh.advertise("/controllers/silver_lifter_controller/command", 10)) - , silver_pusher_pub_(nh.advertise("/controllers/silver_pusher_controller/command", 10)) - , silver_rotator_pub_(nh.advertise("/controllers/silver_rotator_controller/command", 10)) - , gold_pusher_pub_(nh.advertise("/controllers/gold_pusher_controller/command", 10)) - , gold_lifter_pub_(nh.advertise("/controllers/gold_lifter_controller/command", 10)) - , middle_pitch_pub_(nh.advertise("/controllers/middle_pitch_controller/command", 10)) + , left_gripper_pub_(nh.advertise("/controllers/left_gripper_controller/command", 10)) + , right_gripper_pub_(nh.advertise("/controllers/right_gripper_controller/command", 10)) + , lifter_pub_(nh.advertise("/engineer_lifter_cmd", 10)) , tf_listener_(tf_) , is_middleware_control_(false) { @@ -75,14 +68,17 @@ Middleware::Middleware(ros::NodeHandle& nh) nh.getParam("scenes_list", scenes_list); ROS_ASSERT(steps_list.getType() == XmlRpc::XmlRpcValue::Type::TypeStruct); ROS_ASSERT(scenes_list.getType() == XmlRpc::XmlRpcValue::Type::TypeStruct); + rml_ = robot_model_loader::RobotModelLoader("robot_description"); + robot_model_ = rml_.getModel(); + if (!robot_model_) + ROS_ERROR("cannot get robot model."); for (XmlRpc::XmlRpcValue::ValueStruct::const_iterator it = steps_list.begin(); it != steps_list.end(); ++it) { step_queues_.insert(std::make_pair( - it->first, StepQueue(it->second, scenes_list, tf_, arm_group_, chassis_interface_, hand_pub_, - end_effector_pub_, gimbal_pub_, gpio_pub_, reversal_pub_, stone_num_pub_, - planning_result_pub_, point_cloud_pub_, ore_rotate_pub_, ore_lift_pub_, gimbal_lift_pub_, - extend_arm_f_pub_, extend_arm_b_pub_, silver_lifter_pub_, silver_pusher_pub_, - silver_rotator_pub_, gold_pusher_pub_, gold_lifter_pub_, middle_pitch_pub_))); + it->first, StepQueue(it->second, scenes_list, tf_, arm_group_, chassis_interface_, + teaching_client_, hand_pub_, end_effector_pub_, gimbal_pub_, gpio_pub_, + reversal_pub_, planning_result_pub_, point_cloud_pub_, left_gripper_pub_, + right_gripper_pub_, lifter_pub_, robot_model_))); } } else @@ -90,5 +86,6 @@ Middleware::Middleware(ros::NodeHandle& nh) as_.start(); } geometry_msgs::TransformStamped engineer_middleware::JointMotion::arm2base; +geometry_msgs::TransformStamped engineer_middleware::ArmsMotion::arm2base; } // namespace engineer_middleware diff --git a/engineer_planning_adapters/CMakeLists.txt b/engineer_planning_adapters/CMakeLists.txt new file mode 100644 index 0000000..113db18 --- /dev/null +++ b/engineer_planning_adapters/CMakeLists.txt @@ -0,0 +1,46 @@ +cmake_minimum_required(VERSION 3.10) +project(engineer_planning_adapters) + +set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +add_definitions(-Wall -Werror) + +find_package(catkin REQUIRED COMPONENTS + roscpp + moveit_core + moveit_ros_planning + moveit_ros_planning_interface + pluginlib +) + +catkin_package( + INCLUDE_DIRS + include + CATKIN_DEPENDS + roscpp + moveit_core + moveit_ros_planning + pluginlib +) + +include_directories(include ${catkin_INCLUDE_DIRS}) + +add_library(${PROJECT_NAME} + src/special_joint_constraint_adapter.cpp +) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) + +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) + +install(FILES planning_adapter_plugins.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) \ No newline at end of file diff --git a/engineer_planning_adapters/include/engineer_planning_adapters/special_joint_constraint_adapter.h b/engineer_planning_adapters/include/engineer_planning_adapters/special_joint_constraint_adapter.h new file mode 100644 index 0000000..c82e407 --- /dev/null +++ b/engineer_planning_adapters/include/engineer_planning_adapters/special_joint_constraint_adapter.h @@ -0,0 +1,31 @@ +// +// Created by ch on 2026/5/6. +// + +#pragma once + +#include +#include +#include + +namespace engineer_planning_adapters +{ +class SpecialJointConstraintAdapter : public planning_request_adapter::PlanningRequestAdapter +{ +public: + std::string getDescription() const override; + void initialize(const ros::NodeHandle& nh) override; + + 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; + +private: + bool enabled_{true}; + std::string joint_a_; + std::string joint_b_; + double constant_{0.0}; +}; +} // namespace engineer_planning_adapters \ No newline at end of file diff --git a/engineer_planning_adapters/package.xml b/engineer_planning_adapters/package.xml new file mode 100644 index 0000000..dc381cc --- /dev/null +++ b/engineer_planning_adapters/package.xml @@ -0,0 +1,23 @@ + + + engineer_planning_adapters + 0.0.0 + Custom MoveIt request adapters + ch + BSD-3-Clause + + catkin + roscpp + moveit_core + moveit_ros_planning + pluginlib + + roscpp + moveit_core + moveit_ros_planning + pluginlib + + + + + diff --git a/engineer_planning_adapters/planning_adapter_plugins.xml b/engineer_planning_adapters/planning_adapter_plugins.xml new file mode 100644 index 0000000..f0029e7 --- /dev/null +++ b/engineer_planning_adapters/planning_adapter_plugins.xml @@ -0,0 +1,5 @@ + + + \ No newline at end of file diff --git a/engineer_planning_adapters/src/special_joint_constraint_adapter.cpp b/engineer_planning_adapters/src/special_joint_constraint_adapter.cpp new file mode 100644 index 0000000..3faafed --- /dev/null +++ b/engineer_planning_adapters/src/special_joint_constraint_adapter.cpp @@ -0,0 +1,86 @@ +// +// Created by ch on 2026/5/6. +// + +#include + +#include +#include +#include +#include + +namespace engineer_planning_adapters +{ +std::string SpecialJointConstraintAdapter::getDescription() const +{ + return "SpecialJointConstraintAdapter"; +} + +void SpecialJointConstraintAdapter::initialize(const ros::NodeHandle& nh) +{ + ros::NodeHandle pnh(nh, "special_joint_constraint"); + pnh.param("enabled", enabled_, false); + pnh.param("joint_a", joint_a_, std::string()); + pnh.param("joint_b", joint_b_, std::string()); + pnh.param("constant", constant_, 0.0); +} + +bool SpecialJointConstraintAdapter::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 +{ + if (!enabled_) + return planner(planning_scene, req, res); + + if (joint_a_.empty() || joint_b_.empty()) + { + ROS_ERROR_STREAM("SpecialJointConstraintAdapter: joint_a/joint_b not set"); + return false; + } + + planning_scene::PlanningScenePtr scene = planning_scene->diff(); + const robot_model::JointModel* jm_a = scene->getRobotModel()->getJointModel(joint_a_); + const robot_model::JointModel* jm_b = scene->getRobotModel()->getJointModel(joint_b_); + + if (!jm_a || !jm_b) + { + ROS_ERROR_STREAM("SpecialJointConstraintAdapter: unknown joint(s): " + << joint_a_ << ", " << joint_b_); + return false; + } + + if (jm_a->getVariableCount() != 1 || jm_b->getVariableCount() != 1) + { + ROS_ERROR_STREAM("SpecialJointConstraintAdapter: only 1-DOF joints supported"); + return false; + } + + const std::string var_a = jm_a->getVariableNames()[0]; + const std::string var_b = jm_b->getVariableNames()[0]; + const double constant = constant_; + + scene->setStateFeasibilityPredicate( + [var_a, var_b, constant](const robot_state::RobotState& state, bool verbose) -> bool + { + const double a = state.getVariablePosition(var_a); + const double b = state.getVariablePosition(var_b); + if (a >= constant - b) + return true; + + if (verbose) + { + ROS_WARN_STREAM_THROTTLE(1.0, "Joint relation violated: " << var_a + << " < " << constant << " - " << var_b); + } + return false; + }); + + return planner(scene, req, res); +} +} // namespace engineer_planning_adapters + +PLUGINLIB_EXPORT_CLASS(engineer_planning_adapters::SpecialJointConstraintAdapter, + planning_request_adapter::PlanningRequestAdapter) \ No newline at end of file diff --git a/engineer_trajectory_teaching/CMakeLists.txt b/engineer_trajectory_teaching/CMakeLists.txt new file mode 100644 index 0000000..7606e25 --- /dev/null +++ b/engineer_trajectory_teaching/CMakeLists.txt @@ -0,0 +1,121 @@ +cmake_minimum_required(VERSION 3.0.2) +project(engineer_trajectory_teaching) + +## Use C++14 +set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +## By adding -Wall and -Werror, the compiler does not ignore warnings anymore, +## enforcing cleaner code. +add_definitions(-Wall -Werror -Wno-address-of-packed-member) + +## Find catkin macros and libraries +find_package(yaml-cpp REQUIRED) + +find_package(catkin REQUIRED + COMPONENTS + roscpp + rm_msgs + std_msgs + sensor_msgs + trajectory_msgs + control_msgs + actionlib +) + +generate_messages( + DEPENDENCIES + std_msgs + sensor_msgs + trajectory_msgs + control_msgs +) +## Find system libraries +#find_package(Eigen3 REQUIRED) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( + INCLUDE_DIRS + include + LIBRARIES + CATKIN_DEPENDS + roscpp + rm_msgs + std_msgs + sensor_msgs + trajectory_msgs + control_msgs + DEPENDS +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + +## Declare cpp executables +FILE(GLOB ALL_SOURCES "src/*.cpp" ) +add_executable(${PROJECT_NAME} ${ALL_SOURCES}) + +## Add dependencies to exported targets, like ROS msgs or srvs +add_dependencies(${PROJECT_NAME} + ${catkin_EXPORTED_TARGETS} +) + +## Specify libraries to link executable targets against +target_link_libraries(${PROJECT_NAME} + ${catkin_LIBRARIES} + yaml-cpp +) + +############# +## Install ## +############# + +# 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" +) + +## Mark other files for installation +install( + DIRECTORY launch + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +############# +## Testing ## +############# + +#if (${CATKIN_ENABLE_TESTING}) +# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread") +# ## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test +# test/test_ros_package_template.cpp +# test/AlgorithmTest.cpp) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}_core) +#endif () diff --git a/engineer_trajectory_teaching/config/config.yaml b/engineer_trajectory_teaching/config/config.yaml new file mode 100644 index 0000000..8cc601d --- /dev/null +++ b/engineer_trajectory_teaching/config/config.yaml @@ -0,0 +1,3 @@ +record: + record_joint_names: [ "joint1","joint2","joint3","joint4","joint5","joint6" ] + record_interval_threshold: 5.0 diff --git a/engineer_trajectory_teaching/include/engineer_trajectory_teaching/engineer_trajectory_teaching.h b/engineer_trajectory_teaching/include/engineer_trajectory_teaching/engineer_trajectory_teaching.h new file mode 100644 index 0000000..b21e690 --- /dev/null +++ b/engineer_trajectory_teaching/include/engineer_trajectory_teaching/engineer_trajectory_teaching.h @@ -0,0 +1,63 @@ +// +// Created by ch on 25-7-6. +// + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace engineer_trajectory_teaching +{ +class EngineerTrajectoryTeaching +{ +public: + explicit EngineerTrajectoryTeaching(ros::NodeHandle& nh); + void executeCB(const actionlib::SimpleActionServer::GoalConstPtr& goal); + +private: + void startRecord(); + void record(); + void stopRecord(const std::string& file_path); + void jointStateCallback(const sensor_msgs::JointState::ConstPtr& data); + void controllerStateCallback(const control_msgs::JointTrajectoryControllerStateConstPtr& msg); + void recordToYAML(const std::string& file_path); + void playback(const YAML::Node& point, ros::Duration& duration); + void stopPlayback(); + + struct TrajectoryPoint + { + double timestamp; + std::vector joint_position; + }; + struct Trajectory + { + std::vector record_joint_names; + std::vector trajectory_points; + } recorded_trajectory_; + + ros::NodeHandle nh_; + actionlib::SimpleActionServer as_; + rm_msgs::EngineerTrajectoryTeachingFeedback feedback_; + rm_msgs::EngineerTrajectoryTeachingResult result_; + control_msgs::JointTrajectoryControllerState controller_state_; + trajectory_msgs::JointTrajectory trajectory_cmd_; + ros::Subscriber joint_state_sub_, controller_state_sub_; + ros::Publisher trajectory_cmd_pub_, small_joint1_cmd_pub_, small_joint2_cmd_pub_, + small_joint3_cmd_pub_, gpio_cmd_pub_, gimbal_cmd_pub_, middle_pitch_cmd_pub_; + std::map joint_state_; + ros::Time start_record_time_; + ros::Duration last_time_from_start_, time_from_start_; + std::string file_path_; + double record_interval_threshold_ = 0.1, sync_time_ = 0.1,speed_ = 1.0; +}; +} diff --git a/engineer_trajectory_teaching/launch/load.launch b/engineer_trajectory_teaching/launch/load.launch new file mode 100644 index 0000000..b36712d --- /dev/null +++ b/engineer_trajectory_teaching/launch/load.launch @@ -0,0 +1,4 @@ + + + + \ No newline at end of file diff --git a/engineer_trajectory_teaching/package.xml b/engineer_trajectory_teaching/package.xml new file mode 100644 index 0000000..809aa0a --- /dev/null +++ b/engineer_trajectory_teaching/package.xml @@ -0,0 +1,66 @@ + + + engineer_trajectory_teaching + 0.0.0 + The engineer_trajectory_teaching package + + + + + ch + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + roscpp + yaml-cpp + rm_msgs + sensor_msgs + std_msgs + trajectory_msgs + control_msgs + actionlib + + + + + + + diff --git a/engineer_trajectory_teaching/src/engineer_trajectory_teaching.cpp b/engineer_trajectory_teaching/src/engineer_trajectory_teaching.cpp new file mode 100644 index 0000000..4da53c0 --- /dev/null +++ b/engineer_trajectory_teaching/src/engineer_trajectory_teaching.cpp @@ -0,0 +1,285 @@ +// +// Created by ch on 25-7-6. +// +#include "engineer_trajectory_teaching/engineer_trajectory_teaching.h" + +namespace engineer_trajectory_teaching +{ + EngineerTrajectoryTeaching::EngineerTrajectoryTeaching(ros::NodeHandle &nh) + : nh_(nh), + as_(nh_, "trajectory_server", [this](auto&& PH1) { executeCB(std::forward(PH1)); }, false) + { + ROS_INFO_STREAM(nh_.getNamespace().c_str()); + ROS_ASSERT(nh_.getParam("record/record_joint_names", recorded_trajectory_.record_joint_names)); + nh_.param("record/record_interval_threshold", record_interval_threshold_, 0.1); + joint_state_sub_ = nh_.subscribe("/joint_states", 10, &EngineerTrajectoryTeaching::jointStateCallback, this); + controller_state_sub_ = nh_.subscribe("/controllers/arm_trajectory_controller/state", 10, &EngineerTrajectoryTeaching::controllerStateCallback, this); + trajectory_cmd_pub_ = nh_.advertise("/controllers/arm_trajectory_controller/command", 10); + small_joint1_cmd_pub_ = nh_.advertise("/controllers/small_joint1_controller/command", 10); + small_joint2_cmd_pub_ = nh_.advertise("/controllers/small_joint2_controller/command", 10); + small_joint3_cmd_pub_ = nh_.advertise("/controllers/small_joint3_controller/command", 10); + gpio_cmd_pub_ = nh_.advertise("/controllers/gpio_controller/command", 10); + gimbal_cmd_pub_ = nh_.advertise("/controllers/gimbal_controller/command", 10); + middle_pitch_cmd_pub_ = nh_.advertise("/controllers/middle_pitch_controller/command", 10); + as_.start(); + } + + void EngineerTrajectoryTeaching::executeCB(const actionlib::SimpleActionServer::GoalConstPtr &goal) + { + result_.succeed = false; + if (goal->filename.empty()) + { + ROS_ERROR_STREAM("The file path is empty!"); + as_.setAborted(result_); + return; + } + file_path_ = goal->filename; + if (goal->command == rm_msgs::EngineerTrajectoryTeachingGoal_>::RECORD) + { + startRecord(); + while (as_.isActive()) + { + record(); + if (as_.isPreemptRequested() || !ros::ok()) + { + stopRecord(file_path_); + break; + } + } + result_.succeed = true; + as_.setSucceeded(result_); + } + if (goal->command == rm_msgs::EngineerTrajectoryTeachingGoal_>::PLAYBACK) + { + if (goal->sync_time <= 0.0 || goal->speed <= 0.0) + { + ROS_ERROR_STREAM("THe sync_time or speed is invalid!"); + as_.setAborted(result_); + return; + } + sync_time_ = goal->sync_time; + speed_ = goal->speed; + try + { + YAML::Node config = YAML::LoadFile(file_path_); + ros::Time last_time = ros::Time::now(); + + ros::Duration duration = ros::Duration(0.0); + auto joint_names = config["joint_names"].as>(); + trajectory_cmd_.joint_names = joint_names; + auto points = config["points"]; + auto point = points.begin(); + auto last_time_stamp = (*point)["timestamp"].as(); + while (as_.isActive() && point != points.end()) + { + if (ros::Time::now() - last_time >= duration) + { + duration = ros::Duration(((*point)["timestamp"].as() - last_time_stamp) / speed_); + if (point == points.begin()) + duration += ros::Duration(sync_time_); + playback(*point, duration); + last_time_stamp = (*point)["timestamp"].as(); + ++point; + last_time = ros::Time::now(); + } + if (as_.isPreemptRequested() || !ros::ok()) + { + stopPlayback(); + break; + } + } + if (point == points.end()) + duration.sleep(); + result_.succeed = true; + as_.setSucceeded(result_); + } + catch (std::exception &e) + { + stopPlayback(); + ROS_WARN_STREAM("YAML parsing error: " << e.what()); + } + } + } + + void EngineerTrajectoryTeaching::jointStateCallback(const sensor_msgs::JointStateConstPtr &msg) + { + joint_state_.clear(); + for (size_t i = 0; i < msg->name.size(); i++) + { + joint_state_.insert(std::map::value_type(msg->name[i], msg->position[i])); + } + } + + void EngineerTrajectoryTeaching::controllerStateCallback(const control_msgs::JointTrajectoryControllerStateConstPtr &msg) + { + controller_state_ = *msg; + } + + + void EngineerTrajectoryTeaching::record() + { + time_from_start_ = ros::Time::now() - start_record_time_; + if ((time_from_start_ - last_time_from_start_).toSec() < record_interval_threshold_ ) + return; + TrajectoryPoint point; + point.timestamp = time_from_start_.toSec(); + for (const auto& record_joint_name : recorded_trajectory_.record_joint_names) + { + try + { + point.joint_position.push_back(joint_state_.at(record_joint_name)); + } + catch (std::exception &e) + { + ROS_ERROR_STREAM("The joint named: " << record_joint_name << " was not found!"); + return; + } + } + recorded_trajectory_.trajectory_points.push_back(point); + feedback_.mode = "RECORD"; + feedback_.state = rm_msgs::EngineerTrajectoryTeachingFeedback::START; + feedback_.text = "Recording..."; + feedback_.time_from_start = time_from_start_.toSec(); + as_.publishFeedback(feedback_); + last_time_from_start_ = time_from_start_; + } + + void EngineerTrajectoryTeaching::startRecord() + { + recorded_trajectory_.trajectory_points.clear(); + start_record_time_ = ros::Time::now(); + last_time_from_start_ = ros::Duration(-500.0); + ROS_INFO_STREAM("Start recording ..."); + } + + void EngineerTrajectoryTeaching::stopRecord(const std::string& file_path) + { + recordToYAML(file_path); + ROS_INFO_STREAM("Saved trajectory to: " << file_path); + } + + void EngineerTrajectoryTeaching::recordToYAML(const std::string& file_path) + { + try + { + std::ofstream f_out(file_path); + YAML::Emitter emitter(f_out); + emitter << YAML::BeginMap; + emitter << YAML::Key << "joint_names" << YAML::Value << YAML::Flow << recorded_trajectory_.record_joint_names; + emitter << YAML::Key << "points" << YAML::Value << YAML::BeginSeq; + for (const auto& point : recorded_trajectory_.trajectory_points) + { + emitter << YAML::BeginMap; + emitter << YAML::Key << "timestamp" << YAML::Value << point.timestamp; + emitter << YAML::Key << "positions" << YAML::Value << YAML::Flow << point.joint_position; + emitter << YAML::EndMap; + } + emitter << YAML::EndSeq; + emitter << YAML::EndMap; + f_out.close(); + ROS_INFO("Save successfully"); + } + catch (std::exception &e) + { + ROS_ERROR_STREAM("YAML parsing error: " << e.what()); + } + } + + void EngineerTrajectoryTeaching::playback(const YAML::Node &point, ros::Duration& duration) + { + if (point["positions"].IsDefined()) + { + trajectory_cmd_.points.clear(); + trajectory_cmd_.header.stamp = ros::Time(0); + trajectory_msgs::JointTrajectoryPoint trajectory_point; + const auto positions = point["positions"].as>(); + trajectory_point.positions = positions; + if (point["velocities"].IsDefined()) + { + const auto velocities = point["velocities"].as>(); + trajectory_point.velocities = velocities; + } + if (point["accelerations"].IsDefined()) + { + const auto accelerations = point["accelerations"].as>(); + trajectory_point.accelerations = accelerations; + } + trajectory_point.time_from_start = duration; + trajectory_cmd_.points.push_back(trajectory_point); + trajectory_cmd_pub_.publish(trajectory_cmd_); + } + if (point["small_joint1"].IsDefined()) + { + std_msgs::Float64 small_joint1_cmd; + small_joint1_cmd.data = point["small_joint1"].as(); + small_joint1_cmd_pub_.publish(small_joint1_cmd); + } + if (point["small_joint2"].IsDefined()) + { + std_msgs::Float64 small_joint2_cmd; + small_joint2_cmd.data = point["small_joint2"].as(); + small_joint2_cmd_pub_.publish(small_joint2_cmd); + } + if (point["small_joint3"].IsDefined()) + { + std_msgs::Float64 small_joint3_cmd; + small_joint3_cmd.data = point["small_joint3"].as(); + small_joint3_cmd_pub_.publish(small_joint3_cmd); + } + if (point["main_gripper"].IsDefined() || point["small_gripper"].IsDefined() || point["transfer_gripper"].IsDefined()) + { + rm_msgs::GpioData gpio_cmd; + if (point["main_gripper"].IsDefined()) + { + gpio_cmd.gpio_name.emplace_back("main_gripper"); + gpio_cmd.gpio_state.emplace_back(point["main_gripper"].as()); + } + if (point["small_gripper"].IsDefined()) + { + gpio_cmd.gpio_name.emplace_back("small_gripper"); + gpio_cmd.gpio_state.emplace_back(point["small_gripper"].as()); + } + if (point["transfer_gripper"].IsDefined()) + { + gpio_cmd.gpio_name.emplace_back("transfer_gripper"); + gpio_cmd.gpio_state.emplace_back(point["transfer_gripper"].as()); + } + gpio_cmd_pub_.publish(gpio_cmd); + } + if (point["gimbal"].IsDefined()) + { + rm_msgs::GimbalCmd gimbal_cmd; + gimbal_cmd.mode = rm_msgs::GimbalCmd::DIRECT; + gimbal_cmd.target_pos.header.frame_id = "base_link"; + gimbal_cmd.target_pos.point.x = point["gimbal"][0].as(); + gimbal_cmd.target_pos.point.y = point["gimbal"][1].as(); + gimbal_cmd.target_pos.point.z = point["gimbal"][2].as(); + gimbal_cmd_pub_.publish(gimbal_cmd); + } + if (point["middle_pitch"].IsDefined()) + { + std_msgs::Float64 middle_pitch_cmd; + middle_pitch_cmd.data = point["middle_pitch"].as(); + middle_pitch_cmd_pub_.publish(middle_pitch_cmd); + } + feedback_.mode = "PLAYBACK"; + feedback_.state = rm_msgs::EngineerTrajectoryTeachingFeedback::START; + feedback_.text = "Playback..."; + feedback_.time_from_start = point["timestamp"].as(); + as_.publishFeedback(feedback_); + } + + void EngineerTrajectoryTeaching::stopPlayback() + { + trajectory_cmd_.points.clear(); + trajectory_cmd_.header.stamp = ros::Time(0); + trajectory_cmd_.joint_names = controller_state_.joint_names; + trajectory_msgs::JointTrajectoryPoint current_point; + current_point.positions = controller_state_.actual.positions; + current_point.time_from_start = ros::Duration(0.1); + trajectory_cmd_.points.push_back(current_point); + trajectory_cmd_pub_.publish(trajectory_cmd_); + ROS_INFO_STREAM("Stop playback"); + } + +} \ No newline at end of file diff --git a/engineer_trajectory_teaching/src/main.cpp b/engineer_trajectory_teaching/src/main.cpp new file mode 100644 index 0000000..d3804f6 --- /dev/null +++ b/engineer_trajectory_teaching/src/main.cpp @@ -0,0 +1,21 @@ +// +// Created by ch on 25-7-7. +// +#include "engineer_trajectory_teaching/engineer_trajectory_teaching.h" + +using namespace engineer_trajectory_teaching; + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "engineer_trajectory_teaching"); + ros::NodeHandle nh("~"); + engineer_trajectory_teaching::EngineerTrajectoryTeaching engineer_trajectory_teaching(nh); + ros::Rate loop_rate(100); + ros::MultiThreadedSpinner spinner(3); + while (ros::ok()) + { + spinner.spin(); + loop_rate.sleep(); + } + return 0; +} diff --git a/engineer_trajectory_teaching/trajectory/big_change_small.yaml b/engineer_trajectory_teaching/trajectory/big_change_small.yaml new file mode 100644 index 0000000..0421a9d --- /dev/null +++ b/engineer_trajectory_teaching/trajectory/big_change_small.yaml @@ -0,0 +1,57 @@ +joint_names: [joint1, joint2, joint3, joint4, joint5, joint6] +points: + - timestamp: 0.000093433 + positions: [ 0.1, 1.12080810546875, -2.29154296875, -1.568243164062501, 0.08296142578125015, -1.59 ] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + small_joint3: -0.01 + small_joint1: 0.238 + small_joint2: -0.176 + middle_pitch: 0.071 + transfer_gripper: true + small_gripper: true + - timestamp: 0.2 + positions: [ 0.2, 1.12080810546875, -2.29154296875, -1.568243164062501, 0.08296142578125015, -1.59 ] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + - timestamp: 1.15 + positions: [ 0.2, -0.763, -2.096, -1.568243164062501, 0.35, -1.59 ] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + - timestamp: 1.7 + positions: [ 0.2, -0.763, -2.096, -1.568243164062501, 0.35, -1.59 ] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + main_gripper: false + - timestamp: 2.2 + positions: [ 0.4, -0.5, -2.096, -1.568243164062501, 0.35, -1.59 ] + - timestamp: 2.7 + positions: [0.55, -1.281, -2.189, -0.034, 1.575, -0.307] + - timestamp: 3.2 + positions: [0.55, -2.142, -2.189, -0.034, 1.66, -0.307] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + main_gripper: true + - timestamp: 3.5 + positions: [0.55, -2.142, -2.189, -0.034, 1.66, -0.307] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + - timestamp: 3.65 + positions: [0.47, -2.142, -2.189, -0.034, 1.66, -0.307] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + transfer_gripper: false + small_gripper: true + - timestamp: 4.0 + positions: [0.47, -2.142, -2.189, -0.034, 1.66, -0.307] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + - timestamp: 4.2 + positions: [0.55, -2.142, -2.189, -0.034, 1.66, -0.307] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + small_joint3: 1.61 + - timestamp: 4.7 + positions: [0.55, -1.281, -2.189, -0.034, 1.575, -0.307] + - timestamp: 5.7 + positions: [ 0.55, 0.871, -2.407, -0.064, 0.653, 0.01 ] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + small_joint1: 0.1805 + - timestamp: 6.0 + positions: [0.55, 0.871, -2.407, -0.064, 0.653, 0.01] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + middle_pitch: 1.057 + - timestamp: 6.4 + positions: [0.244, 0.871, -2.407, -0.064, 0.653, 0.01] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] \ No newline at end of file diff --git a/engineer_trajectory_teaching/trajectory/big_island1.yaml b/engineer_trajectory_teaching/trajectory/big_island1.yaml new file mode 100644 index 0000000..447dc54 --- /dev/null +++ b/engineer_trajectory_teaching/trajectory/big_island1.yaml @@ -0,0 +1,42 @@ +joint_names: [joint1, joint2, joint3, joint4, joint5, joint6] +points: + - timestamp: 0.300091673 + positions: [0.00804281809659451, 1.093, -2.359, -0.04538916015625039, 0.1205249023437503, 1.60922314453125] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + middle_pitch: 0.06 + - timestamp: 0.500091673 + positions: [0.003484281809659451, 1.093, -2.359, -0.04538916015625039, 0.1205249023437503, 1.60922314453125] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + small_joint3: 0.039 + main_gripper: true + small_gripper: true + transfer_gripper: false + - timestamp: 0.7 + small_joint1: 0.065 + small_joint2: -0.01 + - timestamp: 1.000091753 + positions: [0.003484281809659451, 1.093, -2.359, -1.125914062500001, 0.01550048828125022, -1.588] + - timestamp: 1.500091771 + positions: [0.003484281809659451, 0.689747314453125, -2.032906494140625, -1.414539550781251, 0.006301269531250153, -1.588] + small_joint2: -0.393 + - timestamp: 2.000091855 + positions: [0.003488954516601328, 0.3471875, -1.741845092773437, -1.53182958984375, -0.04582763671875084, -1.588] + - timestamp: 2.500092752 + positions: [0.003493627223543204, 0.3262066650390625, -1.729256591796875, -1.548694824218751, -0.05387695312499935, -1.588] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + small_joint3: -0.07 + - timestamp: 3.000092959 + positions: [0.04434411102172571, 0.3258251953125, -1.742989501953125, -1.565560058593749, -0.05349365234374942, -1.588] + small_joint1: 0.098 + middle_pitch: 0.0 + - timestamp: 3.500093207 + positions: [0.05834932343668896, 0.3258251953125, -1.742989501953125, -1.567859863281251, -0.01593017578124928, -1.588] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + - timestamp: 4.000093288 + positions: [0.05861886429921393, 0.7053875732421875, -2.026421508789062, -1.566709960937501, 0.009750976562499514, -1.588] + - timestamp: 4.500093391 + positions: [0.05837121083129446, 1.099064331054687, -2.281624755859375, -1.564793457031249, 0.06341308593750022, -1.588] + - timestamp: 5.000093433 + positions: [0.05835411828694595, 1.12080810546875, -2.29154296875, -1.568243164062501, 0.08296142578125015, -1.588] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + small_joint2: -0.001 \ No newline at end of file diff --git a/engineer_trajectory_teaching/trajectory/big_island2.yaml b/engineer_trajectory_teaching/trajectory/big_island2.yaml new file mode 100644 index 0000000..d1cdd0c --- /dev/null +++ b/engineer_trajectory_teaching/trajectory/big_island2.yaml @@ -0,0 +1,16 @@ +joint_names: [joint1, joint2, joint3, joint4, joint5, joint6] +points: + - timestamp: 0.000093433 + positions: [ 0.3640007556342396, 1.12080810546875, -2.29154296875, -1.568243164062501, 0.08296142578125015, 1.5528779296875 ] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + small_joint1: 0.24 + small_joint2: -0.176 + - timestamp: 1.000050034 + positions: [0.3640007556342396, 0.871, -2.407, -0.064, 0.653, 0.01] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + middle_pitch: 1.057 + small_joint3: 1.61 + - timestamp: 2.000050034 + positions: [0.244, 0.871, -2.407, -0.064, 0.653, 0.01] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + small_joint1: 0.1805 \ No newline at end of file diff --git a/engineer_trajectory_teaching/trajectory/big_island_left.yaml b/engineer_trajectory_teaching/trajectory/big_island_left.yaml new file mode 100644 index 0000000..20f28f5 --- /dev/null +++ b/engineer_trajectory_teaching/trajectory/big_island_left.yaml @@ -0,0 +1,27 @@ +joint_names: [joint1, joint2, joint3, joint4, joint5, joint6] +points: + - timestamp: 0.300091673 + positions: [0.00804281809659451, 1.093, -2.359, -0.04538916015625039, 0.1205249023437503, 1.60922314453125] + middle_pitch: 0.06 + - timestamp: 0.500091673 + positions: [0.003484281809659451, 1.093, -2.359, -0.04538916015625039, 0.1205249023437503, 1.60922314453125] + main_gripper: true + - timestamp: 1.000091753 + positions: [0.003484281809659451, 1.093, -2.359, -1.125914062500001, 0.01550048828125022, -1.588] + - timestamp: 1.500091771 + positions: [0.003484281809659451, 0.689747314453125, -2.032906494140625, -1.414539550781251, 0.006301269531250153, -1.588] + - timestamp: 2.000091855 + positions: [0.003488954516601328, 0.3471875, -1.741845092773437, -1.53182958984375, -0.04582763671875084, -1.588] + - timestamp: 2.500092752 + positions: [0.003493627223543204, 0.3262066650390625, -1.729256591796875, -1.548694824218751, -0.05387695312499935, -1.588] + - timestamp: 3.000092959 + positions: [0.04034411102172571, 0.3258251953125, -1.742989501953125, -1.565560058593749, -0.05349365234374942, -1.588] + middle_pitch: 0.0 + - timestamp: 3.500093207 + positions: [0.05534932343668896, 0.3258251953125, -1.742989501953125, -1.567859863281251, -0.01593017578124928, -1.588] + - timestamp: 4.000093288 + positions: [0.05361886429921393, 0.7053875732421875, -2.026421508789062, -1.566709960937501, 0.009750976562499514, -1.588] + - timestamp: 4.500093391 + positions: [0.05337121083129446, 1.099064331054687, -2.281624755859375, -1.564793457031249, 0.06341308593750022, -1.588] + - timestamp: 5.000093433 + positions: [0.05335411828694595, 1.12080810546875, -2.29154296875, -1.568243164062501, 0.08296142578125015, -1.588] \ No newline at end of file diff --git a/engineer_trajectory_teaching/trajectory/big_island_left2.yaml b/engineer_trajectory_teaching/trajectory/big_island_left2.yaml new file mode 100644 index 0000000..6f794d1 --- /dev/null +++ b/engineer_trajectory_teaching/trajectory/big_island_left2.yaml @@ -0,0 +1,9 @@ +joint_names: [joint1, joint2, joint3, joint4, joint5, joint6] +points: + - timestamp: 0.000093433 + positions: [ 0.3640007556342396, 1.12080810546875, -2.29154296875, -1.568243164062501, 0.08296142578125015, 1.5528779296875 ] + - timestamp: 1.000050034 + positions: [0.3640007556342396, 0.871, -2.407, -0.064, 0.653, 0.01] + middle_pitch: 1.057 + - timestamp: 2.000050034 + positions: [0.244, 0.871, -2.407, -0.064, 0.653, 0.01] \ No newline at end of file diff --git a/engineer_trajectory_teaching/trajectory/big_island_mid.yaml b/engineer_trajectory_teaching/trajectory/big_island_mid.yaml new file mode 100644 index 0000000..69df467 --- /dev/null +++ b/engineer_trajectory_teaching/trajectory/big_island_mid.yaml @@ -0,0 +1,20 @@ +joint_names: [joint1, joint2, joint3, joint4, joint5, joint6] +points: + - timestamp: 4.2378e-05 + positions: [0.004657131252070519, 1.096012573242187, -2.492577514648437, -0.0004916992187505853, -0.0009105078125003496, -0.001002441406249166] + main_gripper: true + middle_pitch: 0.071 + - timestamp: 0.5 + positions: [0.004657131252070519, 1.058628540039062, -2.173287353515625, -0.4478037109374994, -0.04192369140624985, -1.54] + - timestamp: 1.0 + positions: [0.004657131252070519, 0.7580303955078125, -2.021080932617187, -1.515296386718751, 0.1412940820312499, -1.54] + - timestamp: 1.5 + positions: [0.004657131252070519, 0.3506207275390625, -1.760155639648437, -1.511846679687499, 0.1029640039062499, -1.54] + - timestamp: 2.0 + positions: [0.004657131252070519, -0.08997680664062502, -1.364953002929687, -1.522962402343751, 0.1067970117187492, -1.54] + - timestamp: 2.5 + positions: [0.06931673663141094, -0.08997680664062502, -1.365715942382812, -1.504563964843751, 0.1071803124999992, -1.54] + - timestamp: 3.0 + positions: [0.06931673663141094, 0.4116558837890625, -1.80326171875, -1.5578427734375, 0.1439771874999994, -1.54] + - timestamp: 3.5 + positions: [0.06931673663141094, 1.022770385742187, -2.054268798828125, -1.5164462890625, 0.5084962304687494, -1.54] \ No newline at end of file diff --git a/engineer_trajectory_teaching/trajectory/big_island_mid2.yaml b/engineer_trajectory_teaching/trajectory/big_island_mid2.yaml new file mode 100644 index 0000000..dfa37ca --- /dev/null +++ b/engineer_trajectory_teaching/trajectory/big_island_mid2.yaml @@ -0,0 +1,9 @@ +joint_names: [joint1, joint2, joint3, joint4, joint5, joint6] +points: + - timestamp: 0.000093433 + positions: [ 0.3640007556342396, 1.022770385742187, -2.054268798828125, -1.5164462890625, 0.5084962304687494, -1.54] + - timestamp: 1.000050034 + positions: [0.3640007556342396, 0.871, -2.407, -0.064, 0.653, 0.01] + middle_pitch: 1.057 + - timestamp: 2.000050034 + positions: [0.244, 0.871, -2.407, -0.064, 0.653, 0.01] \ No newline at end of file diff --git a/engineer_trajectory_teaching/trajectory/big_island_right.yaml b/engineer_trajectory_teaching/trajectory/big_island_right.yaml new file mode 100644 index 0000000..56776cd --- /dev/null +++ b/engineer_trajectory_teaching/trajectory/big_island_right.yaml @@ -0,0 +1,16 @@ +joint_names: [joint1, joint2, joint3, joint4, joint5, joint6] +points: + - timestamp: 0.000050034 + positions: [ 0.01, -2.212, 1.862, -1.65, -0.565, -1.54 ] + middle_pitch: 0.06 + - timestamp: 1.0 + positions: [ 0.01, -1.466, 1.474, -1.65, -0.379, -1.54 ] + - timestamp: 1.5 + positions: [ 0.01, -1.466, 1.474, -1.65, -0.379, -1.54 ] + - timestamp: 1.7 + positions: [ 0.059, -1.466, 1.474, -1.65, -0.379, -1.54 ] + middle_pitch: 0.0 + - timestamp: 2.4 + positions: [ 0.059, -2.353, 1.802, -1.65, -0.932, -1.54 ] + - timestamp: 2.8 + positions: [ 0.059, -2.533, 1.806, -1.65, -1.052, -1.54 ] \ No newline at end of file diff --git a/engineer_trajectory_teaching/trajectory/big_island_right2.yaml b/engineer_trajectory_teaching/trajectory/big_island_right2.yaml new file mode 100644 index 0000000..56ac173 --- /dev/null +++ b/engineer_trajectory_teaching/trajectory/big_island_right2.yaml @@ -0,0 +1,9 @@ +joint_names: [joint1, joint2, joint3, joint4, joint5, joint6] +points: + - timestamp: 0.000093433 + positions: [ 0.3640007556342396, 1.12080810546875, -2.29154296875, -1.568243164062501, 0.08296142578125015, 1.5528779296875 ] + - timestamp: 1.000050034 + positions: [0.3640007556342396, 0.871, -2.407, -0.064, 0.653, 0.01] + middle_pitch: 1.057 + - timestamp: 2.000050034 + positions: [0.234, 0.871, -2.407, -0.064, 0.653, 0.01] \ No newline at end of file diff --git a/engineer_trajectory_teaching/trajectory/big_island_right_ready.yaml b/engineer_trajectory_teaching/trajectory/big_island_right_ready.yaml new file mode 100644 index 0000000..2eed9a4 --- /dev/null +++ b/engineer_trajectory_teaching/trajectory/big_island_right_ready.yaml @@ -0,0 +1,6 @@ +joint_names: [joint1, joint2, joint3, joint4, joint5, joint6] +points: + - timestamp: 0.000050034 + positions: [ 0.01, -2.212, 1.862, -1.65, -0.565, -1.54 ] + middle_pitch: 0.06 + main_gripper: true \ No newline at end of file diff --git a/engineer_trajectory_teaching/trajectory/get_second_stone.yaml b/engineer_trajectory_teaching/trajectory/get_second_stone.yaml new file mode 100644 index 0000000..063230c --- /dev/null +++ b/engineer_trajectory_teaching/trajectory/get_second_stone.yaml @@ -0,0 +1,23 @@ +joint_names: [joint1, joint2, joint3, joint4, joint5, joint6] +points: + - timestamp: 0.000050034 + positions: [ 0.3, -1.311, -2.272, -0.062, 1.648, -1.106 ] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + - timestamp: 0.2 + positions: [ 0.3, -1.311, -2.272, -0.062, 1.648, -1.106 ] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + main_gripper: true + - timestamp: 0.6 + positions: [ 0.139, -1.311, -2.272, -0.062, 1.648, -1.106 ] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + - timestamp: 1.0 + positions: [ 0.139, -1.311, -2.272, -0.062, 1.648, -1.106 ] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + - timestamp: 1.5 + positions: [ 0.4, -1.311, -2.272, -0.062, 1.648, -1.106 ] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + small_joint1: 0.001 + small_joint2: -0.001 + small_joint3: 0.001 + small_gripper: false + transfer_gripper: false \ No newline at end of file diff --git a/engineer_trajectory_teaching/trajectory/get_second_stone_side.yaml b/engineer_trajectory_teaching/trajectory/get_second_stone_side.yaml new file mode 100644 index 0000000..8e74e6e --- /dev/null +++ b/engineer_trajectory_teaching/trajectory/get_second_stone_side.yaml @@ -0,0 +1,30 @@ +joint_names: [joint1, joint2, joint3, joint4, joint5, joint6] +points: + - timestamp: 0.000050034 + positions: [ 0.26, 0.259, -2.569, -1.47, 0.738, -1.506 ] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + - timestamp: 0.2 + positions: [ 0.26, 0.259, -2.569, -1.47, 0.738, -1.506 ] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + main_gripper: true + small_joint2: -0.059 + - timestamp: 0.6 + positions: [ 0.055, 0.259, -2.569, -1.47, 0.738, -1.506 ] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + - timestamp: 0.8 + positions: [ 0.055, 0.259, -2.569, -1.47, 0.738, -1.506 ] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + - timestamp: 1.2 + positions: [ 0.055, -0.729, -2.538, -1.471, 0.0, -1.506 ] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + - timestamp: 1.6 + positions: [ 0.055, -0.729, -2.538, -1.471, 0.0, -1.506 ] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + small_joint1: 0.001 + small_joint2: -0.001 + small_joint3: 0.001 + small_gripper: false + transfer_gripper: false + - timestamp: 1.8 + positions: [ 0.3, -0.729, -2.538, -1.471, 0.0, -1.506 ] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] \ No newline at end of file diff --git a/engineer_trajectory_teaching/trajectory/get_transfer_stored.yaml b/engineer_trajectory_teaching/trajectory/get_transfer_stored.yaml new file mode 100644 index 0000000..14785e5 --- /dev/null +++ b/engineer_trajectory_teaching/trajectory/get_transfer_stored.yaml @@ -0,0 +1,26 @@ +joint_names: [joint1, joint2, joint3, joint4, joint5, joint6] +points: + - timestamp: 1.5 + positions: [0.55, -1.281, -2.189, -0.034, 1.575, -0.307] + middle_pitch: 0.071 + - timestamp: 2.0 + positions: [0.55, -2.142, -2.189, -0.034, 1.66, -0.307] + small_joint3: -0.01 + small_joint1: 0.238 + small_joint2: -0.176 + main_gripper: true + - timestamp: 2.3 + positions: [0.55, -2.142, -2.189, -0.034, 1.66, -0.307] + - timestamp: 2.45 + positions: [0.47, -2.142, -2.189, -0.034, 1.66, -0.307] + transfer_gripper: false + small_gripper: false + - timestamp: 2.8 + positions: [0.47, -2.142, -2.189, -0.034, 1.66, -0.307] + - timestamp: 3.0 + positions: [0.55, -2.142, -2.189, -0.034, 1.66, -0.307] + small_joint3: -0.01 + small_joint1: 0.01 + small_joint2: -0.01 + - timestamp: 3.5 + positions: [0.55, -1.281, -2.189, -0.034, 1.575, -0.307] \ No newline at end of file diff --git a/engineer_trajectory_teaching/trajectory/ground_stone1.yaml b/engineer_trajectory_teaching/trajectory/ground_stone1.yaml new file mode 100644 index 0000000..9f2e51e --- /dev/null +++ b/engineer_trajectory_teaching/trajectory/ground_stone1.yaml @@ -0,0 +1,10 @@ +joint_names: [joint1, joint2, joint3, joint4, joint5, joint6] +points: + - timestamp: 0.00004814 + positions: [0.09982151112444874, 1.058628540039062, -2.47007080078125, 0.009422851562500112, 0.4447973632812506, -0.01022265624999985] + - timestamp: 0.5 + positions: [0.003, 1.058628540039062, -2.47007080078125, 0.009422851562500112, 0.4447973632812506, -0.01022265624999985] + middle_pitch: 0.0 + - timestamp: 1.1 + positions: [0.003, -0.112, -2.401, 0.001, 1.128, 1.264] + main_gripper: true \ No newline at end of file diff --git a/engineer_trajectory_teaching/trajectory/ground_stone2.yaml b/engineer_trajectory_teaching/trajectory/ground_stone2.yaml new file mode 100644 index 0000000..aa546e1 --- /dev/null +++ b/engineer_trajectory_teaching/trajectory/ground_stone2.yaml @@ -0,0 +1,13 @@ +joint_names: [joint1, joint2, joint3, joint4, joint5, joint6] +points: + - timestamp: 0.00001 + positions: [0.003, -0.112, -2.401, 0.001, 1.128, 1.264] + main_gripper: true + middle_pitch: 0.585 + - timestamp: 0.9 + positions: [0.376, -0.112, -2.401, 0.001, 1.128, 1.264] + - timestamp: 1.4 + positions: [0.376, 0.871, -2.407, -0.064, 0.653, 0.645] + middle_pitch: 1.057 + - timestamp: 1.9 + positions: [0.234, 0.871, -2.407, -0.064, 0.653, 0.645] \ No newline at end of file diff --git a/engineer_trajectory_teaching/trajectory/small_island1.yaml b/engineer_trajectory_teaching/trajectory/small_island1.yaml new file mode 100644 index 0000000..94641d1 --- /dev/null +++ b/engineer_trajectory_teaching/trajectory/small_island1.yaml @@ -0,0 +1,40 @@ +joint_names: [joint1, joint2, joint3, joint4, joint5, joint6] +points: + - timestamp: 1.00004814 + positions: [0.06082151112444874, 1.058628540039062, -2.47007080078125, 0.009422851562500112, 0.4447973632812506, -0.01022265624999985] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + main_gripper: true + small_gripper: true + transfer_gripper: false + small_joint1: 0.24 + small_joint2: -0.389 + small_joint3: 1.61 + middle_pitch: 1.057 + - timestamp: 1.500048414 + positions: [0.1585558495207434, 0.5, -2.4, 0.007889648437500396, 0.639130859375, -0.2] + - timestamp: 2.2 + positions: [0.1837121461268273, 0.0, -2.1, -0.03120703124999946, 0.691, -0.2] + small_joint1: -0.015 + - timestamp: 2.900048747 + positions: [0.1831078093623446, -0.388, -1.95, -0.1691953125000005, 0.691, -0.2] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + - timestamp: 3.350049087 + positions: [0.152579457342083, -0.388, -1.95, -0.041, 0.691, -0.2] + - timestamp: 3.400049244 + positions: [0.07606699630681203, -0.388, -1.95, -0.041, 0.691, -0.2] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + - timestamp: 3.9 + positions: [0.07606699630681203, -0.388, -1.95, -0.041, 0.691, -0.2] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + - timestamp: 4.0 + positions: [0.1214794775059318, -0.388, -1.95, -0.041, 0.691, -0.2] + - timestamp: 4.4 + positions: [0.311261469950257, -0.388, -1.95, -0.041, 0.691, -0.2] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + - timestamp: 5.0 + positions: [0.3654601977690858, -0.07357360839843752, -2.331978759765625, -0.1216660156250005, 0.653, -0.2] + small_joint1: 0.24 + - timestamp: 5.5 + positions: [0.3640007556342396, 0.871, -2.407, -0.064, 0.653, 0.645] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + small_joint2: -0.176 \ No newline at end of file diff --git a/engineer_trajectory_teaching/trajectory/small_island2.yaml b/engineer_trajectory_teaching/trajectory/small_island2.yaml new file mode 100644 index 0000000..32c6849 --- /dev/null +++ b/engineer_trajectory_teaching/trajectory/small_island2.yaml @@ -0,0 +1,6 @@ +joint_names: [joint1, joint2, joint3, joint4, joint5, joint6] +points: + - timestamp: 0.000050034 + positions: [0.244, 0.871, -2.407, -0.064, 0.653, 0.645] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + small_joint1: 0.1805 \ No newline at end of file diff --git a/engineer_trajectory_teaching/trajectory/test.yaml b/engineer_trajectory_teaching/trajectory/test.yaml new file mode 100644 index 0000000..1db7d9f --- /dev/null +++ b/engineer_trajectory_teaching/trajectory/test.yaml @@ -0,0 +1,10 @@ +joint_names: [joint1, joint2, joint3, joint4, joint5, joint6] +points: + - timestamp: 4.245500000000001e-05 + positions: [0.091, 1.102, -2.499, -0.14, 0.003, 0.003] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] +# accelerations: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + - timestamp: 0.500042926 + positions: [0.091, -0.741, -2.499, -0.14, 0.003, 0.003] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] +# accelerations: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] \ No newline at end of file diff --git a/engineer_trajectory_teaching/trajectory/transfer.yaml b/engineer_trajectory_teaching/trajectory/transfer.yaml new file mode 100644 index 0000000..461892f --- /dev/null +++ b/engineer_trajectory_teaching/trajectory/transfer.yaml @@ -0,0 +1,49 @@ +joint_names: [joint1, joint2, joint3, joint4, joint5, joint6] +points: + - timestamp: 5.4941e-05 + positions: [0.2300330971860355, 0.871326904296875, -2.407509765625, -0.09061865234374911, 0.6533129882812492, 0.6448383789062495] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + small_joint1: 0.1805 + small_joint2: -0.176 + - timestamp: 0.3 + positions: [0.37, 0.2277874755859375, -2.301079711914062, -0.034, 1.575, 0.381] + middle_pitch: 0.071 + - timestamp: 0.6 + positions: [0.53, -0.64, -2.189, -0.034, 1.575, 0.16] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + - timestamp: 0.9 + positions: [0.53, -1.281, -2.189, -0.034, 1.575, 0.16] + - timestamp: 1.2 + positions: [0.53, -2.142, -2.189, -0.034, 1.66, 0.16] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + - timestamp: 1.5 + positions: [0.53, -2.142, -2.189, -0.034, 1.66, 0.16] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + - timestamp: 1.8 + positions: [0.42, -2.142, -2.189, -0.034, 1.66, 0.16] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + small_gripper: true + transfer_gripper: true + - timestamp: 2.1 + positions: [0.42, -2.142, -2.189, -0.034, 1.66, 0.16] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + main_gripper: false + - timestamp: 2.2 + positions: [0.53, -2.142, -2.189, -0.034, 1.66, 0.16] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + - timestamp: 2.7 + positions: [0.466, -1.242, -2.16, -0.538, 1.247, -0.4] + - timestamp: 3.1 + positions: [0.323, -1.242, -2.13, -1.406, 1.103, -1.447] + main_gripper: true + - timestamp: 3.4 + positions: [0.323, -1.779, -2.13, -1.406, 0.785, -1.447] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + - timestamp: 3.8 + positions: [0.323, -1.779, -2.13, -1.406, 0.785, -1.447] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + - timestamp: 4.3 + positions: [0.4, -1.779, -2.13, -1.406, 0.785, -1.447] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + small_gripper: true + transfer_gripper: false \ No newline at end of file diff --git a/engineer_trajectory_teaching/trajectory/transfer_store_stone.yaml b/engineer_trajectory_teaching/trajectory/transfer_store_stone.yaml new file mode 100644 index 0000000..720ddeb --- /dev/null +++ b/engineer_trajectory_teaching/trajectory/transfer_store_stone.yaml @@ -0,0 +1,41 @@ +joint_names: [joint1, joint2, joint3, joint4, joint5, joint6] +points: + - timestamp: 5.4941e-05 + positions: [0.2300330971860355, 0.871326904296875, -2.407509765625, -0.09061865234374911, 0.6533129882812492, 0.6448383789062495] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + middle_pitch: 0.071 + - timestamp: 0.3 + positions: [0.41, 0.2277874755859375, -2.301079711914062, -0.034, 1.575, 0.381] + small_joint3: -0.01 + small_joint1: 0.238 + small_joint2: -0.176 + - timestamp: 0.6 + positions: [0.55, -0.64, -2.189, -0.034, 1.575, 0.16] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + - timestamp: 0.9 + positions: [0.55, -1.281, -2.189, -0.034, 1.575, -0.307] + - timestamp: 1.2 + positions: [0.55, -2.142, -2.189, -0.034, 1.66, -0.307] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + - timestamp: 1.6 + positions: [0.55, -2.142, -2.189, -0.034, 1.66, -0.307] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + - timestamp: 2.1 + positions: [0.47, -2.142, -2.189, -0.034, 1.66, -0.307] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + small_gripper: true + transfer_gripper: true + - timestamp: 2.4 + positions: [0.47, -2.142, -2.189, -0.034, 1.66, -0.307] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + main_gripper: false + - timestamp: 2.6 + positions: [0.55, -2.142, -2.189, -0.034, 1.66, -0.307] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + - timestamp: 2.9 + positions: [0.55, -1.281, -2.189, -0.034, 1.575, -0.307] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + - timestamp: 4.4 + positions: [0.09671880342092676, 1.096012573242187, -2.492577514648437, -0.0004916992187505853, -0.0009105078125003496, -0.001002441406249166] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + small_joint2: -0.01 \ No newline at end of file