feat: refactor controller state update + add regularization configurable parameter for inverse mass matrix computation (#40)

* Bump down python requirement

* Fixed library expoert issues

* Minor controller changes

* Minor controller changes

* Format CMakeLists

* Revert parameters header inclusion

* chore: add regularization to operational space pseudoinverse

* fix: targets exported before dependencies (incorrecttly)

* chore: revert back to nullspace regularization parameter

* fix: dq assignment and num_joint usage

* chore: Update docstrings for updateCurrentState

---------

Co-authored-by: Daniel San José Pro <42489409+danielsanjosepro@users.noreply.github.com>

Author: dmalexa5

CMakeLists.txt

@@ -121,46 +121,45 @@ target_link_libraries(${PROJECT_NAME}
 
 ament_target_dependencies(${PROJECT_NAME}
   PUBLIC
-controller_interface
-hardware_interface
-pluginlib
-rclcpp
-rclcpp_lifecycle
-pinocchio
-generate_parameter_library
-realtime_tools
+    controller_interface
+    hardware_interface
+    pluginlib
+    rclcpp
+    rclcpp_lifecycle
+    pinocchio
+    generate_parameter_library
+    realtime_tools
 )
 
 pluginlib_export_plugin_description_file(
         controller_interface crisp_controllers.xml)
 
 install(
-        TARGETS
-        ${PROJECT_NAME}
-        RUNTIME DESTINATION bin
-        ARCHIVE DESTINATION lib
-        LIBRARY DESTINATION lib
+    TARGETS ${PROJECT_NAME}
+    EXPORT export_${PROJECT_NAME}
+    RUNTIME DESTINATION bin
+    ARCHIVE DESTINATION lib
+    LIBRARY DESTINATION lib
 )
 
 install(
-        DIRECTORY include/
-        DESTINATION include
+    DIRECTORY include/
+    DESTINATION include
 )
 
-ament_export_include_directories(
-        include
-)
-ament_export_libraries(
-        ${PROJECT_NAME}
-)
 ament_export_dependencies(
-        controller_interface
-        pluginlib
-        rclcpp
-        rclcpp_lifecycle
-        hardware_interface
-        Eigen3
-)
+    controller_interface
+    pluginlib
+    rclcpp
+    rclcpp_lifecycle
+    hardware_interface
+    Eigen3
+    pinocchio
+    realtime_tools
+    generate_parameter_library
+)
+
+ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
 
 if(BUILD_TESTING)
   find_package(ament_cmake_gtest REQUIRED)

include/crisp_controllers/cartesian_controller.hpp

@@ -114,6 +114,12 @@ class CartesianController : public controller_interface::ControllerInterface {
    */
   void setStiffnessAndDamping();
 
+  /**
+   * @brief Get the current state of the robot from hardware interfaces and update internal variables
+   * @param initialize If set to true, initialize the exponential moving average filter with the current state 
+   */
+  void updateCurrentState(bool initialize = false);
+
   /**
    * @brief Reads the target pose in realtime loop from the buffer and parses it to be used in the controller.
    */
@@ -199,6 +205,10 @@ class CartesianController : public controller_interface::ControllerInterface {
   pinocchio::SE3 end_effector_pose;
   /** @brief End effector Jacobian matrix */
   pinocchio::Data::Matrix6x J;
+  /** @brief End effector Jacobian matrix pseudoinverse */
+  Eigen::MatrixXd J_pinv;
+  /** @brief Joint-space identity matrix */
+  Eigen::MatrixXd Id_nv;
 
   /** @brief Friction parameters 1 of size nv */
   Eigen::VectorXd fp1;

pyproject.toml

@@ -3,7 +3,7 @@ name = "crisp-controllers"
 version = "0.1.0"
 description = "A collection of ROS2 controllers for the CRISP project."
 readme = "README.md"
-requires-python = ">=3.11"
+requires-python = ">=3.10"
 dependencies = [
     "mkdocs>=1.6.1",
     "mkdocs-material>=9.6.15",

src/cartesian_controller.cpp

@@ -62,41 +62,11 @@ CartesianController::state_interface_configuration() const {
 
 controller_interface::return_type
 CartesianController::update(const rclcpp::Time & time, const rclcpp::Duration & /*period*/) {
-  size_t num_joints = params_.joints.size();
-  for (size_t i = 0; i < num_joints; i++) {
-    // TODO(placeholder): later it might be better to get this thing prepared in the
-    // configuration part (not in the control loop)
-    auto joint_name = params_.joints[i];
-    auto joint_id = model_.getJointId(joint_name);  // pinocchio joind id might be different
-    auto joint = model_.joints[joint_id];
-
-    q_ref[i] = exponential_moving_average(q_ref[i], q_target[i], params_.filter.q_ref);
-
-// Filtering joint position measurement 1 uses previous q, 0 uses new q from measurement.
-// q[i] = exponential_moving_average(q[i], state_interfaces_[i].get_value(), params_.filter.q);
-#if ROS2_VERSION_ABOVE_HUMBLE
-    q[i] = exponential_moving_average(
-      q[i], state_interfaces_[i].get_optional().value_or(q[i]), params_.filter.q);
-#else
-    q[i] = exponential_moving_average(q[i], state_interfaces_[i].get_value(), params_.filter.q);
-#endif
-
-    if (continous_joint_types.count(joint.shortname())) {  // Then we are handling a continous
-                                                           // joint that is SO(2)
-      q_pin[joint.idx_q()] = std::cos(q[i]);
-      q_pin[joint.idx_q() + 1] = std::sin(q[i]);
-    } else {  // simple revolute joint case
-      q_pin[joint.idx_q()] = q[i];
-    }
-#if ROS2_VERSION_ABOVE_HUMBLE
-    dq[i] = exponential_moving_average(
-      dq[i], state_interfaces_[num_joints + i].get_optional().value_or(dq[i]), params_.filter.dq);
-#else
-    dq[i] = exponential_moving_average(
-      dq[i], state_interfaces_[num_joints + i].get_value(), params_.filter.dq);
-#endif
-  }
+  
+  // Update current state information with EMA filtered values
+  updateCurrentState();
 
+  // Check if new targets available
   if (new_target_pose_) {
     parse_target_pose_();
     new_target_pose_ = false;
@@ -147,15 +117,13 @@ CartesianController::update(const rclcpp::Time & time, const rclcpp::Duration &
     : pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED;
   pinocchio::computeFrameJacobian(model_, data_, q_pin, end_effector_frame_id, reference_frame, J);
 
-  Eigen::MatrixXd J_pinv(model_.nv, 6);
   J_pinv = pseudo_inverse(J, params_.nullspace.regularization);
-  Eigen::MatrixXd Id_nv = Eigen::MatrixXd::Identity(model_.nv, model_.nv);
   if (params_.nullspace.projector_type == "dynamic" || params_.use_operational_space) {
     pinocchio::computeMinverse(model_, data_, q_pin);
     data_.Minv.triangularView<Eigen::StrictlyLower>() =
       data_.Minv.transpose().triangularView<Eigen::StrictlyLower>();
     Mx_inv = J * data_.Minv * J.transpose();
-    Mx = pseudo_inverse(Mx_inv);
+    Mx = pseudo_inverse(Mx_inv, params_.operational_space_regularization);
   }
 
   if (params_.nullspace.projector_type == "dynamic") {
@@ -221,7 +189,7 @@ CartesianController::update(const rclcpp::Time & time, const rclcpp::Duration &
   tau_d = exponential_moving_average(tau_d, tau_previous, params_.filter.output_torque);
 
   if (!params_.stop_commands) {
-    for (size_t i = 0; i < num_joints; ++i) {
+    for (size_t i = 0; i < params_.joints.size(); ++i) {
 #if ROS2_VERSION_ABOVE_HUMBLE
       (void)command_interfaces_[i].set_value(tau_d[i]);
 #else
@@ -323,7 +291,8 @@ CartesianController::on_configure(const rclcpp_lifecycle::State & /*previous_sta
       return CallbackReturn::ERROR;
     }
   }
-
+  
+  // Preallocate the matrices and vectors that will be used in the control loop
   end_effector_frame_id = model_.getFrameId(params_.end_effector_frame);
   q = Eigen::VectorXd::Zero(model_.nv);
   q_pin = Eigen::VectorXd::Zero(model_.nq);
@@ -333,6 +302,8 @@ CartesianController::on_configure(const rclcpp_lifecycle::State & /*previous_sta
   dq_ref = Eigen::VectorXd::Zero(model_.nv);
   tau_previous = Eigen::VectorXd::Zero(model_.nv);
   J = Eigen::MatrixXd::Zero(6, model_.nv);
+  J_pinv = Eigen::MatrixXd::Zero(model_.nv, 6);
+  Id_nv = Eigen::MatrixXd::Identity(model_.nv, model_.nv);
 
   // Map the friction parameters to Eigen vectors
   fp1 = Eigen::Map<Eigen::VectorXd>(params_.friction.fp1.data(), model_.nv);
@@ -491,41 +462,51 @@ void CartesianController::setStiffnessAndDamping() {
   }
 }
 
-CallbackReturn
-CartesianController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) {
+void CartesianController::updateCurrentState(bool initialize) {
   auto num_joints = params_.joints.size();
   for (size_t i = 0; i < num_joints; i++) {
-    // TODO(placeholder): later it might be better to get this thing prepared in the
-    // configuration part (not in the control loop)
     auto joint_name = params_.joints[i];
     auto joint_id = model_.getJointId(joint_name);  // pinocchio joind id might be different
     auto joint = model_.joints[joint_id];
 
 #if ROS2_VERSION_ABOVE_HUMBLE
-    q[i] = state_interfaces_[i].get_optional().value_or(q[i]);
+    double q_meas = state_interfaces_[i].get_optional().value_or(q[i]);
+    double dq_meas = state_interfaces_[num_joints + i].get_optional().value_or(dq[i]);
 #else
-    q[i] = state_interfaces_[i].get_value();
+    double q_meas = state_interfaces_[i].get_value();
+    double dq_meas = state_interfaces_[num_joints + i].get_value();
 #endif
-    if (joint.shortname() == "JointModelRZ") {  // simple revolute joint case
-      q_pin[joint.idx_q()] = q[i];
-    } else if (continous_joint_types.count(
-                 joint.shortname())) {  // Then we are handling a continous
-                                        // joint that is SO(2)
+    
+    q_ref[i] = initialize 
+      ? q_meas
+      : exponential_moving_average(q_ref[i], q_target[i], params_.filter.q_ref);
+
+    q[i] = initialize
+      ? q_meas
+      : exponential_moving_average(q[i], q_meas, params_.filter.q);    
+    
+    if (continous_joint_types.count(joint.shortname())) { // Then we are handling a continous
+                                                          // joint that is SO(2)
       q_pin[joint.idx_q()] = std::cos(q[i]);
       q_pin[joint.idx_q() + 1] = std::sin(q[i]);
+    } else {  // simple revolute joint case
+      q_pin[joint.idx_q()] = q[i];
     }
 
-    q_ref[i] = q[i];
-    q_target[i] = q[i];
+    dq[i] = initialize
+      ? dq_meas
+      : exponential_moving_average(dq[i], dq_meas, params_.filter.dq);
 
-#if ROS2_VERSION_ABOVE_HUMBLE
-    dq[i] = state_interfaces_[num_joints + i].get_optional().value_or(dq[i]);
-    dq_ref[i] = dq[i];
-#else
-    dq[i] = state_interfaces_[num_joints + i].get_value();
-    dq_ref[i] = state_interfaces_[num_joints + i].get_value();
-#endif
+    q_target[i] = initialize ? q_meas : q_target[i];
   }
+}
+
+CallbackReturn
+CartesianController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) {
+  
+  // Update the current state with initial measurements (no EMA filtering)
+  // to avoid large initial errors
+  updateCurrentState(true);
 
   pinocchio::forwardKinematics(model_, data_, q_pin, dq);
   pinocchio::updateFramePlacements(model_, data_);

src/cartesian_controller.yaml

@@ -19,7 +19,7 @@ cartesian_controller:
 
   operational_space_regularization:
     type: double
-    default_value: 0.1
+    default_value: 0.01
     description: "Regularization (damping) for Mx pseudo-inverse. Higher values improve stability near singularities but reduce accuracy. Valid range is 0.001-1.0; typical useful values are in the 0.01-0.5 range."
     validation:
       bounds<>: [0.001, 1.0]