feat: add clamp_in_range for a single JointStateVariable

aica-technology · Oct 17, 2024 · 02f10ab · 02f10ab
1 parent b935720
commit 02f10ab
Show file tree

Hide file tree

Showing 2 changed files with 34 additions and 0 deletions.
diff --git a/source/robot_model/include/robot_model/Model.hpp b/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

diff --git a/source/robot_model/src/Model.cpp b/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;
+  }
+  state_representation::JointState 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(),