diff --git a/extensions/rcs_fr3/src/hw/Franka.cpp b/extensions/rcs_fr3/src/hw/Franka.cpp index 53210249..054fa7e0 100644 --- a/extensions/rcs_fr3/src/hw/Franka.cpp +++ b/extensions/rcs_fr3/src/hw/Franka.cpp @@ -147,13 +147,14 @@ void PInverse(const Eigen::MatrixXd &M, Eigen::MatrixXd &M_inv, M_inv = Eigen::MatrixXd(svd.matrixV() * S_inv * svd.matrixU().transpose()); } -void TorqueSafetyGuardFn(std::array &tau_d_array, double min_torque, - double max_torque) { +void TorqueSafetyGuardFn(std::array &tau_d_array, + const std::array &min_torques, + const std::array &max_torques) { for (size_t i = 0; i < tau_d_array.size(); i++) { - if (tau_d_array[i] < min_torque) { - tau_d_array[i] = min_torque; - } else if (tau_d_array[i] > max_torque) { - tau_d_array[i] = max_torque; + if (tau_d_array[i] < min_torques[i]) { + tau_d_array[i] = min_torques[i]; + } else if (tau_d_array[i] > max_torques[i]) { + tau_d_array[i] = max_torques[i]; } } } @@ -419,18 +420,20 @@ void Franka::osc() { dist2joint_max = joint_max_.matrix() - q; dist2joint_min = q - joint_min_.matrix(); + double activation_distance = 0.25; for (int i = 0; i < 7; i++) { - if (dist2joint_max[i] < 0.25 && dist2joint_max[i] > 0.1) - avoidance_force[i] += -avoidance_weights_[i] * dist2joint_max[i]; - if (dist2joint_min[i] < 0.25 && dist2joint_min[i] > 0.1) - avoidance_force[i] += avoidance_weights_[i] * dist2joint_min[i]; + if (dist2joint_max[i] < activation_distance) { + avoidance_force[i] += + -avoidance_weights_[i] * + (1 / std::max(dist2joint_max[i], 0.001) - 1 / activation_distance); + } + if (dist2joint_min[i] < activation_distance) { + avoidance_force[i] += + avoidance_weights_[i] * + (1 / std::max(dist2joint_min[i], 0.001) - 1 / activation_distance); + } } tau_d << tau_d + Nullspace * avoidance_force; - for (int i = 0; i < 7; i++) { - if (dist2joint_max[i] < 0.1 && tau_d[i] > 0.) tau_d[i] = 0.; - if (dist2joint_min[i] < 0.1 && tau_d[i] < 0.) tau_d[i] = 0.; - } - std::array tau_d_array{}; Eigen::VectorXd::Map(&tau_d_array[0], 7) = tau_d; @@ -439,14 +442,16 @@ void Franka::osc() { std::chrono::high_resolution_clock::now(); auto time = std::chrono::duration_cast(t2 - t1); + // clamp the torques to a safe range. + std::array min_torques = {-10.0, -10.0, -10.0, -10.0, + -5.0, -5.0, -5.0}; + std::array max_torques = {10.0, 10.0, 10.0, 10.0, 5.0, 5.0, 5.0}; + TorqueSafetyGuardFn(tau_d_array, min_torques, max_torques); + + // limit the rate of change of the clamped torques. std::array tau_d_rate_limited = franka::limitRate( franka::kMaxTorqueRate, tau_d_array, robot_state.tau_J_d); - // deoxys/config/control_config.yml - double min_torque = -5; - double max_torque = 5; - TorqueSafetyGuardFn(tau_d_rate_limited, min_torque, max_torque); - return tau_d_rate_limited; }); } @@ -514,16 +519,31 @@ void Franka::joint_controller() { tau_d << Kp.cwiseProduct(joint_pos_error) - Kd.cwiseProduct(dq); + // Add joint avoidance potential + Eigen::Array avoidance_weights; + avoidance_weights << 1., 1., 1., 1., 1., 10., 10.; + Eigen::Matrix avoidance_force; + avoidance_force.setZero(); Eigen::Matrix dist2joint_max; Eigen::Matrix dist2joint_min; dist2joint_max = joint_max_.matrix() - q; dist2joint_min = q - joint_min_.matrix(); + double activation_distance = 0.25; for (int i = 0; i < 7; i++) { - if (dist2joint_max[i] < 0.1 && tau_d[i] > 0.) tau_d[i] = 0.; - if (dist2joint_min[i] < 0.1 && tau_d[i] < 0.) tau_d[i] = 0.; + if (dist2joint_max[i] < activation_distance) { + avoidance_force[i] += + -avoidance_weights[i] * + (1 / std::max(dist2joint_max[i], 0.001) - 1 / activation_distance); + } + if (dist2joint_min[i] < activation_distance) { + avoidance_force[i] += + avoidance_weights[i] * + (1 / std::max(dist2joint_min[i], 0.001) - 1 / activation_distance); + } } + tau_d << tau_d + avoidance_force; std::array tau_d_array{}; Eigen::VectorXd::Map(&tau_d_array[0], 7) = tau_d; @@ -533,14 +553,17 @@ void Franka::joint_controller() { std::chrono::high_resolution_clock::now(); auto time = std::chrono::duration_cast(t2 - t1); + // deoxys/config/control_config.yml + // First clamp the torques to a safe range. + std::array min_torques = {-10.0, -10.0, -10.0, -10.0, + -5.0, -5.0, -5.0}; + std::array max_torques = {10.0, 10.0, 10.0, 10.0, 5.0, 5.0, 5.0}; + TorqueSafetyGuardFn(tau_d_array, min_torques, max_torques); + + // Then limit the rate of change of the clamped torques. std::array tau_d_rate_limited = franka::limitRate( franka::kMaxTorqueRate, tau_d_array, robot_state.tau_J_d); - // deoxys/config/control_config.yml - double min_torque = -5; - double max_torque = 5; - TorqueSafetyGuardFn(tau_d_rate_limited, min_torque, max_torque); - return tau_d_rate_limited; }); } diff --git a/include/rcs/LinearPoseTrajInterpolator.h b/include/rcs/LinearPoseTrajInterpolator.h index cecaa754..1bada3a9 100644 --- a/include/rcs/LinearPoseTrajInterpolator.h +++ b/include/rcs/LinearPoseTrajInterpolator.h @@ -10,19 +10,16 @@ class LinearPoseTrajInterpolator { Eigen::Vector3d p_start_; Eigen::Vector3d p_goal_; Eigen::Vector3d last_p_t_; - Eigen::Vector3d prev_p_goal_; Eigen::Quaterniond q_start_; Eigen::Quaterniond q_goal_; Eigen::Quaterniond last_q_t_; - Eigen::Quaterniond prev_q_goal_; double dt_; double last_time_; double max_time_; double start_time_; bool start_; - bool first_goal_; public: inline LinearPoseTrajInterpolator() @@ -30,8 +27,7 @@ class LinearPoseTrajInterpolator { last_time_(0.), max_time_(1.), start_time_(0.), - start_(false), - first_goal_(true){}; + start_(false){}; inline ~LinearPoseTrajInterpolator(){}; @@ -49,22 +45,8 @@ class LinearPoseTrajInterpolator { start_ = false; - if (first_goal_) { - p_start_ = p_start; - q_start_ = q_start; - - prev_p_goal_ = p_start; - prev_q_goal_ = q_start; - first_goal_ = false; - } else { - // If the goal is already set, use prev goal as the starting point of - // interpolation. - prev_p_goal_ = p_goal_; - prev_q_goal_ = q_goal_; - - p_start_ = prev_p_goal_; - q_start_ = prev_q_goal_; - } + p_start_ = p_start; + q_start_ = q_start; p_goal_ = p_goal; q_goal_ = q_goal; @@ -104,14 +86,12 @@ class LinearJointPositionTrajInterpolator { Vector7d q_goal_; Vector7d last_q_t_; - Vector7d prev_q_goal_; double dt_; double last_time_; double max_time_; double start_time_; bool start_; - bool first_goal_; double interpolation_fraction_; // fraction of actual interpolation within an // interval @@ -122,8 +102,7 @@ class LinearJointPositionTrajInterpolator { last_time_(0.), max_time_(1.), start_time_(0.), - start_(false), - first_goal_(true){}; + start_(false){}; inline ~LinearJointPositionTrajInterpolator(){}; @@ -141,15 +120,7 @@ class LinearJointPositionTrajInterpolator { start_ = false; - if (first_goal_) { - q_start_ = q_start; - prev_q_goal_ = q_start; - first_goal_ = false; - // std::cout << "First goal of the interpolation" << std::endl; - } else { - prev_q_goal_ = q_goal_; - q_start_ = prev_q_goal_; - } + q_start_ = q_start; q_goal_ = q_goal; };