39 #include <visp3/core/vpConfig.h>
41 #ifdef VISP_HAVE_FRANKA
43 #include <visp3/core/vpIoTools.h>
44 #include <visp3/robot/vpRobotException.h>
45 #include <visp3/robot/vpRobotFranka.h>
47 #include "vpForceTorqueGenerator_impl.h"
48 #include "vpJointPosTrajGenerator_impl.h"
49 #include "vpJointVelTrajGenerator_impl.h"
57 :
vpRobot(), m_handler(NULL), m_gripper(NULL), m_model(NULL), m_positioningVelocity(20.), m_velControlThread(),
58 m_velControlThreadIsRunning(false), m_velControlThreadStopAsked(false), m_dq_des(), m_v_cart_des(),
59 m_ftControlThread(), m_ftControlThreadIsRunning(false), m_ftControlThreadStopAsked(false), m_tau_J_des(),
60 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(),
73 :
vpRobot(), m_handler(NULL), m_gripper(NULL), m_model(NULL), m_positioningVelocity(20.), m_velControlThread(),
74 m_velControlThreadIsRunning(false), m_velControlThreadStopAsked(false), m_dq_des(), m_v_cart_des(),
75 m_ftControlThread(), m_ftControlThreadIsRunning(false), m_ftControlThreadStopAsked(false), m_tau_J_des(),
76 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(),
80 connect(franka_address, realtime_config);
86 void vpRobotFranka::init()
90 m_q_min = std::array<double, 7>{-2.8973, -1.7628, -2.8973, -3.0718, -2.8973, -0.0175, -2.8973};
91 m_q_max = std::array<double, 7>{2.8973, 1.7628, 2.8973, -0.0698, 2.8973, 3.7525, 2.8973};
92 m_dq_max = std::array<double, 7>{2.1750, 2.1750, 2.1750, 2.1750, 2.6100, 2.6100, 2.6100};
93 m_ddq_max = std::array<double, 7>{15.0, 7.5, 10.0, 12.5, 15.0, 20.0, 20.0};
109 std::cout <<
"Grasped object, will release it now." << std::endl;
128 if (franka_address.empty()) {
134 m_franka_address = franka_address;
135 m_handler =
new franka::Robot(franka_address, realtime_config);
137 std::array<double, 7> lower_torque_thresholds_nominal{{25.0, 25.0, 22.0, 20.0, 19.0, 17.0, 14.}};
138 std::array<double, 7> upper_torque_thresholds_nominal{{35.0, 35.0, 32.0, 30.0, 29.0, 27.0, 24.0}};
139 std::array<double, 7> lower_torque_thresholds_acceleration{{25.0, 25.0, 22.0, 20.0, 19.0, 17.0, 14.0}};
140 std::array<double, 7> upper_torque_thresholds_acceleration{{35.0, 35.0, 32.0, 30.0, 29.0, 27.0, 24.0}};
141 std::array<double, 6> lower_force_thresholds_nominal{{30.0, 30.0, 30.0, 25.0, 25.0, 25.0}};
142 std::array<double, 6> upper_force_thresholds_nominal{{40.0, 40.0, 40.0, 35.0, 35.0, 35.0}};
143 std::array<double, 6> lower_force_thresholds_acceleration{{30.0, 30.0, 30.0, 25.0, 25.0, 25.0}};
144 std::array<double, 6> upper_force_thresholds_acceleration{{40.0, 40.0, 40.0, 35.0, 35.0, 35.0}};
145 m_handler->setCollisionBehavior(lower_torque_thresholds_acceleration, upper_torque_thresholds_acceleration,
146 lower_torque_thresholds_nominal, upper_torque_thresholds_nominal,
147 lower_force_thresholds_acceleration, upper_force_thresholds_acceleration,
148 lower_force_thresholds_nominal, upper_force_thresholds_nominal);
150 m_handler->setJointImpedance({{3000, 3000, 3000, 2500, 2500, 2000, 2000}});
151 m_handler->setCartesianImpedance({{3000, 3000, 3000, 300, 300, 300}});
152 #if (VISP_HAVE_FRANKA_VERSION < 0x000500)
154 m_handler->setFilters(10, 10, 10, 10, 10);
161 m_model =
new franka::Model(m_handler->loadModel());
186 for (
int i = 0; i < 7; i++) {
187 q[i] = robot_state.q[i];
199 for (
size_t i = 0; i < 6; i++) {
200 position[i] = fPe[i];
208 for (
size_t i = 0; i < 6; i++) {
209 position[i] = fPc[i];
240 for (
int i = 0; i < 7; i++)
241 force[i] = robot_state.tau_J[i];
247 for (
int i = 0; i < 6; i++)
248 force[i] = robot_state.K_F_ext_hat_K[i];
254 for (
int i = 0; i < 6; i++)
255 eFe[i] = robot_state.K_F_ext_hat_K[i];
295 for (
int i = 0; i < 7; i++) {
296 d_position[i] = robot_state.dq[i];
318 std::array<double, 7> coriolis_;
322 coriolis_ = m_model->coriolis(robot_state);
325 for (
int i = 0; i < 7; i++) {
326 coriolis[i] = coriolis_[i];
340 std::array<double, 7> gravity_;
344 gravity_ = m_model->gravity(robot_state);
347 for (
int i = 0; i < 7; i++) {
348 gravity[i] = gravity_[i];
362 std::array<double, 49> mass_;
366 mass_ = m_model->mass(robot_state);
369 for (
size_t i = 0; i < 7; i++) {
370 for (
size_t j = 0; j < 7; j++) {
371 mass[i][j] = mass_[j * 7 + i];
392 std::array<double, 7> q_array;
393 for (
size_t i = 0; i < 7; i++)
398 std::array<double, 16> pose_array =
399 m_model->pose(franka::Frame::kEndEffector, q_array, robot_state.F_T_EE, robot_state.EE_T_K);
401 for (
unsigned int i = 0; i < 4; i++) {
402 for (
unsigned int j = 0; j < 4; j++) {
403 fMe[i][j] = pose_array[j * 4 + i];
427 return (fMe * m_eMc);
450 std::array<double, 16> pose_array = robot_state.O_T_EE;
452 for (
unsigned int i = 0; i < 4; i++) {
453 for (
unsigned int j = 0; j < 4; j++) {
454 fMe[i][j] = pose_array[j * 4 + i];
486 std::array<double, 42> jacobian = m_model->bodyJacobian(franka::Frame::kEndEffector, robot_state);
488 for (
size_t i = 0; i < 6; i++) {
489 for (
size_t j = 0; j < 7; j++) {
490 eJe_[i][j] = jacobian[j * 6 + i];
509 std::array<double, 7> q_array;
510 for (
size_t i = 0; i < 7; i++)
513 std::array<double, 42> jacobian = m_model->bodyJacobian(franka::Frame::kEndEffector, q_array, robot_state.F_T_EE,
516 for (
size_t i = 0; i < 6; i++) {
517 for (
size_t j = 0; j < 7; j++) {
518 eJe_[i][j] = jacobian[j * 6 + i];
537 std::array<double, 42> jacobian = m_model->zeroJacobian(franka::Frame::kEndEffector, robot_state);
539 for (
size_t i = 0; i < 6; i++) {
540 for (
size_t j = 0; j < 7; j++) {
541 fJe_[i][j] = jacobian[j * 6 + i];
560 "Cannot get Franka robot fJe jacobian with an input joint position vector [%u] that is not a 7-dim vector",
566 std::array<double, 7> q_array;
567 for (
size_t i = 0; i < 7; i++)
570 std::array<double, 42> jacobian = m_model->zeroJacobian(franka::Frame::kEndEffector, q_array, robot_state.F_T_EE,
573 for (
size_t i = 0; i < 6; i++) {
574 for (
size_t j = 0; j < 7; j++) {
575 fJe_[i][j] = jacobian[j * 6 + i];
592 if (!folder.empty()) {
596 m_log_folder = folder;
599 error =
"Unable to create Franka log folder: " + folder;
603 m_log_folder = folder;
619 std::cout <<
"Robot was not in position-based control. "
620 "Modification of the robot state"
626 double speed_factor = m_positioningVelocity / 100.;
628 std::array<double, 7> q_goal;
629 for (
size_t i = 0; i < 7; i++) {
630 q_goal[i] = position[i];
633 vpJointPosTrajGenerator joint_pos_traj_generator(speed_factor, q_goal);
636 for (
int attempt = 1; attempt <= nbAttempts; attempt++) {
638 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)
649 "Cannot move the robot to a cartesian position. Only joint positioning is implemented"));
676 m_velControlThreadStopAsked =
true;
677 if (m_velControlThread.joinable()) {
678 m_velControlThread.join();
679 m_velControlThreadStopAsked =
false;
680 m_velControlThreadIsRunning =
false;
684 m_ftControlThreadStopAsked =
true;
685 if (m_ftControlThread.joinable()) {
686 m_ftControlThread.join();
687 m_ftControlThreadStopAsked =
false;
688 m_ftControlThreadIsRunning =
false;
695 std::cout <<
"Change the control mode from velocity to position control.\n";
697 m_velControlThreadStopAsked =
true;
698 if (m_velControlThread.joinable()) {
699 m_velControlThread.join();
700 m_velControlThreadStopAsked =
false;
701 m_velControlThreadIsRunning =
false;
704 std::cout <<
"Change the control mode from force/torque to position control.\n";
706 m_ftControlThreadStopAsked =
true;
707 if (m_ftControlThread.joinable()) {
708 m_ftControlThread.join();
709 m_ftControlThreadStopAsked =
false;
710 m_ftControlThreadIsRunning =
false;
717 std::cout <<
"Change the control mode from stop to velocity control.\n";
719 std::cout <<
"Change the control mode from position to velocity control.\n";
721 std::cout <<
"Change the control mode from force/torque to velocity control.\n";
723 m_ftControlThreadStopAsked =
true;
724 if (m_ftControlThread.joinable()) {
725 m_ftControlThread.join();
726 m_ftControlThreadStopAsked =
false;
727 m_ftControlThreadIsRunning =
false;
734 std::cout <<
"Change the control mode from stop to force/torque control.\n";
736 std::cout <<
"Change the control mode from position to force/torque control.\n";
738 std::cout <<
"Change the control mode from velocity to force/torque control.\n";
740 m_velControlThreadStopAsked =
true;
741 if (m_velControlThread.joinable()) {
742 m_velControlThread.join();
743 m_velControlThreadStopAsked =
false;
744 m_velControlThreadIsRunning =
false;
814 "Cannot send a velocity to the robot. "
815 "Use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first.");
821 if (vel.
size() != 7) {
830 for (
size_t i = 0; i < m_dq_des.size(); i++) {
831 m_dq_des[i] = vel_sat[i];
841 if (vel.
size() != 6) {
847 for (
unsigned int i = 0; i < 3; i++)
849 for (
unsigned int i = 3; i < 6; i++)
862 if (!m_velControlThreadIsRunning) {
863 m_velControlThreadIsRunning =
true;
865 std::thread(&vpJointVelTrajGenerator::control_thread, vpJointVelTrajGenerator(), std::ref(m_handler),
866 std::ref(m_velControlThreadStopAsked), m_log_folder, frame, m_eMc, std::ref(m_v_cart_des),
867 std::ref(m_dq_des), std::cref(m_q_min), std::cref(m_q_max), std::cref(m_dq_max),
868 std::cref(m_ddq_max), std::ref(m_robot_state), std::ref(m_mutex));
885 const double &filter_gain,
const bool &activate_pi_controller)
890 if (ft.
size() != 7) {
895 for (
size_t i = 0; i < m_tau_J_des.size(); i++) {
896 m_tau_J_des[i] = ft[i];
907 if (ft.
size() != 6) {
923 if (!m_ftControlThreadIsRunning) {
925 m_ftControlThreadIsRunning =
true;
926 m_ftControlThread = std::thread(&vpForceTorqueGenerator::control_thread, vpForceTorqueGenerator(),
927 std::ref(m_handler), std::ref(m_ftControlThreadStopAsked), m_log_folder, frame,
928 std::ref(m_tau_J_des), std::ref(m_ft_cart_des), std::ref(m_robot_state),
929 std::ref(m_mutex), filter_gain, activate_pi_controller);
938 franka::RobotState robot_state;
940 if (!m_velControlThreadIsRunning && !m_ftControlThreadIsRunning) {
941 robot_state = m_handler->readOnce();
943 std::lock_guard<std::mutex> lock(m_mutex);
944 m_robot_state = robot_state;
946 std::lock_guard<std::mutex> lock(m_mutex);
947 robot_state = m_robot_state;
961 for (
size_t i = 0; i < m_q_min.size(); i++)
962 q_min[i] = m_q_min[i];
974 for (
size_t i = 0; i < m_q_max.size(); i++)
975 q_max[i] = m_q_max[i];
1071 std::ifstream fd(filename.c_str(), std::ios::in);
1073 if (!fd.is_open()) {
1078 std::string key(
"R:");
1079 std::string id(
"#PANDA - Joint position file");
1080 bool pos_found =
false;
1086 while (std::getline(fd, line)) {
1089 if (!(line.compare(0,
id.size(),
id) == 0)) {
1090 std::cout <<
"Error: this position file " << filename <<
" is not for Afma6 robot" << std::endl;
1094 if ((line.compare(0, 1,
"#") == 0)) {
1097 if ((line.compare(0, key.size(), key) == 0)) {
1100 if (chain.size() < njoints + 1)
1102 if (chain.size() < njoints + 1)
1105 std::istringstream ss(line);
1108 for (
unsigned int i = 0; i < njoints; i++)
1116 for (
unsigned int i = 0; i < njoints; i++) {
1123 std::cout <<
"Error: unable to find a position for Panda robot in " << filename << std::endl;
1156 fd = fopen(filename.c_str(),
"w");
1160 fprintf(fd,
"#PANDA - Joint position file\n"
1162 "# R: q1 q2 q3 q4 q5 q6 q7\n"
1163 "# with joint positions q1 to q7 expressed in degrees\n"
1198 if (m_franka_address.empty()) {
1201 if (m_gripper == NULL)
1202 m_gripper =
new franka::Gripper(m_franka_address);
1204 m_gripper->homing();
1218 if (m_franka_address.empty()) {
1221 if (m_gripper == NULL)
1222 m_gripper =
new franka::Gripper(m_franka_address);
1225 franka::GripperState gripper_state = m_gripper->readOnce();
1227 if (gripper_state.max_width < width) {
1228 std::cout <<
"Finger width request is too large for the current fingers on the gripper."
1229 <<
"Maximum possible width is " << gripper_state.max_width << std::endl;
1230 return EXIT_FAILURE;
1233 m_gripper->move(width, 0.1);
1234 return EXIT_SUCCESS;
1257 if (m_franka_address.empty()) {
1260 if (m_gripper == NULL)
1261 m_gripper =
new franka::Gripper(m_franka_address);
1264 franka::GripperState gripper_state = m_gripper->readOnce();
1266 m_gripper->move(gripper_state.max_width, 0.1);
1267 return EXIT_SUCCESS;
1278 if (m_franka_address.empty()) {
1281 if (m_gripper == NULL)
1282 m_gripper =
new franka::Gripper(m_franka_address);
1303 if (m_gripper == NULL)
1304 m_gripper =
new franka::Gripper(m_franka_address);
1307 franka::GripperState gripper_state = m_gripper->readOnce();
1308 std::cout <<
"Gripper max witdh: " << gripper_state.max_width << std::endl;
1309 if (gripper_state.max_width < grasping_width) {
1310 std::cout <<
"Object is too large for the current fingers on the gripper."
1311 <<
"Maximum possible width is " << gripper_state.max_width << std::endl;
1312 return EXIT_FAILURE;
1316 if (!m_gripper->grasp(grasping_width, 0.1, force)) {
1317 std::cout <<
"Failed to grasp object." << std::endl;
1318 return EXIT_FAILURE;
1321 return EXIT_SUCCESS;
1324 #elif !defined(VISP_BUILD_SHARED_LIBS)
1327 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 emited 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 emited by the vpRobot class and its derivates.
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 setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
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 get_eJe(vpMatrix &eJe)
void getCoriolis(vpColVector &coriolis)
void setPositioningVelocity(double velocity)
void get_fJe(vpMatrix &fJe)
void set_eMc(const vpHomogeneousMatrix &eMc)
bool readPosFile(const std::string &filename, vpColVector &q)
vpRobot::vpRobotStateType setRobotState(vpRobot::vpRobotStateType newState)
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &position)
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position)
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