39 #ifndef _vpRobotFranka_h_
40 #define _vpRobotFranka_h_
42 #include <visp3/core/vpConfig.h>
44 #ifdef VISP_HAVE_FRANKA
53 #include <franka/exception.h>
54 #include <franka/robot.h>
55 #include <franka/model.h>
56 #include <franka/gripper.h>
58 #include <visp3/core/vpColVector.h>
59 #include <visp3/robot/vpRobot.h>
60 #include <visp3/core/vpException.h>
237 franka::Robot *m_handler;
238 franka::Gripper *m_gripper;
239 franka::Model *m_model;
240 double m_positionningVelocity;
243 std::thread m_velControlThread;
244 std::atomic_bool m_velControlThreadIsRunning;
245 std::atomic_bool m_velControlThreadStopAsked;
246 std::array<double, 7> m_dq_des;
250 std::thread m_ftControlThread;
251 std::atomic_bool m_ftControlThreadIsRunning;
252 std::atomic_bool m_ftControlThreadStopAsked;
253 std::array<double, 7> m_tau_J_des;
256 std::array<double, 7> m_q_min;
257 std::array<double, 7> m_q_max;
258 std::array<double, 7> m_dq_max;
259 std::array<double, 7> m_ddq_max;
261 franka::RobotState m_robot_state;
265 std::string m_log_folder;
266 std::string m_franka_address;
272 franka::RealtimeConfig realtime_config = franka::RealtimeConfig::kEnforce);
276 void connect(
const std::string &franka_address,
277 franka::RealtimeConfig realtime_config = franka::RealtimeConfig::kEnforce);
293 franka::RobotState getRobotInternalState();
300 franka::Gripper *getGripperHandler() {
314 franka::Robot *getHandler() {
333 int gripperGrasp(
double grasping_width,
double force=60.);
334 void gripperHoming();
335 int gripperMove(
double width);
337 void gripperRelease();
339 void move(
const std::string &filename,
double velocity_percentage=10.);
341 bool readPosFile(
const std::string &filename,
vpColVector &q);
342 bool savePosFile(
const std::string &filename,
const vpColVector &q);
346 const double &filter_gain=0.1,
const bool &activate_pi_controller=
false);
347 void setLogFolder(
const std::string &folder);
349 void setPositioningVelocity(
double velocity);
358 #endif // #ifndef __vpRobotFranka_h_