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" 56 :
vpRobot(), m_handler(NULL), m_gripper(NULL), m_model(NULL), m_positionningVelocity(20.), m_controlThread(), m_controlThreadIsRunning(false),
57 m_controlThreadStopAsked(false), m_q_min(), m_q_max(), m_dq_max(), m_ddq_max(), m_robot_state(),
58 m_mutex(), m_dq_des(), m_eMc(), m_log_folder(), m_franka_address()
70 :
vpRobot(), m_handler(NULL), m_gripper(NULL), m_model(NULL), m_positionningVelocity(20.), m_controlThread(), m_controlThreadIsRunning(false),
71 m_controlThreadStopAsked(false), m_q_min(), m_q_max(), m_dq_max(), m_ddq_max(), m_robot_state(),
72 m_mutex(), m_dq_des(), m_v_cart_des(), m_eMc(),m_log_folder(), m_franka_address()
75 connect(franka_address, realtime_config);
81 void vpRobotFranka::init()
85 m_q_min = std::array<double, 7> {-2.8973, -1.7628, -2.8973, -3.0718, -2.8973, -0.0175, -2.8973};
86 m_q_max = std::array<double, 7> {12.8973, 1.7628, 2.8973, 0.0175, 2.8973, 3.7525, 2.8973};
87 m_dq_max = std::array<double, 7> {2.1750, 2.1750, 2.1750, 2.1750, 2.6100, 2.6100, 2.6100};
88 m_ddq_max = std::array<double, 7> {14.25, 7.125, 11.875, 11.875, 14.25, 19.0, 19.0};
104 std::cout <<
"Grasped object, will release it now." << std::endl;
123 if(franka_address.empty()) {
129 m_franka_address = franka_address;
130 m_handler =
new franka::Robot(franka_address, realtime_config);
132 std::array<double, 7> lower_torque_thresholds_nominal{
133 {25.0, 25.0, 22.0, 20.0, 19.0, 17.0, 14.}};
134 std::array<double, 7> upper_torque_thresholds_nominal{
135 {35.0, 35.0, 32.0, 30.0, 29.0, 27.0, 24.0}};
136 std::array<double, 7> lower_torque_thresholds_acceleration{
137 {25.0, 25.0, 22.0, 20.0, 19.0, 17.0, 14.0}};
138 std::array<double, 7> upper_torque_thresholds_acceleration{
139 {35.0, 35.0, 32.0, 30.0, 29.0, 27.0, 24.0}};
140 std::array<double, 6> lower_force_thresholds_nominal{{30.0, 30.0, 30.0, 25.0, 25.0, 25.0}};
141 std::array<double, 6> upper_force_thresholds_nominal{{40.0, 40.0, 40.0, 35.0, 35.0, 35.0}};
142 std::array<double, 6> lower_force_thresholds_acceleration{{30.0, 30.0, 30.0, 25.0, 25.0, 25.0}};
143 std::array<double, 6> upper_force_thresholds_acceleration{{40.0, 40.0, 40.0, 35.0, 35.0, 35.0}};
144 m_handler->setCollisionBehavior(
145 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());
184 franka::RobotState robot_state = getRobotInternalState();
186 for (
int i=0; i < 7; i++)
187 q[i] = robot_state.q_d[i];
198 for (
size_t i=0; i < 6; i++) {
199 position[i] = fPe[i];
207 for (
size_t i=0; i < 6; i++) {
208 position[i] = fPc[i];
237 franka::RobotState robot_state = getRobotInternalState();
242 for (
int i=0; i < 7; i++)
243 force[i] = robot_state.tau_J[i];
249 for (
int i=0; i < 7; i++)
250 force[i] = robot_state.K_F_ext_hat_K[i];
256 for (
int i=0; i < 7; i++)
257 eFe[i] = robot_state.K_F_ext_hat_K[i];
291 franka::RobotState robot_state = getRobotInternalState();
297 for (
int i=0; i < 7; i++) {
298 d_position[i]=robot_state.dq[i];
320 std::array<double, 7> coriolis_;
322 franka::RobotState robot_state = getRobotInternalState();
324 coriolis_ = m_model->coriolis(robot_state);
327 for (
int i=0; i < 7; i++) {
328 coriolis[i] = coriolis_[i];
342 std::array<double, 7> gravity_;
344 franka::RobotState robot_state = getRobotInternalState();
346 gravity_ = m_model->gravity(robot_state);
349 for (
int i=0; i < 7; i++) {
350 gravity[i] = gravity_[i];
364 std::array<double, 49> mass_;
366 franka::RobotState robot_state = getRobotInternalState();
368 mass_ = m_model->mass(robot_state);
371 for (
size_t i = 0; i < 7; i ++) {
372 for (
size_t j = 0; j < 7; j ++) {
373 mass[i][j] = mass_[j*7 + i];
394 std::array< double, 7 > q_array;
395 for (
size_t i = 0; i < 7; i++)
398 franka::RobotState robot_state = getRobotInternalState();
400 std::array<double, 16> pose_array = m_model->pose(franka::Frame::kEndEffector, q_array, robot_state.F_T_EE, robot_state.EE_T_K);
402 for (
unsigned int i=0; i< 4; i++) {
403 for (
unsigned int j=0; j< 4; j++) {
404 fMe[i][j] = pose_array[j*4 + i];
428 return (fMe * m_eMc);
447 franka::RobotState robot_state = getRobotInternalState();
449 std::array<double, 16> pose_array = robot_state.O_T_EE;
451 for (
unsigned int i=0; i< 4; i++) {
452 for (
unsigned int j=0; j< 4; j++) {
453 fMe[i][j] = pose_array[j*4 + i];
483 franka::RobotState robot_state = getRobotInternalState();
485 std::array<double, 42> jacobian = m_model->bodyJacobian(franka::Frame::kEndEffector, robot_state);
487 for (
size_t i = 0; i < 6; i ++) {
488 for (
size_t j = 0; j < 7; j ++) {
489 eJe[i][j] = jacobian[j*6 + i];
508 franka::RobotState robot_state = getRobotInternalState();
510 std::array< double, 7 > q_array;
511 for (
size_t i = 0; i < 7; i++)
514 std::array<double, 42> jacobian = m_model->bodyJacobian(franka::Frame::kEndEffector, q_array, robot_state.F_T_EE, robot_state.EE_T_K);
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];
536 franka::RobotState robot_state = getRobotInternalState();
538 std::array<double, 42> jacobian = m_model->zeroJacobian(franka::Frame::kEndEffector, robot_state);
540 for (
size_t i = 0; i < 6; i ++) {
541 for (
size_t j = 0; j < 7; j ++) {
542 fJe[i][j] = jacobian[j*6 + i];
563 franka::RobotState robot_state = getRobotInternalState();
565 std::array< double, 7 > q_array;
566 for (
size_t i = 0; i < 7; i++)
569 std::array<double, 42> jacobian = m_model->zeroJacobian(franka::Frame::kEndEffector, q_array, robot_state.F_T_EE, robot_state.EE_T_K);
571 for (
size_t i = 0; i < 6; i ++) {
572 for (
size_t j = 0; j < 7; j ++) {
573 fJe[i][j] = jacobian[j*6 + i];
590 if (!folder.empty()) {
594 m_log_folder = folder;
598 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" << std::endl;
625 double speed_factor = m_positionningVelocity / 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);
639 }
catch (
const franka::ControlException &e) {
640 std::cerr <<
"Warning: communication error: " << e.what() <<
"\nRetry attempt: " << attempt << std::endl;
641 m_handler->automaticErrorRecovery();
642 if (attempt == nbAttempts)
649 "Cannot move the robot to a cartesian position. Only joint positionning is implemented"));
663 m_positionningVelocity = velocity;
679 m_controlThreadStopAsked =
true;
680 if(m_controlThread.joinable()) {
681 m_controlThread.join();
682 m_controlThreadStopAsked =
false;
683 m_controlThreadIsRunning =
false;
690 std::cout <<
"Change the control mode from velocity to position control.\n";
692 m_controlThreadStopAsked =
true;
693 if(m_controlThread.joinable()) {
694 m_controlThread.join();
695 m_controlThreadStopAsked =
false;
696 m_controlThreadIsRunning =
false;
703 std::cout <<
"Change the control mode from stop to velocity control.\n";
771 "Cannot send a velocity to the robot. " 772 "Use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first.");
778 if (vel.
size() != 7) {
780 "Joint velocity vector (%d) is not of size 7", vel.
size());
787 for (
size_t i = 0; i < m_dq_des.size(); i++) {
788 m_dq_des[i] = vel_sat[i];
798 if (vel.
size() != 6) {
800 "Cartesian velocity vector (%d) is not of size 6", vel.
size());
804 for (
unsigned int i = 0; i < 3; i++)
806 for (
unsigned int i = 3; i < 6; i++)
816 "Velocity controller not supported");
820 if(! m_controlThreadIsRunning) {
821 m_controlThreadIsRunning =
true;
822 m_controlThread = std::thread(&vpJointVelTrajGenerator::control_thread, vpJointVelTrajGenerator(),
823 std::ref(m_handler), std::ref(m_controlThreadStopAsked), m_log_folder,
824 frame, m_eMc, std::ref(m_v_cart_des), std::ref(m_dq_des),
825 std::cref(m_q_min), std::cref(m_q_max), std::cref(m_dq_max), std::cref(m_ddq_max),
826 std::ref(m_robot_state), std::ref(m_mutex));
830 franka::RobotState vpRobotFranka::getRobotInternalState()
835 franka::RobotState robot_state;
837 if (! m_controlThreadIsRunning) {
838 robot_state = m_handler->readOnce();
840 std::lock_guard<std::mutex> lock(m_mutex);
841 m_robot_state = robot_state;
844 std::lock_guard<std::mutex> lock(m_mutex);
845 robot_state = m_robot_state;
859 for (
size_t i = 0; i < m_q_min.size(); i ++)
860 q_min[i] = m_q_min[i];
872 for (
size_t i = 0; i < m_q_max.size(); i ++)
873 q_max[i] = m_q_max[i];
975 std::ifstream fd(filename.c_str(), std::ios::in);
982 std::string key(
"R:");
983 std::string id(
"#PANDA - Joint position file");
984 bool pos_found =
false;
990 while (std::getline(fd, line)) {
993 if (!(line.compare(0,
id.size(), id) == 0)) {
994 std::cout <<
"Error: this position file " << filename <<
" is not for Afma6 robot" << std::endl;
998 if ((line.compare(0, 1,
"#") == 0)) {
1001 if ((line.compare(0, key.size(), key) == 0)) {
1004 if (chain.size() < njoints + 1)
1006 if (chain.size() < njoints + 1)
1009 std::istringstream ss(line);
1012 for (
unsigned int i = 0; i < njoints; i++)
1020 for (
unsigned int i = 0; i < njoints; i++) {
1027 std::cout <<
"Error: unable to find a position for Panda robot in " << filename << std::endl;
1060 fd = fopen(filename.c_str(),
"w");
1065 "#PANDA - Joint position file\n" 1067 "# R: q1 q2 q3 q4 q5 q6 q7\n" 1068 "# with joint positions q1 to q7 expressed in degrees\n" 1103 if (m_franka_address.empty()) {
1106 if (m_gripper == NULL)
1107 m_gripper =
new franka::Gripper(m_franka_address);
1109 m_gripper->homing();
1123 if (m_franka_address.empty()) {
1126 if (m_gripper == NULL)
1127 m_gripper =
new franka::Gripper(m_franka_address);
1130 franka::GripperState gripper_state = m_gripper->readOnce();
1132 if (gripper_state.max_width < width) {
1133 std::cout <<
"Finger width request is too large for the current fingers on the gripper." 1134 <<
"Maximum possible width is " << gripper_state.max_width << std::endl;
1135 return EXIT_FAILURE;
1138 m_gripper->move(width, 0.1);
1139 return EXIT_SUCCESS;
1165 if (m_franka_address.empty()) {
1168 if (m_gripper == NULL)
1169 m_gripper =
new franka::Gripper(m_franka_address);
1172 franka::GripperState gripper_state = m_gripper->readOnce();
1174 m_gripper->move(gripper_state.max_width, 0.1);
1175 return EXIT_SUCCESS;
1186 if (m_franka_address.empty()) {
1189 if (m_gripper == NULL)
1190 m_gripper =
new franka::Gripper(m_franka_address);
1211 if (m_gripper == NULL)
1212 m_gripper =
new franka::Gripper(m_franka_address);
1215 franka::GripperState gripper_state = m_gripper->readOnce();
1216 std::cout <<
"Gripper max witdh: " << gripper_state.max_width << std::endl;
1217 if (gripper_state.max_width < grasping_width) {
1218 std::cout <<
"Object is too large for the current fingers on the gripper." 1219 <<
"Maximum possible width is " << gripper_state.max_width << std::endl;
1220 return EXIT_FAILURE;
1224 if (!m_gripper->grasp(grasping_width, 0.1, force)) {
1225 std::cout <<
"Failed to grasp object." << std::endl;
1226 return EXIT_FAILURE;
1229 return EXIT_SUCCESS;
1232 #elif !defined(VISP_BUILD_SHARED_LIBS) 1235 void dummy_vpRobotFranka(){};
int gripperMove(double width)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
Implementation of a matrix and operations on matrices.
vpColVector getJointMax() const
Error that can be emited by the vpRobot class and its derivates.
vpRobot::vpRobotStateType setRobotState(vpRobot::vpRobotStateType newState)
void getCoriolis(vpColVector &coriolis)
Implementation of an homogeneous matrix and operations on such kind of matrices.
bool savePosFile(const std::string &filename, const vpColVector &q)
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &d_position)
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true, const bool recopy_=true)
vpHomogeneousMatrix get_fMe(const vpColVector &q)
void getGravity(vpColVector &gravity)
double getMaxTranslationVelocity(void) const
vpHomogeneousMatrix get_eMc() const
Initialize the position controller.
error that can be emited by ViSP classes.
Class that defines a generic virtual robot.
static vpColVector saturateVelocities(const vpColVector &v_in, const vpColVector &v_max, bool verbose=false)
unsigned int size() const
Return the number of elements of the 2D array.
void getForceTorque(const vpRobot::vpControlFrameType frame, vpColVector &force)
vpColVector getJointMin() const
void setLogFolder(const std::string &folder)
int gripperGrasp(double grasping_width, double force=60.)
void getMass(vpMatrix &mass)
double getMaxRotationVelocity(void) const
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
void set_eMc(const vpHomogeneousMatrix &eMc)
Initialize the velocity controller.
bool readPosFile(const std::string &filename, vpColVector &q)
void get_eJe(vpMatrix &eJe)
static double rad(double deg)
void get_fJe(vpMatrix &fJe)
int nDof
number of degrees of freedom
static double deg(double rad)
vpMatrix eJe
robot Jacobian expressed in the end-effector frame
Implementation of column vector and the associated operations.
Implementation of a pose vector and operations on poses.
vpHomogeneousMatrix inverse() const
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position)
vpHomogeneousMatrix get_fMc(const vpColVector &q)
virtual vpRobotStateType getRobotState(void) const
void move(const std::string &filename, double velocity_percentage=10.)
void setPositioningVelocity(const double velocity)
void connect(const std::string &franka_address, franka::RealtimeConfig realtime_config=franka::RealtimeConfig::kEnforce)
vpPoseVector buildFrom(const double tx, const double ty, const double tz, const double tux, const double tuy, const double tuz)
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &position)
Function not implemented.
vpMatrix fJe
robot Jacobian expressed in the robot reference frame available
void resize(const unsigned int i, const bool flagNullify=true)