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.

Sign up for GitHub

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

Jump to bottom

Fetch use lower limit #293

Open
wants to merge 5 commits into
base: master
Choose a base branch
from
Open

Fetch use lower limit #293

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
ab53c1a
Fetch add option to use lower joint limits file
tonypan1995 Jul 19, 2022
eca0483
fix
tonypan1995 Jul 19, 2022
08d9d65
load limit file when it's available
tonypan1995 Jul 23, 2022
b8e7fdc
Merge branch 'master' into fetch_use_lower_limit
tonypan1995 Jul 23, 2022
f218625
Fixed linting
ChamzasKonstantinos Jul 29, 2022
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
View file
Open in desktop
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
View file
Open in desktop
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