fix gravity compensation in external torque computation

CHANGELOG.md

@@ -20,6 +20,7 @@
 * Improve Gazebo 'stone' world objects
 * Add `connected_to` option to `panda_gazebo.xacro` macro, similar to `panda_arm.xacro`
 * Rename `ns` -> `arm_id` in `hand.xacro` macros to be consistent with the other xacro files
+* Introduce new `tau_ext_lowpass_filter` parameter for `franka_gazebo` to configure the filtering of `tau_ext_hat_filtered`
 
 ## 0.8.1 - 2021-09-08
 

franka_gazebo/config/franka_hw_sim.yaml

@@ -1,5 +1,6 @@
 arm_id:                           $(arg arm_id)
 singularity_warning_threshold:    0.0001               # print a warning if the smallest singular value of J x J^T drops below this value (use -1 to disable)
+tau_ext_lowpass_filter:           1.0                  # Exponential Moving average filter: range between and zero (infinite delay) one (no filtering)
 
 franka_gripper:
   type:    franka_gazebo/FrankaGripperSim

franka_gazebo/include/franka_gazebo/franka_hw_sim.h

@@ -20,6 +20,8 @@
 
 namespace franka_gazebo {
 
+const double kDefaultTauExtLowpassFilter = 1.0;  // no filtering per default of tau_ext_hat_filtered
+
 /**
  * A custom implementation of a [gazebo_ros_control](http://wiki.ros.org/gazebo_ros_control) plugin,
  * which is able to simulate franka interfaces in Gazebo.
@@ -90,6 +92,9 @@ class FrankaHWSim : public gazebo_ros_control::RobotHWSim {
   void eStopActive(const bool active) override;
 
  private:
+  /// If gazebo::Joint::GetForceTorque() yielded already a non-zero value
+  bool efforts_initialized_;
+
   std::array<double, 3> gravity_earth_;
 
   std::string arm_id_;
@@ -104,6 +109,8 @@ class FrankaHWSim : public gazebo_ros_control::RobotHWSim {
   franka::RobotState robot_state_;
   std::unique_ptr<franka_hw::ModelBase> model_;
 
+  double tau_ext_lowpass_filter_;
+
   ros::ServiceServer service_set_ee_;
   ros::ServiceServer service_set_k_;
   ros::ServiceServer service_set_load_;

franka_gazebo/include/franka_gazebo/joint.h

@@ -38,9 +38,13 @@ struct Joint {
   /// The axis of rotation/translation of this joint in local coordinates
   Eigen::Vector3d axis;
 
-  /// The currently applied command from an actuator on this joint either in \f$N\f$ or \f$Nm\f$
+  /// The currently applied command from a controller acting on this joint either in \f$N\f$ or
+  /// \f$Nm\f$ without gravity
   double command = 0;
 
+  /// The currently acting gravity force or torque acting on this joint in \f$N\f$ or \f$Nm\f$
+  double gravity = 0;
+
   /// The current position of this joint either in \f$m\f$ or \f$rad\f$
   double position = 0;
 

franka_gazebo/package.xml

@@ -40,6 +40,7 @@
   <test_depend>gtest</test_depend>
   <test_depend>rostest</test_depend>
   <test_depend>sensor_msgs</test_depend>
+  <test_depend>geometry_msgs</test_depend>
 
   <export>
     <controller_interface plugin="${prefix}/franka_gripper_sim_plugin.xml"/>

franka_gazebo/src/franka_hw_sim.cpp

@@ -32,6 +32,7 @@ bool FrankaHWSim::initSim(const std::string& robot_namespace,
   }
 
   this->robot_ = parent;
+  this->efforts_initialized_ = false;
 
 #if GAZEBO_MAJOR_VERSION >= 8
   gazebo::physics::PhysicsEnginePtr physics = gazebo::physics::get_world()->Physics();
@@ -46,6 +47,9 @@ bool FrankaHWSim::initSim(const std::string& robot_namespace,
   auto gravity = physics->World()->Gravity();
   this->gravity_earth_ = {gravity.X(), gravity.Y(), gravity.Z()};
 
+  model_nh.param<double>("tau_ext_lowpass_filter", this->tau_ext_lowpass_filter_,
+                         kDefaultTauExtLowpassFilter);
+
   // Generate a list of franka_gazebo::Joint to store all relevant information
   for (const auto& transmission : transmissions) {
     if (transmission.type_ != "transmission_interface/SimpleTransmission") {
@@ -316,15 +320,16 @@ void FrankaHWSim::writeSim(ros::Time /*time*/, ros::Duration /*period*/) {
 
   for (auto& pair : this->joints_) {
     auto joint = pair.second;
-    auto command = joint->command;
 
     // Check if this joint is affected by gravity compensation
     std::string prefix = this->arm_id_ + "_joint";
     if (pair.first.rfind(prefix, 0) != std::string::npos) {
       int i = std::stoi(pair.first.substr(prefix.size())) - 1;
-      command += g.at(i);
+      joint->gravity = g.at(i);
     }
 
+    auto command = joint->command + joint->gravity;
+
     if (std::isnan(command)) {
       ROS_WARN_STREAM_NAMED("franka_hw_sim",
                             "Command for " << joint->name << "is NaN, won't send to robot");
@@ -475,16 +480,25 @@ void FrankaHWSim::updateRobotState(ros::Time time) {
     this->robot_state_.tau_J[i] = joint->effort;
     this->robot_state_.dtau_J[i] = joint->jerk;
 
-    this->robot_state_.q_d[i] = joint->position;
-    this->robot_state_.dq_d[i] = joint->velocity;
-    this->robot_state_.ddq_d[i] = joint->acceleration;
+    // since we don't support position or velocity interface yet, we set the desired joint
+    // trajectory to zero indicating we are in torque control mode
+    this->robot_state_.q_d[i] = joint->position;  // special case for resetting motion generators
+    this->robot_state_.dq_d[i] = 0;
+    this->robot_state_.ddq_d[i] = 0;
     this->robot_state_.tau_J_d[i] = joint->command;
 
     // For now we assume no flexible joints
     this->robot_state_.theta[i] = joint->position;
     this->robot_state_.dtheta[i] = joint->velocity;
 
-    this->robot_state_.tau_ext_hat_filtered[i] = joint->effort - joint->command;
+    if (this->efforts_initialized_) {
+      double tau_ext = joint->effort - joint->command + joint->gravity;
+
+      // Exponential moving average filter from tau_ext -> tau_ext_hat_filtered
+      this->robot_state_.tau_ext_hat_filtered[i] =
+          this->tau_ext_lowpass_filter_ * tau_ext +
+          (1 - this->tau_ext_lowpass_filter_) * this->robot_state_.tau_ext_hat_filtered[i];
+    }
 
     this->robot_state_.joint_contact[i] = static_cast<double>(joint->isInContact());
     this->robot_state_.joint_collision[i] = static_cast<double>(joint->isInCollision());
@@ -518,6 +532,8 @@ void FrankaHWSim::updateRobotState(ros::Time time) {
   this->robot_state_.control_command_success_rate = 1.0;
   this->robot_state_.time = franka::Duration(time.toNSec() / 1e6 /*ms*/);
   this->robot_state_.O_T_EE = this->model_->pose(franka::Frame::kEndEffector, this->robot_state_);
+
+  this->efforts_initialized_ = true;
 }
 
 }  // namespace franka_gazebo

franka_gazebo/test/CMakeLists.txt

@@ -22,6 +22,25 @@ target_include_directories(franka_hw_sim_test PUBLIC
   ${catkin_INCLUDE_DIRS}
 )
 
+add_rostest_gtest(franka_hw_sim_gazebo_test
+  launch/franka_hw_sim_gazebo.test
+  franka_hw_sim_gazebo_test.cpp
+)
+
+add_dependencies(franka_hw_sim_gazebo_test
+  ${${PROJECT_NAME}_EXPORTED_TARGETS}
+  ${catkin_EXPORTED_TARGETS}
+)
+
+target_link_libraries(franka_hw_sim_gazebo_test
+  ${CATKIN_LIBRARIES}
+  franka_hw_sim
+)
+
+target_include_directories(franka_hw_sim_gazebo_test PUBLIC
+  ${catkin_INCLUDE_DIRS}
+)
+
 add_rostest_gtest(franka_gripper_sim_test launch/franka_gripper_sim.test
   franka_gripper_sim_test.cpp
   gripper_sim_test_setup.cpp
@@ -61,4 +80,4 @@ target_link_libraries(franka_gripper_sim_test_with_object
 
 target_include_directories(franka_gripper_sim_test_with_object PUBLIC
   ${catkin_INCLUDE_DIRS}
-)
+)

franka_gazebo/test/franka_hw_sim_gazebo_test.cpp

@@ -0,0 +1,33 @@
+#include <geometry_msgs/WrenchStamped.h>
+#include <gtest/gtest.h>
+#include <ros/ros.h>
+
+TEST(
+#if ROS_VERSION_MINIMUM(1, 15, 13)
+    TestSuite, /* noetic & newer */
+#else
+    DISABLED_TestSuite, /* melodic */
+#endif
+    franka_hw_sim_compensates_gravity_on_F_ext) {
+  ros::NodeHandle n;
+
+  for (int i = 0; i < 50; i++) {
+    auto msg = ros::topic::waitForMessage<geometry_msgs::WrenchStamped>(
+        "/franka_state_controller/F_ext", n);
+
+    auto now = msg->header.stamp;
+
+    EXPECT_LE(std::abs(msg->wrench.force.x), 0.5) << "During time: " << now;
+    EXPECT_LE(std::abs(msg->wrench.force.y), 0.5) << "During time: " << now;
+    EXPECT_LE(std::abs(msg->wrench.force.z), 0.5) << "During time: " << now;
+    EXPECT_LE(std::abs(msg->wrench.torque.x), 0.25) << "During time: " << now;
+    EXPECT_LE(std::abs(msg->wrench.torque.y), 0.25) << "During time: " << now;
+    EXPECT_LE(std::abs(msg->wrench.torque.z), 0.25) << "During time: " << now;
+  }
+}
+
+int main(int argc, char** argv) {
+  testing::InitGoogleTest(&argc, argv);
+  ros::init(argc, argv, "franka_hw_sim_test");
+  return RUN_ALL_TESTS();
+}

franka_gazebo/test/launch/franka_hw_sim_gazebo.test

@@ -0,0 +1,12 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="debug" default="false" doc="Bringup Gazebo GUI during the test?" />
+  <include file="$(find franka_gazebo)/launch/panda.launch">
+    <arg name="controller" value="cartesian_impedance_example_controller"/>
+    <arg name="arm_id" value="panda"/>
+    <arg name="x" value="-0.5"/>
+    <arg name="world" value="$(find franka_gazebo)/world/stone.sdf"/>
+    <arg name="headless" value="$(eval not arg('debug'))"/>
+  </include>
+  <test test-name="franka_hw_sim_gazebo_test" pkg="franka_gazebo" type="franka_hw_sim_gazebo_test" time-limit="600.0"/>
+</launch>