Visual Servoing Platform version 3.5.0
testFrankaGetPose.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5 *
6 * This software is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2 of the License, or
9 * (at your option) any later version.
10 * See the file LICENSE.txt at the root directory of this source
11 * distribution for additional information about the GNU GPL.
12 *
13 * For using ViSP with software that can not be combined with the GNU
14 * GPL, please contact Inria about acquiring a ViSP Professional
15 * Edition License.
16 *
17 * See http://visp.inria.fr for more information.
18 *
19 * This software was developed at:
20 * Inria Rennes - Bretagne Atlantique
21 * Campus Universitaire de Beaulieu
22 * 35042 Rennes Cedex
23 * France
24 *
25 * If you have questions regarding the use of this file, please contact
26 * Inria at visp@inria.fr
27 *
28 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30 *
31 * Description:
32 * Test Franka robot behavior
33 *
34 * Authors:
35 * Fabien Spindler
36 *
37 *****************************************************************************/
38
46#include <iostream>
47
48#include <visp3/core/vpConfig.h>
49
50#if defined(VISP_HAVE_FRANKA)
51
52#include <visp3/robot/vpRobotFranka.h>
53
54
55int main(int argc, char **argv)
56{
57 std::string robot_ip = "192.168.1.1";
58
59 for (int i = 1; i < argc; i++) {
60 if (std::string(argv[i]) == "--ip" && i + 1 < argc) {
61 robot_ip = std::string(argv[i + 1]);
62 }
63 else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
64 std::cout << argv[0] << " [--ip 192.168.1.1] [--help] [-h]"
65 << "\n";
66 return EXIT_SUCCESS;
67 }
68 }
69
70 try {
71 std::cout << "-- Start test 1/4" << std::endl;
72 vpRobotFranka robot;
73 robot.connect(robot_ip);
74
76
77 for (unsigned i = 0; i < 10; i++) {
78 robot.getPosition(vpRobot::JOINT_STATE, q);
79 std::cout << "Joint position: " << q.t() << std::endl;
80 vpTime::wait(10);
81 }
82
83 vpPoseVector fPe;
84 robot.getPosition(vpRobot::END_EFFECTOR_FRAME, fPe);
85 std::cout << "fMe pose vector: " << fPe.t() << std::endl;
86 std::cout << "fMe pose matrix: \n" << vpHomogeneousMatrix(fPe) << std::endl;
87 }
88 catch(const vpException &e) {
89 std::cout << "ViSP exception: " << e.what() << std::endl;
90 return EXIT_FAILURE;
91 }
92 catch(const franka::NetworkException &e) {
93 std::cout << "Franka network exception: " << e.what() << std::endl;
94 std::cout << "Check if you are connected to the Franka robot"
95 << " or if you specified the right IP using --ip command"
96 << " line option set by default to 192.168.1.1. " << std::endl;
97 return EXIT_FAILURE;
98 }
99 catch(const std::exception &e) {
100 std::cout << "Franka exception: " << e.what() << std::endl;
101 return EXIT_FAILURE;
102 }
103
104 try {
105 std::cout << "-- Start test 2/4" << std::endl;
106 vpRobotFranka robot(robot_ip);
107
108 franka::Robot *handler = robot.getHandler();
109
110 // Get end-effector cartesian position
111 std::array<double, 16> pose = handler->readOnce().O_T_EE;
113 for (unsigned int i=0; i< 4; i++) {
114 for (unsigned int j=0; j< 4; j++) {
115 oMee[i][j] = pose[j*4 + i];
116 }
117 }
118 std::cout << "oMee: \n" << oMee << std::endl;
119
120 // Get flange to end-effector frame transformation
121 pose = handler->readOnce().F_T_EE;
123 for (unsigned int i=0; i< 4; i++) {
124 for (unsigned int j=0; j< 4; j++) {
125 fMee[i][j] = pose[j*4 + i];
126 }
127 }
128 std::cout << "fMee: \n" << fMee << std::endl;
129
130 // Get end-effector to K frame transformation
131 pose = handler->readOnce().EE_T_K;
133 for (unsigned int i=0; i< 4; i++) {
134 for (unsigned int j=0; j< 4; j++) {
135 eeMk[i][j] = pose[j*4 + i];
136 }
137 }
138 std::cout << "eeMk: \n" << eeMk << std::endl;
139 }
140 catch(const vpException &e) {
141 std::cout << "ViSP exception: " << e.what() << std::endl;
142 return EXIT_FAILURE;
143 }
144 catch(const franka::NetworkException &e) {
145 std::cout << "Franka network exception: " << e.what() << std::endl;
146 std::cout << "Check if you are connected to the Franka robot"
147 << " or if you specified the right IP using --ip command line option set by default to 192.168.1.1. " << std::endl;
148 return EXIT_FAILURE;
149 }
150 catch(const std::exception &e) {
151 std::cout << "Franka exception: " << e.what() << std::endl;
152 return EXIT_FAILURE;
153 }
154
155 try {
156 std::cout << "-- Start test 3/4" << std::endl;
157 vpRobotFranka robot;
158 robot.connect(robot_ip);
159
160 vpMatrix mass;
161 robot.getMass(mass);
162 std::cout << "Mass matrix:\n" << mass << std::endl;
163
164 vpColVector gravity;
165 robot.getGravity(gravity);
166 std::cout << "Gravity vector: " << gravity.t() << std::endl;
167
168 vpColVector coriolis;
169 robot.getCoriolis(coriolis);
170 std::cout << "Coriolis vector: " << coriolis.t() << std::endl;
171 }
172 catch(const vpException &e) {
173 std::cout << "ViSP exception: " << e.what() << std::endl;
174 return EXIT_FAILURE;
175 }
176 catch(const franka::NetworkException &e) {
177 std::cout << "Franka network exception: " << e.what() << std::endl;
178 std::cout << "Check if you are connected to the Franka robot"
179 << " or if you specified the right IP using --ip command line option set by default to 192.168.1.1. " << std::endl;
180 return EXIT_FAILURE;
181 }
182 catch(const std::exception &e) {
183 std::cout << "Franka exception: " << e.what() << std::endl;
184 return EXIT_FAILURE;
185 }
186
187 try {
188 std::cout << "-- Start test 4/4" << std::endl;
189 vpRobotFranka robot;
190 robot.connect(robot_ip);
191
192 vpColVector q;
193 robot.getPosition(vpRobot::JOINT_STATE, q);
194 std::cout << "Joint position: " << q.t() << std::endl;
195
196 vpMatrix fJe;
197 robot.get_fJe(fJe);
198 std::cout << "Jacobian fJe:\n" << fJe << std::endl;
199
200 robot.get_fJe(q, fJe);
201 std::cout << "Jacobian fJe:\n" << fJe << std::endl;
202
203 vpMatrix eJe;
204 robot.get_eJe(eJe);
205 std::cout << "Jacobian eJe:\n" << eJe << std::endl;
206
207 robot.get_eJe(q, eJe);
208 std::cout << "Jacobian eJe:\n" << eJe << std::endl;
209 }
210 catch(const vpException &e) {
211 std::cout << "ViSP exception: " << e.what() << std::endl;
212 return EXIT_FAILURE;
213 }
214 catch(const franka::NetworkException &e) {
215 std::cout << "Franka network exception: " << e.what() << std::endl;
216 std::cout << "Check if you are connected to the Franka robot"
217 << " or if you specified the right IP using --ip command line option set by default to 192.168.1.1. " << std::endl;
218 return EXIT_FAILURE;
219 }
220 catch(const std::exception &e) {
221 std::cout << "Franka exception: " << e.what() << std::endl;
222 return EXIT_FAILURE;
223 }
224
225 std::cout << "The end" << std::endl;
226 return EXIT_SUCCESS;
227}
228
229#else
230int main()
231{
232 std::cout << "ViSP is not build with libfranka..." << std::endl;
233}
234#endif
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
vpRowVector t() const
error that can be emited by ViSP classes.
Definition: vpException.h:72
const char * what() const
Implementation of an homogeneous matrix and operations on such kind of matrices.
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:152
vpRowVector t() const
void get_eJe(vpMatrix &eJe)
@ JOINT_STATE
Definition: vpRobot.h:80
@ END_EFFECTOR_FRAME
Definition: vpRobot.h:81
VISP_EXPORT int wait(double t0, double t)