void FrankaTorqueControlClient::updateServerCommand()

in polymetis/src/clients/franka_panda_client/franka_panda_client.cpp [157:191]

• 27 lines of code
• 5 McCabe index (conditional complexity)

void FrankaTorqueControlClient::updateServerCommand(
    /*
     * Send robot states and receive torque command via a request to the
     * controller server.
     */
    const franka::RobotState &libfranka_robot_state,
    std::array<double, NUM_DOFS> &torque_out) {
  // Record robot states
  if (!mock_franka_) {
    for (int i = 0; i < NUM_DOFS; i++) {
      robot_state_.set_joint_positions(i, libfranka_robot_state.q[i]);
      robot_state_.set_joint_velocities(i, libfranka_robot_state.dq[i]);
      robot_state_.set_motor_torques_measured(i,
                                              libfranka_robot_state.tau_J[i]);
      robot_state_.set_motor_torques_external(
          i, libfranka_robot_state.tau_ext_hat_filtered[i]);
    }
  }
  setTimestampToNow(robot_state_.mutable_timestamp());

  // Retrieve torques
  grpc::ClientContext context;
  status_ = stub_->ControlUpdate(&context, robot_state_, &torque_command_);
  if (!status_.ok()) {
    std::cout << "ControlUpdate rpc failed." << std::endl;
    return;
  }

  assert(torque_command_.joint_torques_size() == NUM_DOFS);
  for (int i = 0; i < NUM_DOFS; i++) {
    torque_out[i] = torque_command_.joint_torques(i);
    robot_state_.set_prev_joint_torques_computed(
        i, torque_command_.joint_torques(i));
  }
}