void FrankaTorqueControlClient::computeSafetyReflex()

in polymetis/src/clients/franka_panda_client/franka_panda_client.cpp [271:315]

• 32 lines of code
• 8 McCabe index (conditional complexity)

void FrankaTorqueControlClient::computeSafetyReflex(
    std::array<double, N> values, std::array<double, N> lower_limit,
    std::array<double, N> upper_limit, bool invert_lower,
    std::array<double, N> &safety_torques, double margin, double k,
    const char *item_name) {
  /*
   * Apply safety mechanisms for a vector based on input values and limits.
   * Throws an error if limits are violated.
   * Also computes & outputs safety controller torques.
   * (Note: invert_lower flips the sign of the lower limit. Used for velocities
   * and torques.)
   */
  double upper_violation, lower_violation;
  double lower_sign = 1.0;

  if (invert_lower) {
    lower_sign = -1.0;
  }

  for (int i = 0; i < N; i++) {
    upper_violation = values[i] - upper_limit[i];
    lower_violation = lower_sign * lower_limit[i] - values[i];

    // Check hard limits
    if (upper_violation > 0 || lower_violation > 0) {
      std::cout << "Safety limits exceeded: "
                << "\n\ttype = \"" << std::string(item_name) << "\""
                << "\n\tdim = " << i
                << "\n\tlimits = " << lower_sign * lower_limit[i] << ", "
                << upper_limit[i] << "\n\tvalue = " << values[i] << "\n";
      throw std::runtime_error(
          "Error: Safety limits exceeded in FrankaTorqueControlClient.\n");
      break;
    }

    // Check soft limits & compute feedback forces (safety controller)
    if (is_safety_controller_active_) {
      if (upper_violation > -margin) {
        safety_torques[i] -= k * (margin + upper_violation);
      } else if (lower_violation > -margin) {
        safety_torques[i] += k * (margin + lower_violation);
      }
    }
  }
}