44 #include <visp3/core/vpConfig.h>
46 #if defined(VISP_HAVE_FRANKA)
48 #include <visp3/robot/vpRobotFranka.h>
50 int main(
int argc,
char **argv)
52 std::string robot_ip =
"192.168.1.1";
54 for (
int i = 1; i < argc; i++) {
55 if (std::string(argv[i]) ==
"--ip" && i + 1 < argc) {
56 robot_ip = std::string(argv[i + 1]);
57 }
else if (std::string(argv[i]) ==
"--help" || std::string(argv[i]) ==
"-h") {
58 std::cout << argv[0] <<
" [--ip 192.168.1.1] [--help] [-h]"
65 std::cout <<
"-- Start test 1/4" << std::endl;
67 robot.connect(robot_ip);
71 for (
unsigned i = 0; i < 10; i++) {
73 std::cout <<
"Joint position: " << q.
t() << std::endl;
79 std::cout <<
"fMe pose vector: " << fPe.
t() << std::endl;
82 std::cout <<
"ViSP exception: " << e.
what() << std::endl;
84 }
catch (
const franka::NetworkException &e) {
85 std::cout <<
"Franka network exception: " << e.what() << std::endl;
86 std::cout <<
"Check if you are connected to the Franka robot"
87 <<
" or if you specified the right IP using --ip command"
88 <<
" line option set by default to 192.168.1.1. " << std::endl;
90 }
catch (
const std::exception &e) {
91 std::cout <<
"Franka exception: " << e.what() << std::endl;
96 std::cout <<
"-- Start test 2/4" << std::endl;
99 franka::Robot *handler = robot.getHandler();
102 std::array<double, 16> pose = handler->readOnce().O_T_EE;
104 for (
unsigned int i = 0; i < 4; i++) {
105 for (
unsigned int j = 0; j < 4; j++) {
106 oMee[i][j] = pose[j * 4 + i];
109 std::cout <<
"oMee: \n" << oMee << std::endl;
112 pose = handler->readOnce().F_T_EE;
114 for (
unsigned int i = 0; i < 4; i++) {
115 for (
unsigned int j = 0; j < 4; j++) {
116 fMee[i][j] = pose[j * 4 + i];
119 std::cout <<
"fMee: \n" << fMee << std::endl;
122 pose = handler->readOnce().EE_T_K;
124 for (
unsigned int i = 0; i < 4; i++) {
125 for (
unsigned int j = 0; j < 4; j++) {
126 eeMk[i][j] = pose[j * 4 + i];
129 std::cout <<
"eeMk: \n" << eeMk << std::endl;
131 std::cout <<
"ViSP exception: " << e.
what() << std::endl;
133 }
catch (
const franka::NetworkException &e) {
134 std::cout <<
"Franka network exception: " << e.what() << std::endl;
135 std::cout <<
"Check if you are connected to the Franka robot"
136 <<
" or if you specified the right IP using --ip command line option set by default to 192.168.1.1. "
139 }
catch (
const std::exception &e) {
140 std::cout <<
"Franka exception: " << e.what() << std::endl;
145 std::cout <<
"-- Start test 3/4" << std::endl;
147 robot.connect(robot_ip);
151 std::cout <<
"Mass matrix:\n" << mass << std::endl;
154 robot.getGravity(gravity);
155 std::cout <<
"Gravity vector: " << gravity.
t() << std::endl;
158 robot.getCoriolis(coriolis);
159 std::cout <<
"Coriolis vector: " << coriolis.
t() << std::endl;
161 std::cout <<
"ViSP exception: " << e.
what() << std::endl;
163 }
catch (
const franka::NetworkException &e) {
164 std::cout <<
"Franka network exception: " << e.what() << std::endl;
165 std::cout <<
"Check if you are connected to the Franka robot"
166 <<
" or if you specified the right IP using --ip command line option set by default to 192.168.1.1. "
169 }
catch (
const std::exception &e) {
170 std::cout <<
"Franka exception: " << e.what() << std::endl;
175 std::cout <<
"-- Start test 4/4" << std::endl;
177 robot.connect(robot_ip);
181 std::cout <<
"Joint position: " << q.
t() << std::endl;
185 std::cout <<
"Jacobian fJe:\n" << fJe << std::endl;
187 robot.get_fJe(q, fJe);
188 std::cout <<
"Jacobian fJe:\n" << fJe << std::endl;
192 std::cout <<
"Jacobian eJe:\n" << eJe << std::endl;
195 std::cout <<
"Jacobian eJe:\n" << eJe << std::endl;
197 std::cout <<
"ViSP exception: " << e.
what() << std::endl;
199 }
catch (
const franka::NetworkException &e) {
200 std::cout <<
"Franka network exception: " << e.what() << std::endl;
201 std::cout <<
"Check if you are connected to the Franka robot"
202 <<
" or if you specified the right IP using --ip command line option set by default to 192.168.1.1. "
205 }
catch (
const std::exception &e) {
206 std::cout <<
"Franka exception: " << e.what() << std::endl;
210 std::cout <<
"The end" << std::endl;
215 int main() { std::cout <<
"ViSP is not build with libfranka..." << std::endl; }
Implementation of column vector and the associated operations.
error that can be emitted by ViSP classes.
const char * what() const
Implementation of an homogeneous matrix and operations on such kind of matrices.
Implementation of a matrix and operations on matrices.
Implementation of a pose vector and operations on poses.
void get_eJe(vpMatrix &eJe) vp_override
VISP_EXPORT int wait(double t0, double t)