From bb94aac0a709fb7bf72e131f0ebaf3d18beb6eff Mon Sep 17 00:00:00 2001
From: HiroIshida <h-ishida@jsk.imi.i.u-tokyo.ac.jp>
Date: Mon, 10 Jun 2024 05:34:21 +0900
Subject: [PATCH] feat: get velocity and effort limits

---
 include/tinyfk.hpp  |  6 ++++++
 src/tinyfk.cpp      | 22 ++++++++++++++++++++++
 test/test_parse.cpp |  9 +++++++++
 3 files changed, 37 insertions(+)

diff --git a/include/tinyfk.hpp b/include/tinyfk.hpp
index c80a3f0..37deb91 100644
--- a/include/tinyfk.hpp
+++ b/include/tinyfk.hpp
@@ -104,6 +104,12 @@ class KinematicModel {
   std::vector<Bound>
   get_joint_position_limits(const std::vector<size_t> &joint_ids) const;
 
+  std::vector<double>
+  get_joint_velocity_limits(const std::vector<size_t> &joint_ids) const;
+
+  std::vector<double>
+  get_joint_effort_limits(const std::vector<size_t> &joint_ids) const;
+
   std::vector<std::string> get_joint_names() const {
     std::vector<std::string> joint_names;
     for (auto &joint : joints_) {
diff --git a/src/tinyfk.cpp b/src/tinyfk.cpp
index e1c130f..2955b34 100644
--- a/src/tinyfk.cpp
+++ b/src/tinyfk.cpp
@@ -169,6 +169,28 @@ std::vector<Bound> KinematicModel::get_joint_position_limits(
   return limits;
 }
 
+std::vector<double> KinematicModel::get_joint_velocity_limits(
+    const std::vector<size_t> &joint_ids) const {
+  const size_t n_joint = joint_ids.size();
+  std::vector<double> limits(n_joint);
+  for (size_t i = 0; i < n_joint; i++) {
+    const auto &joint = joints_[joint_ids[i]];
+    limits[i] = joint->limits->velocity;
+  }
+  return limits;
+}
+
+std::vector<double> KinematicModel::get_joint_effort_limits(
+    const std::vector<size_t> &joint_ids) const {
+  const size_t n_joint = joint_ids.size();
+  std::vector<double> limits(n_joint);
+  for (size_t i = 0; i < n_joint; i++) {
+    const auto &joint = joints_[joint_ids[i]];
+    limits[i] = joint->limits->effort;
+  }
+  return limits;
+}
+
 std::vector<size_t>
 KinematicModel::get_link_ids(std::vector<std::string> link_names) const {
   int n_link = link_names.size();
diff --git a/test/test_parse.cpp b/test/test_parse.cpp
index 1f2910d..ef78611 100644
--- a/test/test_parse.cpp
+++ b/test/test_parse.cpp
@@ -12,22 +12,31 @@ TEST(TEST_PARSE, AllTest) {
   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);
+  auto vel_limits = kin.get_joint_velocity_limits(ids);
+  auto eff_limits = kin.get_joint_effort_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());
+  EXPECT_EQ(vel_limits[0], 3.6);
+  EXPECT_EQ(eff_limits[0], 30);
 
   // 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);
+  EXPECT_EQ(pos_limits[1].second, 0.714601836603);
+  EXPECT_EQ(vel_limits[1], 2.088);
+  EXPECT_EQ(eff_limits[1], 30);
 
   // 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);
+  EXPECT_EQ(vel_limits[2], 0.013);
+  EXPECT_EQ(eff_limits[2], 10000);
 }
 
 int main(int argc, char **argv) {