From 9c0cb674585558e5339a1bfe2424c4aab6732411 Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Tue, 2 Aug 2022 08:00:35 -0600 Subject: [PATCH] Generate parameters from yaml Signed-off-by: Tyler Weaver --- CMakeLists.txt | 8 ++++- package.xml | 7 +++-- src/bio_ik_parameters.yaml | 56 ++++++++++++++++++++++++++++++++++ src/ik_base.h | 2 +- src/ik_evolution_1.cpp | 8 ++--- src/ik_parallel.h | 8 ++--- src/kinematics_plugin.cpp | 61 ++++++++++---------------------------- src/problem.cpp | 6 ++-- src/utils.h | 20 +++---------- upstream.repos | 13 ++++++++ 10 files changed, 111 insertions(+), 78 deletions(-) create mode 100644 src/bio_ik_parameters.yaml create mode 100644 upstream.repos diff --git a/CMakeLists.txt b/CMakeLists.txt index 513804a0..e0061aae 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -19,12 +19,17 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS tf2_ros tf2_kdl tf2_geometry_msgs + generate_parameter_library ) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() +generate_parameter_library(bio_ik_parameters + src/bio_ik_parameters.yaml +) + find_package(OpenMP) # the specific flag is not yet present in cmake 2.8.12 if(OpenMP_CXX_FOUND OR OPENMP_FOUND) @@ -119,6 +124,7 @@ target_link_libraries( PUBLIC ${FANN_LIBRARIES} ${OpenMP_LIBS} + bio_ik_parameters -static-libgcc -static-libstdc++ @@ -146,7 +152,7 @@ install( DESTINATION include ) install( - TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_plugin + TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_plugin bio_ik_parameters EXPORT export_${PROJECT_NAME} LIBRARY DESTINATION lib ARCHIVE DESTINATION lib diff --git a/package.xml b/package.xml index 13457490..cdd99971 100644 --- a/package.xml +++ b/package.xml @@ -14,15 +14,16 @@ ament_cmake_ros - moveit_core - pluginlib eigen + generate_parameter_library + moveit_core moveit_ros_planning + pluginlib rclcpp tf2 tf2_eigen - tf2_kdl tf2_geometry_msgs + tf2_kdl ament_cmake_gtest diff --git a/src/bio_ik_parameters.yaml b/src/bio_ik_parameters.yaml new file mode 100644 index 00000000..ca52d693 --- /dev/null +++ b/src/bio_ik_parameters.yaml @@ -0,0 +1,56 @@ +bio_ik: + mode: + type: string + default_value: "bio2_memetic" + profiler: + type: bool + default_value: false + counter: + type: bool + default_value: false + threads: + type: int + default_value: 0 + random_seed: + type: int + default_value: 0 + description: "The default value of 0 results in seeding random with std::random_device" + dpos: + type: double + default_value: .inf + drot: + type: double + default_value: .inf + dtwist: + type: double + default_value: 0.00001 + no_wipeout: + type: bool + default_value: false + population_size: + type: int + default_value: 8 + elite_count: + type: int + default_value: 4 + linear_fitness: + type: bool + default_value: false + rotation_scale: + type: double + default_value: 0.5 + position_only_ik: + type: bool + default_value: false + center_joints_weight: + type: double + default_value: 0 + avoid_joint_limits_weight: + type: double + default_value: 0 + minimal_displacement_weight: + type: double + default_value: 0 + + + diff --git a/src/ik_base.h b/src/ik_base.h index 74f5de04..b3bcb3e6 100644 --- a/src/ik_base.h +++ b/src/ik_base.h @@ -142,7 +142,7 @@ struct IKBase : Random virtual void setParams(const IKParams& p) {} IKBase(const IKParams& p) - : Random(p.random_seed) + : Random(p.ros_params.random_seed) , model(p.robot_model) , modelInfo(p.robot_model) , params(p) diff --git a/src/ik_evolution_1.cpp b/src/ik_evolution_1.cpp index ecbfa7a1..8f949326 100644 --- a/src/ik_evolution_1.cpp +++ b/src/ik_evolution_1.cpp @@ -132,10 +132,10 @@ struct IKEvolution1 : IKBase void setParams(const IKParams& p) { - opt_no_wipeout = p.opt_no_wipeout; - populationSize = p.population_size; - eliteCount = p.elite_count; - linear_fitness = p.linear_fitness; + opt_no_wipeout = p.ros_params.no_wipeout; + populationSize = p.ros_params.population_size; + eliteCount = p.ros_params.elite_count; + linear_fitness = p.ros_params.linear_fitness; } bool in_final_adjustment_loop; diff --git a/src/ik_parallel.h b/src/ik_parallel.h index 0441a72c..0bbb4e70 100644 --- a/src/ik_parallel.h +++ b/src/ik_parallel.h @@ -111,15 +111,15 @@ struct IKParallel : params(params) { // solver class name - std::string name = params.solver_class_name; + std::string name = params.ros_params.mode; - enable_counter = params.enable_counter; + enable_counter = params.ros_params.counter; // create solvers solvers.emplace_back(IKFactory::create(name, params)); thread_count = solvers.front()->concurrency(); - if(params.thread_count) { - thread_count = params.thread_count; + if(params.ros_params.threads != 0) { + thread_count = params.ros_params.threads; } while(solvers.size() < thread_count) solvers.emplace_back(IKFactory::clone(solvers.front().get())); diff --git a/src/kinematics_plugin.cpp b/src/kinematics_plugin.cpp index 1fc28b96..7ef1302e 100644 --- a/src/kinematics_plugin.cpp +++ b/src/kinematics_plugin.cpp @@ -114,9 +114,8 @@ struct BioIKKinematicsPlugin : kinematics::KinematicsBase { mutable std::unique_ptr temp_state; mutable std::vector tipFrames; RobotInfo robot_info; - bool enable_profiler; - BioIKKinematicsPlugin() { enable_profiler = false; } + BioIKKinematicsPlugin() { ikparams.ros_params.profiler = false; } virtual const std::vector &getJointNames() const { LOG_FNC(); @@ -152,27 +151,20 @@ struct BioIKKinematicsPlugin : kinematics::KinematicsBase { mutable std::vector all_goals; + std::shared_ptr param_listener; IKParams ikparams; mutable Problem problem; - template - void getRosParam(const std::string ¶m, T &val, const T &default_val) - { - const std::string prefix = "robot_description_kinematics." + group_name_ + "."; - if (!node_->has_parameter(prefix + param)) - { - val = node_->declare_parameter(prefix + param, rclcpp::ParameterValue{default_val}).get(); - return; - } - val = node_->get_parameter(prefix + param).get_value(); - } - bool load(std::string group_name) { LOG_FNC(); LOG("bio ik init", node_->getName()); + // Declare/get ros parameters + param_listener = std::make_shared(node_); + ikparams.ros_params = param_listener->get_params(); + joint_model_group = robot_model_->getJointModelGroup(group_name); if (!joint_model_group) { LOG("failed to get joint model group"); @@ -194,32 +186,15 @@ struct BioIKKinematicsPlugin : kinematics::KinematicsBase { link_names = tip_frames_; - // bool enable_profiler; - getRosParam("profiler", enable_profiler, false); - // if(enable_profiler) Profiler::start(); - robot_info = RobotInfo(robot_model_); ikparams.robot_model = robot_model_; ikparams.joint_model_group = joint_model_group; // initialize parameters for IKParallel - getRosParam("mode", ikparams.solver_class_name, - std::string("bio2_memetic")); - getRosParam("counter", ikparams.enable_counter, false); - getRosParam("threads", ikparams.thread_count, 0); - getRosParam("random_seed", ikparams.random_seed, static_cast(std::random_device()())); - - // initialize parameters for Problem - getRosParam("dpos", ikparams.dpos, DBL_MAX); - getRosParam("drot", ikparams.drot, DBL_MAX); - getRosParam("dtwist", ikparams.dtwist, 1e-5); - - // initialize parameters for ik_evolution_1 - getRosParam("no_wipeout", ikparams.opt_no_wipeout, false); - getRosParam("population_size", ikparams.population_size, 8); - getRosParam("elite_count", ikparams.elite_count, 4); - getRosParam("linear_fitness", ikparams.linear_fitness, false); + if (ikparams.ros_params.random_seed == 0) { + ikparams.ros_params.random_seed = static_cast(std::random_device()()); + } temp_state.reset(new moveit::core::RobotState(robot_model_)); @@ -238,12 +213,9 @@ struct BioIKKinematicsPlugin : kinematics::KinematicsBase { // LOG_VAR(goal->link_name); - double rotation_scale = 0.5; - - getRosParam("rotation_scale", rotation_scale, rotation_scale); + double rotation_scale = ikparams.ros_params.rotation_scale; + bool position_only_ik = ikparams.ros_params.position_only_ik; - bool position_only_ik = false; - getRosParam("position_only_ik", position_only_ik, position_only_ik); if (position_only_ik) rotation_scale = 0; @@ -253,8 +225,7 @@ struct BioIKKinematicsPlugin : kinematics::KinematicsBase { } { - double weight = 0; - getRosParam("center_joints_weight", weight, weight); + double weight = ikparams.ros_params.center_joints_weight; if (weight > 0.0) { auto *center_joints_goal = new bio_ik::CenterJointsGoal(); center_joints_goal->setWeight(weight); @@ -263,8 +234,7 @@ struct BioIKKinematicsPlugin : kinematics::KinematicsBase { } { - double weight = 0; - getRosParam("avoid_joint_limits_weight", weight, weight); + double weight = ikparams.ros_params.avoid_joint_limits_weight; if (weight > 0.0) { auto *avoid_joint_limits_goal = new bio_ik::AvoidJointLimitsGoal(); avoid_joint_limits_goal->setWeight(weight); @@ -273,8 +243,7 @@ struct BioIKKinematicsPlugin : kinematics::KinematicsBase { } { - double weight = 0; - getRosParam("minimal_displacement_weight", weight, weight); + double weight = ikparams.ros_params.minimal_displacement_weight; if (weight > 0.0) { auto *minimal_displacement_goal = new bio_ik::MinimalDisplacementGoal(); @@ -395,7 +364,7 @@ struct BioIKKinematicsPlugin : kinematics::KinematicsBase { const moveit::core::RobotState *context_state = NULL) const { std::chrono::time_point> t0 = std::chrono::system_clock::now(); - if (enable_profiler) + if (ikparams.ros_params.profiler) Profiler::start(); auto *bio_ik_options = toBioIKKinematicsQueryOptions(&options); diff --git a/src/problem.cpp b/src/problem.cpp index 5b5f25d1..2e722416 100644 --- a/src/problem.cpp +++ b/src/problem.cpp @@ -87,9 +87,9 @@ void Problem::initialize(moveit::core::RobotModelConstPtr robot_model, const mov if(!ros_params_initrd) { ros_params_initrd = true; - dpos = params.dpos; - drot = params.drot; - dtwist = params.dtwist; + dpos = params.ros_params.dpos; + drot = params.ros_params.drot; + dtwist = params.ros_params.dtwist; if(dpos < 0.0 || dpos >= FLT_MAX || !std::isfinite(dpos)) dpos = DBL_MAX; if(drot < 0.0 || drot >= FLT_MAX || !std::isfinite(drot)) drot = DBL_MAX; if(dtwist < 0.0 || dtwist >= FLT_MAX || !std::isfinite(dtwist)) dtwist = DBL_MAX; diff --git a/src/utils.h b/src/utils.h index 43b5cad6..b2378e37 100644 --- a/src/utils.h +++ b/src/utils.h @@ -53,6 +53,8 @@ //#include #include +#include "bio_ik_parameters.hpp" + namespace bio_ik { @@ -61,22 +63,8 @@ struct IKParams moveit::core::RobotModelConstPtr robot_model; const moveit::core::JointModelGroup* joint_model_group; - // IKParallel parameters - std::string solver_class_name; - bool enable_counter; - int thread_count; - int random_seed; - - //Problem parameters - double dpos; - double drot; - double dtwist; - - // ik_evolution_1 parameters - bool opt_no_wipeout; - int population_size; - int elite_count; - bool linear_fitness; + // ROS Parameters + Params ros_params; }; // Uncomment to enable logging diff --git a/upstream.repos b/upstream.repos new file mode 100644 index 00000000..98ca4f11 --- /dev/null +++ b/upstream.repos @@ -0,0 +1,13 @@ +repositories: + generate_parameter_library: + type: git + url: https://github.com/PickNikRobotics/generate_parameter_library.git + version: main + moveit2: + type: git + url: https://github.com/ros-planning/moveit2.git + version: main + ros2_control: + type: git + url: https://github.com/ros-controls/ros2_control.git + version: master