Visual Servoing Platform  version 3.1.0
franka_print_joint_positions.cpp
1 // Copyright (c) 2017 Franka Emika GmbH
2 // Use of this source code is governed by the Apache-2.0 license, see LICENSE
3 #include <iostream>
4 #include <iterator>
5 
6 #include <visp3/core/vpConfig.h>
7 
8 #ifdef VISP_HAVE_FRANKA
9 
10 #include <franka/exception.h>
11 #include <franka/model.h>
12 
22 template <class T, size_t N> std::ostream &operator<<(std::ostream &ostream, const std::array<T, N> &array)
23 {
24  ostream << "[";
25  std::copy(array.cbegin(), array.cend() - 1, std::ostream_iterator<T>(ostream, ","));
26  std::copy(array.cend() - 1, array.cend(), std::ostream_iterator<T>(ostream));
27  ostream << "]";
28  return ostream;
29 }
30 
31 int main(int argc, char **argv)
32 {
33  if (argc != 2) {
34  std::cerr << "Usage: ./print_joint_positions <robot-hostname>" << std::endl;
35  return -1;
36  }
37 
38  try {
39  franka::Robot robot(argv[1]);
40 
41  franka::RobotState state = robot.readOnce();
42 
43  franka::Model model(robot.loadModel());
44  for (franka::Frame frame = franka::Frame::kJoint1; frame <= franka::Frame::kEndEffector; frame++) {
45  std::cout << model.pose(frame, state) << std::endl;
46  }
47  } catch (franka::Exception const &e) {
48  std::cout << e.what() << std::endl;
49  return -1;
50  }
51 
52  return 0;
53 }
54 
55 #else
56 int main() { std::cout << "This example needs libfranka to control Panda robot." << std::endl; }
57 #endif