feat(robot-model): add clamp_in_range function for individual JointSt…

…ateVariable (#194)

CHANGELOG.md

@@ -20,7 +20,8 @@ Release Versions
 
 - feat(state-representation): add utilities for CartesianStateVariable (#195, #201)
 - feat(state-representation): add utilities for JointStateVariable (#197, #201)
-
+- feat(robot-model): add clamp_in_range function for individual JointStateVariable (#194)
+
 ## 9.0.0
 
 Version 9.0.0 is a new major version of control libraries that is built on Ubuntu 24.04 with Python 3.12. It does not

python/source/robot_model/bind_model.cpp

@@ -115,6 +115,13 @@ void model(py::module_& m) {
   c.def("in_range", [](Model& self, const JointState& joint_state) -> bool { return self.in_range(joint_state); },
         "Check if the joint state variables (positions, velocities & torques) are inside the limits provided by the model", "joint_state"_a);
 
+  c.def(
+      "clamp_in_range",
+      [](Model& self, const JointState& joint_state, const JointStateVariable& state_variable_type) -> JointState {
+        return self.clamp_in_range(joint_state, state_variable_type);
+      },
+      "Clamp the joint state variables (positions, velocities & torques) according to the limits provided by the model",
+      "joint_state"_a, "state_variable_type"_a);
   c.def("clamp_in_range", [](Model& self, const JointState& joint_state) -> JointState { return self.clamp_in_range(joint_state); },
         "Clamp the joint state variables (positions, velocities & torques) according to the limits provided by the model", "joint_state"_a);
 }

source/robot_model/include/robot_model/Model.hpp

@@ -539,6 +539,14 @@ class Model {
    */
   bool in_range(const state_representation::JointState& joint_state) const;
 
+  /**
+   * @brief Clamp the joint state variables of a JointStateVariable according to the limits provided by the model
+   * @param joint_state the joint state to be clamped
+   * @param state_variable_type the type of the joint state variable to be clamped
+   * @return the clamped joint states
+   */
+  state_representation::JointState clamp_in_range(const state_representation::JointState& joint_state, const state_representation::JointStateVariable& state_variable_type) const;
+
   /**
    * @brief Clamp the joint state variables (positions, velocities & torques) according to the limits provided by
    * the model

source/robot_model/src/Model.cpp

@@ -661,6 +661,32 @@ Eigen::VectorXd Model::clamp_in_range(const Eigen::VectorXd& vector,
   return lower_limits.cwiseMax(upper_limits.cwiseMin(vector));
 }
 
+state_representation::JointState Model::clamp_in_range(
+    const state_representation::JointState& joint_state,
+    const state_representation::JointStateVariable& state_variable_type) const {
+  using namespace state_representation;
+  Eigen::VectorXd clamped_vector;
+  switch (state_variable_type) {
+    case JointStateVariable::POSITIONS:
+      clamped_vector = this->clamp_in_range(
+          joint_state.get_positions(), this->robot_model_.lowerPositionLimit, this->robot_model_.upperPositionLimit);
+      break;
+    case JointStateVariable::VELOCITIES:
+      clamped_vector = this->clamp_in_range(
+          joint_state.get_velocities(), -this->robot_model_.velocityLimit, this->robot_model_.velocityLimit);
+      break;
+    case JointStateVariable::TORQUES:
+      clamped_vector = this->clamp_in_range(
+          joint_state.get_torques(), -this->robot_model_.effortLimit, this->robot_model_.effortLimit);
+      break;
+    default:
+      return joint_state;
+  }
+  auto joint_state_clamped(joint_state);
+  joint_state_clamped.set_state_variable(clamped_vector, state_variable_type);
+  return joint_state_clamped;
+}
+
 state_representation::JointState Model::clamp_in_range(const state_representation::JointState& joint_state) const {
   state_representation::JointState joint_state_clamped(joint_state);
   joint_state_clamped.set_positions(this->clamp_in_range(joint_state.get_positions(),