36 #include <visp3/core/vpConfig.h>
38 #if defined(VISP_HAVE_FRANKA) && defined(VISP_HAVE_THREADS)
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;
597 error =
"Unable to create Franka log folder: " + folder;
602 m_log_folder = folder;
618 std::cout <<
"Robot was not in position-based control. "
619 "Modification of the robot state"
625 double speed_factor = m_positioningVelocity / 100.;
627 std::array<double, 7> q_goal;
628 for (
size_t i = 0; i < 7; i++) {
629 q_goal[i] = position[i];
632 vpJointPosTrajGenerator joint_pos_traj_generator(speed_factor, q_goal);
635 for (
int attempt = 1; attempt <= nbAttempts; attempt++) {
637 m_handler->control(joint_pos_traj_generator);
640 catch (
const franka::ControlException &e) {
641 std::cerr <<
"Warning: communication error: " << e.what() <<
"\nRetry attempt: " << attempt << std::endl;
642 m_handler->automaticErrorRecovery();
643 if (attempt == nbAttempts)
650 "Cannot move the robot to a cartesian position. Only joint positioning is implemented"));
677 m_velControlThreadStopAsked =
true;
678 if (m_velControlThread.joinable()) {
679 m_velControlThread.join();
680 m_velControlThreadStopAsked =
false;
681 m_velControlThreadIsRunning =
false;
686 m_ftControlThreadStopAsked =
true;
687 if (m_ftControlThread.joinable()) {
688 m_ftControlThread.join();
689 m_ftControlThreadStopAsked =
false;
690 m_ftControlThreadIsRunning =
false;
697 std::cout <<
"Change the control mode from velocity to position control.\n";
699 m_velControlThreadStopAsked =
true;
700 if (m_velControlThread.joinable()) {
701 m_velControlThread.join();
702 m_velControlThreadStopAsked =
false;
703 m_velControlThreadIsRunning =
false;
707 std::cout <<
"Change the control mode from force/torque to position control.\n";
709 m_ftControlThreadStopAsked =
true;
710 if (m_ftControlThread.joinable()) {
711 m_ftControlThread.join();
712 m_ftControlThreadStopAsked =
false;
713 m_ftControlThreadIsRunning =
false;
720 std::cout <<
"Change the control mode from stop to velocity control.\n";
723 std::cout <<
"Change the control mode from position to velocity control.\n";
726 std::cout <<
"Change the control mode from force/torque to velocity control.\n";
728 m_ftControlThreadStopAsked =
true;
729 if (m_ftControlThread.joinable()) {
730 m_ftControlThread.join();
731 m_ftControlThreadStopAsked =
false;
732 m_ftControlThreadIsRunning =
false;
739 std::cout <<
"Change the control mode from stop to force/torque control.\n";
742 std::cout <<
"Change the control mode from position to force/torque control.\n";
745 std::cout <<
"Change the control mode from velocity to force/torque control.\n";
747 m_velControlThreadStopAsked =
true;
748 if (m_velControlThread.joinable()) {
749 m_velControlThread.join();
750 m_velControlThreadStopAsked =
false;
751 m_velControlThreadIsRunning =
false;
821 "Cannot send a velocity to the robot. "
822 "Use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first.");
828 if (vel.
size() != 7) {
837 for (
size_t i = 0; i < m_dq_des.size(); i++) {
838 m_dq_des[i] = vel_sat[i];
848 if (vel.
size() != 6) {
854 for (
unsigned int i = 0; i < 3; i++)
856 for (
unsigned int i = 3; i < 6; i++)
869 if (!m_velControlThreadIsRunning) {
870 m_velControlThreadIsRunning =
true;
872 std::thread(&vpJointVelTrajGenerator::control_thread, vpJointVelTrajGenerator(), std::ref(m_handler),
873 std::ref(m_velControlThreadStopAsked), m_log_folder, frame, m_eMc, std::ref(m_v_cart_des),
874 std::ref(m_dq_des), std::cref(m_q_min), std::cref(m_q_max), std::cref(m_dq_max),
875 std::cref(m_ddq_max), std::ref(m_robot_state), std::ref(m_mutex));
892 const double &filter_gain,
const bool &activate_pi_controller)
897 if (ft.
size() != 7) {
902 for (
size_t i = 0; i < m_tau_J_des.size(); i++) {
903 m_tau_J_des[i] = ft[i];
914 if (ft.
size() != 6) {
930 if (!m_ftControlThreadIsRunning) {
932 m_ftControlThreadIsRunning =
true;
933 m_ftControlThread = std::thread(&vpForceTorqueGenerator::control_thread, vpForceTorqueGenerator(),
934 std::ref(m_handler), std::ref(m_ftControlThreadStopAsked), m_log_folder, frame,
935 std::ref(m_tau_J_des), std::ref(m_ft_cart_des), std::ref(m_robot_state),
936 std::ref(m_mutex), filter_gain, activate_pi_controller);
945 franka::RobotState robot_state;
947 if (!m_velControlThreadIsRunning && !m_ftControlThreadIsRunning) {
948 robot_state = m_handler->readOnce();
950 std::lock_guard<std::mutex> lock(m_mutex);
951 m_robot_state = robot_state;
954 std::lock_guard<std::mutex> lock(m_mutex);
955 robot_state = m_robot_state;
969 for (
size_t i = 0; i < m_q_min.size(); i++)
970 q_min[i] = m_q_min[i];
982 for (
size_t i = 0; i < m_q_max.size(); i++)
983 q_max[i] = m_q_max[i];
1079 std::ifstream fd(filename.c_str(), std::ios::in);
1081 if (!fd.is_open()) {
1086 std::string key(
"R:");
1087 std::string id(
"#PANDA - Joint position file");
1088 bool pos_found =
false;
1094 while (std::getline(fd, line)) {
1097 if (!(line.compare(0,
id.size(),
id) == 0)) {
1098 std::cout <<
"Error: this position file " << filename <<
" is not for Afma6 robot" << std::endl;
1102 if ((line.compare(0, 1,
"#") == 0)) {
1105 if ((line.compare(0, key.size(), key) == 0)) {
1108 if (chain.size() < njoints + 1)
1110 if (chain.size() < njoints + 1)
1113 std::istringstream ss(line);
1116 for (
unsigned int i = 0; i < njoints; i++)
1124 for (
unsigned int i = 0; i < njoints; i++) {
1131 std::cout <<
"Error: unable to find a position for Panda robot in " << filename << std::endl;
1164 fd = fopen(filename.c_str(),
"w");
1168 fprintf(fd,
"#PANDA - Joint position file\n"
1170 "# R: q1 q2 q3 q4 q5 q6 q7\n"
1171 "# with joint positions q1 to q7 expressed in degrees\n"
1206 if (m_franka_address.empty()) {
1209 if (m_gripper ==
nullptr)
1210 m_gripper =
new franka::Gripper(m_franka_address);
1212 m_gripper->homing();
1226 if (m_franka_address.empty()) {
1229 if (m_gripper ==
nullptr)
1230 m_gripper =
new franka::Gripper(m_franka_address);
1233 franka::GripperState gripper_state = m_gripper->readOnce();
1235 if (gripper_state.max_width < width) {
1236 std::cout <<
"Finger width request is too large for the current fingers on the gripper."
1237 <<
"Maximum possible width is " << gripper_state.max_width << std::endl;
1238 return EXIT_FAILURE;
1241 m_gripper->move(width, 0.1);
1242 return EXIT_SUCCESS;
1265 if (m_franka_address.empty()) {
1268 if (m_gripper ==
nullptr)
1269 m_gripper =
new franka::Gripper(m_franka_address);
1272 franka::GripperState gripper_state = m_gripper->readOnce();
1274 m_gripper->move(gripper_state.max_width, 0.1);
1275 return EXIT_SUCCESS;
1286 if (m_franka_address.empty()) {
1289 if (m_gripper ==
nullptr)
1290 m_gripper =
new franka::Gripper(m_franka_address);
1331 if (m_gripper ==
nullptr)
1332 m_gripper =
new franka::Gripper(m_franka_address);
1335 franka::GripperState gripper_state = m_gripper->readOnce();
1336 std::cout <<
"Gripper max witdh: " << gripper_state.max_width << std::endl;
1337 if (gripper_state.max_width < grasping_width) {
1338 std::cout <<
"Object is too large for the current fingers on the gripper."
1339 <<
"Maximum possible width is " << gripper_state.max_width << std::endl;
1340 return EXIT_FAILURE;
1344 if (!m_gripper->grasp(grasping_width, speed, force)) {
1345 std::cout <<
"Failed to grasp object." << std::endl;
1346 return EXIT_FAILURE;
1349 return EXIT_SUCCESS;
1352 #elif !defined(VISP_BUILD_SHARED_LIBS)
1355 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.)
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position) vp_override
vpColVector getJointMin() const
void setLogFolder(const std::string &folder)
void getGravity(vpColVector &gravity)
int gripperMove(double width)
void getMass(vpMatrix &mass)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel) vp_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 setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &position) vp_override
void setPositioningVelocity(double velocity)
void get_eJe(vpMatrix &eJe) vp_override
void set_eMc(const vpHomogeneousMatrix &eMc)
bool readPosFile(const std::string &filename, vpColVector &q)
vpRobot::vpRobotStateType setRobotState(vpRobot::vpRobotStateType newState)
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)
void get_fJe(vpMatrix &fJe) vp_override
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