diff --git a/robowflex_library/include/robowflex_library/robot.h b/robowflex_library/include/robowflex_library/robot.h index 9153d0686..500f96f80 100644 --- a/robowflex_library/include/robowflex_library/robot.h +++ b/robowflex_library/include/robowflex_library/robot.h @@ -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. diff --git a/robowflex_library/src/robot.cpp b/robowflex_library/src/robot.cpp index 931a6e1eb..f919bbf16 100644 --- a/robowflex_library/src/robot.cpp +++ b/robowflex_library/src/robot.cpp @@ -82,7 +82,7 @@ bool Robot::initialize(const std::string &urdf_file, const std::string &srdf_fil return false; } - initializeInternal(); + initializeInternal(true, limits_file); return true; } @@ -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; @@ -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(); + if (IO::isNode(node["joint_limits"][name]["max_velocity"])) + jlim.max_velocity = node["joint_limits"][name]["max_velocity"].as(); + if (IO::isNode(node["joint_limits"][name]["has_acceleration_limits"])) + jlim.has_acceleration_limits = + node["joint_limits"][name]["has_acceleration_limits"].as(); + if (IO::isNode(node["joint_limits"][name]["max_acceleration"])) + jlim.max_acceleration = node["joint_limits"][name]["max_acceleration"].as(); + } + jmodel->setVariableBounds(joint_limits); + } + } + // set strings on parameter server handler_.setParam(ROBOT_DESCRIPTION, urdf_); handler_.setParam(ROBOT_DESCRIPTION + ROBOT_SEMANTIC, srdf_);