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..fd5c0caf336ff98174906dd942498551b5a7a77e 100644 --- a/franka_example_controllers/src/cartesian_impedance_example_controller.cpp +++ b/franka_example_controllers/src/cartesian_impedance_example_controller.cpp @@ -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..4b3f0cac9afe9304b11560590859ac7801c76f8b 100644 --- a/franka_example_controllers/src/force_example_controller.cpp +++ b/franka_example_controllers/src/force_example_controller.cpp @@ -117,7 +117,7 @@ void ForceExampleController::update(const ros::Time& /*time*/, const ros::Durati tau_error_ = tau_error_ + period.toSec() * (tau_d - tau_ext); // FF + PI control (PI gains are intially 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; }