Skip to content

Commit

Permalink
[MSA] Generate joint_limits.yaml and cartesian_limits.yaml (moveit#1245)
Browse files Browse the repository at this point in the history
  • Loading branch information
@DLu
DLu committed Jun 13, 2022
1 parent 4f0560d commit 4e590b2
Show file tree
Hide file tree
Showing 6 changed files with 166 additions and 133 deletions.
21 changes: 0 additions & 21 deletions moveit_setup_assistant/extra_generated_files_code.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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_);
Expand Down
69 changes: 69 additions & 0 deletions ...etup_assistant/moveit_setup_framework/include/moveit_setup_framework/data/srdf_config.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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:
Expand Down Expand Up @@ -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;
Expand Down
97 changes: 97 additions & 0 deletions moveit_setup_assistant/moveit_setup_framework/src/srdf_config.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
0 ...nt/templates/config/cartesian_limits.yaml → ...rk/templates/config/cartesian_limits.yaml
View file
File renamed without changes.
15 changes: 0 additions & 15 deletions ...setup_assistant/old_assistant/include/moveit/setup_assistant/tools/moveit_config_data.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -132,7 +132,6 @@ class MoveItConfigData
bool outputOMPLPlanningYAML(const std::string& file_path);
bool outputSTOMPPlanningYAML(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);
Expand Down Expand Up @@ -252,20 +251,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
Expand Down
97 changes: 0 additions & 97 deletions moveit_setup_assistant/old_assistant/src/tools/moveit_config_data.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -930,103 +930,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
// ******************************************************************************************
Expand Down

0 comments on commit 4e590b2

Please sign in to comment.