libfranka minimal example: How to query q (pose)

This minimal example uses libfranka to query the current joint pose of a Franka robot.

#include <franka/robot.h>
#include <franka/robot_state.h>
#include <franka/exception.h>
#include <iostream>
#include <array>
#include <chrono>
#include <iostream>
#include <thread>
#include <iomanip>

using std::cout, std::endl;

int main() {
  try {
    // Connect to the robot.
    franka::Robot robot("192.168.0.1");  // Replace with your robot's IP address.

    // Continuously query and print the robot pose.
    while (true) {
      // Get the current robot state.
      franka::RobotState state = robot.readOnce();

      // Extract and print the pose of the end-effector.
      // Use fixed width to align all columns
      cout << "q =" << std::fixed << std::setprecision(5);
      for (size_t i = 0; i < state.q.size(); i++)
      {
        cout << state.q.at(i) << " ";
      }
      cout << endl;
      // Add a small delay to avoid excessive output.
      std::this_thread::sleep_for(std::chrono::milliseconds(1));
    }
  } catch (const franka::Exception& ex) {
    // Handle exceptions thrown by the robot control.
    std::cerr << "Exception: " << ex.what() << std::endl;
    return -1;
  }

  return 0;
}

How to compile

Given that libfranka has been installed system-wide, you can compile this example with:

g++ -o franka_q franka_q.cpp -lfranka

Run it using

./franka_q

Example output

q =-0.04501 -0.75216 -0.01165 -2.35292 -0.07323 1.57705 0.79462 
[...]