36 #include <visp3/core/vpConfig.h>
38 #ifdef VISP_HAVE_FRANKA
40 #include <visp3/core/vpIoTools.h>
41 #include <visp3/robot/vpRobotException.h>
42 #include <visp3/robot/vpRobotFranka.h>
44 #include "vpForceTorqueGenerator_impl.h"
45 #include "vpJointPosTrajGenerator_impl.h"
46 #include "vpJointVelTrajGenerator_impl.h"
54 :
vpRobot(), m_handler(nullptr), m_gripper(nullptr), m_model(nullptr), m_positioningVelocity(20.), m_velControlThread(),
55 m_velControlThreadIsRunning(false), m_velControlThreadStopAsked(false), m_dq_des(), m_v_cart_des(),
56 m_ftControlThread(), m_ftControlThreadIsRunning(false), m_ftControlThreadStopAsked(false), m_tau_J_des(),
57 m_ft_cart_des(), m_q_min(), m_q_max(), m_dq_max(), m_ddq_max(), m_robot_state(), m_mutex(), m_eMc(), m_log_folder(),
70 :
vpRobot(), m_handler(nullptr), m_gripper(nullptr), m_model(nullptr), m_positioningVelocity(20.), m_velControlThread(),
71 m_velControlThreadIsRunning(false), m_velControlThreadStopAsked(false), m_dq_des(), m_v_cart_des(),
72 m_ftControlThread(), m_ftControlThreadIsRunning(false), m_ftControlThreadStopAsked(false), m_tau_J_des(),
73 m_ft_cart_des(), m_q_min(), m_q_max(), m_dq_max(), m_ddq_max(), m_robot_state(), m_mutex(), m_eMc(), m_log_folder(),
77 connect(franka_address, realtime_config);
83 void vpRobotFranka::init()
87 m_q_min = std::array<double, 7>{-2.8973, -1.7628, -2.8973, -3.0718, -2.8973, -0.0175, -2.8973};
88 m_q_max = std::array<double, 7>{2.8973, 1.7628, 2.8973, -0.0698, 2.8973, 3.7525, 2.8973};
89 m_dq_max = std::array<double, 7>{2.1750, 2.1750, 2.1750, 2.1750, 2.6100, 2.6100, 2.6100};
90 m_ddq_max = std::array<double, 7>{15.0, 7.5, 10.0, 12.5, 15.0, 20.0, 20.0};
106 std::cout <<
"Grasped object, will release it now." << std::endl;
125 if (franka_address.empty()) {
131 m_franka_address = franka_address;
132 m_handler =
new franka::Robot(franka_address, realtime_config);
134 std::array<double, 7> lower_torque_thresholds_nominal{{25.0, 25.0, 22.0, 20.0, 19.0, 17.0, 14.}};
135 std::array<double, 7> upper_torque_thresholds_nominal{{35.0, 35.0, 32.0, 30.0, 29.0, 27.0, 24.0}};
136 std::array<double, 7> lower_torque_thresholds_acceleration{{25.0, 25.0, 22.0, 20.0, 19.0, 17.0, 14.0}};
137 std::array<double, 7> upper_torque_thresholds_acceleration{{35.0, 35.0, 32.0, 30.0, 29.0, 27.0, 24.0}};
138 std::array<double, 6> lower_force_thresholds_nominal{{30.0, 30.0, 30.0, 25.0, 25.0, 25.0}};
139 std::array<double, 6> upper_force_thresholds_nominal{{40.0, 40.0, 40.0, 35.0, 35.0, 35.0}};
140 std::array<double, 6> lower_force_thresholds_acceleration{{30.0, 30.0, 30.0, 25.0, 25.0, 25.0}};
141 std::array<double, 6> upper_force_thresholds_acceleration{{40.0, 40.0, 40.0, 35.0, 35.0, 35.0}};
142 m_handler->setCollisionBehavior(lower_torque_thresholds_acceleration, upper_torque_thresholds_acceleration,
143 lower_torque_thresholds_nominal, upper_torque_thresholds_nominal,
144 lower_force_thresholds_acceleration, upper_force_thresholds_acceleration,
145 lower_force_thresholds_nominal, upper_force_thresholds_nominal);
147 m_handler->setJointImpedance({{3000, 3000, 3000, 2500, 2500, 2000, 2000}});
148 m_handler->setCartesianImpedance({{3000, 3000, 3000, 300, 300, 300}});
149 #if (VISP_HAVE_FRANKA_VERSION < 0x000500)
151 m_handler->setFilters(10, 10, 10, 10, 10);
158 m_model =
new franka::Model(m_handler->loadModel());
183 for (
int i = 0; i < 7; i++) {
184 q[i] = robot_state.q[i];
196 for (
size_t i = 0; i < 6; i++) {
197 position[i] = fPe[i];
205 for (
size_t i = 0; i < 6; i++) {
206 position[i] = fPc[i];
237 for (
int i = 0; i < 7; i++)
238 force[i] = robot_state.tau_J[i];
244 for (
int i = 0; i < 6; i++)
245 force[i] = robot_state.K_F_ext_hat_K[i];
251 for (
int i = 0; i < 6; i++)
252 eFe[i] = robot_state.K_F_ext_hat_K[i];
292 for (
int i = 0; i < 7; i++) {
293 d_position[i] = robot_state.dq[i];
315 std::array<double, 7> coriolis_;
319 coriolis_ = m_model->coriolis(robot_state);
322 for (
int i = 0; i < 7; i++) {
323 coriolis[i] = coriolis_[i];
337 std::array<double, 7> gravity_;
341 gravity_ = m_model->gravity(robot_state);
344 for (
int i = 0; i < 7; i++) {
345 gravity[i] = gravity_[i];
359 std::array<double, 49> mass_;
363 mass_ = m_model->mass(robot_state);
366 for (
size_t i = 0; i < 7; i++) {
367 for (
size_t j = 0; j < 7; j++) {
368 mass[i][j] = mass_[j * 7 + i];
389 std::array<double, 7> q_array;
390 for (
size_t i = 0; i < 7; i++)
395 std::array<double, 16> pose_array =
396 m_model->pose(franka::Frame::kEndEffector, q_array, robot_state.F_T_EE, robot_state.EE_T_K);
398 for (
unsigned int i = 0; i < 4; i++) {
399 for (
unsigned int j = 0; j < 4; j++) {
400 fMe[i][j] = pose_array[j * 4 + i];
424 return (fMe * m_eMc);
447 std::array<double, 16> pose_array = robot_state.O_T_EE;
449 for (
unsigned int i = 0; i < 4; i++) {
450 for (
unsigned int j = 0; j < 4; j++) {
451 fMe[i][j] = pose_array[j * 4 + i];
483 std::array<double, 42> jacobian = m_model->bodyJacobian(franka::Frame::kEndEffector, robot_state);
485 for (
size_t i = 0; i < 6; i++) {
486 for (
size_t j = 0; j < 7; j++) {
487 eJe_[i][j] = jacobian[j * 6 + i];
506 std::array<double, 7> q_array;
507 for (
size_t i = 0; i < 7; i++)
510 std::array<double, 42> jacobian = m_model->bodyJacobian(franka::Frame::kEndEffector, q_array, robot_state.F_T_EE,
513 for (
size_t i = 0; i < 6; i++) {
514 for (
size_t j = 0; j < 7; j++) {
515 eJe_[i][j] = jacobian[j * 6 + i];
534 std::array<double, 42> jacobian = m_model->zeroJacobian(franka::Frame::kEndEffector, robot_state);
536 for (
size_t i = 0; i < 6; i++) {
537 for (
size_t j = 0; j < 7; j++) {
538 fJe_[i][j] = jacobian[j * 6 + i];
557 "Cannot get Franka robot fJe jacobian with an input joint position vector [%u] that is not a 7-dim vector",
563 std::array<double, 7> q_array;
564 for (
size_t i = 0; i < 7; i++)
567 std::array<double, 42> jacobian = m_model->zeroJacobian(franka::Frame::kEndEffector, q_array, robot_state.F_T_EE,
570 for (
size_t i = 0; i < 6; i++) {
571 for (
size_t j = 0; j < 7; j++) {
572 fJe_[i][j] = jacobian[j * 6 + i];
589 if (!folder.empty()) {
593 m_log_folder = folder;
596 error =
"Unable to create Franka log folder: " + folder;
600 m_log_folder = folder;
616 std::cout <<
"Robot was not in position-based control. "
617 "Modification of the robot state"
623 double speed_factor = m_positioningVelocity / 100.;
625 std::array<double, 7> q_goal;
626 for (
size_t i = 0; i < 7; i++) {
627 q_goal[i] = position[i];
630 vpJointPosTrajGenerator joint_pos_traj_generator(speed_factor, q_goal);
633 for (
int attempt = 1; attempt <= nbAttempts; attempt++) {
635 m_handler->control(joint_pos_traj_generator);
637 }
catch (
const franka::ControlException &e) {
638 std::cerr <<
"Warning: communication error: " << e.what() <<
"\nRetry attempt: " << attempt << std::endl;
639 m_handler->automaticErrorRecovery();
640 if (attempt == nbAttempts)
646 "Cannot move the robot to a cartesian position. Only joint positioning is implemented"));
673 m_velControlThreadStopAsked =
true;
674 if (m_velControlThread.joinable()) {
675 m_velControlThread.join();
676 m_velControlThreadStopAsked =
false;
677 m_velControlThreadIsRunning =
false;
681 m_ftControlThreadStopAsked =
true;
682 if (m_ftControlThread.joinable()) {
683 m_ftControlThread.join();
684 m_ftControlThreadStopAsked =
false;
685 m_ftControlThreadIsRunning =
false;
692 std::cout <<
"Change the control mode from velocity to position control.\n";
694 m_velControlThreadStopAsked =
true;
695 if (m_velControlThread.joinable()) {
696 m_velControlThread.join();
697 m_velControlThreadStopAsked =
false;
698 m_velControlThreadIsRunning =
false;
701 std::cout <<
"Change the control mode from force/torque to position control.\n";
703 m_ftControlThreadStopAsked =
true;
704 if (m_ftControlThread.joinable()) {
705 m_ftControlThread.join();
706 m_ftControlThreadStopAsked =
false;
707 m_ftControlThreadIsRunning =
false;
714 std::cout <<
"Change the control mode from stop to velocity control.\n";
716 std::cout <<
"Change the control mode from position to velocity control.\n";
718 std::cout <<
"Change the control mode from force/torque to velocity control.\n";
720 m_ftControlThreadStopAsked =
true;
721 if (m_ftControlThread.joinable()) {
722 m_ftControlThread.join();
723 m_ftControlThreadStopAsked =
false;
724 m_ftControlThreadIsRunning =
false;
731 std::cout <<
"Change the control mode from stop to force/torque control.\n";
733 std::cout <<
"Change the control mode from position to force/torque control.\n";
735 std::cout <<
"Change the control mode from velocity to force/torque control.\n";
737 m_velControlThreadStopAsked =
true;
738 if (m_velControlThread.joinable()) {
739 m_velControlThread.join();
740 m_velControlThreadStopAsked =
false;
741 m_velControlThreadIsRunning =
false;
811 "Cannot send a velocity to the robot. "
812 "Use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first.");
818 if (vel.
size() != 7) {
827 for (
size_t i = 0; i < m_dq_des.size(); i++) {
828 m_dq_des[i] = vel_sat[i];
838 if (vel.
size() != 6) {
844 for (
unsigned int i = 0; i < 3; i++)
846 for (
unsigned int i = 3; i < 6; i++)
859 if (!m_velControlThreadIsRunning) {
860 m_velControlThreadIsRunning =
true;
862 std::thread(&vpJointVelTrajGenerator::control_thread, vpJointVelTrajGenerator(), std::ref(m_handler),
863 std::ref(m_velControlThreadStopAsked), m_log_folder, frame, m_eMc, std::ref(m_v_cart_des),
864 std::ref(m_dq_des), std::cref(m_q_min), std::cref(m_q_max), std::cref(m_dq_max),
865 std::cref(m_ddq_max), std::ref(m_robot_state), std::ref(m_mutex));
882 const double &filter_gain,
const bool &activate_pi_controller)
887 if (ft.
size() != 7) {
892 for (
size_t i = 0; i < m_tau_J_des.size(); i++) {
893 m_tau_J_des[i] = ft[i];
904 if (ft.
size() != 6) {
920 if (!m_ftControlThreadIsRunning) {
922 m_ftControlThreadIsRunning =
true;
923 m_ftControlThread = std::thread(&vpForceTorqueGenerator::control_thread, vpForceTorqueGenerator(),
924 std::ref(m_handler), std::ref(m_ftControlThreadStopAsked), m_log_folder, frame,
925 std::ref(m_tau_J_des), std::ref(m_ft_cart_des), std::ref(m_robot_state),
926 std::ref(m_mutex), filter_gain, activate_pi_controller);
935 franka::RobotState robot_state;
937 if (!m_velControlThreadIsRunning && !m_ftControlThreadIsRunning) {
938 robot_state = m_handler->readOnce();
940 std::lock_guard<std::mutex> lock(m_mutex);
941 m_robot_state = robot_state;
943 std::lock_guard<std::mutex> lock(m_mutex);
944 robot_state = m_robot_state;
958 for (
size_t i = 0; i < m_q_min.size(); i++)
959 q_min[i] = m_q_min[i];
971 for (
size_t i = 0; i < m_q_max.size(); i++)
972 q_max[i] = m_q_max[i];
1068 std::ifstream fd(filename.c_str(), std::ios::in);
1070 if (!fd.is_open()) {
1075 std::string key(
"R:");
1076 std::string id(
"#PANDA - Joint position file");
1077 bool pos_found =
false;
1083 while (std::getline(fd, line)) {
1086 if (!(line.compare(0,
id.size(),
id) == 0)) {
1087 std::cout <<
"Error: this position file " << filename <<
" is not for Afma6 robot" << std::endl;
1091 if ((line.compare(0, 1,
"#") == 0)) {
1094 if ((line.compare(0, key.size(), key) == 0)) {
1097 if (chain.size() < njoints + 1)
1099 if (chain.size() < njoints + 1)
1102 std::istringstream ss(line);
1105 for (
unsigned int i = 0; i < njoints; i++)
1113 for (
unsigned int i = 0; i < njoints; i++) {
1120 std::cout <<
"Error: unable to find a position for Panda robot in " << filename << std::endl;
1153 fd = fopen(filename.c_str(),
"w");
1157 fprintf(fd,
"#PANDA - Joint position file\n"
1159 "# R: q1 q2 q3 q4 q5 q6 q7\n"
1160 "# with joint positions q1 to q7 expressed in degrees\n"
1195 if (m_franka_address.empty()) {
1198 if (m_gripper ==
nullptr)
1199 m_gripper =
new franka::Gripper(m_franka_address);
1201 m_gripper->homing();
1215 if (m_franka_address.empty()) {
1218 if (m_gripper ==
nullptr)
1219 m_gripper =
new franka::Gripper(m_franka_address);
1222 franka::GripperState gripper_state = m_gripper->readOnce();
1224 if (gripper_state.max_width < width) {
1225 std::cout <<
"Finger width request is too large for the current fingers on the gripper."
1226 <<
"Maximum possible width is " << gripper_state.max_width << std::endl;
1227 return EXIT_FAILURE;
1230 m_gripper->move(width, 0.1);
1231 return EXIT_SUCCESS;
1254 if (m_franka_address.empty()) {
1257 if (m_gripper ==
nullptr)
1258 m_gripper =
new franka::Gripper(m_franka_address);
1261 franka::GripperState gripper_state = m_gripper->readOnce();
1263 m_gripper->move(gripper_state.max_width, 0.1);
1264 return EXIT_SUCCESS;
1275 if (m_franka_address.empty()) {
1278 if (m_gripper ==
nullptr)
1279 m_gripper =
new franka::Gripper(m_franka_address);
1300 if (m_gripper ==
nullptr)
1301 m_gripper =
new franka::Gripper(m_franka_address);
1304 franka::GripperState gripper_state = m_gripper->readOnce();
1305 std::cout <<
"Gripper max witdh: " << gripper_state.max_width << std::endl;
1306 if (gripper_state.max_width < grasping_width) {
1307 std::cout <<
"Object is too large for the current fingers on the gripper."
1308 <<
"Maximum possible width is " << gripper_state.max_width << std::endl;
1309 return EXIT_FAILURE;
1313 if (!m_gripper->grasp(grasping_width, 0.1, force)) {
1314 std::cout <<
"Failed to grasp object." << std::endl;
1315 return EXIT_FAILURE;
1318 return EXIT_SUCCESS;
1321 #elif !defined(VISP_BUILD_SHARED_LIBS)
1324 void dummy_vpRobotFranka(){};
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
unsigned int size() const
Return the number of elements of the 2D array.
Implementation of column vector and the associated operations.
void resize(unsigned int i, bool flagNullify=true)
error that can be emitted by ViSP classes.
@ functionNotImplementedError
Function not implemented.
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
static double rad(double deg)
static double deg(double rad)
Implementation of a matrix and operations on matrices.
Implementation of a pose vector and operations on poses.
vpPoseVector buildFrom(double tx, double ty, double tz, double tux, double tuy, double tuz)
Error that can be emitted by the vpRobot class and its derivatives.
@ wrongStateError
Wrong robot state.
franka::RobotState getRobotInternalState()
void move(const std::string &filename, double velocity_percentage=10.)
bool savePosFile(const std::string &filename, const vpColVector &q)
void getForceTorque(const vpRobot::vpControlFrameType frame, vpColVector &force)
vpHomogeneousMatrix get_eMc() const
int gripperGrasp(double grasping_width, double force=60.)
vpColVector getJointMin() const
void setLogFolder(const std::string &folder)
void getGravity(vpColVector &gravity)
int gripperMove(double width)
void getMass(vpMatrix &mass)
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &position) override
void setForceTorque(const vpRobot::vpControlFrameType frame, const vpColVector &ft, const double &filter_gain=0.1, const bool &activate_pi_controller=false)
vpColVector getJointMax() const
void getCoriolis(vpColVector &coriolis)
void setPositioningVelocity(double velocity)
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position) override
void get_fJe(vpMatrix &fJe) override
void set_eMc(const vpHomogeneousMatrix &eMc)
bool readPosFile(const std::string &filename, vpColVector &q)
vpRobot::vpRobotStateType setRobotState(vpRobot::vpRobotStateType newState)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel) override
void get_eJe(vpMatrix &eJe) override
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &d_position)
void connect(const std::string &franka_address, franka::RealtimeConfig realtime_config=franka::RealtimeConfig::kEnforce)
vpHomogeneousMatrix get_fMe(const vpColVector &q)
vpHomogeneousMatrix get_fMc(const vpColVector &q)
Class that defines a generic virtual robot.
int nDof
number of degrees of freedom
virtual vpRobotStateType getRobotState(void) const
static vpColVector saturateVelocities(const vpColVector &v_in, const vpColVector &v_max, bool verbose=false)
@ STATE_POSITION_CONTROL
Initialize the position controller.
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
@ STATE_STOP
Stops robot motion especially in velocity and acceleration control.
@ STATE_FORCE_TORQUE_CONTROL
Initialize the force/torque controller.
double getMaxRotationVelocity(void) const
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
double getMaxTranslationVelocity(void) const