Update Force Example Controller.

franka_example_controllers/cfg/desired_mass_param.cfg

@@ -5,8 +5,8 @@ from dynamic_reconfigure.parameter_generator_catkin import *
 
 gen = ParameterGenerator()
 
-gen.add("desired_mass", double_t, 0, "desired mass for rendered force due to gravity applied in the z axis", 0.0,  0.0, 5.0)
-gen.add("k_p", double_t, 0, "force P gain", 0.0,  0.0, 20.0)
-gen.add("k_i", double_t, 0, "force I gain", 0.0,  0.0, 20.0)
+gen.add("desired_mass", double_t, 0, "desired mass for rendered force due to gravity applied in the z axis", 0.0,  0.0, 2.0)
+gen.add("k_p", double_t, 0, "force P gain", 0.0,  0.0, 2.0)
+gen.add("k_i", double_t, 0, "force I gain", 0.0,  0.0, 2.0)
 
 exit(gen.generate(PACKAGE, "dynamic_mass", "desired_mass_param"))

franka_example_controllers/include/franka_example_controllers/force_example_controller.h

@@ -30,9 +30,16 @@ class ForceExampleController : public controller_interface::MultiInterfaceContro
   void update(const ros::Time&, const ros::Duration& period) override;
 
  private:
+  // 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);
+
   std::unique_ptr<franka_hw::FrankaModelHandle> model_handle_;
   std::unique_ptr<franka_hw::FrankaStateHandle> state_handle_;
   std::vector<hardware_interface::JointHandle> joint_handles_;
+
   double desired_mass_{0.0};
   double target_mass_{0.0};
   double k_p_{0.0};
@@ -42,6 +49,7 @@ class ForceExampleController : public controller_interface::MultiInterfaceContro
   double filter_gain_{0.001};
   Eigen::Matrix<double, 7, 1> tau_ext_initial_;
   Eigen::Matrix<double, 7, 1> tau_error_;
+  const double delta_tau_max_{1.0};
 
   // Dynamic reconfigure
   std::unique_ptr<dynamic_reconfigure::Server<franka_example_controllers::desired_mass_paramConfig>>

franka_example_controllers/src/force_example_controller.cpp

@@ -89,7 +89,7 @@ bool ForceExampleController::init(hardware_interface::RobotHW* robot_hw,
 void ForceExampleController::starting(const ros::Time& /*time*/) {
   franka::RobotState robot_state = state_handle_->getRobotState();
   std::array<double, 7> gravity_array =
-      model_handle_->getGravity(robot_state, 0.0, {{0.0, 0.0, 0.0}});
+      model_handle_->getGravity(robot_state, robot_state.m_total, robot_state.F_x_Ctotal);
   Eigen::Map<Eigen::Matrix<double, 7, 1> > tau_measured(robot_state.tau_J.data());
   Eigen::Map<Eigen::Matrix<double, 7, 1> > gravity(gravity_array.data());
   // Bias correction for the current external torque
@@ -102,9 +102,11 @@ void ForceExampleController::update(const ros::Time& /*time*/, const ros::Durati
   std::array<double, 42> jacobian_array =
       model_handle_->getZeroJacobian(franka::Frame::kEndEffector, robot_state);
   std::array<double, 7> gravity_array =
-      model_handle_->getGravity(robot_state, 0.0, {{0.0, 0.0, 0.0}});
+      model_handle_->getGravity(robot_state, robot_state.m_total, robot_state.F_x_Ctotal);
   Eigen::Map<Eigen::Matrix<double, 6, 7> > jacobian(jacobian_array.data());
   Eigen::Map<Eigen::Matrix<double, 7, 1> > tau_measured(robot_state.tau_J.data());
+  Eigen::Map<Eigen::Matrix<double, 7, 1> > tau_J_d(  // NOLINT (readability-identifier-naming)
+      robot_state.tau_J_d.data());
   Eigen::Map<Eigen::Matrix<double, 7, 1> > gravity(gravity_array.data());
 
   Eigen::VectorXd tau_d(7), desired_force_torque(6), tau_cmd(7), tau_ext(7);
@@ -115,6 +117,8 @@ 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);
+
   for (size_t i = 0; i < 7; ++i) {
     joint_handles_[i].setCommand(tau_cmd(i));
   }
@@ -133,6 +137,20 @@ void ForceExampleController::desiredMassParamCallback(
   target_k_i_ = config.k_i;
 }
 
+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) {
+  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, delta_tau_max_), -delta_tau_max_);
+  }
+  return tau_d_saturated;
+}
+
 }  // namespace franka_example_controllers
 
 PLUGINLIB_EXPORT_CLASS(franka_example_controllers::ForceExampleController,