// Copyright (c) 2023 Franka Robotics GmbH // Use of this source code is governed by the Apache-2.0 license, see LICENSE #include #include #include #include #include #include #include #include #include #include #include #include #include #include "examples_common.h" namespace { template std::ostream& operator<<(std::ostream& ostream, const std::array& array) { ostream << "["; std::copy(array.cbegin(), array.cend() - 1, std::ostream_iterator(ostream, ",")); std::copy(array.cend() - 1, array.cend(), std::ostream_iterator(ostream)); ostream << "]"; return ostream; } } // anonymous namespace /** * @example joint_impedance_control.cpp * An example showing a joint impedance type control that executes a Cartesian motion in the shape * of a circle. The example illustrates how to use the internal inverse kinematics to map a * Cartesian trajectory to joint space. The joint space target is tracked by an impedance control * that additionally compensates coriolis terms using the libfranka model library. This example * also serves to compare commanded vs. measured torques. The results are printed from a separate * thread to avoid blocking print functions in the real-time loop. */ int main(int argc, char** argv) { // NOLINT(readability-function-cognitive-complexity) // Check whether the required arguments were passed. if (argc != 2) { std::cerr << "Usage: " << argv[0] << " " << std::endl; return -1; } // Set and initialize trajectory parameters. const double radius = 0.05; const double vel_max = 0.25; const double acceleration_time = 2.0; const double run_time = 20.0; // Set print rate for comparing commanded vs. measured torques. const double print_rate = 10.0; double vel_current = 0.0; double angle = 0.0; double time = 0.0; // Initialize data fields for the print thread. struct { std::mutex mutex; bool has_data; std::array tau_d_last; franka::RobotState robot_state; std::array gravity; } print_data{}; std::atomic_bool running{true}; // Start print thread. std::thread print_thread([print_rate, &print_data, &running]() { while (running) { // Sleep to achieve the desired print rate. std::this_thread::sleep_for( std::chrono::milliseconds(static_cast((1.0 / print_rate * 1000.0)))); // Try to lock data to avoid read write collisions. if (print_data.mutex.try_lock()) { if (print_data.has_data) { std::array tau_error{}; double error_rms(0.0); std::array tau_d_actual{}; for (size_t i = 0; i < 7; ++i) { tau_d_actual[i] = print_data.tau_d_last[i] + print_data.gravity[i]; tau_error[i] = tau_d_actual[i] - print_data.robot_state.tau_J[i]; error_rms += std::pow(tau_error[i], 2.0) / tau_error.size(); } error_rms = std::sqrt(error_rms); // Print data to console std::cout << "tau_error [Nm]: " << tau_error << std::endl << "tau_commanded [Nm]: " << tau_d_actual << std::endl << "tau_measured [Nm]: " << print_data.robot_state.tau_J << std::endl << "root mean square of tau_error [Nm]: " << error_rms << std::endl << "-----------------------" << std::endl; print_data.has_data = false; } print_data.mutex.unlock(); } } }); try { // Connect to robot. franka::Robot robot(argv[1]); setDefaultBehavior(robot); // First move the robot to a suitable joint configuration std::array q_goal = {{0, -M_PI_4, 0, -3 * M_PI_4, 0, M_PI_2, M_PI_4}}; MotionGenerator motion_generator(0.5, q_goal); std::cout << "WARNING: This example will move the robot! " << "Please make sure to have the user stop button at hand!" << std::endl << "Press Enter to continue..." << std::endl; std::cin.ignore(); robot.control(motion_generator); std::cout << "Finished moving to initial joint configuration." << std::endl; // Set additional parameters always before the control loop, NEVER in the control loop! // Set collision behavior. robot.setCollisionBehavior( {{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}}, {{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}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}); // Load the kinematics and dynamics model. franka::Model model = robot.loadModel(); std::array initial_pose; // Define callback function to send Cartesian pose goals to get inverse kinematics solved. auto cartesian_pose_callback = [=, &time, &vel_current, &running, &angle, &initial_pose]( const franka::RobotState& robot_state, franka::Duration period) -> franka::CartesianPose { // Update time. time += period.toSec(); if (time == 0.0) { // Read the initial pose to start the motion from in the first time step. initial_pose = robot_state.O_T_EE_c; } // Compute Cartesian velocity. if (vel_current < vel_max && time < run_time) { vel_current += period.toSec() * std::fabs(vel_max / acceleration_time); } if (vel_current > 0.0 && time > run_time) { vel_current -= period.toSec() * std::fabs(vel_max / acceleration_time); } vel_current = std::fmax(vel_current, 0.0); vel_current = std::fmin(vel_current, vel_max); // Compute new angle for our circular trajectory. angle += period.toSec() * vel_current / std::fabs(radius); if (angle > 2 * M_PI) { angle -= 2 * M_PI; } // Compute relative y and z positions of desired pose. double delta_y = radius * (1 - std::cos(angle)); double delta_z = radius * std::sin(angle); franka::CartesianPose pose_desired = initial_pose; pose_desired.O_T_EE[13] += delta_y; pose_desired.O_T_EE[14] += delta_z; // Send desired pose. if (time >= run_time + acceleration_time) { running = false; return franka::MotionFinished(pose_desired); } return pose_desired; }; // Set gains for the joint impedance control. // Stiffness const std::array k_gains = {{600.0, 600.0, 600.0, 600.0, 250.0, 150.0, 50.0}}; // Damping const std::array d_gains = {{50.0, 50.0, 50.0, 50.0, 30.0, 25.0, 15.0}}; // Define callback for the joint torque control loop. std::function impedance_control_callback = [&print_data, &model, k_gains, d_gains]( const franka::RobotState& state, franka::Duration /*period*/) -> franka::Torques { // Read current coriolis terms from model. std::array coriolis = model.coriolis(state); // Compute torque command from joint impedance control law. // Note: The answer to our Cartesian pose inverse kinematics is always in state.q_d with one // time step delay. std::array tau_d_calculated; for (size_t i = 0; i < 7; i++) { tau_d_calculated[i] = k_gains[i] * (state.q_d[i] - state.q[i]) - d_gains[i] * state.dq[i] + coriolis[i]; } // The following line is only necessary for printing the rate limited torque. As we activated // rate limiting for the control loop (activated by default), the torque would anyway be // adjusted! std::array tau_d_rate_limited = franka::limitRate(franka::kMaxTorqueRate, tau_d_calculated, state.tau_J_d); // Update data to print. if (print_data.mutex.try_lock()) { print_data.has_data = true; print_data.robot_state = state; print_data.tau_d_last = tau_d_rate_limited; print_data.gravity = model.gravity(state); print_data.mutex.unlock(); } // Send torque command. return tau_d_rate_limited; }; // Start real-time control loop. robot.control(impedance_control_callback, cartesian_pose_callback); } catch (const franka::Exception& ex) { running = false; std::cerr << ex.what() << std::endl; } if (print_thread.joinable()) { print_thread.join(); } return 0; }