diff --git a/extensions/rcs_fr3/src/hw/Franka.cpp b/extensions/rcs_fr3/src/hw/Franka.cpp index 1c7165ff..71069a08 100644 --- a/extensions/rcs_fr3/src/hw/Franka.cpp +++ b/extensions/rcs_fr3/src/hw/Franka.cpp @@ -172,6 +172,7 @@ void TorqueSafetyGuardFn(std::array& tau_d_array, double min_torque, } void Franka::controller_set_joint_position(const common::Vector7d& desired_q) { + this->check_for_background_errors(); // from deoxys/config/osc-position-controller.yml double traj_interpolation_time_fraction = 1.0; // in s // form deoxys/config/charmander.yml @@ -206,8 +207,18 @@ void Franka::controller_set_joint_position(const common::Vector7d& desired_q) { } } +void Franka::check_for_background_errors() { + std::lock_guard lock(this->exception_mutex); + if (this->background_exception) { + std::exception_ptr ex = this->background_exception; + this->background_exception = nullptr; + std::rethrow_exception(ex); + } +} + void Franka::osc_set_cartesian_position( const common::Pose& desired_pose_EE_in_base_frame) { + this->check_for_background_errors(); // from deoxys/config/osc-position-controller.yml double traj_interpolation_time_fraction = 1.0; // form deoxys/config/charmander.yml @@ -301,168 +312,178 @@ void Franka::osc() { joint_min_ << -2.8973, -1.7628, -2.8973, -3.0718, -2.8973, -0.0175, -2.8973; avoidance_weights_ << 1., 1., 1., 1., 1., 10., 10.; - this->robot.control([&](const franka::RobotState& robot_state, - franka::Duration period) -> franka::Torques { - std::chrono::high_resolution_clock::time_point t1 = - std::chrono::high_resolution_clock::now(); - - // torques handler - if (this->running_controller == Controller::none) { - franka::Torques zero_torques{{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}; - return franka::MotionFinished(zero_torques); - } - // TO BE replaced - // if (!this->control_thread_running && dq.maxCoeff() < 0.0001) { - // return franka::MotionFinished(franka::Torques(tau_d_array)); - // } - - Eigen::Vector3d desired_pos_EE_in_base_frame; - Eigen::Quaterniond desired_quat_EE_in_base_frame; - - common::Pose pose(robot_state.O_T_EE); - // form deoxys/config/charmander.yml - int policy_rate = 20; - int traj_rate = 500; - - this->interpolator_mutex.lock(); - this->curr_state = robot_state; - this->controller_time += period.toSec(); - this->traj_interpolator.next_step(this->controller_time, - desired_pos_EE_in_base_frame, - desired_quat_EE_in_base_frame); - this->interpolator_mutex.unlock(); - - // end torques handler - - Eigen::Matrix tau_d; - - std::array mass_array = model.mass(robot_state); - Eigen::Map> M(mass_array.data()); - - M = M + Eigen::Matrix(residual_mass_vec_.asDiagonal()); - - // coriolis and gravity - std::array coriolis_array = model.coriolis(robot_state); - Eigen::Map> coriolis( - coriolis_array.data()); - - std::array gravity_array = model.gravity(robot_state); - Eigen::Map> gravity(gravity_array.data()); - - std::array jacobian_array = - model.zeroJacobian(franka::Frame::kEndEffector, robot_state); - Eigen::Map> jacobian( - jacobian_array.data()); - - Eigen::MatrixXd jacobian_pos(3, 7); - Eigen::MatrixXd jacobian_ori(3, 7); - jacobian_pos << jacobian.block(0, 0, 3, 7); - jacobian_ori << jacobian.block(3, 0, 3, 7); - - // End effector pose in base frame - Eigen::Affine3d T_EE_in_base_frame( - Eigen::Matrix4d::Map(robot_state.O_T_EE.data())); - Eigen::Vector3d pos_EE_in_base_frame(T_EE_in_base_frame.translation()); - Eigen::Quaterniond quat_EE_in_base_frame(T_EE_in_base_frame.linear()); - - // Nullspace goal - Eigen::Map> q(robot_state.q.data()); - - // Joint velocity - Eigen::Map> dq(robot_state.dq.data()); - - if (desired_quat_EE_in_base_frame.coeffs().dot( - quat_EE_in_base_frame.coeffs()) < 0.0) { - quat_EE_in_base_frame.coeffs() << -quat_EE_in_base_frame.coeffs(); - } - - Eigen::Vector3d pos_error; - - pos_error << desired_pos_EE_in_base_frame - pos_EE_in_base_frame; - Eigen::Quaterniond quat_error(desired_quat_EE_in_base_frame.inverse() * - quat_EE_in_base_frame); - Eigen::Vector3d ori_error; - ori_error << quat_error.x(), quat_error.y(), quat_error.z(); - ori_error << -T_EE_in_base_frame.linear() * ori_error; - - // Compute matrices - Eigen::Matrix M_inv(M.inverse()); - Eigen::MatrixXd Lambda_inv(6, 6); - Lambda_inv << jacobian * M_inv * jacobian.transpose(); - Eigen::MatrixXd Lambda(6, 6); - PInverse(Lambda_inv, Lambda); - - Eigen::Matrix J_inv; - J_inv << M_inv * jacobian.transpose() * Lambda; - Eigen::Matrix Nullspace; - Nullspace << Eigen::MatrixXd::Identity(7, 7) - - jacobian.transpose() * J_inv.transpose(); - - // Decoupled mass matrices - Eigen::MatrixXd Lambda_pos_inv(3, 3); - Lambda_pos_inv << jacobian_pos * M_inv * jacobian_pos.transpose(); - Eigen::MatrixXd Lambda_ori_inv(3, 3); - Lambda_ori_inv << jacobian_ori * M_inv * jacobian_ori.transpose(); - - Eigen::MatrixXd Lambda_pos(3, 3); - Eigen::MatrixXd Lambda_ori(3, 3); - PInverse(Lambda_pos_inv, Lambda_pos); - PInverse(Lambda_ori_inv, Lambda_ori); - - pos_error = - pos_error.unaryExpr([](double x) { return (abs(x) < 1e-4) ? 0. : x; }); - ori_error = - ori_error.unaryExpr([](double x) { return (abs(x) < 5e-3) ? 0. : x; }); - - tau_d << jacobian_pos.transpose() * - (Lambda_pos * - (Kp_p * pos_error - Kd_p * (jacobian_pos * dq))) + - jacobian_ori.transpose() * - (Lambda_ori * - (Kp_r * ori_error - Kd_r * (jacobian_ori * dq))); - - // nullspace control - tau_d << tau_d + Nullspace * (static_q_task_ - q); - - // Add joint avoidance potential - 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(); - - 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]; - } - 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; - - // end of controller - std::chrono::high_resolution_clock::time_point t2 = - std::chrono::high_resolution_clock::now(); - auto time = std::chrono::duration_cast(t2 - t1); - - 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); + try { + this->robot.control([&](const franka::RobotState& robot_state, + franka::Duration period) -> franka::Torques { + std::chrono::high_resolution_clock::time_point t1 = + std::chrono::high_resolution_clock::now(); + + // torques handler + if (this->running_controller == Controller::none) { + franka::Torques zero_torques{{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}; + return franka::MotionFinished(zero_torques); + } + // TO BE replaced + // if (!this->control_thread_running && dq.maxCoeff() < 0.0001) { + // return franka::MotionFinished(franka::Torques(tau_d_array)); + // } + + Eigen::Vector3d desired_pos_EE_in_base_frame; + Eigen::Quaterniond desired_quat_EE_in_base_frame; + + common::Pose pose(robot_state.O_T_EE); + // form deoxys/config/charmander.yml + int policy_rate = 20; + int traj_rate = 500; + + this->interpolator_mutex.lock(); + this->curr_state = robot_state; + this->controller_time += period.toSec(); + this->traj_interpolator.next_step(this->controller_time, + desired_pos_EE_in_base_frame, + desired_quat_EE_in_base_frame); + this->interpolator_mutex.unlock(); + + // end torques handler + + Eigen::Matrix tau_d; + + std::array mass_array = model.mass(robot_state); + Eigen::Map> M(mass_array.data()); + + M = M + Eigen::Matrix(residual_mass_vec_.asDiagonal()); + + // coriolis and gravity + std::array coriolis_array = model.coriolis(robot_state); + Eigen::Map> coriolis( + coriolis_array.data()); + + std::array gravity_array = model.gravity(robot_state); + Eigen::Map> gravity( + gravity_array.data()); + + std::array jacobian_array = + model.zeroJacobian(franka::Frame::kEndEffector, robot_state); + Eigen::Map> jacobian( + jacobian_array.data()); + + Eigen::MatrixXd jacobian_pos(3, 7); + Eigen::MatrixXd jacobian_ori(3, 7); + jacobian_pos << jacobian.block(0, 0, 3, 7); + jacobian_ori << jacobian.block(3, 0, 3, 7); + + // End effector pose in base frame + Eigen::Affine3d T_EE_in_base_frame( + Eigen::Matrix4d::Map(robot_state.O_T_EE.data())); + Eigen::Vector3d pos_EE_in_base_frame(T_EE_in_base_frame.translation()); + Eigen::Quaterniond quat_EE_in_base_frame(T_EE_in_base_frame.linear()); + + // Nullspace goal + Eigen::Map> q(robot_state.q.data()); + + // Joint velocity + Eigen::Map> dq(robot_state.dq.data()); + + if (desired_quat_EE_in_base_frame.coeffs().dot( + quat_EE_in_base_frame.coeffs()) < 0.0) { + quat_EE_in_base_frame.coeffs() << -quat_EE_in_base_frame.coeffs(); + } + + Eigen::Vector3d pos_error; + + pos_error << desired_pos_EE_in_base_frame - pos_EE_in_base_frame; + Eigen::Quaterniond quat_error(desired_quat_EE_in_base_frame.inverse() * + quat_EE_in_base_frame); + Eigen::Vector3d ori_error; + ori_error << quat_error.x(), quat_error.y(), quat_error.z(); + ori_error << -T_EE_in_base_frame.linear() * ori_error; + + // Compute matrices + Eigen::Matrix M_inv(M.inverse()); + Eigen::MatrixXd Lambda_inv(6, 6); + Lambda_inv << jacobian * M_inv * jacobian.transpose(); + Eigen::MatrixXd Lambda(6, 6); + PInverse(Lambda_inv, Lambda); + + Eigen::Matrix J_inv; + J_inv << M_inv * jacobian.transpose() * Lambda; + Eigen::Matrix Nullspace; + Nullspace << Eigen::MatrixXd::Identity(7, 7) - + jacobian.transpose() * J_inv.transpose(); + + // Decoupled mass matrices + Eigen::MatrixXd Lambda_pos_inv(3, 3); + Lambda_pos_inv << jacobian_pos * M_inv * jacobian_pos.transpose(); + Eigen::MatrixXd Lambda_ori_inv(3, 3); + Lambda_ori_inv << jacobian_ori * M_inv * jacobian_ori.transpose(); + + Eigen::MatrixXd Lambda_pos(3, 3); + Eigen::MatrixXd Lambda_ori(3, 3); + PInverse(Lambda_pos_inv, Lambda_pos); + PInverse(Lambda_ori_inv, Lambda_ori); + + pos_error = pos_error.unaryExpr( + [](double x) { return (abs(x) < 1e-4) ? 0. : x; }); + ori_error = ori_error.unaryExpr( + [](double x) { return (abs(x) < 5e-3) ? 0. : x; }); + + tau_d << jacobian_pos.transpose() * + (Lambda_pos * + (Kp_p * pos_error - Kd_p * (jacobian_pos * dq))) + + jacobian_ori.transpose() * + (Lambda_ori * + (Kp_r * ori_error - Kd_r * (jacobian_ori * dq))); + + // nullspace control + tau_d << tau_d + Nullspace * (static_q_task_ - q); + + // Add joint avoidance potential + 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(); + + 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]; + } + 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; + + // end of controller + std::chrono::high_resolution_clock::time_point t2 = + std::chrono::high_resolution_clock::now(); + auto time = + std::chrono::duration_cast(t2 - t1); + + 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; + }); + } catch (...) { + std::lock_guard lock(this->exception_mutex); + this->background_exception = std::current_exception(); + } - return tau_d_rate_limited; - }); + // Ensure we mark the controller as stopped so we can restart later + this->running_controller = Controller::none; } void Franka::joint_controller() { @@ -492,71 +513,77 @@ void Franka::joint_controller() { joint_max_ << 2.8978, 1.7628, 2.8973, -0.0698, 2.8973, 3.7525, 2.8973; joint_min_ << -2.8973, -1.7628, -2.8973, -3.0718, -2.8973, -0.0175, -2.8973; - this->robot.control([&](const franka::RobotState& robot_state, - franka::Duration period) -> franka::Torques { - std::chrono::high_resolution_clock::time_point t1 = - std::chrono::high_resolution_clock::now(); + try { + this->robot.control([&](const franka::RobotState& robot_state, + franka::Duration period) -> franka::Torques { + std::chrono::high_resolution_clock::time_point t1 = + std::chrono::high_resolution_clock::now(); - // torques handler - if (this->running_controller == Controller::none) { - // TODO: test if this also works when the robot is moving - franka::Torques zero_torques{{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}; - return franka::MotionFinished(zero_torques); - } + // torques handler + if (this->running_controller == Controller::none) { + // TODO: test if this also works when the robot is moving + franka::Torques zero_torques{{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}; + return franka::MotionFinished(zero_torques); + } - common::Vector7d desired_q; - common::Pose pose(robot_state.O_T_EE); + common::Vector7d desired_q; + common::Pose pose(robot_state.O_T_EE); - this->interpolator_mutex.lock(); - this->curr_state = robot_state; - this->controller_time += period.toSec(); - this->joint_interpolator.next_step(this->controller_time, desired_q); - this->interpolator_mutex.unlock(); - // end torques handler + this->interpolator_mutex.lock(); + this->curr_state = robot_state; + this->controller_time += period.toSec(); + this->joint_interpolator.next_step(this->controller_time, desired_q); + this->interpolator_mutex.unlock(); + // end torques handler - Eigen::Matrix tau_d; + Eigen::Matrix tau_d; - // Current joint velocity - Eigen::Map> dq(robot_state.dq.data()); + // Current joint velocity + Eigen::Map> dq(robot_state.dq.data()); - // Current joint position - Eigen::Map> q(robot_state.q.data()); + // Current joint position + Eigen::Map> q(robot_state.q.data()); - Eigen::MatrixXd joint_pos_error(7, 1); + Eigen::MatrixXd joint_pos_error(7, 1); - joint_pos_error << desired_q - q; + joint_pos_error << desired_q - q; - tau_d << Kp.cwiseProduct(joint_pos_error) - Kd.cwiseProduct(dq); + tau_d << Kp.cwiseProduct(joint_pos_error) - Kd.cwiseProduct(dq); - Eigen::Matrix dist2joint_max; - Eigen::Matrix dist2joint_min; + Eigen::Matrix dist2joint_max; + Eigen::Matrix dist2joint_min; - dist2joint_max = joint_max_.matrix() - q; - dist2joint_min = q - joint_min_.matrix(); + dist2joint_max = joint_max_.matrix() - q; + dist2joint_min = q - joint_min_.matrix(); - 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.; - } + 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; + std::array tau_d_array{}; + Eigen::VectorXd::Map(&tau_d_array[0], 7) = tau_d; - // end of controller - std::chrono::high_resolution_clock::time_point t2 = - std::chrono::high_resolution_clock::now(); - auto time = std::chrono::duration_cast(t2 - t1); + // end of controller + std::chrono::high_resolution_clock::time_point t2 = + std::chrono::high_resolution_clock::now(); + auto time = + std::chrono::duration_cast(t2 - t1); - std::array tau_d_rate_limited = franka::limitRate( - franka::kMaxTorqueRate, tau_d_array, robot_state.tau_J_d); + 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); + // 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; - }); + return tau_d_rate_limited; + }); + } catch (...) { + std::lock_guard lock(this->exception_mutex); + this->background_exception = std::current_exception(); + } } void Franka::zero_torque_guiding() { diff --git a/extensions/rcs_fr3/src/hw/Franka.h b/extensions/rcs_fr3/src/hw/Franka.h index 6712b67b..70f85ec7 100644 --- a/extensions/rcs_fr3/src/hw/Franka.h +++ b/extensions/rcs_fr3/src/hw/Franka.h @@ -66,9 +66,12 @@ class Franka : public common::Robot { franka::RobotState curr_state; std::mutex interpolator_mutex; Controller running_controller = Controller::none; + std::exception_ptr background_exception = nullptr; + std::mutex exception_mutex; void osc(); void joint_controller(); void zero_torque_controller(); + void check_for_background_errors(); public: Franka(const std::string& ip, diff --git a/extensions/rcs_panda/src/rcs_panda/_core/__init__.pyi b/extensions/rcs_panda/src/rcs_panda/_core/__init__.pyi index 5d8ef9b0..b838a116 100644 --- a/extensions/rcs_panda/src/rcs_panda/_core/__init__.pyi +++ b/extensions/rcs_panda/src/rcs_panda/_core/__init__.pyi @@ -16,4 +16,4 @@ from __future__ import annotations from . import hw __all__: list[str] = ["hw"] -__version__: str = "0.5.2" +__version__: str = "0.6.0" diff --git a/pyproject.toml b/pyproject.toml index 864318a5..a3e06871 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -164,7 +164,7 @@ version_files = [ # --- Panda Extension --- "extensions/rcs_panda/CMakeLists.txt:VERSION", - "extensions/rcs_panda/src/rcs_fr3/_core/__init__.pyi:__version__", + "extensions/rcs_panda/src/rcs_panda/_core/__init__.pyi:__version__", "extensions/rcs_panda/pyproject.toml:version", "extensions/rcs_panda/pyproject.toml:\"rcs>=(.*)\"",