39#include <visp3/core/vpConfig.h>
41#ifdef VISP_HAVE_FRANKA
43#include <visp3/robot/vpRobotException.h>
44#include <visp3/robot/vpRobotFranka.h>
45#include <visp3/core/vpIoTools.h>
47#include "vpJointPosTrajGenerator_impl.h"
48#include "vpJointVelTrajGenerator_impl.h"
49#include "vpForceTorqueGenerator_impl.h"
57 :
vpRobot(), m_handler(NULL), m_gripper(NULL), m_model(NULL), m_positionningVelocity(20.),
58 m_velControlThread(), m_velControlThreadIsRunning(false), m_velControlThreadStopAsked(false),
59 m_dq_des(), m_v_cart_des(),
60 m_ftControlThread(), m_ftControlThreadIsRunning(false), m_ftControlThreadStopAsked(false),
61 m_tau_J_des(), m_ft_cart_des(),
62 m_q_min(), m_q_max(), m_dq_max(), m_ddq_max(), m_robot_state(),
63 m_mutex(), m_eMc(), m_log_folder(), m_franka_address()
75 :
vpRobot(), m_handler(NULL), m_gripper(NULL), m_model(NULL), m_positionningVelocity(20.),
76 m_velControlThread(), m_velControlThreadIsRunning(false), m_velControlThreadStopAsked(false),
77 m_dq_des(), m_v_cart_des(),
78 m_ftControlThread(), m_ftControlThreadIsRunning(false), m_ftControlThreadStopAsked(false),
79 m_tau_J_des(), m_ft_cart_des(),
80 m_q_min(), m_q_max(), m_dq_max(), m_ddq_max(), m_robot_state(),
81 m_mutex(), m_eMc(),m_log_folder(), m_franka_address()
84 connect(franka_address, realtime_config);
90void vpRobotFranka::init()
94 m_q_min = std::array<double, 7> {-2.8973, -1.7628, -2.8973, -3.0718, -2.8973, -0.0175, -2.8973};
95 m_q_max = std::array<double, 7> {2.8973, 1.7628, 2.8973, -0.0698, 2.8973, 3.7525, 2.8973};
96 m_dq_max = std::array<double, 7> {2.1750, 2.1750, 2.1750, 2.1750, 2.6100, 2.6100, 2.6100};
97 m_ddq_max = std::array<double, 7> {15.0, 7.5, 10.0, 12.5, 15.0, 20.0, 20.0};
113 std::cout <<
"Grasped object, will release it now." << std::endl;
132 if(franka_address.empty()) {
138 m_franka_address = franka_address;
139 m_handler =
new franka::Robot(franka_address, realtime_config);
141 std::array<double, 7> lower_torque_thresholds_nominal{
142 {25.0, 25.0, 22.0, 20.0, 19.0, 17.0, 14.}};
143 std::array<double, 7> upper_torque_thresholds_nominal{
144 {35.0, 35.0, 32.0, 30.0, 29.0, 27.0, 24.0}};
145 std::array<double, 7> lower_torque_thresholds_acceleration{
146 {25.0, 25.0, 22.0, 20.0, 19.0, 17.0, 14.0}};
147 std::array<double, 7> upper_torque_thresholds_acceleration{
148 {35.0, 35.0, 32.0, 30.0, 29.0, 27.0, 24.0}};
149 std::array<double, 6> lower_force_thresholds_nominal{{30.0, 30.0, 30.0, 25.0, 25.0, 25.0}};
150 std::array<double, 6> upper_force_thresholds_nominal{{40.0, 40.0, 40.0, 35.0, 35.0, 35.0}};
151 std::array<double, 6> lower_force_thresholds_acceleration{{30.0, 30.0, 30.0, 25.0, 25.0, 25.0}};
152 std::array<double, 6> upper_force_thresholds_acceleration{{40.0, 40.0, 40.0, 35.0, 35.0, 35.0}};
153 m_handler->setCollisionBehavior(
154 lower_torque_thresholds_acceleration, upper_torque_thresholds_acceleration,
155 lower_torque_thresholds_nominal, upper_torque_thresholds_nominal,
156 lower_force_thresholds_acceleration, upper_force_thresholds_acceleration,
157 lower_force_thresholds_nominal, upper_force_thresholds_nominal);
159 m_handler->setJointImpedance({{3000, 3000, 3000, 2500, 2500, 2000, 2000}});
160 m_handler->setCartesianImpedance({{3000, 3000, 3000, 300, 300, 300}});
161#if (VISP_HAVE_FRANKA_VERSION < 0x000500)
163 m_handler->setFilters(10, 10, 10, 10, 10);
170 m_model =
new franka::Model(m_handler->loadModel());
195 for (
int i=0; i < 7; i++) {
196 q[i] = robot_state.q[i];
208 for (
size_t i=0; i < 6; i++) {
209 position[i] = fPe[i];
217 for (
size_t i=0; i < 6; i++) {
218 position[i] = fPc[i];
252 for (
int i=0; i < 7; i++)
253 force[i] = robot_state.tau_J[i];
259 for (
int i=0; i < 6; i++)
260 force[i] = robot_state.K_F_ext_hat_K[i];
266 for (
int i=0; i < 6; i++)
267 eFe[i] = robot_state.K_F_ext_hat_K[i];
307 for (
int i=0; i < 7; i++) {
308 d_position[i]=robot_state.dq[i];
330 std::array<double, 7> coriolis_;
334 coriolis_ = m_model->coriolis(robot_state);
337 for (
int i=0; i < 7; i++) {
338 coriolis[i] = coriolis_[i];
352 std::array<double, 7> gravity_;
356 gravity_ = m_model->gravity(robot_state);
359 for (
int i=0; i < 7; i++) {
360 gravity[i] = gravity_[i];
374 std::array<double, 49> mass_;
378 mass_ = m_model->mass(robot_state);
381 for (
size_t i = 0; i < 7; i ++) {
382 for (
size_t j = 0; j < 7; j ++) {
383 mass[i][j] = mass_[j*7 + i];
404 std::array< double, 7 > q_array;
405 for (
size_t i = 0; i < 7; i++)
410 std::array<double, 16> pose_array = m_model->pose(franka::Frame::kEndEffector, q_array, robot_state.F_T_EE, robot_state.EE_T_K);
412 for (
unsigned int i=0; i< 4; i++) {
413 for (
unsigned int j=0; j< 4; j++) {
414 fMe[i][j] = pose_array[j*4 + i];
438 return (fMe * m_eMc);
462 std::array<double, 16> pose_array = robot_state.O_T_EE;
464 for (
unsigned int i=0; i< 4; i++) {
465 for (
unsigned int j=0; j< 4; j++) {
466 fMe[i][j] = pose_array[j*4 + i];
498 std::array<double, 42> jacobian = m_model->bodyJacobian(franka::Frame::kEndEffector, robot_state);
500 for (
size_t i = 0; i < 6; i ++) {
501 for (
size_t j = 0; j < 7; j ++) {
502 eJe_[i][j] = jacobian[j*6 + i];
522 std::array< double, 7 > q_array;
523 for (
size_t i = 0; i < 7; i++)
526 std::array<double, 42> jacobian = m_model->bodyJacobian(franka::Frame::kEndEffector, q_array, robot_state.F_T_EE, robot_state.EE_T_K);
528 for (
size_t i = 0; i < 6; i ++) {
529 for (
size_t j = 0; j < 7; j ++) {
530 eJe_[i][j] = jacobian[j*6 + i];
550 std::array<double, 42> jacobian = m_model->zeroJacobian(franka::Frame::kEndEffector, robot_state);
552 for (
size_t i = 0; i < 6; i ++) {
553 for (
size_t j = 0; j < 7; j ++) {
554 fJe_[i][j] = jacobian[j*6 + i];
577 std::array< double, 7 > q_array;
578 for (
size_t i = 0; i < 7; i++)
581 std::array<double, 42> jacobian = m_model->zeroJacobian(franka::Frame::kEndEffector, q_array, robot_state.F_T_EE, robot_state.EE_T_K);
583 for (
size_t i = 0; i < 6; i ++) {
584 for (
size_t j = 0; j < 7; j ++) {
585 fJe_[i][j] = jacobian[j*6 + i];
602 if (!folder.empty()) {
606 m_log_folder = folder;
610 error =
"Unable to create Franka log folder: " + folder;
615 m_log_folder = folder;
631 std::cout <<
"Robot was not in position-based control. "
632 "Modification of the robot state" << std::endl;
637 double speed_factor = m_positionningVelocity / 100.;
639 std::array<double, 7> q_goal;
640 for (
size_t i = 0; i < 7; i++) {
641 q_goal[i] = position[i];
644 vpJointPosTrajGenerator joint_pos_traj_generator(speed_factor, q_goal);
647 for (
int attempt = 1; attempt <= nbAttempts; attempt++) {
649 m_handler->control(joint_pos_traj_generator);
651 }
catch (
const franka::ControlException &e) {
652 std::cerr <<
"Warning: communication error: " << e.what() <<
"\nRetry attempt: " << attempt << std::endl;
653 m_handler->automaticErrorRecovery();
654 if (attempt == nbAttempts)
661 "Cannot move the robot to a cartesian position. Only joint positionning is implemented"));
675 m_positionningVelocity = velocity;
691 m_velControlThreadStopAsked =
true;
692 if(m_velControlThread.joinable()) {
693 m_velControlThread.join();
694 m_velControlThreadStopAsked =
false;
695 m_velControlThreadIsRunning =
false;
700 m_ftControlThreadStopAsked =
true;
701 if(m_ftControlThread.joinable()) {
702 m_ftControlThread.join();
703 m_ftControlThreadStopAsked =
false;
704 m_ftControlThreadIsRunning =
false;
711 std::cout <<
"Change the control mode from velocity to position control.\n";
713 m_velControlThreadStopAsked =
true;
714 if(m_velControlThread.joinable()) {
715 m_velControlThread.join();
716 m_velControlThreadStopAsked =
false;
717 m_velControlThreadIsRunning =
false;
721 std::cout <<
"Change the control mode from force/torque to position 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 velocity control.\n";
737 std::cout <<
"Change the control mode from position to velocity control.\n";
740 std::cout <<
"Change the control mode from force/torque to velocity control.\n";
742 m_ftControlThreadStopAsked =
true;
743 if(m_ftControlThread.joinable()) {
744 m_ftControlThread.join();
745 m_ftControlThreadStopAsked =
false;
746 m_ftControlThreadIsRunning =
false;
753 std::cout <<
"Change the control mode from stop to force/torque control.\n";
756 std::cout <<
"Change the control mode from position to force/torque control.\n";
759 std::cout <<
"Change the control mode from velocity to force/torque control.\n";
761 m_velControlThreadStopAsked =
true;
762 if(m_velControlThread.joinable()) {
763 m_velControlThread.join();
764 m_velControlThreadStopAsked =
false;
765 m_velControlThreadIsRunning =
false;
835 "Cannot send a velocity to the robot. "
836 "Use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first.");
842 if (vel.
size() != 7) {
844 "Joint velocity vector (%d) is not of size 7", vel.
size());
851 for (
size_t i = 0; i < m_dq_des.size(); i++) {
852 m_dq_des[i] = vel_sat[i];
862 if (vel.
size() != 6) {
864 "Cartesian velocity vector (%d) is not of size 6", vel.
size());
868 for (
unsigned int i = 0; i < 3; i++)
870 for (
unsigned int i = 3; i < 6; i++)
880 "Velocity controller not supported");
884 if(! m_velControlThreadIsRunning) {
885 m_velControlThreadIsRunning =
true;
886 m_velControlThread = std::thread(&vpJointVelTrajGenerator::control_thread, vpJointVelTrajGenerator(),
887 std::ref(m_handler), std::ref(m_velControlThreadStopAsked), m_log_folder,
888 frame, m_eMc, std::ref(m_v_cart_des), std::ref(m_dq_des),
889 std::cref(m_q_min), std::cref(m_q_max), std::cref(m_dq_max), std::cref(m_ddq_max),
890 std::ref(m_robot_state), std::ref(m_mutex));
907 const double &filter_gain,
const bool &activate_pi_controller)
912 if (ft.
size() != 7) {
914 "Joint torques vector (%d) is not of size 7", ft.
size());
917 for (
size_t i = 0; i < m_tau_J_des.size(); i++) {
918 m_tau_J_des[i] = ft[i];
929 if (ft.
size() != 6) {
931 "Cartesian force/torque vector (%d) is not of size 6", ft.
size());
942 "Velocity controller not supported");
946 if(! m_ftControlThreadIsRunning) {
948 m_ftControlThreadIsRunning =
true;
949 m_ftControlThread = std::thread(&vpForceTorqueGenerator::control_thread, vpForceTorqueGenerator(),
950 std::ref(m_handler), std::ref(m_ftControlThreadStopAsked), m_log_folder,
951 frame, std::ref(m_tau_J_des), std::ref(m_ft_cart_des), std::ref(m_robot_state),
952 std::ref(m_mutex), filter_gain, activate_pi_controller);
961 franka::RobotState robot_state;
963 if (! m_velControlThreadIsRunning && ! m_ftControlThreadIsRunning) {
964 robot_state = m_handler->readOnce();
966 std::lock_guard<std::mutex> lock(m_mutex);
967 m_robot_state = robot_state;
970 std::lock_guard<std::mutex> lock(m_mutex);
971 robot_state = m_robot_state;
985 for (
size_t i = 0; i < m_q_min.size(); i ++)
986 q_min[i] = m_q_min[i];
998 for (
size_t i = 0; i < m_q_max.size(); i ++)
999 q_max[i] = m_q_max[i];
1101 std::ifstream fd(filename.c_str(), std::ios::in);
1103 if (!fd.is_open()) {
1108 std::string key(
"R:");
1109 std::string id(
"#PANDA - Joint position file");
1110 bool pos_found =
false;
1116 while (std::getline(fd, line)) {
1119 if (!(line.compare(0,
id.size(),
id) == 0)) {
1120 std::cout <<
"Error: this position file " << filename <<
" is not for Afma6 robot" << std::endl;
1124 if ((line.compare(0, 1,
"#") == 0)) {
1127 if ((line.compare(0, key.size(), key) == 0)) {
1130 if (chain.size() < njoints + 1)
1132 if (chain.size() < njoints + 1)
1135 std::istringstream ss(line);
1138 for (
unsigned int i = 0; i < njoints; i++)
1146 for (
unsigned int i = 0; i < njoints; i++) {
1153 std::cout <<
"Error: unable to find a position for Panda robot in " << filename << std::endl;
1186 fd = fopen(filename.c_str(),
"w");
1191 "#PANDA - Joint position file\n"
1193 "# R: q1 q2 q3 q4 q5 q6 q7\n"
1194 "# with joint positions q1 to q7 expressed in degrees\n"
1229 if (m_franka_address.empty()) {
1232 if (m_gripper == NULL)
1233 m_gripper =
new franka::Gripper(m_franka_address);
1235 m_gripper->homing();
1249 if (m_franka_address.empty()) {
1252 if (m_gripper == NULL)
1253 m_gripper =
new franka::Gripper(m_franka_address);
1256 franka::GripperState gripper_state = m_gripper->readOnce();
1258 if (gripper_state.max_width < width) {
1259 std::cout <<
"Finger width request is too large for the current fingers on the gripper."
1260 <<
"Maximum possible width is " << gripper_state.max_width << std::endl;
1261 return EXIT_FAILURE;
1264 m_gripper->move(width, 0.1);
1265 return EXIT_SUCCESS;
1291 if (m_franka_address.empty()) {
1294 if (m_gripper == NULL)
1295 m_gripper =
new franka::Gripper(m_franka_address);
1298 franka::GripperState gripper_state = m_gripper->readOnce();
1300 m_gripper->move(gripper_state.max_width, 0.1);
1301 return EXIT_SUCCESS;
1312 if (m_franka_address.empty()) {
1315 if (m_gripper == NULL)
1316 m_gripper =
new franka::Gripper(m_franka_address);
1337 if (m_gripper == NULL)
1338 m_gripper =
new franka::Gripper(m_franka_address);
1341 franka::GripperState gripper_state = m_gripper->readOnce();
1342 std::cout <<
"Gripper max witdh: " << gripper_state.max_width << std::endl;
1343 if (gripper_state.max_width < grasping_width) {
1344 std::cout <<
"Object is too large for the current fingers on the gripper."
1345 <<
"Maximum possible width is " << gripper_state.max_width << std::endl;
1346 return EXIT_FAILURE;
1350 if (!m_gripper->grasp(grasping_width, 0.1, force)) {
1351 std::cout <<
"Failed to grasp object." << std::endl;
1352 return EXIT_FAILURE;
1355 return EXIT_SUCCESS;
1358#elif !defined(VISP_BUILD_SHARED_LIBS)
1361void 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