fix: joint limit for continous must be -inf, inf (#74)

* fix: joint limit for continous must be -inf, inf

* fix: dont break pybing side

CMakeLists.txt

@@ -33,13 +33,15 @@ if(BUILD_TEST)
     setup_tinyfk_test(test_others test/test_others.cpp)
     setup_tinyfk_test(test_data_structure test/test_data_structure.cpp)
     setup_tinyfk_test(test_kinematics test/test_kinematics.cpp)
+    setup_tinyfk_test(test_parse test/test_parse.cpp)
 
     set(nlohmann_json_INCLUDE_DIRS third_party/json/include)
     target_include_directories(test_kinematics PUBLIC ${nlohmann_json_INCLUDE_DIRS})
 
     enable_testing()
     gtest_discover_tests(test_data_structure)
     gtest_discover_tests(test_kinematics)
+    gtest_discover_tests(test_parse)
 endif(BUILD_TEST)
 
 # benchmark (kdl)

include/tinyfk.hpp

@@ -23,7 +23,7 @@ tinyfk: https://github.com/HiroIshida/tinyfk
 
 namespace tinyfk {
 
-using AngleLimit = std::pair<double, double>;
+using Bound = std::pair<double, double>;
 using Transform = urdf::Pose;
 using Vector3 = urdf::Vector3;
 using Rotation = urdf::Rotation;
@@ -101,8 +101,8 @@ class KinematicModel {
 
   std::vector<size_t> get_joint_ids(std::vector<std::string> joint_names) const;
 
-  std::vector<AngleLimit>
-  get_joint_limits(const std::vector<size_t> &joint_ids) const;
+  std::vector<Bound>
+  get_joint_position_limits(const std::vector<size_t> &joint_ids) const;
 
   std::vector<std::string> get_joint_names() const {
     std::vector<std::string> joint_names;

python/tinyfk/__init__.py

@@ -204,7 +204,7 @@ def get_joint_limits(self, joint_ids) -> List[Tuple[Optional[float], Optional[fl
         ret = self._robot.get_joint_limits(joint_ids)
         limits: List[Tuple[Optional[float], Optional[float]]] = []
         for lower, upper in ret:
-            if lower == 0.0 and upper == 0.0:
+            if lower == -np.inf and upper == np.inf:
                 # NOTE: if no limit is set, the value is set to 0.0 in urdfdom
                 # TODO: I assume that if joint limit is not set, both lower and upper
                 # are not set. Is this assumption correct?

src/tinyfk.cpp

@@ -152,16 +152,19 @@ KinematicModel::get_joint_ids(std::vector<std::string> joint_names) const {
   return joint_ids;
 }
 
-std::vector<AngleLimit>
-KinematicModel::get_joint_limits(const std::vector<size_t> &joint_ids) const {
+std::vector<Bound> KinematicModel::get_joint_position_limits(
+    const std::vector<size_t> &joint_ids) const {
   const size_t n_joint = joint_ids.size();
-  std::vector<AngleLimit> limits(n_joint, AngleLimit());
+  std::vector<Bound> limits(n_joint, Bound());
   for (size_t i = 0; i < n_joint; i++) {
-
     const auto &joint = joints_[joint_ids[i]];
-    auto &limit = limits[i];
-    limit.first = joint->limits->lower;
-    limit.second = joint->limits->upper;
+    if (joint->type == urdf::Joint::CONTINUOUS) {
+      limits[i].first = -std::numeric_limits<double>::infinity();
+      limits[i].second = std::numeric_limits<double>::infinity();
+    } else {
+      limits[i].first = joint->limits->lower;
+      limits[i].second = joint->limits->upper;
+    }
   }
   return limits;
 }

src/wrapper.cpp

@@ -238,7 +238,12 @@ PYBIND11_MODULE(_tinyfk, m) {
            &KinematicsModelPyWrapper::compute_total_inertia_matrix)
       .def("get_joint_names", &KinematicsModelPyWrapper::get_joint_names)
       .def("get_joint_ids", &KinematicsModelPyWrapper::get_joint_ids)
-      .def("get_joint_limits", &KinematicsModelPyWrapper::get_joint_limits)
+      .def("get_joint_limits",
+           &KinematicsModelPyWrapper::
+               get_joint_position_limits) // rename to
+                                          // get_joint_position_limits?
+                                          // but breaking
+                                          // change
       .def("get_base_pose", &KinematicsModelPyWrapper::get_base_pose)
       .def("set_base_pose", py::overload_cast<const std::vector<double> &>(
                                 &KinematicsModelPyWrapper::set_base_pose))

test/test_parse.cpp

@@ -0,0 +1,36 @@
+#include "tinyfk.hpp"
+#include <gtest/gtest.h>
+
+using namespace tinyfk;
+
+TEST(TEST_PARSE, AllTest) {
+  const std::string urdf_file = "../data/pr2.urdf";
+  const auto xml_string = load_urdf(urdf_file);
+  auto kin = KinematicModel(xml_string);
+
+  // continuous, revolute, prismatic
+  auto ids = kin.get_joint_ids(
+      {"r_forearm_roll_joint", "r_shoulder_pan_joint", "torso_lift_joint"});
+  auto pos_limits = kin.get_joint_position_limits(ids);
+
+  // check that limit of r_forearm_roll_joint is continuous
+  // <limit effort="30" velocity="3.6"/>
+  EXPECT_EQ(pos_limits[0].first, -std::numeric_limits<double>::infinity());
+  EXPECT_EQ(pos_limits[0].second, std::numeric_limits<double>::infinity());
+
+  // check that limit of r_shoulder_pan_joint is
+  // <limit effort="30" lower="-2.2853981634" upper="0.714601836603"
+  // velocity="2.088"/>
+  EXPECT_EQ(pos_limits[1].first, -2.2853981634);
+  EXPECT_EQ(pos_limits[1].second, 0.714601836603);
+
+  // check that limit of torso_lift_joint is
+  // <limit effort="10000" lower="0.0" upper="0.33" velocity="0.013"/>
+  EXPECT_EQ(pos_limits[2].first, 0.0);
+  EXPECT_EQ(pos_limits[2].second, 0.33);
+}
+
+int main(int argc, char **argv) {
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}