6 #include <visp3/core/vpConfig.h> 8 #ifdef VISP_HAVE_FRANKA 18 #include <franka/duration.h> 19 #include <franka/exception.h> 20 #include <franka/model.h> 21 #include <franka/robot.h> 25 template <
class T,
size_t N> std::ostream &operator<<(std::ostream &ostream, const std::array<T, N> &array)
28 std::copy(array.cbegin(), array.cend() - 1, std::ostream_iterator<T>(ostream,
","));
29 std::copy(array.cend() - 1, array.cend(), std::ostream_iterator<T>(ostream));
51 int main(
int argc,
char **argv)
55 std::cerr <<
"Usage: ./" << argv[0] <<
" <robot-hostname>" 57 <<
" <vel_max in [m/s]>" 58 <<
" <print_rate in [Hz]>" << std::endl;
63 const double radius = std::stod(argv[2]);
64 const double vel_max = std::stod(argv[3]);
65 const double acceleration_time = 2.0;
66 double vel_current = 0.0;
70 const double run_time = 20.0;
73 double print_rate = std::stod(argv[4]);
74 if (print_rate < 0.0) {
75 std::cerr <<
"print_rate too small, must be >= 0.0" << std::endl;
83 std::array<double, 7> tau_d_last;
84 franka::RobotState robot_state;
85 std::array<double, 7> gravity;
87 std::atomic_bool running{
true};
90 std::thread print_thread([print_rate, &print_data, &running]() {
93 std::this_thread::sleep_for(std::chrono::milliseconds(static_cast<int>((1.0 / print_rate * 1000.0))));
96 if (print_data.mutex.try_lock() && print_data.has_data) {
97 std::array<double, 7> tau_error{};
98 double error_rms(0.0);
99 std::array<double, 7> tau_d_actual{};
100 for (
size_t i = 0; i < 7; ++i) {
101 tau_d_actual[i] = print_data.tau_d_last[i] + print_data.gravity[i];
102 tau_error[i] = tau_d_actual[i] - print_data.robot_state.tau_J[i];
103 error_rms += std::sqrt(std::pow(tau_error[i], 2.0)) / tau_error.size();
106 std::cout <<
"tau_error [Nm]: " << tau_error << std::endl
107 <<
"tau_commanded [Nm]: " << tau_d_actual << std::endl
108 <<
"tau_measured [Nm]: " << print_data.robot_state.tau_J << std::endl
109 <<
"root mean square of tau_error [Nm]: " << error_rms << std::endl
110 <<
"-----------------------" << std::endl;
111 print_data.has_data =
false;
112 print_data.mutex.unlock();
119 franka::Robot robot(argv[1]);
122 robot.setCollisionBehavior(
123 {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}},
124 {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}},
125 {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}},
126 {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}});
129 franka::Model model = robot.loadModel();
132 std::array<double, 16> initial_pose = robot.readOnce().O_T_EE;
136 std::function<franka::CartesianPose(const franka::RobotState &, franka::Duration)> cartesian_pose_callback =
137 [=, &time, &vel_current, &running, &angle](
const franka::RobotState & ,
138 franka::Duration period) -> franka::CartesianPose {
140 time += period.toSec();
143 if (vel_current < vel_max && time < run_time) {
144 vel_current += period.toSec() * std::fabs(vel_max / acceleration_time);
146 if (vel_current > 0.0 && time > run_time) {
147 vel_current -= period.toSec() * std::fabs(vel_max / acceleration_time);
149 vel_current = std::fmax(vel_current, 0.0);
150 vel_current = std::fmin(vel_current, vel_max);
153 angle += period.toSec() * vel_current / std::fabs(radius);
154 if (angle > 2 * M_PI) {
159 double delta_y = radius * (1 - std::cos(angle));
160 double delta_z = radius * std::sin(angle);
161 franka::CartesianPose pose_desired = initial_pose;
162 pose_desired.O_T_EE[13] += delta_y;
163 pose_desired.O_T_EE[14] += delta_z;
166 if (time >= run_time + acceleration_time) {
168 return franka::MotionFinished(pose_desired);
176 const std::array<double, 7> k_gains = {{1000.0, 1000.0, 1000.0, 1000.0, 500.0, 300.0, 100.0}};
178 const std::array<double, 7> d_gains = {{100.0, 100.0, 100.0, 100.0, 50.0, 30.0, 10.0}};
181 std::function<franka::Torques(const franka::RobotState &, franka::Duration)> impedance_control_callback =
182 [&print_data, &model, k_gains, d_gains](
const franka::RobotState &state,
183 franka::Duration ) -> franka::Torques {
185 std::array<double, 7> coriolis =
186 model.coriolis(state, {{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}, 0.0, {{0.0, 0.0, 0.0}});
191 std::array<double, 7> tau_d;
192 for (
size_t i = 0; i < 7; i++) {
193 tau_d[i] = k_gains[i] * (state.q_d[i] - state.q[i]) - d_gains[i] * state.dq[i] + coriolis[i];
197 if (print_data.mutex.try_lock()) {
198 print_data.has_data =
true;
199 print_data.robot_state = state;
200 print_data.tau_d_last = tau_d;
201 print_data.gravity = model.gravity(state, 0.0, {{0.0, 0.0, 0.0}});
202 print_data.mutex.unlock();
210 robot.control(impedance_control_callback, cartesian_pose_callback);
212 }
catch (
const franka::Exception &ex) {
214 std::cerr << ex.what() << std::endl;
217 if (print_thread.joinable()) {
224 int main() { std::cout <<
"This example needs libfranka to control Panda robot." << std::endl; }