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> 234 franka::RobotState getRobotInternalState();
237 franka::Robot *m_handler;
238 franka::Gripper *m_gripper;
239 franka::Model *m_model;
240 double m_positionningVelocity;
242 std::thread m_controlThread;
243 std::atomic_bool m_controlThreadIsRunning;
244 std::atomic_bool m_controlThreadStopAsked;
246 std::array<double, 7> m_q_min;
247 std::array<double, 7> m_q_max;
248 std::array<double, 7> m_dq_max;
249 std::array<double, 7> m_ddq_max;
251 franka::RobotState m_robot_state;
254 std::array<double, 7> m_dq_des;
257 std::string m_log_folder;
258 std::string m_franka_address;
264 franka::RealtimeConfig realtime_config = franka::RealtimeConfig::kEnforce);
268 void connect(
const std::string &franka_address,
269 franka::RealtimeConfig realtime_config = franka::RealtimeConfig::kEnforce);
323 int gripperGrasp(
double grasping_width,
double force=60.);
324 void gripperHoming();
325 int gripperMove(
double width);
327 void gripperRelease();
329 void move(
const std::string &filename,
double velocity_percentage=10.);
331 bool readPosFile(
const std::string &filename,
vpColVector &q);
332 bool savePosFile(
const std::string &filename,
const vpColVector &q);
335 void setLogFolder(
const std::string &folder);
337 void setPositioningVelocity(
const double velocity);
346 #endif // #ifndef __vpRobotFranka_h_ Implementation of a matrix and operations on matrices.
virtual void getDisplacement(const vpRobot::vpControlFrameType frame, vpColVector &q)=0
Implementation of an homogeneous matrix and operations on such kind of matrices.
error that can be emited by ViSP classes.
virtual void get_eJe(vpMatrix &_eJe)=0
Get the robot Jacobian expressed in the end-effector frame.
virtual void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &q)=0
Set a displacement (frame has to be specified) in position control.
Class that defines a generic virtual robot.
franka::Gripper * getGripperHandler()
franka::Robot * getHandler()
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
virtual void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q)=0
Get the robot position (frame has to be specified).
virtual void get_fJe(vpMatrix &_fJe)=0
Implementation of column vector and the associated operations.
Implementation of a pose vector and operations on poses.
virtual void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)=0