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
[...]
If this post helped you, please consider buying me a coffee or donating via PayPal to support research & publishing of new posts on TechOverflow