Add scene methods to get joint limit vectors (#162)

Author: sea-bass

bindings/src/modules/core.cpp

@@ -2,6 +2,7 @@
 #include <nanobind/nanobind.h>
 #include <nanobind/stl/filesystem.h>
 #include <nanobind/stl/optional.h>
+#include <nanobind/stl/pair.h>
 #include <nanobind/stl/shared_ptr.h>
 #include <nanobind/stl/string.h>
 #include <nanobind/stl/vector.h>
@@ -217,6 +218,14 @@ void init_core_scene(nanobind::module_& m) {
            "Set the joint positions for the full robot state.", "positions"_a)
       .def("getJointPositionIndices", &Scene::getJointPositionIndices,
            "Get the joint position indices for a set of joint names.", "joint_names"_a)
+      .def("getPositionLimitVectors", unwrap_expected(&Scene::getPositionLimitVectors),
+           "Get the joint position limit vectors for a specified group.", "group_name"_a = "")
+      .def("getVelocityLimitVectors", unwrap_expected(&Scene::getVelocityLimitVectors),
+           "Get the joint velocity limit vectors for a specified group.", "group_name"_a = "")
+      .def("getAccelerationLimitVectors", unwrap_expected(&Scene::getAccelerationLimitVectors),
+           "Get the joint acceleration limit vectors for a specified group.", "group_name"_a = "")
+      .def("getJerkLimitVectors", unwrap_expected(&Scene::getJerkLimitVectors),
+           "Get the joint jerk limit vectors for a specified group.", "group_name"_a = "")
       .def("addBoxGeometry", unwrap_expected(&Scene::addBoxGeometry),
            "Adds a box geometry to the scene.", "name"_a, "parent_frame"_a, "box"_a, "tform"_a,
            "color"_a)

bindings/src/roboplan/roboplan_ext/core.pyi

@@ -378,6 +378,18 @@ class Scene:
     def getJointPositionIndices(self, joint_names: Sequence[str]) -> Annotated[NDArray[numpy.int32], dict(shape=(None,), order='C')]:
         """Get the joint position indices for a set of joint names."""
 
+    def getPositionLimitVectors(self, group_name: str = '') -> tuple[Annotated[NDArray[numpy.float64], dict(shape=(None,), order='C')], Annotated[NDArray[numpy.float64], dict(shape=(None,), order='C')]]:
+        """Get the joint position limit vectors for a specified group."""
+
+    def getVelocityLimitVectors(self, group_name: str = '') -> tuple[Annotated[NDArray[numpy.float64], dict(shape=(None,), order='C')], Annotated[NDArray[numpy.float64], dict(shape=(None,), order='C')]]:
+        """Get the joint velocity limit vectors for a specified group."""
+
+    def getAccelerationLimitVectors(self, group_name: str = '') -> tuple[Annotated[NDArray[numpy.float64], dict(shape=(None,), order='C')], Annotated[NDArray[numpy.float64], dict(shape=(None,), order='C')]]:
+        """Get the joint acceleration limit vectors for a specified group."""
+
+    def getJerkLimitVectors(self, group_name: str = '') -> tuple[Annotated[NDArray[numpy.float64], dict(shape=(None,), order='C')], Annotated[NDArray[numpy.float64], dict(shape=(None,), order='C')]]:
+        """Get the joint jerk limit vectors for a specified group."""
+
     def addBoxGeometry(self, name: str, parent_frame: str, box: Box, tform: Annotated[NDArray[numpy.float64], dict(shape=(4, 4), order='F')], color: Annotated[NDArray[numpy.float64], dict(shape=(4), order='C')]) -> None:
         """Adds a box geometry to the scene."""
 
@@ -422,7 +434,7 @@ class PathShortcutter:
     def getNormalizedPathScaling(self, path: JointPath) -> Annotated[NDArray[numpy.float64], dict(shape=(None,), order='C')]:
         """Computes length-normalized scaling values along a JointPath."""
 
-    def getConfigurationfromNormalizedPathScaling(self, path: JointPath, path_scalings: Annotated[NDArray[numpy.float64], dict(shape=(None,), order='C')], value: float) -> "std::pair<Eigen::Matrix<double, -1, 1, 0, -1, 1>, unsigned long>":
+    def getConfigurationfromNormalizedPathScaling(self, path: JointPath, path_scalings: Annotated[NDArray[numpy.float64], dict(shape=(None,), order='C')], value: float) -> tuple[Annotated[NDArray[numpy.float64], dict(shape=(None,), order='C')], int]:
         """Gets joint configurations from a path with normalized joint scalings."""
 
 def collapseContinuousJointPositions(scene: Scene, group_name: str, q_orig: Annotated[NDArray[numpy.float64], dict(shape=(None,), order='C')]) -> Annotated[NDArray[numpy.float64], dict(shape=(None,), order='C')]:

bindings/test/test_scene.py

@@ -210,6 +210,70 @@ def test_set_collisions(test_scene: Scene) -> None:
     assert str(exc_info.value) == expected_error
 
 
+def test_position_limits_vector(test_scene: Scene) -> None:
+    expected_lower_limits = np.array(
+        [-3.14159, -3.14159, -3.14159, -3.14159, -3.14159, -3.14159]
+    )
+    expected_upper_limits = np.array(
+        [3.14159, 3.14159, 3.14159, 3.14159, 3.14159, 3.14159]
+    )
+
+    # Default group (all joints)
+    lower_limits, upper_limits = test_scene.getPositionLimitVectors()
+    assert np.allclose(lower_limits, expected_lower_limits)
+    assert np.allclose(upper_limits, expected_upper_limits)
+
+    # Specific group (in this case, it's the same as all joints)
+    lower_limits, upper_limits = test_scene.getPositionLimitVectors("arm")
+    assert np.allclose(lower_limits, expected_lower_limits)
+    assert np.allclose(upper_limits, expected_upper_limits)
+
+
+def test_velocity_limits_vector(test_scene: Scene) -> None:
+    expected_lower_limits = np.array([-3.15, -3.15, -3.15, -3.2, -3.2, -3.2])
+    expected_upper_limits = np.array([3.15, 3.15, 3.15, 3.2, 3.2, 3.2])
+
+    # Default group (all joints)
+    lower_limits, upper_limits = test_scene.getVelocityLimitVectors()
+    assert np.allclose(lower_limits, expected_lower_limits)
+    assert np.allclose(upper_limits, expected_upper_limits)
+
+    # Specific group (in this case, it's the same as all joints)
+    lower_limits, upper_limits = test_scene.getVelocityLimitVectors("arm")
+    assert np.allclose(lower_limits, expected_lower_limits)
+    assert np.allclose(upper_limits, expected_upper_limits)
+
+
+def test_acceleration_limits_vector(test_scene: Scene) -> None:
+    expected_lower_limits = np.array([-2.0, -2.0, -2.0, -2.0, -2.0, -2.0])
+    expected_upper_limits = np.array([2.0, 2.0, 2.0, 2.0, 2.0, 2.0])
+
+    # Default group (all joints)
+    lower_limits, upper_limits = test_scene.getAccelerationLimitVectors()
+    assert np.allclose(lower_limits, expected_lower_limits)
+    assert np.allclose(upper_limits, expected_upper_limits)
+
+    # Specific group (in this case, it's the same as all joints)
+    lower_limits, upper_limits = test_scene.getAccelerationLimitVectors("arm")
+    assert np.allclose(lower_limits, expected_lower_limits)
+    assert np.allclose(upper_limits, expected_upper_limits)
+
+
+def test_jerk_limits_vector(test_scene: Scene) -> None:
+    expected_lower_limits = np.array([-10.0, -10.0, -10.0, -10.0, -10.0, -10.0])
+    expected_upper_limits = np.array([10.0, 10.0, 10.0, 10.0, 10.0, 10.0])
+
+    # Default group (all joints)
+    lower_limits, upper_limits = test_scene.getJerkLimitVectors()
+    assert np.allclose(lower_limits, expected_lower_limits)
+    assert np.allclose(upper_limits, expected_upper_limits)
+
+    # Specific group (in this case, it's the same as all joints)
+    lower_limits, upper_limits = test_scene.getJerkLimitVectors("arm")
+    assert np.allclose(lower_limits, expected_lower_limits)
+    assert np.allclose(upper_limits, expected_upper_limits)
+
+
 def test_mimics() -> None:
     # Equivalent to the C++ test, but the updated joint state is returned as a new
     # object rather than updated in place.

roboplan/include/roboplan/core/scene.hpp

@@ -164,6 +164,34 @@ class Scene {
   /// @return The corresponding joint position indices.
   Eigen::VectorXi getJointPositionIndices(const std::vector<std::string>& joint_names) const;
 
+  /// @brief Get the joint position limit vectors for a specified group.
+  /// @param group_name The name of the group. Defaults to the complete robot model.
+  /// @return A pair of vectors for the lower and upper joint position limits, if successful,
+  /// or a string describing any errors.
+  tl::expected<EigenVectorPair, std::string>
+  getPositionLimitVectors(const std::string& group_name = "") const;
+
+  /// @brief Get the joint velocity limit vectors for a specified group.
+  /// @param group_name The name of the group. Defaults to the complete robot model.
+  /// @return A pair of vectors for the lower and upper joint velocity limits, if successful,
+  /// or a string describing any errors.
+  tl::expected<EigenVectorPair, std::string>
+  getVelocityLimitVectors(const std::string& group_name = "") const;
+
+  /// @brief Get the joint acceleration limit vectors for a specified group.
+  /// @param group_name The name of the group. Defaults to the complete robot model.
+  /// @return A pair of vectors for the lower and upper joint acceleration limits, if successful,
+  /// or a string describing any errors.
+  tl::expected<EigenVectorPair, std::string>
+  getAccelerationLimitVectors(const std::string& group_name = "") const;
+
+  /// @brief Get the joint jerk limit vectors for a specified group.
+  /// @param group_name The name of the group. Defaults to the complete robot model.
+  /// @return A pair of vectors for the lower and upper joint jerk limits, if successful,
+  /// or a string describing any errors.
+  tl::expected<EigenVectorPair, std::string>
+  getJerkLimitVectors(const std::string& group_name = "") const;
+
   /// @brief Adds a box geometry to the scene.
   /// @param name The name of the object to add.
   /// @param parent_frame The name of the parent frame to add the object to.

roboplan/include/roboplan/core/types.hpp

@@ -3,12 +3,15 @@
 #include <iostream>
 #include <optional>
 #include <string>
+#include <utility>
 #include <vector>
 
 #include <Eigen/Dense>
 
 namespace roboplan {
 
+using EigenVectorPair = std::pair<Eigen::VectorXd, Eigen::VectorXd>;
+
 /// @brief Represents a robot joint configuration.
 /// @details Creating and validating these structures are handled by separate
 /// utility functions.

roboplan/src/core/scene.cpp

@@ -85,11 +85,30 @@ Scene::Scene(const std::string& name, const std::string& urdf, const std::string
                                joint.shortname() + "' but this is not in the RoboPlan joint map.");
     }
     auto info = JointInfo(kPinocchioJointTypeMap.at(joint.shortname()));
-    for (int idx = 0; idx < joint.nq(); ++idx) {
-      info.limits.min_position[idx] = mimic_model.lowerPositionLimit(q_idx);
-      info.limits.max_position[idx] = mimic_model.upperPositionLimit(q_idx);
-      ++q_idx;
+    switch (info.type) {
+    case (JointType::PRISMATIC):
+    case (JointType::REVOLUTE):
+      info.limits.min_position[0] = mimic_model.lowerPositionLimit(q_idx);
+      info.limits.max_position[0] = mimic_model.upperPositionLimit(q_idx);
+      break;
+    case (JointType::PLANAR):
+      // Only the position limits need to be incorporated, as orientation is unlimited.
+      for (size_t dof = 0; dof < 2; ++dof) {
+        info.limits.min_position[dof] = mimic_model.lowerPositionLimit(q_idx + dof);
+        info.limits.max_position[dof] = mimic_model.upperPositionLimit(q_idx + dof);
+      }
+      break;
+    case (JointType::FLOATING):
+      // Only the position limits need to be incorporated, as orientation is unlimited.
+      for (size_t dof = 0; dof < 3; ++dof) {
+        info.limits.min_position[dof] = mimic_model.lowerPositionLimit(q_idx + dof);
+        info.limits.max_position[dof] = mimic_model.upperPositionLimit(q_idx + dof);
+      }
+      break;
+    default:  // Includes continuous joints, where no operation is needed.
+      break;
     }
+    q_idx += info.num_position_dofs;
 
     std::optional<YAML::Node> maybe_acc_limits;
     std::optional<YAML::Node> maybe_jerk_limits;
@@ -400,6 +419,173 @@ Eigen::VectorXi Scene::getJointPositionIndices(const std::vector<std::string>& j
   return Eigen::VectorXi::Map(q_indices.data(), q_indices.size());
 }
 
+tl::expected<EigenVectorPair, std::string>
+Scene::getPositionLimitVectors(const std::string& group_name) const {
+  const auto maybe_joint_group_info = getJointGroupInfo(group_name);
+  if (!maybe_joint_group_info) {
+    return tl::make_unexpected("Failed to get position limit vectors: " +
+                               maybe_joint_group_info.error());
+  }
+  const auto& joint_group_info = maybe_joint_group_info.value();
+
+  // Initialize all limits as infinity and only set the joint DOFs that are finite.
+  Eigen::VectorXd lower_limits = Eigen::VectorXd::Constant(
+      joint_group_info.nq_collapsed, -std::numeric_limits<double>::infinity());
+  Eigen::VectorXd upper_limits = Eigen::VectorXd::Constant(joint_group_info.nq_collapsed,
+                                                           std::numeric_limits<double>::infinity());
+  size_t q_idx = 0;
+  for (size_t j_idx = 0; j_idx < joint_group_info.joint_names.size(); ++j_idx) {
+    const auto& joint_name = joint_group_info.joint_names.at(j_idx);
+    const auto maybe_joint_info = getJointInfo(joint_name);
+    if (!maybe_joint_info) {
+      return tl::make_unexpected("Failed to get position limit vectors: " +
+                                 maybe_joint_info.error());
+    }
+    const auto& joint_info = maybe_joint_info.value();
+
+    switch (joint_info.type) {
+    case JointType::FLOATING:
+      // Position limits can be finite, orientation stays unlimited.
+      for (int dof = 0; dof < 3; ++dof) {
+        if (joint_info.limits.min_position.size() > dof) {
+          lower_limits(q_idx + dof) = joint_info.limits.min_position(dof);
+        }
+        if (joint_info.limits.max_position.size() > dof) {
+          upper_limits(q_idx + dof) = joint_info.limits.max_position(dof);
+        }
+      }
+      q_idx += 6;
+      break;
+    case JointType::PLANAR:
+      // Position limits can be finite, orientation stays unlimited.
+      for (int dof = 0; dof < 2; ++dof) {
+        if (joint_info.limits.min_position.size() > dof) {
+          lower_limits(q_idx + dof) = joint_info.limits.min_position(dof);
+        }
+        if (joint_info.limits.max_position.size() > dof) {
+          upper_limits(q_idx + dof) = joint_info.limits.max_position(dof);
+        }
+      }
+      q_idx += 3;
+      break;
+    case JointType::CONTINUOUS:
+      // Already has infinite limits, no action needed.
+      ++q_idx;
+      break;
+    default:  // Prismatic or revolute.
+      if (joint_info.limits.min_position.size() > 0) {
+        lower_limits(q_idx) = joint_info.limits.min_position(0);
+      }
+      if (joint_info.limits.max_position.size() > 0) {
+        upper_limits(q_idx) = joint_info.limits.max_position(0);
+      }
+      ++q_idx;
+    }
+  }
+  return std::make_pair(lower_limits, upper_limits);
+}
+
+tl::expected<EigenVectorPair, std::string>
+Scene::getVelocityLimitVectors(const std::string& group_name) const {
+  const auto maybe_joint_group_info = getJointGroupInfo(group_name);
+  if (!maybe_joint_group_info) {
+    return tl::make_unexpected("Failed to get velocity limit vectors: " +
+                               maybe_joint_group_info.error());
+  }
+  const auto& joint_group_info = maybe_joint_group_info.value();
+
+  // Initialize all limits as infinity and only set the joint DOFs that are finite.
+  Eigen::VectorXd lower_limits = Eigen::VectorXd::Constant(
+      joint_group_info.v_indices.size(), -std::numeric_limits<double>::infinity());
+  Eigen::VectorXd upper_limits = Eigen::VectorXd::Constant(joint_group_info.v_indices.size(),
+                                                           std::numeric_limits<double>::infinity());
+  size_t v_idx = 0;
+  for (size_t j_idx = 0; j_idx < joint_group_info.joint_names.size(); ++j_idx) {
+    const auto& joint_name = joint_group_info.joint_names.at(j_idx);
+    const auto maybe_joint_info = getJointInfo(joint_name);
+    if (!maybe_joint_info) {
+      return tl::make_unexpected("Failed to get velocity limit vectors: " +
+                                 maybe_joint_info.error());
+    }
+    const auto& joint_info = maybe_joint_info.value();
+
+    for (size_t dof = 0; dof < joint_info.num_velocity_dofs; ++dof) {
+      const auto& max_vel = joint_info.limits.max_velocity(dof);
+      lower_limits(v_idx + dof) = -max_vel;
+      upper_limits(v_idx + dof) = max_vel;
+    }
+    v_idx += joint_info.num_velocity_dofs;
+  }
+  return std::make_pair(lower_limits, upper_limits);
+}
+
+tl::expected<EigenVectorPair, std::string>
+Scene::getAccelerationLimitVectors(const std::string& group_name) const {
+  const auto maybe_joint_group_info = getJointGroupInfo(group_name);
+  if (!maybe_joint_group_info) {
+    return tl::make_unexpected("Failed to get acceleration limit vectors: " +
+                               maybe_joint_group_info.error());
+  }
+  const auto& joint_group_info = maybe_joint_group_info.value();
+
+  // Initialize all limits as infinity and only set the joint DOFs that are finite.
+  Eigen::VectorXd lower_limits = Eigen::VectorXd::Constant(
+      joint_group_info.v_indices.size(), -std::numeric_limits<double>::infinity());
+  Eigen::VectorXd upper_limits = Eigen::VectorXd::Constant(joint_group_info.v_indices.size(),
+                                                           std::numeric_limits<double>::infinity());
+  size_t v_idx = 0;
+  for (size_t j_idx = 0; j_idx < joint_group_info.joint_names.size(); ++j_idx) {
+    const auto& joint_name = joint_group_info.joint_names.at(j_idx);
+    const auto maybe_joint_info = getJointInfo(joint_name);
+    if (!maybe_joint_info) {
+      return tl::make_unexpected("Failed to get acceleration limit vectors: " +
+                                 maybe_joint_info.error());
+    }
+    const auto& joint_info = maybe_joint_info.value();
+
+    for (size_t dof = 0; dof < joint_info.num_velocity_dofs; ++dof) {
+      const auto& max_accel = joint_info.limits.max_acceleration(dof);
+      lower_limits(v_idx + dof) = -max_accel;
+      upper_limits(v_idx + dof) = max_accel;
+    }
+    v_idx += joint_info.num_velocity_dofs;
+  }
+  return std::make_pair(lower_limits, upper_limits);
+}
+
+tl::expected<EigenVectorPair, std::string>
+Scene::getJerkLimitVectors(const std::string& group_name) const {
+  const auto maybe_joint_group_info = getJointGroupInfo(group_name);
+  if (!maybe_joint_group_info) {
+    return tl::make_unexpected("Failed to get jerk limit vectors: " +
+                               maybe_joint_group_info.error());
+  }
+  const auto& joint_group_info = maybe_joint_group_info.value();
+
+  // Initialize all limits as infinity and only set the joint DOFs that are finite.
+  Eigen::VectorXd lower_limits = Eigen::VectorXd::Constant(
+      joint_group_info.v_indices.size(), -std::numeric_limits<double>::infinity());
+  Eigen::VectorXd upper_limits = Eigen::VectorXd::Constant(joint_group_info.v_indices.size(),
+                                                           std::numeric_limits<double>::infinity());
+  size_t v_idx = 0;
+  for (size_t j_idx = 0; j_idx < joint_group_info.joint_names.size(); ++j_idx) {
+    const auto& joint_name = joint_group_info.joint_names.at(j_idx);
+    const auto maybe_joint_info = getJointInfo(joint_name);
+    if (!maybe_joint_info) {
+      return tl::make_unexpected("Failed to get jerk limit vectors: " + maybe_joint_info.error());
+    }
+    const auto& joint_info = maybe_joint_info.value();
+
+    for (size_t dof = 0; dof < joint_info.num_velocity_dofs; ++dof) {
+      const auto& max_jerk = joint_info.limits.max_jerk(dof);
+      lower_limits(v_idx + dof) = -max_jerk;
+      upper_limits(v_idx + dof) = max_jerk;
+    }
+    v_idx += joint_info.num_velocity_dofs;
+  }
+  return std::make_pair(lower_limits, upper_limits);
+}
+
 tl::expected<void, std::string> Scene::addBoxGeometry(const std::string& name,
                                                       const std::string& parent_frame,
                                                       const Box& box, const Eigen::Matrix4d& tform,