set velocities to zero in case of errors

ICube-Robotics · Feb 12, 2024 · 0fdf88b · 0fdf88b
1 parent 3967a78
commit 0fdf88b
Showing 1 changed file with 5 additions and 0 deletions.
diff --git a/cartesian_admittance_controller/src/cartesian_admittance_controller.cpp b/cartesian_admittance_controller/src/cartesian_admittance_controller.cpp
@@ -141,6 +141,11 @@ controller_interface::return_type CartesianAdmittanceController::update(
       joint_command_
     );
     if (ret_admittance != controller_interface::return_type::OK) {
+      std::fill(
+        joint_command_.accelerations.begin(), joint_command_.accelerations.end(), 0.0);
+      std::fill(
+        joint_command_.velocities.begin(), joint_command_.velocities.end(), 0.0);
+      write_state_to_hardware(joint_command_);
       return controller_interface::return_type::ERROR;
     }
   }