diff --git a/include/ignition/gazebo/Link.hh b/include/ignition/gazebo/Link.hh index 9dd3d5ccc7..df967d30ca 100644 --- a/include/ignition/gazebo/Link.hh +++ b/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 diff --git a/src/Link.cc b/src/Link.cc index 0d07f17140..82d860fdb1 100644 --- a/src/Link.cc +++ b/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(this->dataPtr->id); + + if (vel == nullptr) + { + _ecm.CreateComponent( + this->dataPtr->id, + components::AngularVelocityCmd(_vel)); + } + else + { + vel->Data() = _vel; + } +} + ////////////////////////////////////////////////// std::optional Link::WorldAngularAcceleration( const EntityComponentManager &_ecm) const diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index 18df36ba58..bcd6146868 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -2150,7 +2150,6 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm) worldAngularVelFeature->SetWorldAngularVelocity( math::eigen3::convert(worldAngularVel)); - return true; }); diff --git a/src/systems/trajectory_follower/TrajectoryFollower.cc b/src/systems/trajectory_follower/TrajectoryFollower.cc index c48ba3d8f9..8366c6065d 100644 --- a/src/systems/trajectory_follower/TrajectoryFollower.cc +++ b/src/systems/trajectory_follower/TrajectoryFollower.cc @@ -33,6 +33,7 @@ #include #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("bearing_tolerance"); + // Parse the optional element. + if (_sdf->HasElement("zero_vel_on_bearing_reached")) + this->forceZeroAngVel = _sdf->Get("zero_vel_on_bearing_reached"); + // Parse the optional 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( + this->dataPtr->link.Entity()); + if (angVelCmdComp) + { + _ecm.RemoveComponent( + 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)); diff --git a/src/systems/trajectory_follower/TrajectoryFollower.hh b/src/systems/trajectory_follower/TrajectoryFollower.hh index c1d52aca06..57fbfcaae9 100644 --- a/src/systems/trajectory_follower/TrajectoryFollower.hh +++ b/src/systems/trajectory_follower/TrajectoryFollower.hh @@ -66,6 +66,11 @@ namespace systems /// : If the bearing to the current waypoint is within /// +- this tolerance, a torque won't be applied (degree) /// The default value is 2 deg. + /// : 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. /// : The force to apply at every plugin iteration in the X direction /// of the link (N). The default value is 60. /// : The torque to apply at every plugin iteration in the Yaw diff --git a/test/integration/link.cc b/test/integration/link.cc index 53a1dc9f7c..d6b70cccf0 100644 --- a/test/integration/link.cc +++ b/test/integration/link.cc @@ -22,6 +22,7 @@ #include #include +#include #include #include #include @@ -500,6 +501,23 @@ TEST_F(LinkIntegrationTest, LinkSetVelocity) link.SetLinearVelocity(ecm, linVel2); EXPECT_EQ(linVel2, ecm.Component(eLink)->Data()); + + // No AngularVelocityCmd should exist by default + EXPECT_EQ(nullptr, ecm.Component(eLink)); + + math::Vector3d angVel(0, 0, 1); + link.SetAngularVelocity(ecm, angVel); + + // AngularVelocity cmd should exist + EXPECT_NE(nullptr, ecm.Component(eLink)); + EXPECT_EQ(angVel, + ecm.Component(eLink)->Data()); + + // Make sure the angular velocity is updated + math::Vector3d angVel2(0, 0, 0); + link.SetAngularVelocity(ecm, angVel2); + EXPECT_EQ(angVel2, + ecm.Component(eLink)->Data()); } //////////////////////////////////////////////////