diff --git a/examples/simulation/eurofusion/cpp/eurofusion_cartesian_motion_controller.cpp b/examples/simulation/eurofusion/cpp/eurofusion_cartesian_motion_controller.cpp
index ff6099d..5009c7e 100644
--- a/examples/simulation/eurofusion/cpp/eurofusion_cartesian_motion_controller.cpp
+++ b/examples/simulation/eurofusion/cpp/eurofusion_cartesian_motion_controller.cpp
@@ -14,6 +14,8 @@ using namespace Eigen;
 using namespace sdu_controllers;
 using namespace sdu_controllers::utils;
 
+constexpr double pi = 3.14159265358979323846;
+
 int main()
 {
   // Setup writing of output trajectory to csv.
@@ -87,9 +89,9 @@ int main()
     pos_desired[2] += 2;
 
     Vector3d rpy_zyz_desired = rpy_zyz0;
-    rpy_zyz_desired[0] += M_PI / 8;
+    rpy_zyz_desired[0] += pi / 8;
     rpy_zyz_desired[1] += 0;
-    rpy_zyz_desired[2] += M_PI / 2;
+    rpy_zyz_desired[2] += pi / 2;
 
     x_d << pos_desired, rpy_zyz_desired;
     dx_d.setZero();
diff --git a/include/sdu_controllers/controllers/operational_space_controller.hpp b/include/sdu_controllers/controllers/operational_space_controller.hpp
index 165e212..b367ecd 100644
--- a/include/sdu_controllers/controllers/operational_space_controller.hpp
+++ b/include/sdu_controllers/controllers/operational_space_controller.hpp
@@ -51,10 +51,16 @@ namespace sdu_controllers::controllers
      */
     void set_kappa(double kappa);
 
+    /**
+     * Set damping used for maintaining joint positions on parallel joints. 
+     */
+    void set_Kd_null(double Kd_null);
+
   private:
     Eigen::VectorXd y_;
     Eigen::MatrixXd Kp_, Kd_;
     double kappa_;
+    double Kd_null_;
     std::shared_ptr<models::RobotModel> robot_model_;
   };
 
diff --git a/include/sdu_controllers/models/parameter_robot_model.hpp b/include/sdu_controllers/models/parameter_robot_model.hpp
index 3441c68..7e86fa2 100644
--- a/include/sdu_controllers/models/parameter_robot_model.hpp
+++ b/include/sdu_controllers/models/parameter_robot_model.hpp
@@ -11,6 +11,7 @@
 #include <optional>
 #include <vector>
 #include <string>
+#include <filesystem>
 
 namespace sdu_controllers::models
 {
@@ -24,6 +25,7 @@ namespace sdu_controllers::models
     explicit ParameterRobotModel() = default;
     explicit ParameterRobotModel(const std::string &yaml_filepath);
     explicit ParameterRobotModel(const RobotParameters &params);
+    explicit ParameterRobotModel(const std::filesystem::path &config_path) : ParameterRobotModel(config_path.string()) {}
     virtual ~ParameterRobotModel() = default;
 
     /**
diff --git a/src/controllers/operational_space_controller.cpp b/src/controllers/operational_space_controller.cpp
index d0a225f..13399a0 100644
--- a/src/controllers/operational_space_controller.cpp
+++ b/src/controllers/operational_space_controller.cpp
@@ -16,7 +16,8 @@ namespace sdu_controllers::controllers
         Kd_(std::move(Kd)),
         robot_model_(std::move(robot_model))
   {
-    kappa_ = 0;
+    kappa_ = 0.0;
+    Kd_null_ = 5.0;
   }
 
   void OperationalSpaceController::step(
@@ -51,10 +52,25 @@ namespace sdu_controllers::controllers
     kappaI.setIdentity(6, 6);
     kappaI *= kappa_*kappa_; // todo: should be user configurable
 
-    // The following implements damped least-squares, see eq. (3.59)
-    y_ = J_A.transpose() * (J_A * J_A.transpose() + kappaI).fullPivHouseholderQr().solve(
-      ddx_d + Kd_ * dx_tilde + Kp_ * x_tilde - Jdot_A * dq
-    );
+    // Explicitly calculate the DLS pseudo-inverse, see eq. (3.59)
+    Eigen::MatrixXd J_pinv = J_A.transpose() * 
+        (J_A * J_A.transpose() + kappaI).fullPivHouseholderQr().solve(Eigen::MatrixXd::Identity(6, 6));
+
+    // Calculate the primary task command
+    Eigen::VectorXd task_cmd = ddx_d + Kd_ * dx_tilde + Kp_ * x_tilde - Jdot_A * dq;
+
+    // Project primary task into joint space
+    Eigen::VectorXd y_primary = J_pinv * task_cmd;
+
+    // Calculate the null-space projection matrix: N = I - J_pinv * J_A
+    Eigen::MatrixXd I = Eigen::MatrixXd::Identity(6, 6);
+    Eigen::MatrixXd N = I - J_pinv * J_A;
+
+    // Secondary task (Joint Damping to stop drift)
+    Eigen::VectorXd secondary_cmd = -Kd_null_ * dq; 
+
+    // Combine primary and secondary tasks (see eq. 3.54)
+    y_ = y_primary + N * secondary_cmd;
   }
 
   void OperationalSpaceController::reset()
@@ -62,7 +78,8 @@ namespace sdu_controllers::controllers
     Kp_.setZero();
     Kd_.setZero();
     y_.setZero();
-    kappa_ = 0;
+    kappa_ = 0.0;
+    Kd_null_ = 5.0;
   }
 
   VectorXd OperationalSpaceController::get_output()
@@ -75,4 +92,9 @@ namespace sdu_controllers::controllers
     kappa_ = kappa;
   }
 
+  void OperationalSpaceController::set_Kd_null(double Kd_null)
+  {
+    Kd_null_ = Kd_null;
+  }
+
 }  // namespace sdu_controllers::controllers