Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fetch use lower limit #293

Open
wants to merge 5 commits into
base: master
Choose a base branch
from
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion robowflex_library/include/robowflex_library/robot.h
Original file line number Diff line number Diff line change
Expand Up @@ -771,7 +771,7 @@ namespace robowflex
* \param[in] namespaced Whether or not the parameter server description is under the handler
* namespace.
*/
void initializeInternal(bool namespaced = true);
void initializeInternal(bool namespaced = true, const std::string &limits_file = "");

/** \brief Loads a robot model from the loaded information on the parameter server.
* \param[in] description Robot description on parameter server.
Expand Down
33 changes: 31 additions & 2 deletions robowflex_library/src/robot.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -82,7 +82,7 @@ bool Robot::initialize(const std::string &urdf_file, const std::string &srdf_fil
return false;
}

initializeInternal();
initializeInternal(true, limits_file);
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Why the limits file is not correctly loaded here . If there is a problem with this function, Then I think it should be fixed and not add a custom function inside robot.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Because when the underlying moveIt loads the robot, if it has access to the URDF and SRDF files, it skips loading the joint_limits file. Instead it only gets the limits from the URDF. So if we provide all of these three files, hoping to use our custom joint_limits file, moveIt would never read it and it's not used (see line 105 here).

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Inside initializeInternal is where we call the moveIt loadModel API. So the fix of loading the limits_file should follow that (if not in MoveIt).

return true;
}

Expand Down Expand Up @@ -305,7 +305,7 @@ void Robot::updateXMLString(std::string &string, const PostProcessXMLFunction &f
}
}

void Robot::initializeInternal(bool namespaced)
void Robot::initializeInternal(bool namespaced, const std::string &limits_file)
{
const std::string &description = ((namespaced) ? handler_.getNamespace() : "") + "/" + ROBOT_DESCRIPTION;

Expand All @@ -323,6 +323,35 @@ void Robot::initializeInternal(bool namespaced)
loadRobotModel(description);
}

// Load the joint limits if the file is available.
const auto &yaml = IO::loadFileToYAML(limits_file);
if (yaml.first && IO::isNode(yaml.second["joint_limits"]))
{
const auto &node = yaml.second;
for (auto &jmodel : model_->getJointModels())
{
auto bounds = jmodel->getVariableBounds();
auto jnames = jmodel->getVariableNames();
auto joint_limits = jmodel->getVariableBoundsMsg();
for (auto &jlim : joint_limits)
{
auto name = jlim.joint_name;
if (!IO::isNode(node["joint_limits"][name]))
continue;
if (IO::isNode(node["joint_limits"][name]["has_velocity_limits"]))
jlim.has_velocity_limits = node["joint_limits"][name]["has_velocity_limits"].as<bool>();
if (IO::isNode(node["joint_limits"][name]["max_velocity"]))
jlim.max_velocity = node["joint_limits"][name]["max_velocity"].as<double>();
if (IO::isNode(node["joint_limits"][name]["has_acceleration_limits"]))
jlim.has_acceleration_limits =
node["joint_limits"][name]["has_acceleration_limits"].as<bool>();
if (IO::isNode(node["joint_limits"][name]["max_acceleration"]))
jlim.max_acceleration = node["joint_limits"][name]["max_acceleration"].as<double>();
}
jmodel->setVariableBounds(joint_limits);
}
}

// set strings on parameter server
handler_.setParam(ROBOT_DESCRIPTION, urdf_);
handler_.setParam(ROBOT_DESCRIPTION + ROBOT_SEMANTIC, srdf_);
Expand Down