[ros2] Adding option to select the frame where the force will be applied

gazebo_plugins/include/gazebo_plugins/gazebo_ros_force.hpp

@@ -46,6 +46,9 @@ class GazeboRosForcePrivate;
       <!-- Name of link within model which will receive the force -->
       <link_name>link</link_name>
 
+      <!-- Frame where the force/torque will be applied (options: world; link)-->
+      <force_frame>link</force_frame>
+
     </plugin>
   \endcode
 */

gazebo_plugins/src/gazebo_ros_force.cpp

@@ -43,6 +43,9 @@ class GazeboRosForcePrivate
 
   // Pointer to the update event connection
   gazebo::event::ConnectionPtr update_connection_;
+
+  /// Indicates that the force should be applied on the world frame instead of the link frame
+  bool force_on_world_frame_;
 };
 
 GazeboRosForce::GazeboRosForce()
@@ -72,6 +75,23 @@ void GazeboRosForce::Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf)
     return;
   }
 
+  // Force frame
+  if (!sdf->HasElement("force_frame")) {
+    RCLCPP_INFO(logger, "Force plugin missing <force_frame> wasn't set,"
+      "therefore it's been set as 'world'. The other option is 'link'.");
+    impl_->force_on_world_frame_ = true;
+  } else {
+    auto force_frame = sdf->GetElement("force_frame")->Get<std::string>();
+    if (force_frame == "world") {
+      impl_->force_on_world_frame_ = true;
+    } else if (force_frame == "link") {
+      impl_->force_on_world_frame_ = false;
+    } else {
+      RCLCPP_ERROR(logger, "Force plugin <force_frame> can only be 'world' or 'link'");
+      return;
+    }
+  }
+
   // Subscribe to wrench messages
   impl_->ros_node_ = gazebo_ros::Node::Get(sdf);
 
@@ -96,8 +116,17 @@ void GazeboRosForce::OnRosWrenchMsg(const geometry_msgs::msg::Wrench::SharedPtr
 
 void GazeboRosForce::OnUpdate()
 {
-  impl_->link_->AddForce(gazebo_ros::Convert<ignition::math::Vector3d>(impl_->wrench_msg_.force));
-  impl_->link_->AddTorque(gazebo_ros::Convert<ignition::math::Vector3d>(impl_->wrench_msg_.torque));
+  if (impl_->force_on_world_frame_) {
+    impl_->link_->AddForce(
+      gazebo_ros::Convert<ignition::math::Vector3d>(impl_->wrench_msg_.force));
+    impl_->link_->AddTorque(
+      gazebo_ros::Convert<ignition::math::Vector3d>(impl_->wrench_msg_.torque));
+  } else {
+    impl_->link_->AddRelativeForce(
+      gazebo_ros::Convert<ignition::math::Vector3d>(impl_->wrench_msg_.force));
+    impl_->link_->AddRelativeTorque(
+      gazebo_ros::Convert<ignition::math::Vector3d>(impl_->wrench_msg_.torque));
+  }
 }
 
 GZ_REGISTER_MODEL_PLUGIN(GazeboRosForce)

gazebo_plugins/test/test_gazebo_ros_force.cpp

@@ -20,67 +20,95 @@
 
 #define tol 10e-10
 
+enum AXIS
+{
+  X, Y, Z
+};
+
 class GazeboRosForceTest : public gazebo::ServerFixture
 {
+public:
+  void ApplyForceTorque(AXIS force_axis, double init_rotation = 0)
+  {
+    // World
+    auto world = gazebo::physics::get_world();
+    ASSERT_NE(nullptr, world);
+
+    // Box
+    auto box = world->ModelByName("box");
+    ASSERT_NE(nullptr, box);
+
+    // Check plugin was loaded
+    world->Step(100);
+    EXPECT_EQ(1u, box->GetPluginCount());
+
+    // Check box is at world origin
+    EXPECT_NEAR(0.0, box->WorldPose().Pos().X(), tol);
+    EXPECT_NEAR(init_rotation, box->WorldPose().Rot().Yaw(), tol);
+
+    // Create ROS publisher
+    auto node = std::make_shared<rclcpp::Node>("gazebo_ros_force_test");
+    ASSERT_NE(nullptr, node);
+
+    auto pub = node->create_publisher<geometry_msgs::msg::Wrench>(
+      "test/force_test", rclcpp::QoS(rclcpp::KeepLast(1)));
+
+    // Wait for subscriber to come up
+    unsigned int sleep = 0;
+    unsigned int max_sleep = 30;
+    while (sleep < max_sleep && pub->get_subscription_count() == 0u) {
+      gazebo::common::Time::MSleep(100);
+      sleep++;
+    }
+    EXPECT_LT(0u, pub->get_subscription_count());
+    EXPECT_LT(sleep, max_sleep);
+
+    // Apply force
+    auto msg = geometry_msgs::msg::Wrench();
+    switch (force_axis) {
+      case AXIS::Y:
+        msg.force.y = 10.0;
+        break;
+      case AXIS::Z:
+        msg.force.z = 10.0;
+        break;
+      default:
+        msg.force.x = 10.0;
+        break;
+    }
+    msg.torque.z = 10.0;
+    pub->publish(msg);
+
+    // Wait until box moves
+    sleep = 0;
+    double target_dist{0.1};
+    while (sleep < max_sleep && box->WorldPose().Pos().X() < target_dist) {
+      world->Step(100);
+      gazebo::common::Time::MSleep(100);
+      sleep++;
+    }
+
+    // Check box moved
+    EXPECT_LT(sleep, max_sleep);
+    EXPECT_LT(target_dist, box->WorldPose().Pos().X());
+    EXPECT_LT(target_dist + init_rotation, box->WorldPose().Rot().Yaw());
+  }
 };
 
-TEST_F(GazeboRosForceTest, ApplyForceTorque)
+TEST_F(GazeboRosForceTest, ApplyForceTorqueWorld)
 {
   // Load test world and start paused
   this->Load("worlds/gazebo_ros_force.world", true);
 
-  // World
-  auto world = gazebo::physics::get_world();
-  ASSERT_NE(nullptr, world);
-
-  // Box
-  auto box = world->ModelByName("box");
-  ASSERT_NE(nullptr, box);
-
-  // Check plugin was loaded
-  world->Step(100);
-  EXPECT_EQ(1u, box->GetPluginCount());
-
-  // Check box is at world origin
-  EXPECT_NEAR(0.0, box->WorldPose().Pos().X(), tol);
-  EXPECT_NEAR(0.0, box->WorldPose().Rot().Z(), tol);
-
-  // Create ROS publisher
-  auto node = std::make_shared<rclcpp::Node>("gazebo_ros_force_test");
-  ASSERT_NE(nullptr, node);
-
-  auto pub = node->create_publisher<geometry_msgs::msg::Wrench>(
-    "test/force_test", rclcpp::QoS(rclcpp::KeepLast(1)));
+  this->ApplyForceTorque(AXIS::X);
+}
 
-  // Wait for subscriber to come up
-  unsigned int sleep = 0;
-  unsigned int max_sleep = 30;
-  while (sleep < max_sleep && pub->get_subscription_count() == 0u) {
-    gazebo::common::Time::MSleep(100);
-    sleep++;
-  }
-  EXPECT_LT(0u, pub->get_subscription_count());
-  EXPECT_LT(sleep, max_sleep);
-
-  // Apply force on X
-  auto msg = geometry_msgs::msg::Wrench();
-  msg.force.x = 10.0;
-  msg.torque.z = 10.0;
-  pub->publish(msg);
-
-  // Wait until box moves
-  sleep = 0;
-  double target_dist{0.1};
-  while (sleep < max_sleep && box->WorldPose().Pos().X() < target_dist) {
-    world->Step(100);
-    gazebo::common::Time::MSleep(100);
-    sleep++;
-  }
+TEST_F(GazeboRosForceTest, ApplyForceTorqueLink)
+{
+  // Load test world and start paused
+  this->Load("worlds/gazebo_ros_force_link.world", true);
 
-  // Check box moved
-  EXPECT_LT(sleep, max_sleep);
-  EXPECT_LT(target_dist, box->WorldPose().Pos().X());
-  EXPECT_LT(target_dist, box->WorldPose().Rot().Z());
+  this->ApplyForceTorque(AXIS::Y, -M_PI / 2);
 }
 
 int main(int argc, char ** argv)

gazebo_plugins/test/worlds/gazebo_ros_force.world

@@ -9,7 +9,7 @@
     </include>
     <model name="box">
       <pose>0 0 0.5 0 0 0</pose>
-      <link name="link">
+      <link name="box_link">
         <collision name="collision">
           <geometry>
             <box>
@@ -30,7 +30,8 @@
           <namespace>/test</namespace>
           <argument>gazebo_ros_force:=force_test</argument>
         </ros>
-        <link_name>link</link_name>
+        <link_name>box_link</link_name>
+        <force_frame>world</force_frame>
       </plugin>
     </model>
   </world>

gazebo_plugins/test/worlds/gazebo_ros_force_link.world

@@ -0,0 +1,38 @@
+<?xml version="1.0"?>
+<sdf version="1.6">
+  <world name="default">
+    <include>
+      <uri>model://ground_plane</uri>
+    </include>
+    <include>
+      <uri>model://sun</uri>
+    </include>
+    <model name="box">
+      <pose>0 0 0.5 0 0 -1.5707963267948966</pose>
+      <link name="box_link">
+        <collision name="collision">
+          <geometry>
+            <box>
+              <size>1 1 1</size>
+            </box>
+          </geometry>
+        </collision>
+        <visual name="visual">
+          <geometry>
+            <box>
+              <size>1 1 1</size>
+            </box>
+          </geometry>
+        </visual>
+      </link>
+      <plugin name="gazebo_ros_force" filename="libgazebo_ros_force.so">
+        <ros>
+          <namespace>/test</namespace>
+          <argument>gazebo_ros_force:=force_test</argument>
+        </ros>
+        <link_name>box_link</link_name>
+        <force_frame>link</force_frame>
+      </plugin>
+    </model>
+  </world>
+</sdf>

gazebo_plugins/worlds/gazebo_ros_force_demo.world

@@ -2,9 +2,18 @@
 <!--
   Gazebo ROS force plugin demo
 
+  If you want the force to be applied on the world frame, use
+    <force_frame>world</force_frame>
+  If you want the force to be applied on the link frame, use
+    <force_frame>link</force_frame>
+
   Try for example:
 
-  ros2 topic pub -1 /demo/force_demo geometry_msgs/Wrench "force: {x: 10.0}"
+  ros2 topic pub -1 /demo/world/force_demo geometry_msgs/Wrench "force: {x: 10.0}"
+
+  and
+
+  ros2 topic pub -1 /demo/link/force_demo geometry_msgs/Wrench "force: {x: 10.0}"
 -->
 <sdf version="1.6">
   <world name="default">
@@ -14,8 +23,35 @@
     <include>
       <uri>model://sun</uri>
     </include>
-    <model name="box">
-      <pose>0 0 0.5 0 0 0</pose>
+    <model name="force_on_world_frame">
+      <pose>0 3 0.5 0 0 1.57</pose>
+      <link name="link">
+        <collision name="collision">
+          <geometry>
+            <box>
+              <size>1 1 1</size>
+            </box>
+          </geometry>
+        </collision>
+        <visual name="visual">
+          <geometry>
+            <box>
+              <size>1 1 1</size>
+            </box>
+          </geometry>
+        </visual>
+      </link>
+      <plugin name="gazebo_ros_force" filename="libgazebo_ros_force.so">
+        <ros>
+          <namespace>/demo/world</namespace>
+          <argument>gazebo_ros_force:=force_demo</argument>
+        </ros>
+        <link_name>link</link_name>
+        <force_frame>world</force_frame>
+      </plugin>
+    </model>
+    <model name="force_on_link_frame">
+      <pose>0 -3 0.5 0 0 1.57</pose>
       <link name="link">
         <collision name="collision">
           <geometry>
@@ -34,10 +70,11 @@
       </link>
       <plugin name="gazebo_ros_force" filename="libgazebo_ros_force.so">
         <ros>
-          <namespace>/demo</namespace>
+          <namespace>/demo/link</namespace>
           <argument>gazebo_ros_force:=force_demo</argument>
         </ros>
         <link_name>link</link_name>
+        <force_frame>link</force_frame>
       </plugin>
     </model>
   </world>