diff --git a/CHANGELOG.md b/CHANGELOG.md index 3ac9632ebdc615e7ca250fc29106399c9cf7f47a..f2f3b48abe934ba482328ade77bbe3477617c141 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -2,10 +2,13 @@ ## 0.2.0 - UNRELEASED +Requires `libfranka` >= 0.2.0 + * Added missing run-time dependencies to `franka_description` and `franka_control` - * Added `m_ee`, `F_x_Cee`, `I_ee`, `m_total`, `F_x_Ctotal` and `I_total` - to the robot state - * Updated and improved examples + * Added `tau_J_d`, `m_ee`, `F_x_Cee`, `I_ee`, `m_total`, `F_x_Ctotal`, `I_total`, + `theta` and `dtheta` to `franka_msgs/FrankaState` + * Added new errors to `franka_msgs/Errors` + * Updated and improved examples in `franka_example_controllers` * Fixed includes for Eigen3 in `franka_example_controllers` * Fixed gripper state publishing in `franka_gripper_node` diff --git a/Jenkinsfile b/Jenkinsfile index 9751530fa6e61fe193c2a68c846802e910a154ac..870cc7beb65717e78019be4b37b6e6b70306fb3f 100644 --- a/Jenkinsfile +++ b/Jenkinsfile @@ -17,11 +17,11 @@ node('docker') { projectName: "SWDEV/libfranka/${java.net.URLEncoder.encode(env.BRANCH_NAME, "UTF-8")}", selector: [$class: 'StatusBuildSelector', stable: false]]) } catch (e) { - // Fall back to master branch. + // Fall back to develop branch. step([$class: 'CopyArtifact', filter: 'libfranka-*-amd64.tar.gz', fingerprintArtifacts: true, - projectName: "SWDEV/libfranka/master", + projectName: "SWDEV/libfranka/develop", selector: [$class: 'StatusBuildSelector', stable: false]]) } sh ''' diff --git a/franka_control/src/franka_state_controller.cpp b/franka_control/src/franka_state_controller.cpp index 2fdcdbad2d65c137d1ca0915d46cc73b139a53e8..2faa5aea54f6cf4c66a145cc0c2b844de04f4ef1 100644 --- a/franka_control/src/franka_state_controller.cpp +++ b/franka_control/src/franka_state_controller.cpp @@ -120,6 +120,13 @@ franka_msgs::Errors errorsToMessage(const franka::Errors& error) { error.communication_constraints_violation); message.power_limit_violation = static_cast<decltype(message.power_limit_violation)>(error.power_limit_violation); + message.joint_p2p_insufficient_torque_for_planning = + static_cast<decltype(message.joint_p2p_insufficient_torque_for_planning)>( + error.joint_p2p_insufficient_torque_for_planning); + message.tau_j_range_violation = + static_cast<decltype(message.tau_j_range_violation)>(error.tau_j_range_violation); + message.instability_detected = + static_cast<decltype(message.instability_detected)>(error.instability_detected); return message; } @@ -255,6 +262,12 @@ void FrankaStateController::publishFrankaStates(const ros::Time& time) { "Robot state joint members do not have same size"); static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.dtau_J), "Robot state joint members do not have same size"); + static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.tau_J_d), + "Robot state joint members do not have same size"); + static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.theta), + "Robot state joint members do not have same size"); + static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.dtheta), + "Robot state joint members do not have same size"); static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.joint_collision), "Robot state joint members do not have same size"); static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.joint_contact), @@ -268,6 +281,9 @@ void FrankaStateController::publishFrankaStates(const ros::Time& time) { publisher_franka_states_.msg_.dq_d[i] = robot_state_.dq_d[i]; publisher_franka_states_.msg_.tau_J[i] = robot_state_.tau_J[i]; publisher_franka_states_.msg_.dtau_J[i] = robot_state_.dtau_J[i]; + publisher_franka_states_.msg_.tau_J_d[i] = robot_state_.tau_J_d[i]; + publisher_franka_states_.msg_.theta[i] = robot_state_.theta[i]; + publisher_franka_states_.msg_.dtheta[i] = robot_state_.dtheta[i]; publisher_franka_states_.msg_.joint_collision[i] = robot_state_.joint_collision[i]; publisher_franka_states_.msg_.joint_contact[i] = robot_state_.joint_contact[i]; publisher_franka_states_.msg_.tau_ext_hat_filtered[i] = robot_state_.tau_ext_hat_filtered[i]; diff --git a/franka_example_controllers/include/franka_example_controllers/cartesian_impedance_example_controller.h b/franka_example_controllers/include/franka_example_controllers/cartesian_impedance_example_controller.h index 070328d6f5ed6aa8a37517bdf4544628f5d53e21..2e5d63460152947ad31dd95467083e95dd7cbd20 100644 --- a/franka_example_controllers/include/franka_example_controllers/cartesian_impedance_example_controller.h +++ b/franka_example_controllers/include/franka_example_controllers/cartesian_impedance_example_controller.h @@ -34,8 +34,7 @@ class CartesianImpedanceExampleController : public controller_interface::MultiIn // Saturation Eigen::Matrix<double, 7, 1> saturateTorqueRate( const Eigen::Matrix<double, 7, 1>& tau_d_calculated, - const Eigen::Matrix<double, 7, 1>& tau_J_d, // NOLINT (readability-identifier-naming) - const Eigen::Matrix<double, 7, 1>& gravity); + const Eigen::Matrix<double, 7, 1>& tau_J_d); // NOLINT (readability-identifier-naming) std::unique_ptr<franka_hw::FrankaStateHandle> state_handle_; std::unique_ptr<franka_hw::FrankaModelHandle> model_handle_; diff --git a/franka_example_controllers/include/franka_example_controllers/force_example_controller.h b/franka_example_controllers/include/franka_example_controllers/force_example_controller.h index 053d86ae59eb6bfc9af5762064326ac0081100e9..1b7eb7d75906e673bbcce9f03694a971fa9bae9b 100644 --- a/franka_example_controllers/include/franka_example_controllers/force_example_controller.h +++ b/franka_example_controllers/include/franka_example_controllers/force_example_controller.h @@ -33,8 +33,7 @@ class ForceExampleController : public controller_interface::MultiInterfaceContro // Saturation Eigen::Matrix<double, 7, 1> saturateTorqueRate( const Eigen::Matrix<double, 7, 1>& tau_d_calculated, - const Eigen::Matrix<double, 7, 1>& tau_J_d, // NOLINT (readability-identifier-naming) - const Eigen::Matrix<double, 7, 1>& gravity); + const Eigen::Matrix<double, 7, 1>& tau_J_d); // NOLINT (readability-identifier-naming) std::unique_ptr<franka_hw::FrankaModelHandle> model_handle_; std::unique_ptr<franka_hw::FrankaStateHandle> state_handle_; diff --git a/franka_example_controllers/include/franka_example_controllers/joint_impedance_example_controller.h b/franka_example_controllers/include/franka_example_controllers/joint_impedance_example_controller.h index e9734c4690b8c9f9b964d21e3d1139f0a88420e0..526629a463dfbd43e530978543afe166976b202f 100644 --- a/franka_example_controllers/include/franka_example_controllers/joint_impedance_example_controller.h +++ b/franka_example_controllers/include/franka_example_controllers/joint_impedance_example_controller.h @@ -33,8 +33,7 @@ class JointImpedanceExampleController : public controller_interface::MultiInterf // Saturation std::array<double, 7> saturateTorqueRate( const std::array<double, 7>& tau_d_calculated, - const std::array<double, 7>& tau_J_d, // NOLINT (readability-identifier-naming) - const std::array<double, 7>& gravity); + const std::array<double, 7>& tau_J_d); // NOLINT (readability-identifier-naming) std::unique_ptr<franka_hw::FrankaCartesianPoseHandle> cartesian_pose_handle_; std::unique_ptr<franka_hw::FrankaModelHandle> model_handle_; diff --git a/franka_example_controllers/src/cartesian_impedance_example_controller.cpp b/franka_example_controllers/src/cartesian_impedance_example_controller.cpp index 5b275fbaa1fe3caaf0ef17748194f8ab6c36f4d0..286cad571afc03e82e9e04b2458c26c09ece8de9 100644 --- a/franka_example_controllers/src/cartesian_impedance_example_controller.cpp +++ b/franka_example_controllers/src/cartesian_impedance_example_controller.cpp @@ -134,8 +134,6 @@ void CartesianImpedanceExampleController::update(const ros::Time& /*time*/, const ros::Duration& /*period*/) { // get state variables franka::RobotState robot_state = state_handle_->getRobotState(); - std::array<double, 7> gravity_array = - model_handle_->getGravity(robot_state.m_total, robot_state.F_x_Ctotal); std::array<double, 7> coriolis_array = model_handle_->getCoriolis(robot_state.I_total, robot_state.m_total, robot_state.F_x_Ctotal); std::array<double, 42> jacobian_array = @@ -143,7 +141,6 @@ void CartesianImpedanceExampleController::update(const ros::Time& /*time*/, // convert to Eigen Eigen::Map<Eigen::Matrix<double, 7, 1> > coriolis(coriolis_array.data()); - Eigen::Map<Eigen::Matrix<double, 7, 1> > gravity(gravity_array.data()); Eigen::Map<Eigen::Matrix<double, 6, 7> > jacobian(jacobian_array.data()); Eigen::Map<Eigen::Matrix<double, 7, 1> > q(robot_state.q.data()); Eigen::Map<Eigen::Matrix<double, 7, 1> > dq(robot_state.dq.data()); @@ -159,6 +156,9 @@ void CartesianImpedanceExampleController::update(const ros::Time& /*time*/, error.head(3) << position - position_d_; // orientation error + if (orientation_d_.coeffs().dot(orientation.coeffs()) < 0.0) { + orientation.coeffs() << -orientation.coeffs(); + } // "difference" quaternion Eigen::Quaterniond error_quaternion(orientation * orientation_d_.inverse()); // convert to axis angle @@ -186,7 +186,7 @@ void CartesianImpedanceExampleController::update(const ros::Time& /*time*/, // Desired torque tau_d << tau_task + tau_nullspace + coriolis; // Saturate torque rate to avoid discontinuities - tau_d << saturateTorqueRate(tau_d, tau_J_d, gravity); + tau_d << saturateTorqueRate(tau_d, tau_J_d); for (size_t i = 0; i < 7; ++i) { joint_handles_[i].setCommand(tau_d(i)); } @@ -211,14 +211,12 @@ void CartesianImpedanceExampleController::update(const ros::Time& /*time*/, Eigen::Matrix<double, 7, 1> CartesianImpedanceExampleController::saturateTorqueRate( const Eigen::Matrix<double, 7, 1>& tau_d_calculated, - const Eigen::Matrix<double, 7, 1>& tau_J_d, // NOLINT (readability-identifier-naming) - const Eigen::Matrix<double, 7, 1>& gravity) { + const Eigen::Matrix<double, 7, 1>& tau_J_d) { // NOLINT (readability-identifier-naming) Eigen::Matrix<double, 7, 1> tau_d_saturated{}; for (size_t i = 0; i < 7; i++) { - // TODO(sga): After gravity is removed from tau_J_d, do not subtract it any more. - double difference = tau_d_calculated[i] - (tau_J_d[i] - gravity[i]); + double difference = tau_d_calculated[i] - tau_J_d[i]; tau_d_saturated[i] = - (tau_J_d[i] - gravity[i]) + std::max(std::min(difference, delta_tau_max_), -delta_tau_max_); + tau_J_d[i] + std::max(std::min(difference, delta_tau_max_), -delta_tau_max_); } return tau_d_saturated; } diff --git a/franka_example_controllers/src/force_example_controller.cpp b/franka_example_controllers/src/force_example_controller.cpp index d249ce05cab61b932db6a19e87e54414fad72c8b..a31848a374d072fb52b4ec0276df6b37da63ca62 100644 --- a/franka_example_controllers/src/force_example_controller.cpp +++ b/franka_example_controllers/src/force_example_controller.cpp @@ -115,9 +115,9 @@ void ForceExampleController::update(const ros::Time& /*time*/, const ros::Durati tau_ext = tau_measured - gravity - tau_ext_initial_; tau_d << jacobian.transpose() * desired_force_torque; tau_error_ = tau_error_ + period.toSec() * (tau_d - tau_ext); - // FF + PI control (PI gains are intially all 0) + // FF + PI control (PI gains are initially all 0) tau_cmd = tau_d + k_p_ * (tau_d - tau_ext) + k_i_ * tau_error_; - tau_cmd << saturateTorqueRate(tau_cmd, tau_J_d, gravity); + tau_cmd << saturateTorqueRate(tau_cmd, tau_J_d); for (size_t i = 0; i < 7; ++i) { joint_handles_[i].setCommand(tau_cmd(i)); @@ -139,14 +139,11 @@ void ForceExampleController::desiredMassParamCallback( Eigen::Matrix<double, 7, 1> ForceExampleController::saturateTorqueRate( const Eigen::Matrix<double, 7, 1>& tau_d_calculated, - const Eigen::Matrix<double, 7, 1>& tau_J_d, // NOLINT (readability-identifier-naming) - const Eigen::Matrix<double, 7, 1>& gravity) { + const Eigen::Matrix<double, 7, 1>& tau_J_d) { // NOLINT (readability-identifier-naming) Eigen::Matrix<double, 7, 1> tau_d_saturated{}; for (size_t i = 0; i < 7; i++) { - // TODO(sga): After gravity is removed from tau_J_d, do not subtract it any more. - double difference = tau_d_calculated[i] - (tau_J_d[i] - gravity[i]); - tau_d_saturated[i] = - (tau_J_d[i] - gravity[i]) + std::max(std::min(difference, kDeltaTauMax), -kDeltaTauMax); + double difference = tau_d_calculated[i] - tau_J_d[i]; + tau_d_saturated[i] = tau_J_d[i] + std::max(std::min(difference, kDeltaTauMax), -kDeltaTauMax); } return tau_d_saturated; } diff --git a/franka_example_controllers/src/joint_impedance_example_controller.cpp b/franka_example_controllers/src/joint_impedance_example_controller.cpp index f00a447f6454aeca8c392254c874d0071431a65d..6838b2f9402743d9f4c9c10c6d6bc5c3aa1045a0 100644 --- a/franka_example_controllers/src/joint_impedance_example_controller.cpp +++ b/franka_example_controllers/src/joint_impedance_example_controller.cpp @@ -173,8 +173,7 @@ void JointImpedanceExampleController::update(const ros::Time& /*time*/, // Maximum torque difference with a sampling rate of 1 kHz. The maximum torque rate is // 1000 * (1 / sampling_time). - std::array<double, 7> tau_d_saturated = - saturateTorqueRate(tau_d_calculated, robot_state.tau_J_d, gravity); + std::array<double, 7> tau_d_saturated = saturateTorqueRate(tau_d_calculated, robot_state.tau_J_d); for (size_t i = 0; i < 7; ++i) { joint_handles_[i].setCommand(tau_d_saturated[i]); @@ -204,14 +203,11 @@ void JointImpedanceExampleController::update(const ros::Time& /*time*/, std::array<double, 7> JointImpedanceExampleController::saturateTorqueRate( const std::array<double, 7>& tau_d_calculated, - const std::array<double, 7>& tau_J_d, // NOLINT (readability-identifier-naming) - const std::array<double, 7>& gravity) { + const std::array<double, 7>& tau_J_d) { // NOLINT (readability-identifier-naming) std::array<double, 7> tau_d_saturated{}; for (size_t i = 0; i < 7; i++) { - // TODO(sga): After gravity is removed from tau_J_d, do not subtract it any more. - double difference = tau_d_calculated[i] - (tau_J_d[i] - gravity[i]); - tau_d_saturated[i] = - (tau_J_d[i] - gravity[i]) + std::max(std::min(difference, kDeltaTauMax), -kDeltaTauMax); + double difference = tau_d_calculated[i] - tau_J_d[i]; + tau_d_saturated[i] = tau_J_d[i] + std::max(std::min(difference, kDeltaTauMax), -kDeltaTauMax); } return tau_d_saturated; } diff --git a/franka_msgs/msg/Errors.msg b/franka_msgs/msg/Errors.msg index cddeed88f7e33a89a1a968150725803482744025..a50780fb726d63bbd6a2aaffbf502558999a2db5 100644 --- a/franka_msgs/msg/Errors.msg +++ b/franka_msgs/msg/Errors.msg @@ -31,3 +31,6 @@ bool controller_torque_discontinuity bool start_elbow_sign_inconsistent bool communication_constraints_violation bool power_limit_violation +bool joint_p2p_insufficient_torque_for_planning +bool tau_j_range_violation +bool instability_detected diff --git a/franka_msgs/msg/FrankaState.msg b/franka_msgs/msg/FrankaState.msg index fea9184f82ca538953f37027a330cbf132d9eda5..7f8f376a1d21fa6166e05ee4952995bbf1b3b2d5 100644 --- a/franka_msgs/msg/FrankaState.msg +++ b/franka_msgs/msg/FrankaState.msg @@ -5,8 +5,11 @@ float64[7] q float64[7] q_d float64[7] dq float64[7] dq_d +float64[7] theta +float64[7] dtheta float64[7] tau_J float64[7] dtau_J +float64[7] tau_J_d float64[6] K_F_ext_hat_K float64[2] elbow float64[2] elbow_d