Validate stiffness and damping

Negative values could cause the controller to explode
Default to reasonable new damping when updating stiffness from Tracker

Author: nickswalker

examples/manual_guidance_joint_impedance.py

@@ -22,24 +22,24 @@
     parser.add_argument(
         "--stiffness",
         type=float,
-        default=6.0,
-        help="Uniform joint stiffness used for manual guidance",
+        default=2.0,
+        help="Joint stiffness",
     )
     parser.add_argument(
         "--lock-joint6",
         action="store_true",
-        help="Hold joint 6 at its starting angle while leaving the other joints compliant",
+        help="Hold joint 6 at its starting angle",
     )
     parser.add_argument(
         "--lock-joint7",
         action="store_true",
-        help="Hold joint 7 at its starting angle while leaving the other joints compliant",
+        help="Hold joint 7 at its starting angle",
     )
     parser.add_argument(
         "--lock-stiffness",
         type=float,
         default=40.0,
-        help="Extra stiffness used for locked joints [Nm/rad]",
+        help="Stiffness used for locked joints [Nm/rad]",
     )
 
     args = parser.parse_args()
@@ -80,7 +80,7 @@
         period=0.001,
     ) as tracker:
         while tracker.tick():
-            q_ref = list(robot.current_joint_positions)
+            q_ref = robot.current_joint_positions
             if args.lock_joint6:
                 q_ref[5] = locked_targets[5]
             if args.lock_joint7:

franky/tracker.py

@@ -23,6 +23,24 @@ def _is_premption_exception(exc: ControlException) -> bool:
     return "Move command preempted!" in str(exc)
 
 
+_DEFAULT_JOINT_STIFFNESS = np.full(7, 50.0)
+
+
+def _default_joint_damping(stiffness: np.ndarray) -> np.ndarray:
+    return 2.0 * np.sqrt(stiffness)
+
+
+def _as_joint_gain(name: str, value) -> np.ndarray:
+    vector = np.asarray(value, dtype=float)
+    if vector.shape != (7,):
+        raise ValueError(f"{name} must contain exactly 7 values")
+    if not np.all(np.isfinite(vector)):
+        raise ValueError(f"{name} must contain only finite values")
+    if np.any(vector < 0.0):
+        raise ValueError(f"{name} must contain only non-negative values")
+    return vector.copy()
+
+
 class CartesianImpedanceTracker:
     """A long-lived session for streaming Cartesian impedance tracking commands.
 
@@ -277,16 +295,21 @@ def __init__(
         q = self._robot.current_joint_positions
         self._reference_handle.set(q)
 
-        # Seed gains handle with initial values.
-        stiffness_init = (
-            np.asarray(stiffness) if stiffness is not None else np.full(7, 50.0)
+        # Seed gains handle with initial values. When damping is omitted, use
+        # critical damping for unit inertia so zero stiffness implies zero damping.
+        stiffness_init = _as_joint_gain(
+            "stiffness", _DEFAULT_JOINT_STIFFNESS if stiffness is None else stiffness
+        )
+        damping_init = (
+            _as_joint_gain("damping", damping)
+            if damping is not None
+            else _default_joint_damping(stiffness_init)
         )
-        damping_init = np.asarray(damping) if damping is not None else np.full(7, 10.0)
         self._gains_handle.set(stiffness_init, damping_init)
 
         kwargs = {
-            "stiffness": stiffness,
-            "damping": damping,
+            "stiffness": stiffness_init,
+            "damping": damping_init,
             "constant_torque_offset": constant_torque_offset,
             "compensate_coriolis": compensate_coriolis,
             "max_delta_tau": max_delta_tau,
@@ -351,18 +374,27 @@ def set_gains(
     ) -> None:
         """Update joint impedance gains. Smoothed in the RT loop via exponential interpolation.
 
-        Only the provided gains are changed; omitted gains keep their current target values.
+        When stiffness is changed and damping is omitted, damping is updated to
+        the critical damping heuristic 2*sqrt(stiffness). Otherwise, omitted
+        gains keep their current target values.
         """
         current = self._gains_handle.get() if self._gains_handle.has_gains else None
-        k = (
-            np.asarray(stiffness)
-            if stiffness is not None
-            else (current.stiffness if current else np.full(7, 50.0))
+        k = _as_joint_gain(
+            "stiffness",
+            (
+                stiffness
+                if stiffness is not None
+                else (current.stiffness if current else _DEFAULT_JOINT_STIFFNESS)
+            ),
         )
         d = (
-            np.asarray(damping)
+            _as_joint_gain("damping", damping)
             if damping is not None
-            else (current.damping if current else np.full(7, 10.0))
+            else (
+                _default_joint_damping(k)
+                if stiffness is not None or current is None
+                else _as_joint_gain("damping", current.damping)
+            )
         )
         self._gains_handle.set(k, d)
 

include/franky/motion/impedance_gains_handle.hpp

@@ -8,6 +8,14 @@
 
 namespace franky {
 
+inline Vector7d defaultJointImpedanceStiffness() { return Vector7d::Constant(50.0); }
+
+inline Vector7d defaultJointImpedanceDamping(const Vector7d &stiffness) { return 2.0 * stiffness.cwiseSqrt(); }
+
+inline Vector7d defaultJointImpedanceDamping() {
+  return defaultJointImpedanceDamping(defaultJointImpedanceStiffness());
+}
+
 /**
  * @brief Target gains for a Cartesian impedance controller.
  */
@@ -45,8 +53,8 @@ class CartesianImpedanceGainsHandle {
 struct JointImpedanceGains {
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
-  Vector7d stiffness{Vector7d::Constant(50.0)};
-  Vector7d damping{Vector7d::Constant(10.0)};
+  Vector7d stiffness{defaultJointImpedanceStiffness()};
+  Vector7d damping{defaultJointImpedanceDamping()};
 };
 
 /**

include/franky/motion/joint_impedance_motion.hpp

@@ -45,10 +45,10 @@ struct TorqueSafetyParams {
  */
 struct JointImpedanceParams : public TorqueSafetyParams {
   /** Joint stiffness gains in [Nm/rad]. */
-  Vector7d stiffness{Vector7d::Constant(50.0)};
+  Vector7d stiffness{defaultJointImpedanceStiffness()};
 
   /** Joint damping gains in [Nms/rad]. */
-  Vector7d damping{Vector7d::Constant(10.0)};
+  Vector7d damping{defaultJointImpedanceDamping()};
 
   /** Constant torque offset added to every command in [Nm]. */
   Vector7d constant_torque_offset{Vector7d::Zero()};

python/bind_motion_torque.cpp

@@ -2,6 +2,9 @@
 #include <pybind11/pybind11.h>
 #include <pybind11/stl.h>
 
+#include <cmath>
+#include <string>
+
 #include "franky.hpp"
 
 namespace py = pybind11;
@@ -26,15 +29,30 @@ JointReference toJointReference(
   return reference;
 }
 
+void validateNonNegativeFinite(const Vector7d &values, const char *name) {
+  for (int i = 0; i < values.size(); ++i) {
+    if (!std::isfinite(values[i]) || values[i] < 0.0) {
+      throw py::value_error(std::string(name) + " must contain only finite, non-negative values");
+    }
+  }
+}
+
 JointImpedanceParams makeJointImpedanceParams(
     const std::optional<Vector7d> &stiffness, const std::optional<Vector7d> &damping,
     const std::optional<Vector7d> &constant_torque_offset, const std::optional<Vector7d> &lower_joint_limits,
     const std::optional<Vector7d> &upper_joint_limits, bool compensate_coriolis, double max_delta_tau,
     double joint_limit_activation_distance, double joint_limit_stiffness, double joint_limit_damping,
     double joint_limit_max_torque) {
   auto params = JointImpedanceParams{};
-  if (stiffness.has_value()) params.stiffness = stiffness.value();
-  if (damping.has_value()) params.damping = damping.value();
+  if (stiffness.has_value()) {
+    validateNonNegativeFinite(stiffness.value(), "stiffness");
+    params.stiffness = stiffness.value();
+    if (!damping.has_value()) params.damping = defaultJointImpedanceDamping(params.stiffness);
+  }
+  if (damping.has_value()) {
+    validateNonNegativeFinite(damping.value(), "damping");
+    params.damping = damping.value();
+  }
   if (constant_torque_offset.has_value()) params.constant_torque_offset = constant_torque_offset.value();
   if (lower_joint_limits.has_value() && upper_joint_limits.has_value()) {
     params.joint_limit_repulsion_active = true;
@@ -130,8 +148,15 @@ void bind_motion_torque(py::module &m) {
       .def(
           py::init<>([](const std::optional<Vector7d> &stiffness, const std::optional<Vector7d> &damping) {
             JointImpedanceGains g;
-            if (stiffness.has_value()) g.stiffness = stiffness.value();
-            if (damping.has_value()) g.damping = damping.value();
+            if (stiffness.has_value()) {
+              validateNonNegativeFinite(stiffness.value(), "stiffness");
+              g.stiffness = stiffness.value();
+              if (!damping.has_value()) g.damping = defaultJointImpedanceDamping(g.stiffness);
+            }
+            if (damping.has_value()) {
+              validateNonNegativeFinite(damping.value(), "damping");
+              g.damping = damping.value();
+            }
             return g;
           }),
           "stiffness"_a = std::nullopt,