Add parameter to TrajectoryFollower stop rotation when bearing is reached

include/ignition/gazebo/Link.hh

@@ -227,6 +227,14 @@ namespace ignition
       public: void SetLinearVelocity(EntityComponentManager &_ecm,
           const math::Vector3d &_vel) const;
 
+
+      /// \brief Set the angular velocity on this link. If this is set, wrenches
+      /// on this link will be ignored for the current time step.
+      /// \param[in] _ecm Entity Component manager.
+      /// \param[in] _vel Angular velocity to set in Link's Frame.
+      public: void SetAngularVelocity(EntityComponentManager &_ecm,
+          const math::Vector3d &_vel) const;
+
       /// \brief Get the angular acceleration of the body in the world frame.
       /// \param[in] _ecm Entity-component manager.
       /// \return Angular acceleration of the body in the world frame or

src/Link.cc

@@ -19,6 +19,7 @@
 
 #include "ignition/gazebo/components/AngularAcceleration.hh"
 #include "ignition/gazebo/components/AngularVelocity.hh"
+#include "ignition/gazebo/components/AngularVelocityCmd.hh"
 #include "ignition/gazebo/components/CanonicalLink.hh"
 #include "ignition/gazebo/components/Collision.hh"
 #include "ignition/gazebo/components/ExternalWorldWrenchCmd.hh"
@@ -266,6 +267,25 @@ void Link::SetLinearVelocity(EntityComponentManager &_ecm,
     }
 }
 
+//////////////////////////////////////////////////
+void Link::SetAngularVelocity(EntityComponentManager &_ecm,
+  const math::Vector3d &_vel) const
+{
+    auto vel =
+      _ecm.Component<components::AngularVelocityCmd>(this->dataPtr->id);
+
+    if (vel == nullptr)
+    {
+      _ecm.CreateComponent(
+          this->dataPtr->id,
+          components::AngularVelocityCmd(_vel));
+    }
+    else
+    {
+      vel->Data() = _vel;
+    }
+}
+
 //////////////////////////////////////////////////
 std::optional<math::Vector3d> Link::WorldAngularAcceleration(
     const EntityComponentManager &_ecm) const

src/systems/physics/Physics.cc

@@ -2150,7 +2150,6 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm)
 
         worldAngularVelFeature->SetWorldAngularVelocity(
             math::eigen3::convert(worldAngularVel));
-
         return true;
       });
 

src/systems/trajectory_follower/TrajectoryFollower.cc

@@ -33,6 +33,7 @@
 #include <sdf/sdf.hh>
 
 #include "ignition/gazebo/components/Pose.hh"
+#include "ignition/gazebo/components/AngularVelocityCmd.hh"
 #include "ignition/gazebo/Link.hh"
 #include "ignition/gazebo/Model.hh"
 #include "ignition/gazebo/Util.hh"
@@ -109,6 +110,12 @@ class ignition::gazebo::systems::TrajectoryFollowerPrivate
 
   /// \brief Whether the trajectory follower behavior should be paused or not.
   public: bool paused = false;
+
+  /// \brief Angular velocity set to zero
+  public: bool zeroAngVelSet = false;
+
+  /// \brief Force angular velocity to be zero when bearing is reached
+  public: bool forceZeroAngVel = false;
 };
 
 //////////////////////////////////////////////////
@@ -249,6 +256,10 @@ void TrajectoryFollowerPrivate::Load(const EntityComponentManager &_ecm,
   if (_sdf->HasElement("bearing_tolerance"))
     this->bearingTolerance = _sdf->Get<double>("bearing_tolerance");
 
+  // Parse the optional <zero_vel_on_bearing_reached> element.
+  if (_sdf->HasElement("zero_vel_on_bearing_reached"))
+    this->forceZeroAngVel = _sdf->Get<bool>("zero_vel_on_bearing_reached");
+
   // Parse the optional <topic> element.
   this->topic = "/model/" + this->model.Name(_ecm) +
     "/trajectory_follower/pause";
@@ -376,11 +387,33 @@ void TrajectoryFollower::PreUpdate(
   {
     forceWorld = (*comPose).Rot().RotateVector(
       ignition::math::Vector3d(this->dataPtr->forceToApply, 0, 0));
-  }
 
+    // force angular velocity to be zero when bearing is reached
+    if (this->dataPtr->forceZeroAngVel && !this->dataPtr->zeroAngVelSet &&
+        math::equal (std::abs(bearing.Degree()), 0.0,
+        this->dataPtr->bearingTolerance * 0.5))
+    {
+      this->dataPtr->link.SetAngularVelocity(_ecm, math::Vector3d::Zero);
+      this->dataPtr->zeroAngVelSet = true;
+    }
+  }
   ignition::math::Vector3d torqueWorld;
   if (std::abs(bearing.Degree()) > this->dataPtr->bearingTolerance)
   {
+    // remove angular velocity component otherwise the physics system will set
+    // the zero ang vel command every iteration
+    if (this->dataPtr->forceZeroAngVel && this->dataPtr->zeroAngVelSet)
+    {
+      auto angVelCmdComp = _ecm.Component<components::AngularVelocityCmd>(
+          this->dataPtr->link.Entity());
+      if (angVelCmdComp)
+      {
+        _ecm.RemoveComponent<components::AngularVelocityCmd>(
+          this->dataPtr->link.Entity());
+        this->dataPtr->zeroAngVelSet = false;
+      }
+    }
+
     int sign = std::abs(bearing.Degree()) / bearing.Degree();
     torqueWorld = (*comPose).Rot().RotateVector(
        ignition::math::Vector3d(0, 0, sign * this->dataPtr->torqueToApply));

src/systems/trajectory_follower/TrajectoryFollower.hh

@@ -66,6 +66,11 @@ namespace systems
   /// <bearing_tolerance>: If the bearing to the current waypoint is within
   ///                      +- this tolerance, a torque won't be applied (degree)
   ///                      The default value is 2 deg.
+  /// <zero_vel_on_bearing_reached>: Force angular velocity to be zero when
+  ///                                target bearing is reached.
+  ///                                Default is false.
+  ///                                Note: this is an experimental parameter
+  ///                                and may be removed in the future.
   /// <force>: The force to apply at every plugin iteration in the X direction
   ///          of the link (N). The default value is 60.
   /// <torque>: The torque to apply at every plugin iteration in the Yaw

test/integration/link.cc

@@ -22,6 +22,7 @@
 
 #include <ignition/gazebo/components/AngularAcceleration.hh>
 #include <ignition/gazebo/components/AngularVelocity.hh>
+#include <ignition/gazebo/components/AngularVelocityCmd.hh>
 #include <ignition/gazebo/components/CanonicalLink.hh>
 #include <ignition/gazebo/components/Collision.hh>
 #include <ignition/gazebo/components/ExternalWorldWrenchCmd.hh>
@@ -500,6 +501,23 @@ TEST_F(LinkIntegrationTest, LinkSetVelocity)
   link.SetLinearVelocity(ecm, linVel2);
   EXPECT_EQ(linVel2,
     ecm.Component<components::LinearVelocityCmd>(eLink)->Data());
+
+  // No AngularVelocityCmd should exist by default
+  EXPECT_EQ(nullptr, ecm.Component<components::AngularVelocityCmd>(eLink));
+
+  math::Vector3d angVel(0, 0, 1);
+  link.SetAngularVelocity(ecm, angVel);
+
+  // AngularVelocity cmd should exist
+  EXPECT_NE(nullptr, ecm.Component<components::AngularVelocityCmd>(eLink));
+  EXPECT_EQ(angVel,
+    ecm.Component<components::AngularVelocityCmd>(eLink)->Data());
+
+  // Make sure the angular velocity is updated
+  math::Vector3d angVel2(0, 0, 0);
+  link.SetAngularVelocity(ecm, angVel2);
+  EXPECT_EQ(angVel2,
+    ecm.Component<components::AngularVelocityCmd>(eLink)->Data());
 }
 
 //////////////////////////////////////////////////