diff --git a/examples/worlds/pendulum_links.sdf b/examples/worlds/pendulum_links.sdf new file mode 100644 index 0000000000..2f62bab25c --- /dev/null +++ b/examples/worlds/pendulum_links.sdf @@ -0,0 +1,279 @@ + + + + + + + + 0.001 + 1.0 + + + + + + + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + + + + + + + 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 + + + + + + + + + 100 + + + 0 0 0.01 0 0 0 + + + 0.8 + 0.02 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + -0.275 0 1.1 0 0 0 + + + 0.2 0.2 2.2 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 0.01 0 0 0 + + + 0.8 + 0.02 + + + + + -0.275 0 1.1 0 0 0 + + + 0.2 0.2 2.2 + + + + + + + 0 0 2.1 -1.5708 0 0 + 0 + + 0 0 0.5 0 0 0 + + + -0.05 0 0 0 1.5708 0 + + + 0.1 + 0.3 + + + + 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 1.5708 0 + + + 0.1 + 0.2 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 0.5 0 0 0 + + + 0.1 + 0.9 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + -0.05 0 0 0 1.5708 0 + + + 0.1 + 0.3 + + + + + 0 0 1.0 0 1.5708 0 + + + 0.1 + 0.2 + + + + + 0 0 0.5 0 0 0 + + + 0.1 + 0.9 + + + + + + + 0.25 1.0 2.1 -2 0 0 + 0 + + 0 0 0.5 0 0 0 + + + 0 0 0 0 1.5708 0 + + + 0.08 + 0.3 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 0.5 0 0 0 + + + 0.1 + 0.9 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 0 0 1.5708 0 + + + 0.08 + 0.3 + + + + + 0 0 0.5 0 0 0 + + + 0.1 + 0.9 + + + + + + + base + upper_link + + 1.0 0 0 + + + + + upper_link + lower_link + + 1.0 0 0 + + + + + base + upper_link + lower_link + + + + diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index 672d9bb828..72ccc67339 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -1563,6 +1563,111 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm) return true; }); + // Update link angular velocity + _ecm.Each( + [&](const Entity &_entity, const components::Link *, + const components::AngularVelocityCmd *_angularVelocityCmd) + { + if (!this->entityLinkMap.HasEntity(_entity)) + { + ignwarn << "Failed to find link [" << _entity + << "]." << std::endl; + return true; + } + + auto linkPtrPhys = this->entityLinkMap.Get(_entity); + if (nullptr == linkPtrPhys) + return true; + + auto freeGroup = linkPtrPhys->FindFreeGroup(); + if (!freeGroup) + return true; + this->entityFreeGroupMap.AddEntity(_entity, freeGroup); + + auto worldAngularVelFeature = + this->entityFreeGroupMap + .EntityCast(_entity); + + if (!worldAngularVelFeature) + { + static bool informed{false}; + if (!informed) + { + igndbg << "Attempting to set link angular velocity, but the " + << "physics engine doesn't support velocity commands. " + << "Velocity won't be set." + << std::endl; + informed = true; + } + return true; + } + // velocity in world frame = world_to_model_tf * model_to_link_tf * vel + Entity modelEntity = topLevelModel(_entity, _ecm); + const components::Pose *modelEntityPoseComp = + _ecm.Component(modelEntity); + math::Pose3d modelToLinkTransform = this->RelativePose( + modelEntity, _entity, _ecm); + math::Vector3d worldAngularVel = modelEntityPoseComp->Data().Rot() + * modelToLinkTransform.Rot() * _angularVelocityCmd->Data(); + worldAngularVelFeature->SetWorldAngularVelocity( + math::eigen3::convert(worldAngularVel)); + + return true; + }); + + // Update link linear velocity + _ecm.Each( + [&](const Entity &_entity, const components::Link *, + const components::LinearVelocityCmd *_linearVelocityCmd) + { + if (!this->entityLinkMap.HasEntity(_entity)) + { + ignwarn << "Failed to find link [" << _entity + << "]." << std::endl; + return true; + } + + auto linkPtrPhys = this->entityLinkMap.Get(_entity); + if (nullptr == linkPtrPhys) + return true; + + auto freeGroup = linkPtrPhys->FindFreeGroup(); + if (!freeGroup) + return true; + this->entityFreeGroupMap.AddEntity(_entity, freeGroup); + + auto worldLinearVelFeature = + this->entityFreeGroupMap + .EntityCast(_entity); + if (!worldLinearVelFeature) + { + static bool informed{false}; + if (!informed) + { + igndbg << "Attempting to set link linear velocity, but the " + << "physics engine doesn't support velocity commands. " + << "Velocity won't be set." + << std::endl; + informed = true; + } + return true; + } + + // velocity in world frame = world_to_model_tf * model_to_link_tf * vel + Entity modelEntity = topLevelModel(_entity, _ecm); + const components::Pose *modelEntityPoseComp = + _ecm.Component(modelEntity); + math::Pose3d modelToLinkTransform = this->RelativePose( + modelEntity, _entity, _ecm); + math::Vector3d worldLinearVel = modelEntityPoseComp->Data().Rot() + * modelToLinkTransform.Rot() * _linearVelocityCmd->Data(); + worldLinearVelFeature->SetWorldLinearVelocity( + math::eigen3::convert(worldLinearVel)); + + return true; + }); + + // Populate bounding box info // Only compute bounding box if component exists to avoid unnecessary // computations @@ -2027,6 +2132,20 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm) return true; }); + _ecm.Each( + [&](const Entity &, components::AngularVelocityCmd *_vel) -> bool + { + _vel->Data() = math::Vector3d::Zero; + return true; + }); + + _ecm.Each( + [&](const Entity &, components::LinearVelocityCmd *_vel) -> bool + { + _vel->Data() = math::Vector3d::Zero; + return true; + }); + // Update joint positions _ecm.Each( [&](const Entity &_entity, components::Joint *, diff --git a/src/systems/velocity_control/VelocityControl.cc b/src/systems/velocity_control/VelocityControl.cc index d6e07f48e7..7ed361c627 100644 --- a/src/systems/velocity_control/VelocityControl.cc +++ b/src/systems/velocity_control/VelocityControl.cc @@ -15,8 +15,11 @@ * */ +#include #include #include +#include +#include #include #include @@ -35,10 +38,15 @@ using namespace systems; class ignition::gazebo::systems::VelocityControlPrivate { - /// \brief Callback for velocity subscription + /// \brief Callback for model velocity subscription /// \param[in] _msg Velocity message public: void OnCmdVel(const ignition::msgs::Twist &_msg); + /// \brief Callback for link velocity subscription + /// \param[in] _msg Velocity message + public: void OnLinkCmdVel(const ignition::msgs::Twist &_msg, + const ignition::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 @@ -46,23 +54,45 @@ class ignition::gazebo::systems::VelocityControlPrivate public: void UpdateVelocity(const ignition::gazebo::UpdateInfo &_info, const ignition::gazebo::EntityComponentManager &_ecm); + /// \brief Update link velocity. + /// \param[in] _info System update information. + /// \param[in] _ecm The EntityComponentManager of the given simulation + /// instance. + public: void UpdateLinkVelocity(const ignition::gazebo::UpdateInfo &_info, + const ignition::gazebo::EntityComponentManager &_ecm); + /// \brief Ignition communication node. public: transport::Node node; + /// \brief Model interface + public: Model model{kNullEntity}; + /// \brief Angular velocity of a model public: math::Vector3d angularVelocity{0, 0, 0}; /// \brief Linear velocity of a model public: math::Vector3d linearVelocity{0, 0, 0}; - /// \brief Model interface - public: Model model{kNullEntity}; - /// \brief Last target velocity requested. public: msgs::Twist targetVel; - /// \brief A mutex to protect the target velocity command. + /// \brief A mutex to protect the model velocity command. public: std::mutex mutex; + + /// \brief Link names + public: std::vector linkNames; + + /// \brief Link entities in a model + public: std::unordered_map links; + + /// \brief Angular velocities of links + public: std::unordered_map angularVelocities; + + /// \brief Linear velocities of links + public: std::unordered_map linearVelocities; + + /// \brief All link velocites + public: std::unordered_map linkVels; }; ////////////////////////////////////////////////// @@ -86,15 +116,45 @@ void VelocityControl::Configure(const Entity &_entity, return; } - // Subscribe to commands - std::string topic{"/model/" + this->dataPtr->model.Name(_ecm) + "/cmd_vel"}; + // Subscribe to model commands + std::string modelTopic{ + "/model/" + this->dataPtr->model.Name(_ecm) + "/cmd_vel"}; if (_sdf->HasElement("topic")) - topic = _sdf->Get("topic"); - this->dataPtr->node.Subscribe( - topic, &VelocityControlPrivate::OnCmdVel, this->dataPtr.get()); + modelTopic = _sdf->Get("topic"); + modelTopic = transport::TopicUtils::AsValidTopic(modelTopic); - ignmsg << "VelocityControl subscribing to twist messages on [" << topic << "]" + this->dataPtr->node.Subscribe( + modelTopic, &VelocityControlPrivate::OnCmdVel, this->dataPtr.get()); + ignmsg << "VelocityControl subscribing to twist messages on [" + << modelTopic << "]" << std::endl; + + // Ugly, but needed because the sdf::Element::GetElement is not a const + // function and _sdf is a const shared pointer to a const sdf::Element. + auto ptr = const_cast(_sdf.get()); + + if (!ptr->HasElement("link_name")) + return; + + sdf::ElementPtr sdfElem = ptr->GetElement("link_name"); + while (sdfElem) + { + this->dataPtr->linkNames.push_back(sdfElem->Get()); + sdfElem = sdfElem->GetNextElement("link_name"); + } + + // Subscribe to link commands + for (const auto &linkName : this->dataPtr->linkNames) + { + std::string linkTopic{"/model/" + this->dataPtr->model.Name(_ecm) + + "/link/" + linkName + "/cmd_vel"}; + linkTopic = transport::TopicUtils::AsValidTopic(linkTopic); + this->dataPtr->node.Subscribe( + linkTopic, &VelocityControlPrivate::OnLinkCmdVel, this->dataPtr.get()); + ignmsg << "VelocityControl subscribing to twist messages on [" + << linkTopic << "]" + << std::endl; + } } ////////////////////////////////////////////////// @@ -116,11 +176,11 @@ void VelocityControl::PreUpdate(const ignition::gazebo::UpdateInfo &_info, return; // update angular velocity of model - auto angularVel = + auto modelAngularVel = _ecm.Component( this->dataPtr->model.Entity()); - if (angularVel == nullptr) + if (modelAngularVel == nullptr) { _ecm.CreateComponent( this->dataPtr->model.Entity(), @@ -128,16 +188,16 @@ void VelocityControl::PreUpdate(const ignition::gazebo::UpdateInfo &_info, } else { - *angularVel = + *modelAngularVel = components::AngularVelocityCmd({this->dataPtr->angularVelocity}); } // update linear velocity of model - auto linearVel = + auto modelLinearVel = _ecm.Component( this->dataPtr->model.Entity()); - if (linearVel == nullptr) + if (modelLinearVel == nullptr) { _ecm.CreateComponent( this->dataPtr->model.Entity(), @@ -145,9 +205,84 @@ void VelocityControl::PreUpdate(const ignition::gazebo::UpdateInfo &_info, } else { - *linearVel = + *modelLinearVel = components::LinearVelocityCmd({this->dataPtr->linearVelocity}); } + + // If there are links, create link components + // If the link hasn't been identified yet, look for it + auto modelName = this->dataPtr->model.Name(_ecm); + + if (this->dataPtr->linkNames.empty()) + return; + + // find all the link entity ids + if (this->dataPtr->links.size() != this->dataPtr->linkNames.size()) + { + for (const auto &linkName : this->dataPtr->linkNames) + { + if (this->dataPtr->links.find(linkName) == this->dataPtr->links.end()) + { + Entity link = this->dataPtr->model.LinkByName(_ecm, linkName); + if (link != kNullEntity) + this->dataPtr->links.insert({linkName, link}); + else + { + ignwarn << "Failed to find link [" << linkName + << "] for model [" << modelName << "]" << std::endl; + } + } + } + } + + // update link velocities + for (const auto& [linkName, angularVel] : this->dataPtr->angularVelocities) + { + auto it = this->dataPtr->links.find(linkName); + if (it != this->dataPtr->links.end()) + { + auto linkAngularVelComp = + _ecm.Component(it->second); + if (!linkAngularVelComp) + { + _ecm.CreateComponent(it->second, + components::AngularVelocityCmd({angularVel})); + } + else + { + *linkAngularVelComp = components::AngularVelocityCmd(angularVel); + } + } + else + { + ignwarn << "No link found for angular velocity cmd [" + << linkName << "]" << std::endl; + } + } + + for (const auto& [linkName, linearVel] : this->dataPtr->linearVelocities) + { + auto it = this->dataPtr->links.find(linkName); + if (it != this->dataPtr->links.end()) + { + auto linkLinearVelComp = + _ecm.Component(it->second); + if (!linkLinearVelComp) + { + _ecm.CreateComponent(it->second, + components::LinearVelocityCmd({linearVel})); + } + else + { + *linkLinearVelComp = components::LinearVelocityCmd(linearVel); + } + } + else + { + ignwarn << "No link found for linear velocity cmd [" + << linkName << "]" << std::endl; + } + } } ////////////////////////////////////////////////// @@ -159,10 +294,12 @@ 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 ignition::gazebo::UpdateInfo &/*_info*/, @@ -170,18 +307,27 @@ void VelocityControlPrivate::UpdateVelocity( { IGN_PROFILE("VeocityControl::UpdateVelocity"); - double linVel; - double angVel; + std::lock_guard lock(this->mutex); + this->linearVelocity = msgs::Convert(this->targetVel.linear()); + this->angularVelocity = msgs::Convert(this->targetVel.angular()); +} + +////////////////////////////////////////////////// +void VelocityControlPrivate::UpdateLinkVelocity( + const ignition::gazebo::UpdateInfo &/*_info*/, + const ignition::gazebo::EntityComponentManager &/*_ecm*/) +{ + IGN_PROFILE("VelocityControl::UpdateLinkVelocity"); + + std::lock_guard lock(this->mutex); + for (const auto& [linkName, msg] : this->linkVels) { - std::lock_guard lock(this->mutex); - linVel = this->targetVel.linear().x(); - angVel = this->targetVel.angular().z(); + auto linearVel = msgs::Convert(msg.linear()); + auto angularVel = msgs::Convert(msg.angular()); + this->linearVelocities[linkName] = linearVel; + this->angularVelocities[linkName] = angularVel; } - - this->linearVelocity = math::Vector3d( - linVel, this->targetVel.linear().y(), this->targetVel.linear().z()); - this->angularVelocity = math::Vector3d( - this->targetVel.angular().x(), this->targetVel.angular().y(), angVel); + this->linkVels.clear(); } ////////////////////////////////////////////////// @@ -191,6 +337,21 @@ void VelocityControlPrivate::OnCmdVel(const msgs::Twist &_msg) this->targetVel = _msg; } +////////////////////////////////////////////////// +void VelocityControlPrivate::OnLinkCmdVel(const msgs::Twist &_msg, + const transport::MessageInfo &_info) +{ + std::lock_guard lock(this->mutex); + for (const auto &linkName : this->linkNames) + { + if (_info.Topic().find("/" + linkName + "/cmd_vel") != std::string::npos) + { + this->linkVels.insert({linkName, _msg}); + break; + } + } +} + IGNITION_ADD_PLUGIN(VelocityControl, ignition::gazebo::System, VelocityControl::ISystemConfigure, diff --git a/test/integration/velocity_control_system.cc b/test/integration/velocity_control_system.cc index df1354b924..5eb7722440 100644 --- a/test/integration/velocity_control_system.cc +++ b/test/integration/velocity_control_system.cc @@ -21,6 +21,7 @@ #include #include +#include "ignition/gazebo/components/Link.hh" #include "ignition/gazebo/components/Name.hh" #include "ignition/gazebo/components/Model.hh" #include "ignition/gazebo/components/Pose.hh" @@ -126,6 +127,112 @@ class VelocityControlTest : public ::testing::TestWithParam EXPECT_GT(poses[i].Rot().Euler().Z(), poses[i-1].Rot().Euler().Z()) << i; } } + + /// \param[in] _sdfFile SDF file to load. + /// \param[in] _cmdVelTopic Command velocity topic. + protected: void TestPublishLinkCmd(const std::string &_sdfFile, + const std::string &_cmdVelTopic) + { + // Start server + ServerConfig serverConfig; + serverConfig.SetSdfFile(_sdfFile); + + // currently only tpe supports link velocity cmds + serverConfig.SetPhysicsEngine("ignition-physics-tpe-plugin"); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + // Create a system that records the vehicle link poses + test::Relay testSystem; + + std::vector modelPoses; + std::vector linkPoses; + testSystem.OnPostUpdate([&linkPoses, &modelPoses]( + const gazebo::UpdateInfo &, + const gazebo::EntityComponentManager &_ecm) + { + auto modelId = _ecm.EntityByComponents( + components::Model(), + components::Name("vehicle_blue")); + EXPECT_NE(kNullEntity, modelId); + + auto modelPoseComp = _ecm.Component(modelId); + ASSERT_NE(nullptr, modelPoseComp); + + modelPoses.push_back(modelPoseComp->Data()); + + auto linkId = _ecm.EntityByComponents( + components::Link(), + components::Name("caster")); + EXPECT_NE(kNullEntity, linkId); + + auto linkPoseComp = _ecm.Component(linkId); + ASSERT_NE(nullptr, linkPoseComp); + + linkPoses.push_back(linkPoseComp->Data()); + + }); + server.AddSystem(testSystem.systemPtr); + + // Run server and check that vehicle didn't move + server.Run(true, 1000, false); + + EXPECT_EQ(1000u, modelPoses.size()); + + for (const auto &pose : modelPoses) + { + EXPECT_EQ(modelPoses[0], pose); + } + for (const auto &pose : linkPoses) + { + EXPECT_EQ(linkPoses[0], pose); + } + + // Publish command and check that link moved + transport::Node node; + auto pub = node.Advertise(_cmdVelTopic); + + msgs::Twist msg; + + const double desiredLinVel = 10.5; + const double desiredAngVel = 0.2; + msgs::Set(msg.mutable_linear(), + math::Vector3d(desiredLinVel, 0, 0)); + msgs::Set(msg.mutable_angular(), + math::Vector3d(0.0, 0, desiredAngVel)); + pub.Publish(msg); + + // Give some time for message to be received + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + + server.Run(true, 3000, false); + + // Poses for 4s + ASSERT_EQ(4000u, modelPoses.size()); + ASSERT_EQ(4000u, linkPoses.size()); + + // verify that the model is stationary + for (unsigned int i = 1001; i < modelPoses.size(); ++i) + { + EXPECT_EQ(modelPoses[0], modelPoses[i]); + } + + // verify that the link is moving in +x and rotating about its origin + for (unsigned int i = 1001; i < linkPoses.size(); ++i) + { + EXPECT_GT(linkPoses[i].Pos().X(), linkPoses[i-1].Pos().X()) << i; + EXPECT_NEAR(linkPoses[i].Pos().Y(), linkPoses[i-1].Pos().Y(), 1e-5); + EXPECT_NEAR(linkPoses[i].Pos().Z(), linkPoses[i-1].Pos().Z(), 1e-5); + EXPECT_NEAR(linkPoses[i].Rot().Euler().X(), + linkPoses[i-1].Rot().Euler().X(), 1e-5) << i; + EXPECT_NEAR(linkPoses[i].Rot().Euler().Y(), + linkPoses[i-1].Rot().Euler().Y(), 1e-5) << i; + EXPECT_GT(linkPoses[i].Rot().Euler().Z(), + linkPoses[i-1].Rot().Euler().Z()) << i; + } + } }; ///////////////////////////////////////////////// @@ -136,6 +243,14 @@ TEST_P(VelocityControlTest, PublishCmd) "/model/vehicle_blue/cmd_vel"); } +///////////////////////////////////////////////// +TEST_P(VelocityControlTest, PublishLinkCmd) +{ + TestPublishLinkCmd( + std::string(PROJECT_SOURCE_PATH) + "/test/worlds/velocity_control.sdf", + "/model/vehicle_blue/link/caster/cmd_vel"); +} + // Run multiple times INSTANTIATE_TEST_SUITE_P(ServerRepeat, VelocityControlTest, ::testing::Range(1, 2)); diff --git a/test/worlds/velocity_control.sdf b/test/worlds/velocity_control.sdf index 2e104e5f08..bae5e14df4 100644 --- a/test/worlds/velocity_control.sdf +++ b/test/worlds/velocity_control.sdf @@ -310,6 +310,7 @@ + caster