From 8dc50e4630acf7da1bc060ccbba91eb996216c0a Mon Sep 17 00:00:00 2001 From: "David V. Lu!!" Date: Fri, 13 May 2022 12:22:22 -0400 Subject: [PATCH] [MSA] Generate joint_limits.yaml and cartesian_limits.yaml (#1245) --- .../extra_generated_files_code.txt | 21 ---- .../data/srdf_config.hpp | 69 +++++++++++++ .../src/srdf_config.cpp | 97 +++++++++++++++++++ .../templates/config/cartesian_limits.yaml | 0 .../tools/moveit_config_data.hpp | 15 --- .../src/tools/moveit_config_data.cpp | 97 ------------------- 6 files changed, 166 insertions(+), 133 deletions(-) rename moveit_setup_assistant/{old_assistant => moveit_setup_framework}/templates/config/cartesian_limits.yaml (100%) diff --git a/moveit_setup_assistant/extra_generated_files_code.txt b/moveit_setup_assistant/extra_generated_files_code.txt index 0f2c31dd27..92d4603ac8 100644 --- a/moveit_setup_assistant/extra_generated_files_code.txt +++ b/moveit_setup_assistant/extra_generated_files_code.txt @@ -19,27 +19,6 @@ file.gen_func_ = std::bind(&MoveItConfigData::outputCHOMPPlanningYAML, config_data_, std::placeholders::_1); file.write_on_changes = MoveItConfigData::GROUPS; // need to double check if this is actually correct! - // joint_limits.yaml -------------------------------------------------------------------------------------- - file.file_name_ = "joint_limits.yaml"; - file.rel_path_ = config_data_->appendPaths(config_path, file.file_name_); - file.description_ = "Contains additional information about joints that appear in your planning groups that is not " - "contained in the URDF, as well as allowing you to set maximum and minimum limits for velocity " - "and acceleration than those contained in your URDF. This information is used by our trajectory " - "filtering system to assign reasonable velocities and timing for the trajectory before it is " - "passed to the robots controllers."; - file.gen_func_ = std::bind(&MoveItConfigData::outputJointLimitsYAML, config_data_, std::placeholders::_1); - file.write_on_changes = 0; // Can they be changed? - - // cartesian_limits.yaml -------------------------------------------------------------------------------------- - file.file_name_ = "cartesian_limits.yaml"; - file.rel_path_ = config_data_->appendPaths(config_path, file.file_name_); - template_path = config_data_->appendPaths(config_data_->template_package_path_, file.rel_path_); - file.description_ = "Cartesian velocity for planning in the workspace." - "The velocity is used by pilz industrial motion planner as maximum velocity for cartesian " - "planning requests scaled by the velocity scaling factor of an individual planning request."; - file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1); - file.write_on_changes = 0; // Can they be changed? - // fake_controllers.yaml -------------------------------------------------------------------------------------- file.file_name_ = "fake_controllers.yaml"; file.rel_path_ = config_data_->appendPaths(config_path, file.file_name_); diff --git a/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/data/srdf_config.hpp b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/data/srdf_config.hpp index 7be44762d5..563c6163c5 100644 --- a/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/data/srdf_config.hpp +++ b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/data/srdf_config.hpp @@ -36,7 +36,9 @@ #pragma once #include +#include #include +#include #include #include // for getting kinematic model #include // for writing srdf data @@ -57,6 +59,9 @@ enum InformationFields OTHER = 1 << 8, }; +static const std::string JOINT_LIMITS_FILE = "config/joint_limits.yaml"; +static const std::string CARTESIAN_LIMITS_FILE = "config/cartesian_limits.yaml"; + class SRDFConfig : public SetupConfig { public: @@ -181,10 +186,74 @@ class SRDFConfig : public SetupConfig SRDFConfig& parent_; }; + class GeneratedJointLimits : public YamlGeneratedFile + { + public: + GeneratedJointLimits(const std::string& package_path, const std::time_t& last_gen_time, SRDFConfig& parent) + : YamlGeneratedFile(package_path, last_gen_time), parent_(parent) + { + } + + std::string getRelativePath() const override + { + return JOINT_LIMITS_FILE; + } + + std::string getDescription() const override + { + return "Contains additional information about joints that appear in your planning groups that is not " + "contained in the URDF, as well as allowing you to set maximum and minimum limits for velocity " + "and acceleration than those contained in your URDF. This information is used by our trajectory " + "filtering system to assign reasonable velocities and timing for the trajectory before it is " + "passed to the robot's controllers."; + } + + bool hasChanges() const override + { + return false; // Can't be changed just yet + } + + bool writeYaml(YAML::Emitter& emitter) override; + + protected: + SRDFConfig& parent_; + }; + + class GeneratedCartesianLimits : public TemplatedGeneratedFile + { + public: + using TemplatedGeneratedFile::TemplatedGeneratedFile; + + std::string getRelativePath() const override + { + return CARTESIAN_LIMITS_FILE; + } + + std::string getTemplatePath() const override + { + return appendPaths(ament_index_cpp::get_package_share_directory("moveit_setup_framework"), + appendPaths("templates", CARTESIAN_LIMITS_FILE)); + } + + std::string getDescription() const override + { + return "Cartesian velocity for planning in the workspace." + "The velocity is used by pilz industrial motion planner as maximum velocity for cartesian " + "planning requests scaled by the velocity scaling factor of an individual planning request."; + } + + bool hasChanges() const override + { + return false; + } + }; + void collectFiles(const std::string& package_path, const std::time_t& last_gen_time, std::vector& files) override { files.push_back(std::make_shared(package_path, last_gen_time, *this)); + files.push_back(std::make_shared(package_path, last_gen_time, *this)); + files.push_back(std::make_shared(package_path, last_gen_time)); } void collectVariables(std::vector& variables) override; diff --git a/moveit_setup_assistant/moveit_setup_framework/src/srdf_config.cpp b/moveit_setup_assistant/moveit_setup_framework/src/srdf_config.cpp index d34d17f8be..93fda3c519 100644 --- a/moveit_setup_assistant/moveit_setup_framework/src/srdf_config.cpp +++ b/moveit_setup_assistant/moveit_setup_framework/src/srdf_config.cpp @@ -197,6 +197,103 @@ void SRDFConfig::collectVariables(std::vector& variables) variables.push_back(TemplateVariable("PLANNING_FRAME", robot_model_->getModelFrame())); } +/** + * \brief Custom std::set comparator, used for sorting the joint_limits.yaml file into alphabetical order + * \param jm1 - a pointer to the first joint model to compare + * \param jm2 - a pointer to the second joint model to compare + * \return bool of alphabetical sorting comparison + */ +struct JointModelCompare +{ + bool operator()(const moveit::core::JointModel* jm1, const moveit::core::JointModel* jm2) const + { + return jm1->getName() < jm2->getName(); + } +}; + +bool SRDFConfig::GeneratedJointLimits::writeYaml(YAML::Emitter& emitter) +{ + emitter << YAML::Comment("joint_limits.yaml allows the dynamics properties specified in the URDF " + "to be overwritten or augmented as needed"); + emitter << YAML::Newline; + + emitter << YAML::BeginMap; + + emitter << YAML::Comment("For beginners, we downscale velocity and acceleration limits.") << YAML::Newline; + emitter << YAML::Comment("You can always specify higher scaling factors (<= 1.0) in your motion requests."); + emitter << YAML::Comment("Increase the values below to 1.0 to always move at maximum speed."); + emitter << YAML::Key << "default_velocity_scaling_factor"; + emitter << YAML::Value << "0.1"; + + emitter << YAML::Key << "default_acceleration_scaling_factor"; + emitter << YAML::Value << "0.1"; + + emitter << YAML::Newline << YAML::Newline; + emitter << YAML::Comment("Specific joint properties can be changed with the keys " + "[max_position, min_position, max_velocity, max_acceleration]") + << YAML::Newline; + emitter << YAML::Comment("Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits]"); + + emitter << YAML::Key << "joint_limits"; + emitter << YAML::Value << YAML::BeginMap; + + // Union all the joints in groups. Uses a custom comparator to allow the joints to be sorted by name + std::set joints; + + // Loop through groups + for (srdf::Model::Group& group : parent_.srdf_.groups_) + { + // Get list of associated joints + const moveit::core::JointModelGroup* joint_model_group = parent_.getRobotModel()->getJointModelGroup(group.name_); + + const std::vector& joint_models = joint_model_group->getJointModels(); + + // Iterate through the joints + for (const moveit::core::JointModel* joint_model : joint_models) + { + // Check that this joint only represents 1 variable. + if (joint_model->getVariableCount() == 1) + joints.insert(joint_model); + } + } + + // Add joints to yaml file, if no more than 1 dof + for (const moveit::core::JointModel* joint : joints) + { + emitter << YAML::Key << joint->getName(); + emitter << YAML::Value << YAML::BeginMap; + + const moveit::core::VariableBounds& b = joint->getVariableBounds()[0]; + + // Output property + emitter << YAML::Key << "has_velocity_limits"; + if (b.velocity_bounded_) + emitter << YAML::Value << "true"; + else + emitter << YAML::Value << "false"; + + // Output property + emitter << YAML::Key << "max_velocity"; + emitter << YAML::Value << std::min(fabs(b.max_velocity_), fabs(b.min_velocity_)); + + // Output property + emitter << YAML::Key << "has_acceleration_limits"; + if (b.acceleration_bounded_) + emitter << YAML::Value << "true"; + else + emitter << YAML::Value << "false"; + + // Output property + emitter << YAML::Key << "max_acceleration"; + emitter << YAML::Value << std::min(fabs(b.max_acceleration_), fabs(b.min_acceleration_)); + + emitter << YAML::EndMap; + } + + emitter << YAML::EndMap; + return true; // file created successfully +} + } // namespace moveit_setup_framework #include // NOLINT diff --git a/moveit_setup_assistant/old_assistant/templates/config/cartesian_limits.yaml b/moveit_setup_assistant/moveit_setup_framework/templates/config/cartesian_limits.yaml similarity index 100% rename from moveit_setup_assistant/old_assistant/templates/config/cartesian_limits.yaml rename to moveit_setup_assistant/moveit_setup_framework/templates/config/cartesian_limits.yaml diff --git a/moveit_setup_assistant/old_assistant/include/moveit/setup_assistant/tools/moveit_config_data.hpp b/moveit_setup_assistant/old_assistant/include/moveit/setup_assistant/tools/moveit_config_data.hpp index 6fb890f719..a06932efee 100644 --- a/moveit_setup_assistant/old_assistant/include/moveit/setup_assistant/tools/moveit_config_data.hpp +++ b/moveit_setup_assistant/old_assistant/include/moveit/setup_assistant/tools/moveit_config_data.hpp @@ -131,7 +131,6 @@ class MoveItConfigData bool outputOMPLPlanningYAML(const std::string& file_path); bool outputCHOMPPlanningYAML(const std::string& file_path); bool outputKinematicsYAML(const std::string& file_path); - bool outputJointLimitsYAML(const std::string& file_path); bool outputFakeControllersYAML(const std::string& file_path); bool outputSimpleControllersYAML(const std::string& file_path); bool outputROSControllersYAML(const std::string& file_path); @@ -251,20 +250,6 @@ class MoveItConfigData */ srdf::Model::GroupState getDefaultStartPose(); - /** - * \brief Custom std::set comparator, used for sorting the joint_limits.yaml file into alphabetical order - * \param jm1 - a pointer to the first joint model to compare - * \param jm2 - a pointer to the second joint model to compare - * \return bool of alphabetical sorting comparison - */ - struct JointModelCompare - { - bool operator()(const moveit::core::JointModel* jm1, const moveit::core::JointModel* jm2) const - { - return jm1->getName() < jm2->getName(); - } - }; - private: // ****************************************************************************************** // Private Vars diff --git a/moveit_setup_assistant/old_assistant/src/tools/moveit_config_data.cpp b/moveit_setup_assistant/old_assistant/src/tools/moveit_config_data.cpp index 7342fa60c9..ffc50a81b6 100644 --- a/moveit_setup_assistant/old_assistant/src/tools/moveit_config_data.cpp +++ b/moveit_setup_assistant/old_assistant/src/tools/moveit_config_data.cpp @@ -1036,103 +1036,6 @@ bool MoveItConfigData::outputROSControllersYAML(const std::string& file_path) return true; // file created successfully } -// ****************************************************************************************** -// Output joint limits config files -// ****************************************************************************************** -bool MoveItConfigData::outputJointLimitsYAML(const std::string& file_path) -{ - YAML::Emitter emitter; - emitter << YAML::Comment("joint_limits.yaml allows the dynamics properties specified in the URDF " - "to be overwritten or augmented as needed"); - emitter << YAML::Newline; - - emitter << YAML::BeginMap; - - emitter << YAML::Comment("For beginners, we downscale velocity and acceleration limits.") << YAML::Newline; - emitter << YAML::Comment("You can always specify higher scaling factors (<= 1.0) in your motion requests."); - emitter << YAML::Comment("Increase the values below to 1.0 to always move at maximum speed."); - emitter << YAML::Key << "default_velocity_scaling_factor"; - emitter << YAML::Value << "0.1"; - - emitter << YAML::Key << "default_acceleration_scaling_factor"; - emitter << YAML::Value << "0.1"; - - emitter << YAML::Newline << YAML::Newline; - emitter << YAML::Comment("Specific joint properties can be changed with the keys " - "[max_position, min_position, max_velocity, max_acceleration]") - << YAML::Newline; - emitter << YAML::Comment("Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits]"); - - emitter << YAML::Key << "joint_limits"; - emitter << YAML::Value << YAML::BeginMap; - - // Union all the joints in groups. Uses a custom comparator to allow the joints to be sorted by name - std::set joints; - - // Loop through groups - for (srdf::Model::Group& group : srdf_->groups_) - { - // Get list of associated joints - const moveit::core::JointModelGroup* joint_model_group = getRobotModel()->getJointModelGroup(group.name_); - - const std::vector& joint_models = joint_model_group->getJointModels(); - - // Iterate through the joints - for (const moveit::core::JointModel* joint_model : joint_models) - { - // Check that this joint only represents 1 variable. - if (joint_model->getVariableCount() == 1) - joints.insert(joint_model); - } - } - - // Add joints to yaml file, if no more than 1 dof - for (const moveit::core::JointModel* joint : joints) - { - emitter << YAML::Key << joint->getName(); - emitter << YAML::Value << YAML::BeginMap; - - const moveit::core::VariableBounds& b = joint->getVariableBounds()[0]; - - // Output property - emitter << YAML::Key << "has_velocity_limits"; - if (b.velocity_bounded_) - emitter << YAML::Value << "true"; - else - emitter << YAML::Value << "false"; - - // Output property - emitter << YAML::Key << "max_velocity"; - emitter << YAML::Value << std::min(fabs(b.max_velocity_), fabs(b.min_velocity_)); - - // Output property - emitter << YAML::Key << "has_acceleration_limits"; - if (b.acceleration_bounded_) - emitter << YAML::Value << "true"; - else - emitter << YAML::Value << "false"; - - // Output property - emitter << YAML::Key << "max_acceleration"; - emitter << YAML::Value << std::min(fabs(b.max_acceleration_), fabs(b.min_acceleration_)); - - emitter << YAML::EndMap; - } - - emitter << YAML::EndMap; - - std::ofstream output_stream(file_path.c_str(), std::ios_base::trunc); - if (!output_stream.good()) - { - RCLCPP_ERROR_STREAM(LOGGER, "Unable to open file for writing " << file_path); - return false; - } - output_stream << emitter.c_str(); - output_stream.close(); - - return true; // file created successfully -} - // ****************************************************************************************** // Decide the best two joints to be used for the projection evaluator // ******************************************************************************************