From ab53c1a15d19f3aa552d2332510e5629579cddbc Mon Sep 17 00:00:00 2001 From: Tianyang Pan <1045380922@qq.com> Date: Tue, 19 Jul 2022 16:21:31 -0500 Subject: [PATCH 1/4] Fetch add option to use lower joint limits file --- .../include/robowflex_library/detail/fetch.h | 3 ++- robowflex_library/src/detail/fetch.cpp | 11 ++++++++--- 2 files changed, 10 insertions(+), 4 deletions(-) diff --git a/robowflex_library/include/robowflex_library/detail/fetch.h b/robowflex_library/include/robowflex_library/detail/fetch.h index f784e63e0..a22f556d0 100644 --- a/robowflex_library/include/robowflex_library/detail/fetch.h +++ b/robowflex_library/include/robowflex_library/detail/fetch.h @@ -32,9 +32,10 @@ namespace robowflex /** \brief Initialize the robot with arm and arm_with_torso kinematics. * \param[in] addVirtual flag to add virtual joint. + * \param[in] use_low_limits flag to use lower joint limits. * \return True on success, false on failure. */ - bool initialize(bool addVirtual = true); + bool initialize(bool addVirtual = true, bool use_low_limits = false); /** \brief Inserts the caster links if they don't exist. * \param[in] doc urdf description to be processed. diff --git a/robowflex_library/src/detail/fetch.cpp b/robowflex_library/src/detail/fetch.cpp index f8a686da8..fa0333c04 100644 --- a/robowflex_library/src/detail/fetch.cpp +++ b/robowflex_library/src/detail/fetch.cpp @@ -18,6 +18,8 @@ const std::string // const std::string FetchRobot::RESOURCE_URDF{"package://robowflex_resources/fetch/robots/fetch.urdf"}; const std::string FetchRobot::RESOURCE_SRDF{"package://robowflex_resources/fetch/config/fetch.srdf"}; const std::string FetchRobot::RESOURCE_LIMITS{"package://robowflex_resources/fetch/config/joint_limits.yaml"}; +const std::string // + FetchRobot::RESOURCE_LIMITS_LOW{"package://robowflex_resources/fetch/config/joint_limits_low.yaml"}; const std::string // FetchRobot::RESOURCE_KINEMATICS{"package://robowflex_resources/fetch/config/kinematics.yaml"}; const std::string // @@ -28,7 +30,7 @@ FetchRobot::FetchRobot() : Robot("fetch") { } -bool FetchRobot::initialize(bool addVirtual) +bool FetchRobot::initialize(bool addVirtual, bool use_low_limits) { if (addVirtual) setSRDFPostProcessAddPlanarJoint("base_joint"); @@ -36,7 +38,6 @@ bool FetchRobot::initialize(bool addVirtual) setURDFPostProcessFunction([this](tinyxml2::XMLDocument &doc) { return addCastersURDF(doc); }); bool success = false; - // First attempt the `robowflex_resources` package, then attempt the "actual" resource files. if (IO::resolvePackage(RESOURCE_URDF).empty() or IO::resolvePackage(RESOURCE_SRDF).empty()) { @@ -46,7 +47,11 @@ bool FetchRobot::initialize(bool addVirtual) else { RBX_INFO("Initializing Fetch with `robowflex_resources`"); - success = Robot::initialize(RESOURCE_URDF, RESOURCE_SRDF, RESOURCE_LIMITS, RESOURCE_KINEMATICS); + if (use_low_limits) + success = + Robot::initialize(RESOURCE_URDF, RESOURCE_SRDF, RESOURCE_LIMITS_LOW, RESOURCE_KINEMATICS); + else + success = Robot::initialize(RESOURCE_URDF, RESOURCE_SRDF, RESOURCE_LIMITS, RESOURCE_KINEMATICS); } loadKinematics("arm"); From eca0483e622fdd2c8d969319a58079671ecf423a Mon Sep 17 00:00:00 2001 From: Tianyang Pan <1045380922@qq.com> Date: Tue, 19 Jul 2022 16:36:15 -0500 Subject: [PATCH 2/4] fix --- robowflex_library/include/robowflex_library/detail/fetch.h | 1 + 1 file changed, 1 insertion(+) diff --git a/robowflex_library/include/robowflex_library/detail/fetch.h b/robowflex_library/include/robowflex_library/detail/fetch.h index a22f556d0..d30d19679 100644 --- a/robowflex_library/include/robowflex_library/detail/fetch.h +++ b/robowflex_library/include/robowflex_library/detail/fetch.h @@ -72,6 +72,7 @@ namespace robowflex static const std::string RESOURCE_URDF; ///< URDF from robowflex_resources static const std::string RESOURCE_SRDF; ///< SRDF from robowflex_resources static const std::string RESOURCE_LIMITS; ///< Limits from robowflex_resources + static const std::string RESOURCE_LIMITS_LOW; ///< Lower limits from robowflex_resources static const std::string RESOURCE_KINEMATICS; ///< kinematics from robowflex_resources }; From 08d9d659de0a2ed7a04554086172bc2c9e22ac5c Mon Sep 17 00:00:00 2001 From: Tianyang Pan <1045380922@qq.com> Date: Sat, 23 Jul 2022 11:00:36 -0500 Subject: [PATCH 3/4] load limit file when it's available --- .../include/robowflex_library/robot.h | 2 +- robowflex_library/src/robot.cpp | 112 ++++++++++++------ 2 files changed, 75 insertions(+), 39 deletions(-) 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..ba9af5910 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_); @@ -421,32 +450,36 @@ bool Robot::loadKinematics(const std::string &group_name, bool load_subgroups) void Robot::setSRDFPostProcessAddPlanarJoint(const std::string &name) { - setSRDFPostProcessFunction([&, name](tinyxml2::XMLDocument &doc) -> bool { - tinyxml2::XMLElement *virtual_joint = doc.NewElement("virtual_joint"); - virtual_joint->SetAttribute("name", name.c_str()); - virtual_joint->SetAttribute("type", "planar"); - virtual_joint->SetAttribute("parent_frame", "world"); - virtual_joint->SetAttribute("child_link", model_->getRootLink()->getName().c_str()); + setSRDFPostProcessFunction( + [&, name](tinyxml2::XMLDocument &doc) -> bool + { + tinyxml2::XMLElement *virtual_joint = doc.NewElement("virtual_joint"); + virtual_joint->SetAttribute("name", name.c_str()); + virtual_joint->SetAttribute("type", "planar"); + virtual_joint->SetAttribute("parent_frame", "world"); + virtual_joint->SetAttribute("child_link", model_->getRootLink()->getName().c_str()); - doc.FirstChildElement("robot")->InsertFirstChild(virtual_joint); + doc.FirstChildElement("robot")->InsertFirstChild(virtual_joint); - return true; - }); + return true; + }); } void Robot::setSRDFPostProcessAddFloatingJoint(const std::string &name) { - setSRDFPostProcessFunction([&, name](tinyxml2::XMLDocument &doc) -> bool { - tinyxml2::XMLElement *virtual_joint = doc.NewElement("virtual_joint"); - virtual_joint->SetAttribute("name", name.c_str()); - virtual_joint->SetAttribute("type", "floating"); - virtual_joint->SetAttribute("parent_frame", "world"); - virtual_joint->SetAttribute("child_link", model_->getRootLink()->getName().c_str()); + setSRDFPostProcessFunction( + [&, name](tinyxml2::XMLDocument &doc) -> bool + { + tinyxml2::XMLElement *virtual_joint = doc.NewElement("virtual_joint"); + virtual_joint->SetAttribute("name", name.c_str()); + virtual_joint->SetAttribute("type", "floating"); + virtual_joint->SetAttribute("parent_frame", "world"); + virtual_joint->SetAttribute("child_link", model_->getRootLink()->getName().c_str()); - doc.FirstChildElement("robot")->InsertFirstChild(virtual_joint); + doc.FirstChildElement("robot")->InsertFirstChild(virtual_joint); - return true; - }); + return true; + }); } const std::string &Robot::getModelName() const @@ -757,34 +790,37 @@ void Robot::IKQuery::addMetric(const Metric &metric_function) void Robot::IKQuery::addDistanceMetric(double weight) { addMetric([weight](const robot_state::RobotState &state, const SceneConstPtr &scene, - const kinematic_constraints::ConstraintEvaluationResult &result) { - return weight * result.distance; - }); + const kinematic_constraints::ConstraintEvaluationResult &result) + { return weight * result.distance; }); } void Robot::IKQuery::addCenteringMetric(double weight) { - addMetric([&, weight](const robot_state::RobotState &state, const SceneConstPtr &scene, - const kinematic_constraints::ConstraintEvaluationResult &result) { - const auto &jmg = state.getJointModelGroup(group); - const auto &min = state.getMinDistanceToPositionBounds(jmg); - double extent = min.second->getMaximumExtent() / 2.; - return weight * (extent - min.first) / extent; - }); + addMetric( + [&, weight](const robot_state::RobotState &state, const SceneConstPtr &scene, + const kinematic_constraints::ConstraintEvaluationResult &result) + { + const auto &jmg = state.getJointModelGroup(group); + const auto &min = state.getMinDistanceToPositionBounds(jmg); + double extent = min.second->getMaximumExtent() / 2.; + return weight * (extent - min.first) / extent; + }); } void Robot::IKQuery::addClearanceMetric(double weight) { - addMetric([&, weight](const robot_state::RobotState &state, const SceneConstPtr &scene, - const kinematic_constraints::ConstraintEvaluationResult &result) { - if (scene) + addMetric( + [&, weight](const robot_state::RobotState &state, const SceneConstPtr &scene, + const kinematic_constraints::ConstraintEvaluationResult &result) { - double v = scene->distanceToCollision(state); - return weight * v; - } + if (scene) + { + double v = scene->distanceToCollision(state); + return weight * v; + } - return 0.; - }); + return 0.; + }); } bool Robot::IKQuery::sampleRegion(RobotPose &pose, std::size_t index) const From f218625c701f3103706b08967e1dfa8d45e36918 Mon Sep 17 00:00:00 2001 From: ChamzasKonstantinos Date: Fri, 29 Jul 2022 10:30:30 +0800 Subject: [PATCH 4/4] Fixed linting --- robowflex_library/src/robot.cpp | 79 +++++++++++++++------------------ 1 file changed, 36 insertions(+), 43 deletions(-) diff --git a/robowflex_library/src/robot.cpp b/robowflex_library/src/robot.cpp index ba9af5910..f919bbf16 100644 --- a/robowflex_library/src/robot.cpp +++ b/robowflex_library/src/robot.cpp @@ -450,36 +450,32 @@ bool Robot::loadKinematics(const std::string &group_name, bool load_subgroups) void Robot::setSRDFPostProcessAddPlanarJoint(const std::string &name) { - setSRDFPostProcessFunction( - [&, name](tinyxml2::XMLDocument &doc) -> bool - { - tinyxml2::XMLElement *virtual_joint = doc.NewElement("virtual_joint"); - virtual_joint->SetAttribute("name", name.c_str()); - virtual_joint->SetAttribute("type", "planar"); - virtual_joint->SetAttribute("parent_frame", "world"); - virtual_joint->SetAttribute("child_link", model_->getRootLink()->getName().c_str()); + setSRDFPostProcessFunction([&, name](tinyxml2::XMLDocument &doc) -> bool { + tinyxml2::XMLElement *virtual_joint = doc.NewElement("virtual_joint"); + virtual_joint->SetAttribute("name", name.c_str()); + virtual_joint->SetAttribute("type", "planar"); + virtual_joint->SetAttribute("parent_frame", "world"); + virtual_joint->SetAttribute("child_link", model_->getRootLink()->getName().c_str()); - doc.FirstChildElement("robot")->InsertFirstChild(virtual_joint); + doc.FirstChildElement("robot")->InsertFirstChild(virtual_joint); - return true; - }); + return true; + }); } void Robot::setSRDFPostProcessAddFloatingJoint(const std::string &name) { - setSRDFPostProcessFunction( - [&, name](tinyxml2::XMLDocument &doc) -> bool - { - tinyxml2::XMLElement *virtual_joint = doc.NewElement("virtual_joint"); - virtual_joint->SetAttribute("name", name.c_str()); - virtual_joint->SetAttribute("type", "floating"); - virtual_joint->SetAttribute("parent_frame", "world"); - virtual_joint->SetAttribute("child_link", model_->getRootLink()->getName().c_str()); + setSRDFPostProcessFunction([&, name](tinyxml2::XMLDocument &doc) -> bool { + tinyxml2::XMLElement *virtual_joint = doc.NewElement("virtual_joint"); + virtual_joint->SetAttribute("name", name.c_str()); + virtual_joint->SetAttribute("type", "floating"); + virtual_joint->SetAttribute("parent_frame", "world"); + virtual_joint->SetAttribute("child_link", model_->getRootLink()->getName().c_str()); - doc.FirstChildElement("robot")->InsertFirstChild(virtual_joint); + doc.FirstChildElement("robot")->InsertFirstChild(virtual_joint); - return true; - }); + return true; + }); } const std::string &Robot::getModelName() const @@ -790,37 +786,34 @@ void Robot::IKQuery::addMetric(const Metric &metric_function) void Robot::IKQuery::addDistanceMetric(double weight) { addMetric([weight](const robot_state::RobotState &state, const SceneConstPtr &scene, - const kinematic_constraints::ConstraintEvaluationResult &result) - { return weight * result.distance; }); + const kinematic_constraints::ConstraintEvaluationResult &result) { + return weight * result.distance; + }); } void Robot::IKQuery::addCenteringMetric(double weight) { - addMetric( - [&, weight](const robot_state::RobotState &state, const SceneConstPtr &scene, - const kinematic_constraints::ConstraintEvaluationResult &result) - { - const auto &jmg = state.getJointModelGroup(group); - const auto &min = state.getMinDistanceToPositionBounds(jmg); - double extent = min.second->getMaximumExtent() / 2.; - return weight * (extent - min.first) / extent; - }); + addMetric([&, weight](const robot_state::RobotState &state, const SceneConstPtr &scene, + const kinematic_constraints::ConstraintEvaluationResult &result) { + const auto &jmg = state.getJointModelGroup(group); + const auto &min = state.getMinDistanceToPositionBounds(jmg); + double extent = min.second->getMaximumExtent() / 2.; + return weight * (extent - min.first) / extent; + }); } void Robot::IKQuery::addClearanceMetric(double weight) { - addMetric( - [&, weight](const robot_state::RobotState &state, const SceneConstPtr &scene, - const kinematic_constraints::ConstraintEvaluationResult &result) + addMetric([&, weight](const robot_state::RobotState &state, const SceneConstPtr &scene, + const kinematic_constraints::ConstraintEvaluationResult &result) { + if (scene) { - if (scene) - { - double v = scene->distanceToCollision(state); - return weight * v; - } + double v = scene->distanceToCollision(state); + return weight * v; + } - return 0.; - }); + return 0.; + }); } bool Robot::IKQuery::sampleRegion(RobotPose &pose, std::size_t index) const