From c2b98dd0da50063d103bdb95c98a993bcdf95b92 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Fri, 10 Nov 2023 11:53:22 -0800 Subject: [PATCH 1/3] velocity_control: set model velocity cmds directly Refactor to set model linear and angular velocity commands directly in the OnCmdVel callback. Remove the targetVel member variable and UpdateVelocity method. Signed-off-by: Steve Peters --- .../velocity_control/VelocityControl.cc | 31 ++----------------- 1 file changed, 2 insertions(+), 29 deletions(-) diff --git a/src/systems/velocity_control/VelocityControl.cc b/src/systems/velocity_control/VelocityControl.cc index 75f8111962..cbbe839bb3 100644 --- a/src/systems/velocity_control/VelocityControl.cc +++ b/src/systems/velocity_control/VelocityControl.cc @@ -51,13 +51,6 @@ class gz::sim::systems::VelocityControlPrivate public: void OnLinkCmdVel(const msgs::Twist &_msg, const transport::MessageInfo &_info); - /// \brief Update the linear and angular velocities. - /// \param[in] _info System update information. - /// \param[in] _ecm The EntityComponentManager of the given simulation - /// instance. - public: void UpdateVelocity(const UpdateInfo &_info, - const EntityComponentManager &_ecm); - /// \brief Update link velocity. /// \param[in] _info System update information. /// \param[in] _ecm The EntityComponentManager of the given simulation @@ -77,9 +70,6 @@ class gz::sim::systems::VelocityControlPrivate /// \brief Linear velocity of a model public: math::Vector3d linearVelocity{0, 0, 0}; - /// \brief Last target velocity requested. - public: msgs::Twist targetVel; - /// \brief A mutex to protect the model velocity command. public: std::mutex mutex; @@ -123,8 +113,6 @@ void VelocityControl::Configure(const Entity &_entity, if (_sdf->HasElement("initial_linear")) { this->dataPtr->linearVelocity = _sdf->Get("initial_linear"); - msgs::Set(this->dataPtr->targetVel.mutable_linear(), - this->dataPtr->linearVelocity); gzmsg << "Linear velocity initialized to [" << this->dataPtr->linearVelocity << "]" << std::endl; } @@ -133,8 +121,6 @@ void VelocityControl::Configure(const Entity &_entity, { this->dataPtr->angularVelocity = _sdf->Get("initial_angular"); - msgs::Set(this->dataPtr->targetVel.mutable_angular(), - this->dataPtr->angularVelocity); gzmsg << "Angular velocity initialized to [" << this->dataPtr->angularVelocity << "]" << std::endl; } @@ -275,24 +261,10 @@ void VelocityControl::PostUpdate(const UpdateInfo &_info, if (_info.paused) return; - // update model velocities - this->dataPtr->UpdateVelocity(_info, _ecm); // update link velocities this->dataPtr->UpdateLinkVelocity(_info, _ecm); } -////////////////////////////////////////////////// -void VelocityControlPrivate::UpdateVelocity( - const UpdateInfo &/*_info*/, - const EntityComponentManager &/*_ecm*/) -{ - GZ_PROFILE("VeocityControl::UpdateVelocity"); - - std::lock_guard lock(this->mutex); - this->linearVelocity = msgs::Convert(this->targetVel.linear()); - this->angularVelocity = msgs::Convert(this->targetVel.angular()); -} - ////////////////////////////////////////////////// void VelocityControlPrivate::UpdateLinkVelocity( const UpdateInfo &/*_info*/, @@ -315,7 +287,8 @@ void VelocityControlPrivate::UpdateLinkVelocity( void VelocityControlPrivate::OnCmdVel(const msgs::Twist &_msg) { std::lock_guard lock(this->mutex); - this->targetVel = _msg; + this->linearVelocity = msgs::Convert(_msg.linear()); + this->angularVelocity = msgs::Convert(_msg.angular()); } ////////////////////////////////////////////////// From 607004a6664b60fa13f0310aa1eb0520fbb194ca Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Tue, 14 Nov 2023 22:15:41 -0800 Subject: [PATCH 2/3] Support non-persistent initial velocity Currently the velocity_control system commands velocities in a persistent manner. This change adds a @persistent attribute (default true) to the //initial_linear and //initial_angular XML tags of the velocity_control system. By setting @persistent=false, the initial_* values will correspond to initial velocities that may change over time. Any twist values published to the cmd_vel topic will still be interpreted as persistent velocity commands. Signed-off-by: Steve Peters --- .../velocity_control/VelocityControl.cc | 68 +++++++++++++++---- 1 file changed, 53 insertions(+), 15 deletions(-) diff --git a/src/systems/velocity_control/VelocityControl.cc b/src/systems/velocity_control/VelocityControl.cc index cbbe839bb3..f679a4459d 100644 --- a/src/systems/velocity_control/VelocityControl.cc +++ b/src/systems/velocity_control/VelocityControl.cc @@ -19,6 +19,7 @@ #include #include +#include #include #include #include @@ -64,11 +65,19 @@ class gz::sim::systems::VelocityControlPrivate /// \brief Model interface public: Model model{kNullEntity}; - /// \brief Angular velocity of a model - public: math::Vector3d angularVelocity{0, 0, 0}; + /// \brief Model angular velocity command, initialized to zero. + public: std::optional angularVelocity = math::Vector3d::Zero; - /// \brief Linear velocity of a model - public: math::Vector3d linearVelocity{0, 0, 0}; + /// \brief Flag indicating whether the model angular velocity command should + /// persist across multiple time steps. + public: bool persistentAngularVelocity = true; + + /// \brief Model linear velocity command, initialized to zero. + public: std::optional linearVelocity = math::Vector3d::Zero; + + /// \brief Flag indicating whether the model linear velocity command should + /// persist across multiple time steps. + public: bool persistentLinearVelocity = true; /// \brief A mutex to protect the model velocity command. public: std::mutex mutex; @@ -112,17 +121,26 @@ void VelocityControl::Configure(const Entity &_entity, if (_sdf->HasElement("initial_linear")) { - this->dataPtr->linearVelocity = _sdf->Get("initial_linear"); + auto elem = _sdf->FindElement("initial_linear"); + this->dataPtr->linearVelocity = elem->Get(); gzmsg << "Linear velocity initialized to [" - << this->dataPtr->linearVelocity << "]" << std::endl; + << *this->dataPtr->linearVelocity << "]" << std::endl; + if (elem->HasAttribute("persistent")) + { + this->dataPtr->persistentLinearVelocity = elem->Get("persistent"); + } } if (_sdf->HasElement("initial_angular")) { - this->dataPtr->angularVelocity = - _sdf->Get("initial_angular"); + auto elem = _sdf->FindElement("initial_angular"); + this->dataPtr->angularVelocity = elem->Get(); gzmsg << "Angular velocity initialized to [" - << this->dataPtr->angularVelocity << "]" << std::endl; + << *this->dataPtr->angularVelocity << "]" << std::endl; + if (elem->HasAttribute("persistent")) + { + this->dataPtr->persistentAngularVelocity = elem->Get("persistent"); + } } // Subscribe to model commands @@ -186,13 +204,31 @@ void VelocityControl::PreUpdate(const UpdateInfo &_info, if (_info.paused) return; - // update angular velocity of model - _ecm.SetComponentData( - this->dataPtr->model.Entity(), {this->dataPtr->angularVelocity}); + { + std::lock_guard lock(this->dataPtr->mutex); - // update linear velocity of model - _ecm.SetComponentData( - this->dataPtr->model.Entity(), {this->dataPtr->linearVelocity}); + // update angular velocity of model + if (this->dataPtr->angularVelocity) + { + _ecm.SetComponentData( + this->dataPtr->model.Entity(), {*this->dataPtr->angularVelocity}); + if (!this->dataPtr->persistentAngularVelocity) + { + this->dataPtr->angularVelocity.reset(); + } + } + + // update linear velocity of model + if (this->dataPtr->linearVelocity) + { + _ecm.SetComponentData( + this->dataPtr->model.Entity(), {*this->dataPtr->linearVelocity}); + if (!this->dataPtr->persistentLinearVelocity) + { + this->dataPtr->linearVelocity.reset(); + } + } + } // If there are links, create link components // If the link hasn't been identified yet, look for it @@ -289,6 +325,8 @@ void VelocityControlPrivate::OnCmdVel(const msgs::Twist &_msg) std::lock_guard lock(this->mutex); this->linearVelocity = msgs::Convert(_msg.linear()); this->angularVelocity = msgs::Convert(_msg.angular()); + this->persistentLinearVelocity = true; + this->persistentAngularVelocity = true; } ////////////////////////////////////////////////// From fdf943880fbd250ebd19c85aafe186b23d271140 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Wed, 15 Nov 2023 15:18:08 -0800 Subject: [PATCH 3/3] initial_velocity.sdf example world Signed-off-by: Steve Peters --- examples/worlds/initial_velocity.sdf | 232 +++++++++++++++++++++++++++ 1 file changed, 232 insertions(+) create mode 100644 examples/worlds/initial_velocity.sdf diff --git a/examples/worlds/initial_velocity.sdf b/examples/worlds/initial_velocity.sdf new file mode 100644 index 0000000000..8f5363863d --- /dev/null +++ b/examples/worlds/initial_velocity.sdf @@ -0,0 +1,232 @@ + + + + + + + 0.001 + 1.0 + + + + + + + + + + true + 0 0 10 0 0 0 + 1 1 1 1 + 0.5 0.5 0.5 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 0 0 1.0 0 0 0 + + + + + + 0.1 0.4 0.9 + + + + 0.5 0.5 1.0 1 + 0.5 0.5 1.0 1 + 0.0 0.0 1.0 1 + + + + + + 0.1 0.4 0.9 + + + + + + + + + + 0 1 1.0 0 0 0 + + + + + + 0.1 0.4 0.9 + + + + 0.5 0.5 1.0 1 + 0.5 0.5 1.0 1 + 0.0 0.0 1.0 1 + + + + + + 0.1 0.4 0.9 + + + + + + 0 0 0 + 0 0 0 + + + + + 0 2 1.0 0 0 0 + + + + + + 0.1 0.4 0.9 + + + + 0.5 0.5 1.0 1 + 0.5 0.5 1.0 1 + 0.0 0.0 1.0 1 + + + + + + 0.1 0.4 0.9 + + + + + + 0 0 0 + 0 0 0 + + + + + 0 3 1.0 0 0 0 + + + + + + 0.1 0.4 0.9 + + + + 0.5 0.5 1.0 1 + 0.5 0.5 1.0 1 + 0.0 0.0 1.0 1 + + + + + + 0.1 0.4 0.9 + + + + + + 0 0 0 + 0 0 0 + + + + + 0 -1 1.0 0 0 0 + + + + + + 0.1 0.4 0.9 + + + + 0.5 0.5 1.0 1 + 0.5 0.5 1.0 1 + 0.0 0.0 1.0 1 + + + + + + 0.1 0.4 0.9 + + + + + + 0.1 5 0.1 + 4 -2 10 + + + + +