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/gripper.h>
55 #include <franka/model.h>
56 #include <franka/robot.h>
58 #include <visp3/core/vpColVector.h>
59 #include <visp3/core/vpException.h>
60 #include <visp3/robot/vpRobot.h>
239 franka::Robot *m_handler;
240 franka::Gripper *m_gripper;
241 franka::Model *m_model;
242 double m_positionningVelocity;
245 std::thread m_velControlThread;
246 std::atomic_bool m_velControlThreadIsRunning;
247 std::atomic_bool m_velControlThreadStopAsked;
248 std::array<double, 7> m_dq_des;
252 std::thread m_ftControlThread;
253 std::atomic_bool m_ftControlThreadIsRunning;
254 std::atomic_bool m_ftControlThreadStopAsked;
255 std::array<double, 7> m_tau_J_des;
258 std::array<double, 7> m_q_min;
259 std::array<double, 7> m_q_max;
260 std::array<double, 7> m_dq_max;
261 std::array<double, 7> m_ddq_max;
263 franka::RobotState m_robot_state;
267 std::string m_log_folder;
268 std::string m_franka_address;
274 franka::RealtimeConfig realtime_config = franka::RealtimeConfig::kEnforce);
278 void connect(
const std::string &franka_address,
279 franka::RealtimeConfig realtime_config = franka::RealtimeConfig::kEnforce);
295 franka::RobotState getRobotInternalState();
336 int gripperGrasp(
double grasping_width,
double force = 60.);
337 void gripperHoming();
338 int gripperMove(
double width);
340 void gripperRelease();
342 void move(
const std::string &filename,
double velocity_percentage = 10.);
344 bool readPosFile(
const std::string &filename,
vpColVector &q);
345 bool savePosFile(
const std::string &filename,
const vpColVector &q);
349 const bool &activate_pi_controller =
false);
350 void setLogFolder(
const std::string &folder);
352 void setPositioningVelocity(
double velocity);
Implementation of column vector and the associated operations.
error that can be emited by ViSP classes.
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.
franka::Robot * getHandler()
franka::Gripper * getGripperHandler()
Class that defines a generic virtual robot.
virtual void get_eJe(vpMatrix &_eJe)=0
Get the robot Jacobian expressed in the end-effector frame.
virtual void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)=0
virtual void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position)=0
Get the robot position (frame has to be specified).
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
virtual void get_fJe(vpMatrix &_fJe)=0
virtual void getDisplacement(const vpRobot::vpControlFrameType frame, vpColVector &q)=0
virtual void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &q)=0
Set a displacement (frame has to be specified) in position control.