void FrankaTorqueControlClient::run()

in polymetis/src/clients/franka_panda_client/franka_panda_client.cpp [106:155]

• 36 lines of code
• 6 McCabe index (conditional complexity)

void FrankaTorqueControlClient::run() {
  // Create callback function that relays information between gRPC server and
  // robot
  auto control_callback = [&](const franka::RobotState &libfranka_robot_state,
                              franka::Duration) -> franka::Torques {
    // Compute torque components
    updateServerCommand(libfranka_robot_state, torque_commanded_);
    checkStateLimits(libfranka_robot_state, torque_safety_);

    // Aggregate & clamp torques
    for (int i = 0; i < NUM_DOFS; i++) {
      torque_applied_[i] = torque_commanded_[i] + torque_safety_[i];
    }
    checkTorqueLimits(torque_applied_);

    // Record final applied torques
    for (int i = 0; i < NUM_DOFS; i++) {
      robot_state_.set_prev_joint_torques_computed_safened(i,
                                                           torque_applied_[i]);
    }

    return torque_applied_;
  };

  // Send lambda function
  try {
    if (!mock_franka_) {
      robot_ptr_->control(control_callback);
    } else {
      franka::RobotState dummy_robot_state;
      franka::Duration dummy_duration;

      int period = 1.0 / FRANKA_HZ;
      int period_ns = period * 1.0e9;

      struct timespec abs_target_time;
      while (true) {
        clock_gettime(CLOCK_REALTIME, &abs_target_time);
        abs_target_time.tv_nsec += period_ns;

        control_callback(dummy_robot_state, dummy_duration);

        clock_nanosleep(CLOCK_REALTIME, TIMER_ABSTIME, &abs_target_time,
                        nullptr);
      }
    }
  } catch (const std::exception &ex) {
    std::cout << ex.what() << std::endl;
  }
}