diff --git a/franka_example_controllers/src/cartesian_impedance_example_controller.cpp b/franka_example_controllers/src/cartesian_impedance_example_controller.cpp index 81e78c27444cb172033b73c725973771806d31e2..8371b712e13542f14e783ed6fbacaf1407c6b9d5 100644 --- a/franka_example_controllers/src/cartesian_impedance_example_controller.cpp +++ b/franka_example_controllers/src/cartesian_impedance_example_controller.cpp @@ -148,7 +148,6 @@ void CartesianImpedanceExampleController::update(const ros::Time& /*time*/, 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()); - Eigen::Map<Eigen::Matrix<double, 7, 1> > tau_ext(robot_state.tau_ext_hat_filtered.data()); Eigen::Map<Eigen::Matrix<double, 7, 1> > tau_J_d( // NOLINT (readability-identifier-naming) robot_state.tau_J_d.data()); Eigen::Affine3d transform(Eigen::Matrix4d::Map(robot_state.O_T_EE.data())); @@ -186,7 +185,7 @@ void CartesianImpedanceExampleController::update(const ros::Time& /*time*/, (nullspace_stiffness_ * (q_d_nullspace_ - q) - (2.0 * sqrt(nullspace_stiffness_)) * dq); // Desired torque - tau_d << tau_task + tau_nullspace + coriolis - tau_ext; + tau_d << tau_task + tau_nullspace + coriolis; // Saturate torque rate to avoid discontinuities tau_d << saturateTorqueRate(tau_d, tau_J_d, gravity); for (size_t i = 0; i < 7; ++i) {