[MSA] Generate joint_limits.yaml and cartesian_limits.yaml

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_);

moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/data/srdf_config.hpp

@@ -36,7 +36,9 @@
 #pragma once
 
 #include <moveit_setup_framework/config.hpp>
+#include <moveit_setup_framework/templates.hpp>
 #include <moveit_setup_framework/data/urdf_config.hpp>
+#include <ament_index_cpp/get_package_share_directory.hpp>
 #include <moveit/robot_model/robot_model.h>
 #include <moveit/planning_scene/planning_scene.h>  // for getting kinematic model
 #include <srdfdom/srdf_writer.h>                   // 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<GeneratedFilePtr>& files) override
   {
     files.push_back(std::make_shared<GeneratedSRDF>(package_path, last_gen_time, *this));
+    files.push_back(std::make_shared<GeneratedJointLimits>(package_path, last_gen_time, *this));
+    files.push_back(std::make_shared<GeneratedCartesianLimits>(package_path, last_gen_time));
   }
 
   void collectVariables(std::vector<TemplateVariable>& variables) override;

moveit_setup_assistant/moveit_setup_framework/src/srdf_config.cpp

@@ -197,6 +197,103 @@ void SRDFConfig::collectVariables(std::vector<TemplateVariable>& 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<const moveit::core::JointModel*, JointModelCompare> 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<const moveit::core::JointModel*>& 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 <pluginlib/class_list_macros.hpp>  // NOLINT

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

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<const moveit::core::JointModel*, JointModelCompare> 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<const moveit::core::JointModel*>& 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
 // ******************************************************************************************