From 5c457e20ee6c6fcdb7755a203b021d22368b68b5 Mon Sep 17 00:00:00 2001
From: Kevin Zhang <kevinleezhang@gmail.com>
Date: Fri, 13 Sep 2024 16:05:00 -0400
Subject: [PATCH] Preventing from publishing nans.
---
franka-interface/src/skills/impedance_control_skill.cpp | 4 +++-
1 file changed, 3 insertions(+), 1 deletion(-)
diff --git a/franka-interface/src/skills/impedance_control_skill.cpp b/franka-interface/src/skills/impedance_control_skill.cpp
index c82b4f6..57e7fa4 100644
--- a/franka-interface/src/skills/impedance_control_skill.cpp
+++ b/franka-interface/src/skills/impedance_control_skill.cpp
@@ -132,7 +132,9 @@ void ImpedanceControlSkill::execute_skill_on_franka(run_loop* run_loop,
for(int i = 0; i < 7; i++) {
current_joint_torques_[i] = feedback_controller_->tau_d_array_[i];
}
-
+ if (current_period_ == 0.0) {
+ current_period_ = 0.001;
+ }
limit_current_joint_torques(current_period_);
for(int i = 0; i < 7; i++) {