[delta][control] clamp iniital variable by bounds

jsk-ros-pkg · Nov 29, 2024 · 92882b7 · 92882b7
1 parent cc9d2f9
commit 92882b7
Showing 1 changed file with 3 additions and 1 deletion.
diff --git a/robots/delta/src/control/delta_ground_controller.cpp b/robots/delta/src/control/delta_ground_controller.cpp
@@ -591,7 +591,9 @@ void RollingController::nonlinearGroundWrenchAllocation()
   // joint torque part (use osqp solution)
   for(int i = 0; i < n_variables - 2 * motor_num_; i++)
     {
-      opt_x.at(2 * motor_num_ + i) = osqp_solution_(2 * motor_num_ + i);
+      opt_x.at(2 * motor_num_ + i) = std::clamp(osqp_solution_(2 * motor_num_ + i),
+                                                lb.at(2 * motor_num_ + i),
+                                                ub.at(2 * motor_num_ + i));
     }
 
   for(int i = 0; i < n_variables; i++)