diff --git a/CHANGELOG.md b/CHANGELOG.md index e9433a95d..e4770326d 100644 --- a/CHANGELOG.md +++ b/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 diff --git a/franka_gazebo/config/franka_hw_sim.yaml b/franka_gazebo/config/franka_hw_sim.yaml index fde8beaa5..6703a1682 100644 --- a/franka_gazebo/config/franka_hw_sim.yaml +++ b/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 diff --git a/franka_gazebo/include/franka_gazebo/franka_hw_sim.h b/franka_gazebo/include/franka_gazebo/franka_hw_sim.h index 8a85a07da..2cbd1fdd3 100644 --- a/franka_gazebo/include/franka_gazebo/franka_hw_sim.h +++ b/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 gravity_earth_; std::string arm_id_; @@ -104,6 +109,8 @@ class FrankaHWSim : public gazebo_ros_control::RobotHWSim { franka::RobotState robot_state_; std::unique_ptr model_; + double tau_ext_lowpass_filter_; + ros::ServiceServer service_set_ee_; ros::ServiceServer service_set_k_; ros::ServiceServer service_set_load_; diff --git a/franka_gazebo/include/franka_gazebo/joint.h b/franka_gazebo/include/franka_gazebo/joint.h index 92ae7337c..be8afcf3c 100644 --- a/franka_gazebo/include/franka_gazebo/joint.h +++ b/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; diff --git a/franka_gazebo/package.xml b/franka_gazebo/package.xml index e98826a70..02a3a8d7f 100644 --- a/franka_gazebo/package.xml +++ b/franka_gazebo/package.xml @@ -40,6 +40,7 @@ gtest rostest sensor_msgs + geometry_msgs diff --git a/franka_gazebo/src/franka_hw_sim.cpp b/franka_gazebo/src/franka_hw_sim.cpp index 723ea4891..d34d344fe 100644 --- a/franka_gazebo/src/franka_hw_sim.cpp +++ b/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("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(joint->isInContact()); this->robot_state_.joint_collision[i] = static_cast(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 diff --git a/franka_gazebo/test/CMakeLists.txt b/franka_gazebo/test/CMakeLists.txt index e9e778a39..8ffb29eb5 100644 --- a/franka_gazebo/test/CMakeLists.txt +++ b/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} -) \ No newline at end of file +) diff --git a/franka_gazebo/test/franka_hw_sim_gazebo_test.cpp b/franka_gazebo/test/franka_hw_sim_gazebo_test.cpp new file mode 100644 index 000000000..10600bd9c --- /dev/null +++ b/franka_gazebo/test/franka_hw_sim_gazebo_test.cpp @@ -0,0 +1,33 @@ +#include +#include +#include + +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( + "/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(); +} diff --git a/franka_gazebo/test/launch/franka_hw_sim_gazebo.test b/franka_gazebo/test/launch/franka_hw_sim_gazebo.test new file mode 100644 index 000000000..7edc56c4f --- /dev/null +++ b/franka_gazebo/test/launch/franka_hw_sim_gazebo.test @@ -0,0 +1,12 @@ + + + + + + + + + + + +