diff --git a/tpe/lib/src/Entity.hh b/tpe/lib/src/Entity.hh index 7935f014d..740b74067 100644 --- a/tpe/lib/src/Entity.hh +++ b/tpe/lib/src/Entity.hh @@ -173,7 +173,7 @@ class IGNITION_PHYSICS_TPELIB_VISIBLE Entity /// \brief Get number of children /// \return Map of child id's to child entities - protected: std::map> &GetChildren() + public: std::map> &GetChildren() const; /// \brief Update the entity bounding box diff --git a/tpe/lib/src/Link.cc b/tpe/lib/src/Link.cc index d464141c1..c14076b5f 100644 --- a/tpe/lib/src/Link.cc +++ b/tpe/lib/src/Link.cc @@ -42,3 +42,41 @@ Entity &Link::AddCollision() this->ChildrenChanged(); return *it->second.get(); } + +////////////////////////////////////////////////// +void Link::SetLinearVelocity(const math::Vector3d &_velocity) +{ + this->linearVelocity = _velocity; +} + +////////////////////////////////////////////////// +math::Vector3d Link::GetLinearVelocity() const +{ + return this->linearVelocity; +} + +////////////////////////////////////////////////// +void Link::SetAngularVelocity(const math::Vector3d &_velocity) +{ + this->angularVelocity = _velocity; +} + +////////////////////////////////////////////////// +math::Vector3d Link::GetAngularVelocity() const +{ + return this->angularVelocity; +} + +////////////////////////////////////////////////// +void Link::UpdatePose(double _timeStep) +{ + if (this->linearVelocity == math::Vector3d::Zero && + this->angularVelocity == math::Vector3d::Zero) + return; + + math::Pose3d currentPose = this->GetPose(); + math::Pose3d nextPose( + currentPose.Pos() + this->linearVelocity * _timeStep, + currentPose.Rot().Integrate(this->angularVelocity, _timeStep)); + this->SetPose(nextPose); +} diff --git a/tpe/lib/src/Link.hh b/tpe/lib/src/Link.hh index 6cf68f55b..05cfccf65 100644 --- a/tpe/lib/src/Link.hh +++ b/tpe/lib/src/Link.hh @@ -18,6 +18,8 @@ #ifndef IGNITION_PHYSICS_TPE_LIB_SRC_LINK_HH_ #define IGNITION_PHYSICS_TPE_LIB_SRC_LINK_HH_ +#include + #include "ignition/physics/tpelib/Export.hh" #include "Entity.hh" @@ -42,6 +44,34 @@ class IGNITION_PHYSICS_TPELIB_VISIBLE Link : public Entity /// \brief Add a collision /// \return Newly created Collision public: Entity &AddCollision(); + + /// \brief Set the linear velocity of link relative to parent + /// \param[in] _velocity linear velocity in meters per second + public: void SetLinearVelocity(const math::Vector3d &_velocity); + + /// \brief Get the linear velocity of link relative to parent + /// \return linear velocity of link in meters per second + public: math::Vector3d GetLinearVelocity() const; + + /// \brief Set the angular velocity of link relative to parent + /// \param[in] _velocity angular velocity in radians per second + public: void SetAngularVelocity(const math::Vector3d &_velocity); + + /// \brief Get the angular velocity of link relative to parent + /// \return angular velocity in radians per second + public: math::Vector3d GetAngularVelocity() const; + + /// \brief Update the pose of the entity + /// \param[in] _timeStep current world timestep in seconds + public: virtual void UpdatePose(double _timeStep); + + IGN_UTILS_WARN_IGNORE__DLL_INTERFACE_MISSING + /// \brief linear velocity of link + protected: math::Vector3d linearVelocity; + + /// \brief angular velocity of link + protected: math::Vector3d angularVelocity; + IGN_UTILS_WARN_RESUME__DLL_INTERFACE_MISSING }; } diff --git a/tpe/lib/src/Link_TEST.cc b/tpe/lib/src/Link_TEST.cc index 6481b47c6..f086901d8 100644 --- a/tpe/lib/src/Link_TEST.cc +++ b/tpe/lib/src/Link_TEST.cc @@ -35,8 +35,9 @@ TEST(Link, BasicAPI) link.SetName("link_1"); EXPECT_EQ("link_1", link.GetName()); - link.SetPose(math::Pose3d(1, 2, 3, 0.1, 0.2, 0.3)); - EXPECT_EQ(math::Pose3d(1, 2, 3, 0.1, 0.2, 0.3), link.GetPose()); + math::Pose3d link1Pose(1, 2, 3, 0.1, 0.2, 0.3); + link.SetPose(link1Pose); + EXPECT_EQ(link1Pose, link.GetPose()); Model model; auto modelPose = math::Pose3d(10, 0, 2, 1, 0, 0); @@ -44,10 +45,26 @@ TEST(Link, BasicAPI) Entity &linkEnt = model.AddLink(); ASSERT_NE(nullptr, linkEnt.GetParent()); - linkEnt.SetPose(math::Pose3d(0, 0.2, 0.5, 0, 1, 0)); + math::Pose3d linkEntPose(0, 0.2, 0.5, 0, 1, 0); + linkEnt.SetPose(linkEntPose); EXPECT_EQ(math::Pose3d(10, -0.312675, 2.43845, 1.23686, 0.471978, 0.918989), linkEnt.GetWorldPose()); + math::Vector3d linVel(0, 0.1, 0); + link.SetLinearVelocity(linVel); + EXPECT_EQ(linVel, link.GetLinearVelocity()); + + math::Vector3d angVel(0.2, 0, 1); + link.SetAngularVelocity(angVel); + EXPECT_EQ(angVel, link.GetAngularVelocity()); + + double timeStep = 0.1; + math::Pose3d expectedPose( + link1Pose.Pos() + linVel * timeStep, + link1Pose.Rot().Integrate(angVel, timeStep)); + link.UpdatePose(timeStep); + EXPECT_EQ(expectedPose, link.GetPose()); + Link link2; EXPECT_NE(link.GetId(), link2.GetId()); } diff --git a/tpe/lib/src/Model.cc b/tpe/lib/src/Model.cc index eaec716b8..3a95140cf 100644 --- a/tpe/lib/src/Model.cc +++ b/tpe/lib/src/Model.cc @@ -65,7 +65,10 @@ Entity &Model::AddLink() std::size_t linkId = Entity::GetNextId(); if (this->GetChildren().empty()) + { this->dataPtr->firstLinkId = linkId; + this->dataPtr->canonicalLinkId = linkId; + } const auto[it, success] = this->GetChildren().insert( {linkId, std::make_shared(linkId)}); @@ -127,7 +130,7 @@ Entity &Model::GetCanonicalLink() } ////////////////////////////////////////////////// -void Model::SetLinearVelocity(const math::Vector3d _velocity) +void Model::SetLinearVelocity(const math::Vector3d &_velocity) { this->linearVelocity = _velocity; } @@ -140,7 +143,7 @@ math::Vector3d Model::GetLinearVelocity() const } ////////////////////////////////////////////////// -void Model::SetAngularVelocity(const math::Vector3d _velocity) +void Model::SetAngularVelocity(const math::Vector3d &_velocity) { this->angularVelocity = _velocity; } @@ -153,20 +156,17 @@ math::Vector3d Model::GetAngularVelocity() const } ////////////////////////////////////////////////// -void Model::UpdatePose( - const double _timeStep, - const math::Vector3d _linearVelocity, - const math::Vector3d _angularVelocity) +void Model::UpdatePose(double _timeStep) { IGN_PROFILE("tpelib::Model::UpdatePose"); - if (_linearVelocity == math::Vector3d::Zero && - _angularVelocity == math::Vector3d::Zero) + if (this->linearVelocity == math::Vector3d::Zero && + this->angularVelocity == math::Vector3d::Zero) return; math::Pose3d currentPose = this->GetPose(); math::Pose3d nextPose( - currentPose.Pos() + _linearVelocity * _timeStep, - currentPose.Rot().Integrate(_angularVelocity, _timeStep)); + currentPose.Pos() + this->linearVelocity * _timeStep, + currentPose.Rot().Integrate(this->angularVelocity, _timeStep)); this->SetPose(nextPose); } diff --git a/tpe/lib/src/Model.hh b/tpe/lib/src/Model.hh index 8e910d45b..b62322179 100644 --- a/tpe/lib/src/Model.hh +++ b/tpe/lib/src/Model.hh @@ -60,30 +60,25 @@ class IGNITION_PHYSICS_TPELIB_VISIBLE Model : public Entity /// \return Entity the canonical (first) link public: Entity &GetCanonicalLink(); - /// \brief Set the linear velocity of model - /// \param[in] _velocity linear velocity - public: void SetLinearVelocity(const math::Vector3d _velocity); + /// \brief Set the linear velocity of model relative to parent + /// \param[in] _velocity linear velocity in meters per second + public: void SetLinearVelocity(const math::Vector3d &_velocity); - /// \brief Get the linear velocity of model - /// \return linear velocity of model + /// \brief Get the linear velocity of model relative to parent + /// \return linear velocity of model in meters per second public: math::Vector3d GetLinearVelocity() const; - /// \brief Set the angular velocity of model - /// \param[in] _velocity angular velocity from world - public: void SetAngularVelocity(const math::Vector3d _velocity); + /// \brief Set the angular velocity of model relative to parent + /// \param[in] _velocity angular velocity from world in radians per second + public: void SetAngularVelocity(const math::Vector3d &_velocity); - /// \brief Get the angular velocity of model - /// \return angular velocity + /// \brief Get the angular velocity of model relative to parent + /// \return angular velocity in radians per second public: math::Vector3d GetAngularVelocity() const; /// \brief Update the pose of the entity - /// \param[in] _timeStep current world timestep - /// \param[in] _linearVelocity linear velocity - /// \param[in] _angularVelocity angular velocity - public: virtual void UpdatePose( - const double _timeStep, - const math::Vector3d _linearVelocity, - const math::Vector3d _angularVelocity); + /// \param[in] _timeStep current world timestep in seconds + public: virtual void UpdatePose(double _timeStep); IGN_UTILS_WARN_IGNORE__DLL_INTERFACE_MISSING /// \brief linear velocity of model diff --git a/tpe/lib/src/Model_TEST.cc b/tpe/lib/src/Model_TEST.cc index 310a95f89..930eaa11d 100644 --- a/tpe/lib/src/Model_TEST.cc +++ b/tpe/lib/src/Model_TEST.cc @@ -47,7 +47,7 @@ TEST(Model, BasicAPI) model.SetAngularVelocity(math::Vector3d(1.0, 1.0, 1.0)); EXPECT_EQ(math::Vector3d(1.0, 1.0, 1.0), model.GetAngularVelocity()); - model.UpdatePose(0.1, model.GetLinearVelocity(), model.GetAngularVelocity()); + model.UpdatePose(0.1); EXPECT_EQ(model.GetPose(), model.GetWorldPose()); EXPECT_FALSE(model.GetStatic()); @@ -66,8 +66,7 @@ TEST(Model, BasicAPI) math::Pose3d expectedPose( originalPose.Pos() + math::Vector3d(0.1, 0.1, 0.1) * timeStep, originalPose.Rot().Integrate(math::Vector3d(1.0, 0, 0), timeStep)); - model2.UpdatePose( - timeStep, model2.GetLinearVelocity(), model2.GetAngularVelocity()); + model2.UpdatePose(timeStep); EXPECT_EQ(expectedPose, model2.GetPose()); } diff --git a/tpe/lib/src/World.cc b/tpe/lib/src/World.cc index 33dfc313d..1b623e3ae 100644 --- a/tpe/lib/src/World.cc +++ b/tpe/lib/src/World.cc @@ -23,6 +23,7 @@ #include #include "World.hh" #include "Model.hh" +#include "Link.hh" using namespace ignition; using namespace physics; @@ -66,10 +67,17 @@ void World::Step() for (auto it = children.begin(); it != children.end(); ++it) { auto model = std::dynamic_pointer_cast(it->second); - model->UpdatePose( - this->timeStep, - model->GetLinearVelocity(), - model->GetAngularVelocity()); + model->UpdatePose(this->timeStep); + auto &ents = model->GetChildren(); + for (auto linkIt = ents.begin(); linkIt != ents.end(); ++linkIt) + { + // if child of model is link + auto link = std::dynamic_pointer_cast(linkIt->second); + if (link) + { + link->UpdatePose(this->timeStep); + } + } } // check colliisions diff --git a/tpe/plugin/src/FreeGroupFeatures.cc b/tpe/plugin/src/FreeGroupFeatures.cc index 2d306c2a1..81c8d12e8 100644 --- a/tpe/plugin/src/FreeGroupFeatures.cc +++ b/tpe/plugin/src/FreeGroupFeatures.cc @@ -63,6 +63,10 @@ Identity FreeGroupFeatures::GetFreeGroupCanonicalLink( if (modelIt != this->models.end() && modelIt->second != nullptr) { // assume canonical link is the first link in model + // note the canonical link of a free group is renamed to root link in + // ign-physics4. The canonical link / root link of a free group can be + // different from the canonical link of a model. + // Here we treat them the same and return the model's canonical link tpelib::Entity &link = modelIt->second->model->GetCanonicalLink(); auto linkPtr = std::make_shared(); linkPtr->link = static_cast(&link); @@ -141,24 +145,46 @@ void FreeGroupFeatures::SetFreeGroupWorldLinearVelocity( const Identity &_groupID, const LinearVelocity &_linearVelocity) { - // assume no canonical link for now - // assume groupID ~= modelID auto it = this->models.find(_groupID.id); // set model linear velocity if (it != this->models.end() && it->second != nullptr) + { it->second->model->SetLinearVelocity( math::eigen3::convert(_linearVelocity)); + } + else + { + auto linkIt = this->links.find(_groupID.id); + if (linkIt != this->links.end() && linkIt->second != nullptr) + { + math::Pose3d linkWorldPose = linkIt->second->link->GetWorldPose(); + linkIt->second->link->SetLinearVelocity( + linkWorldPose.Rot().Inverse() * + math::eigen3::convert( _linearVelocity)); + } + } } ///////////////////////////////////////////////// void FreeGroupFeatures::SetFreeGroupWorldAngularVelocity( const Identity &_groupID, const AngularVelocity &_angularVelocity) { - // assume no canonical link for now - // assume groupID ~= modelID auto it = this->models.find(_groupID.id); // set model angular velocity if (it != this->models.end() && it->second != nullptr) + { it->second->model->SetAngularVelocity( math::eigen3::convert(_angularVelocity)); + } + else + { + auto linkIt = this->links.find(_groupID.id); + if (linkIt != this->links.end() && linkIt->second != nullptr) + { + math::Pose3d linkWorldPose = linkIt->second->link->GetWorldPose(); + linkIt->second->link->SetAngularVelocity( + linkWorldPose.Rot().Inverse() * + math::eigen3::convert(_angularVelocity)); + } + } } diff --git a/tpe/plugin/src/SDFFeatures.cc b/tpe/plugin/src/SDFFeatures.cc index bb250beb6..b07311b66 100644 --- a/tpe/plugin/src/SDFFeatures.cc +++ b/tpe/plugin/src/SDFFeatures.cc @@ -114,16 +114,19 @@ Identity SDFFeatures::ConstructSdfModel( this->ConstructSdfLink(modelIdentity, *_sdfModel.LinkByIndex(i)); } - // set canonical link id - if (_sdfModel.CanonicalLink() != nullptr) + if (_sdfModel.LinkCount() > 0u) { - std::string canonicalLinkName = _sdfModel.CanonicalLinkName(); - tpelib::Entity &canonicalLink = model->GetChildByName(canonicalLinkName); - model->SetCanonicalLink(canonicalLink.GetId()); - } - else - { - model->SetCanonicalLink(); + // set canonical link id + if (_sdfModel.CanonicalLink() != nullptr) + { + std::string canonicalLinkName = _sdfModel.CanonicalLinkName(); + tpelib::Entity &canonicalLink = model->GetChildByName(canonicalLinkName); + model->SetCanonicalLink(canonicalLink.GetId()); + } + else + { + model->SetCanonicalLink(); + } } return modelIdentity; @@ -185,16 +188,19 @@ Identity SDFFeatures::ConstructSdfNestedModel( this->ConstructSdfLink(modelIdentity, *_sdfModel.LinkByIndex(i)); } - // set canonical link id - if (_sdfModel.CanonicalLink() != nullptr) + if (_sdfModel.LinkCount() > 0u) { - std::string canonicalLinkName = _sdfModel.CanonicalLinkName(); - tpelib::Entity &canonicalLink = model->GetChildByName(canonicalLinkName); - model->SetCanonicalLink(canonicalLink.GetId()); - } - else - { - model->SetCanonicalLink(); + // set canonical link id + if (_sdfModel.CanonicalLink() != nullptr) + { + std::string canonicalLinkName = _sdfModel.CanonicalLinkName(); + tpelib::Entity &canonicalLink = model->GetChildByName(canonicalLinkName); + model->SetCanonicalLink(canonicalLink.GetId()); + } + else + { + model->SetCanonicalLink(); + } } // construct nested models