void FrankaTorqueControlClient::checkStateLimits()

in polymetis/src/clients/franka_panda_client/franka_panda_client.cpp [193:252]
42 lines of code
5 McCabe index (conditional complexity)

void FrankaTorqueControlClient::checkStateLimits(
    const franka::RobotState &libfranka_robot_state,
    std::array<double, NUM_DOFS> &torque_out) {
  /*
   * Compute robot state limit violations and apply safety mechanisms.
   */
  std::array<double, 3> ee_pos_buf, force_buf;
  std::array<double, 1> elbow_vel_buf, elbow_lim_buf, dummy;

  // No safety checks in mock mode
  if (mock_franka_) {
    return;
  }

  // Reset reflex torques
  for (int i = 0; i < NUM_DOFS; i++) {
    torque_out[i] = 0.0;
  }
  for (int i = 0; i < 3; i++) {
    force_buf[i] = 0.0;
  }

  // Cartesian position limits
  for (int i = 0; i < 3; i++) {
    ee_pos_buf[i] = libfranka_robot_state.O_T_EE[12 + i];
  }
  computeSafetyReflex(ee_pos_buf, cartesian_pos_llimits_,
                      cartesian_pos_ulimits_, false, force_buf,
                      margin_cartesian_pos_, k_cartesian_pos_, "EE position");

  std::array<double, 6 *NUM_DOFS> jacobian_array = model_ptr_->zeroJacobian(
      franka::Frame::kEndEffector, libfranka_robot_state);
  Eigen::Map<const Eigen::Matrix<double, 6, NUM_DOFS>> jacobian(
      jacobian_array.data());
  Eigen::Map<const Eigen::Vector3d> force_xyz_vec(force_buf.data());

  Eigen::VectorXd force_vec(6);
  force_vec.head(3) << force_xyz_vec;
  force_vec.tail(3) << Eigen::Vector3d::Zero();

  Eigen::VectorXd torque_vec(NUM_DOFS);
  torque_vec << jacobian.transpose() * force_vec;
  Eigen::VectorXd::Map(&torque_out[0], NUM_DOFS) = torque_vec;

  // Joint position limits
  computeSafetyReflex(libfranka_robot_state.q, joint_pos_llimits_,
                      joint_pos_ulimits_, false, torque_out, margin_joint_pos_,
                      k_joint_pos_, "Joint position");

  // Joint velocity limits
  computeSafetyReflex(libfranka_robot_state.dq, joint_vel_limits_,
                      joint_vel_limits_, true, torque_out, margin_joint_vel_,
                      k_joint_vel_, "Joint velocity");

  // Miscellaneous velocity limits
  elbow_vel_buf[0] = libfranka_robot_state.delbow_c[0];
  elbow_lim_buf[0] = elbow_vel_limit_;
  computeSafetyReflex(elbow_vel_buf, elbow_lim_buf, elbow_lim_buf, true, dummy,
                      0.0, 0.0, "Elbow velocity");
}