diff --git a/.github/ISSUE_TEMPLATE/bug_report.md b/.github/ISSUE_TEMPLATE/bug_report.md deleted file mode 100644 index 3d89f6414..000000000 --- a/.github/ISSUE_TEMPLATE/bug_report.md +++ /dev/null @@ -1,29 +0,0 @@ ---- -name: Bug report -about: Report a bug -labels: bug ---- - - - -## Environment -* OS Version: -* Source or binary build? - - - -## Description -* Expected behavior: -* Actual behavior: - -## Steps to reproduce - - -1. -2. -3. - -## Output - diff --git a/.github/ISSUE_TEMPLATE/feature_request.md b/.github/ISSUE_TEMPLATE/feature_request.md deleted file mode 100644 index f49727a0c..000000000 --- a/.github/ISSUE_TEMPLATE/feature_request.md +++ /dev/null @@ -1,23 +0,0 @@ ---- -name: Feature request -about: Request a new feature -labels: enhancement ---- - - - -## Desired behavior - - -## Alternatives considered - - -## Implementation suggestion - - -## Additional context - diff --git a/.github/PULL_REQUEST_TEMPLATE.md b/.github/PULL_REQUEST_TEMPLATE.md deleted file mode 100644 index 6c88bd546..000000000 --- a/.github/PULL_REQUEST_TEMPLATE.md +++ /dev/null @@ -1,52 +0,0 @@ - - -# Bug Report - -Fixes issue # - -## Summary - - -## Checklist -- [ ] Signed all commits for DCO -- [ ] Added tests -- [ ] Updated documentation (as needed) -- [ ] Updated migration guide (as needed) -- [ ] `codecheck` passed (See - [contributing](https://ignitionrobotics.org/docs/all/contributing#contributing-code)) -- [ ] All tests passed (See - [test coverage](https://ignitionrobotics.org/docs/all/contributing#test-coverage)) -- [ ] While waiting for a review on your PR, please help review -[another open pull request](https://github.com/pulls?q=is%3Aopen+is%3Apr+user%3Aignitionrobotics+archived%3Afalse+) -to support the maintainers - -**Note to maintainers**: Remember to use **Squash-Merge** - ---- - -# New feature - -Closes # - -## Summary - - -## Test it - - -## Checklist -- [ ] Signed all commits for DCO -- [ ] Added tests -- [ ] Added example world and/or tutorial -- [ ] Updated documentation (as needed) -- [ ] Updated migration guide (as needed) -- [ ] `codecheck` passed (See [contributing](https://ignitionrobotics.org/docs/all/contributing#contributing-code)) -- [ ] All tests passed (See [test coverage](https://ignitionrobotics.org/docs/all/contributing#test-coverage)) -- [ ] While waiting for a review on your PR, please help review -[another open pull request](https://github.com/pulls?q=is%3Aopen+is%3Apr+user%3Aignitionrobotics+archived%3Afalse+) -to support the maintainers - -**Note to maintainers**: Remember to use **Squash-Merge** diff --git a/.github/PULL_REQUEST_TEMPLATE/port.md b/.github/PULL_REQUEST_TEMPLATE/port.md deleted file mode 100644 index c30d08588..000000000 --- a/.github/PULL_REQUEST_TEMPLATE/port.md +++ /dev/null @@ -1,6 +0,0 @@ -Port to - -Branch comparison: https://github.com/ignitionrobotics/ign-physics/compare/... - -**Note to maintainers**: Remember to **Merge** with commit (not squash-merge -or rebase) diff --git a/CMakeLists.txt b/CMakeLists.txt index 403b653a6..ea6bdf12d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -13,7 +13,8 @@ find_package(ignition-cmake2 2.1.0 REQUIRED) #============================================================================ # Configure the project #============================================================================ -ign_configure_project(VERSION_SUFFIX) +ign_configure_project() + #============================================================================ # Set project-specific options diff --git a/Changelog.md b/Changelog.md index e2b1cc5d5..8960e26a5 100644 --- a/Changelog.md +++ b/Changelog.md @@ -85,6 +85,62 @@ ### Ignition Physics 3.x.x (20XX-XX-XX) +### Ignition Physics 3.2.0 (2021-04-28) + +1. Infrastructure + * [Pull request #221](https://github.com/ignitionrobotics/ign-physics/pull/221) + * [Pull request #215](https://github.com/ignitionrobotics/ign-physics/pull/215) + * [Pull request #211](https://github.com/ignitionrobotics/ign-physics/pull/211) + * [Pull request #210](https://github.com/ignitionrobotics/ign-physics/pull/210) + +1. Linters + * [Pull request #201](https://github.com/ignitionrobotics/ign-physics/pull/201) + * [Pull request #154](https://github.com/ignitionrobotics/ign-physics/pull/154) + +1. Documentation + * [Pull request #164](https://github.com/ignitionrobotics/ign-physics/pull/164) + * [Pull request #163](https://github.com/ignitionrobotics/ign-physics/pull/163) + * [Pull request #187](https://github.com/ignitionrobotics/ign-physics/pull/187) + * [Pull request #162](https://github.com/ignitionrobotics/ign-physics/pull/162) + * [Pull request #120](https://github.com/ignitionrobotics/ign-physics/pull/120) + * [Pull request #144](https://github.com/ignitionrobotics/ign-physics/pull/144) + * [Pull request #131](https://github.com/ignitionrobotics/ign-physics/pull/131) + * [Pull request #128](https://github.com/ignitionrobotics/ign-physics/pull/128) + * [Pull request #115](https://github.com/ignitionrobotics/ign-physics/pull/115) + * [Pull request #104](https://github.com/ignitionrobotics/ign-physics/pull/104) + * [Pull request #105](https://github.com/ignitionrobotics/ign-physics/pull/105) + * [Pull request #106](https://github.com/ignitionrobotics/ign-physics/pull/106) + +1. Add Windows installation + * [Pull request #194](https://github.com/ignitionrobotics/ign-physics/pull/194) + +1. dartsim-plugin windows build fixes + * [Pull request #148](https://github.com/ignitionrobotics/ign-physics/pull/148) + +1. [TPE] Skip computing collisions for static objects + * [Pull request #181](https://github.com/ignitionrobotics/ign-physics/pull/181) + +1. [TPE] Update link pose and velocity + * [Pull request #179](https://github.com/ignitionrobotics/ign-physics/pull/179) + +1. [TPE] Fix poseDirty getter + * [Pull request #182](https://github.com/ignitionrobotics/ign-physics/pull/182) + +1. Add restitution coefficient support for bouncing + * [Pull request #139](https://github.com/ignitionrobotics/ign-physics/pull/139) + +1. Fix compilation with gcc 10.2.0 + * [Pull request #185](https://github.com/ignitionrobotics/ign-physics/pull/185) + +1. Support setting canonical link + * [Pull request #142](https://github.com/ignitionrobotics/ign-physics/pull/142) + +1. Ignore invalid joint commands + * [Pull request #137](https://github.com/ignitionrobotics/ign-physics/pull/137) + +1. Fix CONFIG arg in ign_find_package(DART) call + * [Pull request #119](https://github.com/ignitionrobotics/ign-physics/pull/119) + ### Ignition Physics 3.1.0 (2020-10-18) 1. Support for slip compliance in the dartsim-plugin. @@ -120,6 +176,48 @@ ### Ignition Physics 2.x.x (20XX-XX-XX) +### Ignition Physics 2.4.0 (2021-04-14) + +1. [TPE] Update link pose and velocity + * [Pull request #179](https://github.com/ignitionrobotics/ign-physics/pull/179) + +1. Infrastructure + * [Pull request #221](https://github.com/ignitionrobotics/ign-physics/pull/221) + * [Pull request #211](https://github.com/ignitionrobotics/ign-physics/pull/211) + * [Pull request #130](https://github.com/ignitionrobotics/ign-physics/pull/130) + * [Pull request #118](https://github.com/ignitionrobotics/ign-physics/pull/118) + +1. Documentation + * [Pull request #187](https://github.com/ignitionrobotics/ign-physics/pull/187) + * [Pull request #194](https://github.com/ignitionrobotics/ign-physics/pull/194) + +1. TPE: Skip computing collisions for static objects + * [Pull request #181](https://github.com/ignitionrobotics/ign-physics/pull/181) + +1. Add restitution coefficient support for bouncing + * [Pull request #139](https://github.com/ignitionrobotics/ign-physics/pull/139) + +1. Fix compilation with gcc 10.2.0 + * [Pull request #185](https://github.com/ignitionrobotics/ign-physics/pull/185) + +1. Fix TPE poseDirty getter + * [Pull request #182](https://github.com/ignitionrobotics/ign-physics/pull/182) + +1. Support setting canonical link + * [Pull request #142](https://github.com/ignitionrobotics/ign-physics/pull/142) + +1. Resolved codecheck issues + * [Pull request #154](https://github.com/ignitionrobotics/ign-physics/pull/154) + +1. Ignore invalid joint commands + * [Pull request #137](https://github.com/ignitionrobotics/ign-physics/pull/137) + +1. Support getting shape AABB in world frame + * [Pull request #127](https://github.com/ignitionrobotics/ign-physics/pull/127) + +1. Fix CONFIG arg in `ign_find_package(DART)` call + * [Pull request #119](https://github.com/ignitionrobotics/ign-physics/pull/119) + ### Ignition Physics 2.3.0 (2020-09-29) 1. Support for slip compliance in the dartsim-plugin. diff --git a/README.md b/README.md index 7088c45c7..ec363fa6d 100644 --- a/README.md +++ b/README.md @@ -9,9 +9,9 @@ Build | Status -- | -- -Test coverage | [![codecov](https://codecov.io/gh/ignitionrobotics/ign-physics/branch/main/graph/badge.svg)](https://codecov.io/gh/ignitionrobotics/ign-physics) -Ubuntu Bionic | [![Build Status](https://build.osrfoundation.org/buildStatus/icon?job=ignition_physics-ci-main-bionic-amd64)](https://build.osrfoundation.org/job/ignition_physics-ci-main-bionic-amd64) -Homebrew | [![Build Status](https://build.osrfoundation.org/buildStatus/icon?job=ignition_physics-ci-main-homebrew-amd64)](https://build.osrfoundation.org/job/ignition_physics-ci-main-homebrew-amd64) +Test coverage | [![codecov](https://codecov.io/gh/ignitionrobotics/ign-physics/branch/ign-physics4/graph/badge.svg)](https://codecov.io/gh/ignitionrobotics/ign-physics) +Ubuntu Bionic | [![Build Status](https://build.osrfoundation.org/buildStatus/icon?job=ignition_physics-ci-ign-physics4-bionic-amd64)](https://build.osrfoundation.org/job/ignition_physics-ci-ign-physics4-bionic-amd64) +Homebrew | [![Build Status](https://build.osrfoundation.org/buildStatus/icon?job=ignition_physics-ci-ign-physics4-homebrew-amd64)](https://build.osrfoundation.org/job/ignition_physics-ci-ign-physics4-homebrew-amd64) Windows | [![Build Status](https://build.osrfoundation.org/buildStatus/icon?job=ign_physics-ci-win)](https://build.osrfoundation.org/job/ign_physics-ci-win) Ignition Physics, a component of [Ignition @@ -74,7 +74,7 @@ See the [installation tutorial](https://ignitionrobotics.org/api/physics/4.0/ins # Usage -Please refer to the [examples directory](https://github.com/ignitionrobotics/ign-physics/raw/main/examples/). +Please refer to the [examples directory](https://github.com/ignitionrobotics/ign-physics/raw/ign-physics4/examples/). # Documentation @@ -91,7 +91,7 @@ You can also generate the documentation from a clone of this repository by follo 2. Clone the repository ``` - git clone https://github.com/ignitionrobotics/ign-physics -b main + git clone https://github.com/ignitionrobotics/ign-physics -b ign-physics4 ``` 3. Configure and build the documentation. 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 53d82287c..4adc5b3d6 100644 --- a/tpe/lib/src/Model.cc +++ b/tpe/lib/src/Model.cc @@ -70,8 +70,11 @@ Entity &Model::AddLink() { std::size_t linkId = Entity::GetNextId(); - if (this->GetLinkCount() == 0u) + if (this->GetChildren().empty()) + { this->dataPtr->firstLinkId = linkId; + this->dataPtr->canonicalLinkId = linkId; + } const auto[it, success] = this->GetChildren().insert( {linkId, std::make_shared(linkId)}); @@ -147,7 +150,7 @@ Entity &Model::GetCanonicalLink() } ////////////////////////////////////////////////// -void Model::SetLinearVelocity(const math::Vector3d _velocity) +void Model::SetLinearVelocity(const math::Vector3d &_velocity) { this->linearVelocity = _velocity; } @@ -160,7 +163,7 @@ math::Vector3d Model::GetLinearVelocity() const } ////////////////////////////////////////////////// -void Model::SetAngularVelocity(const math::Vector3d _velocity) +void Model::SetAngularVelocity(const math::Vector3d &_velocity) { this->angularVelocity = _velocity; } @@ -173,21 +176,18 @@ 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 9b6cc6e8a..9ae9fc3fa 100644 --- a/tpe/lib/src/Model.hh +++ b/tpe/lib/src/Model.hh @@ -68,30 +68,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); /// \brief Removes a child entity (either a link or model) from the /// appropriate child entity containers 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 dbee78514..d62562bb7 100644 --- a/tpe/plugin/src/FreeGroupFeatures.cc +++ b/tpe/plugin/src/FreeGroupFeatures.cc @@ -62,6 +62,10 @@ Identity FreeGroupFeatures::GetFreeGroupRootLink(const Identity &_groupID) const 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); @@ -140,24 +144,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 583c467dd..10fba3159 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 diff --git a/tutorials/02_installation.md b/tutorials/02_installation.md index a6e0fa619..b400c79ef 100644 --- a/tutorials/02_installation.md +++ b/tutorials/02_installation.md @@ -208,7 +208,7 @@ You can also generate the documentation from a clone of this repository by follo 2. Clone the repository ``` - git clone https://github.com/ignitionrobotics/ign-physics -b main + git clone https://github.com/ignitionrobotics/ign-physics -b ign-physics4 ``` 3. Configure and build the documentation. diff --git a/tutorials/03_physics_plugins.md b/tutorials/03_physics_plugins.md index 6045a70be..6187080ac 100644 --- a/tutorials/03_physics_plugins.md +++ b/tutorials/03_physics_plugins.md @@ -78,12 +78,12 @@ Users do not need to organize their own plugin implementations this way. Dart ([Dynamic Animation and Robotics Toolkit](https://dartsim.github.io/)) is an open source library that provides data structures and algorithms for kinematic and dynamic applications in robotics and computer animation. It is the default physics engine used in Ignition Simulation. -The source code for Dartsim plugin can be found in [Ignition Physics repository](https://github.com/ignitionrobotics/ign-physics/tree/main) under `dartsim` directory. +The source code for Dartsim plugin can be found in [Ignition Physics repository](https://github.com/ignitionrobotics/ign-physics/tree/ign-physics4) under `dartsim` directory. -TPE ([Trivial Physics Engine](https://github.com/ignitionrobotics/ign-physics/tree/main/tpe)) is an open source library created by Open Robotics that enables fast, inexpensive kinematics simulation for entities at large scale. +TPE ([Trivial Physics Engine](https://github.com/ignitionrobotics/ign-physics/tree/ign-physics4/tpe)) is an open source library created by Open Robotics that enables fast, inexpensive kinematics simulation for entities at large scale. It supports higher-order fleet dynamics without real physics (eg. gravity, force, constraint etc.) and multi-machine synchronization. Ignition support for TPE targets [Citadel](https://ignitionrobotics.org/docs/citadel) and onward releases. -The source code for TPE plugin can be found in [Ignition Physics repository](https://github.com/ignitionrobotics/ign-physics/tree/main) under the `tpe/plugin` directory. +The source code for TPE plugin can be found in [Ignition Physics repository](https://github.com/ignitionrobotics/ign-physics/tree/ign-physics4) under the `tpe/plugin` directory. The following is a list of features supported by each physics engine to help users select one that fits their needs. diff --git a/tutorials/06-physics-simulation-concepts.md b/tutorials/06-physics-simulation-concepts.md index 99879a897..f0bbc2f9d 100644 --- a/tutorials/06-physics-simulation-concepts.md +++ b/tutorials/06-physics-simulation-concepts.md @@ -69,9 +69,9 @@ second in Z-axis. Note that the mechanism to move the car is different depending on the used physics engine. Using [dartsim](https://github.com/ignitionrobotics/ign-physics/tree/main/dartsim), -the car is moved by applying force on the joints, please see [DiffDrive.cc](https://github.com/ignitionrobotics/ign-gazebo/blob/ign-gazebo6/src/systems/diff_drive/DiffDrive.cc#L333) code. +the car is moved by applying force on the joints, please see [DiffDrive.cc](https://github.com/ignitionrobotics/ign-gazebo/blob/main/src/systems/diff_drive/DiffDrive.cc#L333) code. Using [TPE](https://github.com/ignitionrobotics/ign-physics/tree/main/tpe), -TPE directly sets model velocity in [VelocityControl.cc](https://github.com/ignitionrobotics/ign-gazebo/blob/ign-gazebo6/src/systems/velocity_control/VelocityControl.cc#L117) code. +TPE directly sets model velocity in [VelocityControl.cc](https://github.com/ignitionrobotics/ign-gazebo/blob/main/src/systems/velocity_control/VelocityControl.cc#L117) code. #### Monitoring the model and its links pose @@ -136,7 +136,7 @@ and the blades will stop after some time due to friction. The command applies a constant torque to the rotor rod, together with the mechanism to compute the upward/downward lift and drag force due to the wind pressure simulation supported by Ignition Physics, the cube is lifted. -For more detail, please see the [LiftDrag.cc](https://github.com/ignitionrobotics/ign-gazebo/blob/ign-gazebo6/src/systems/lift_drag/LiftDrag.cc) +For more detail, please see the [LiftDrag.cc](https://github.com/ignitionrobotics/ign-gazebo/blob/main/src/systems/lift_drag/LiftDrag.cc) code. ### Buoyancy diff --git a/tutorials/08-implementing-a-custom-feature.md b/tutorials/08-implementing-a-custom-feature.md index 8c5d58f26..ef55f0f30 100644 --- a/tutorials/08-implementing-a-custom-feature.md +++ b/tutorials/08-implementing-a-custom-feature.md @@ -11,7 +11,7 @@ In the last \ref createphysicsplugin "Implement a physics plugin" tutorial, we know how to implement a dummy physics engine as a plugin and load it using \ref ignition::physics "Ignition Physics API". In this tutorial, we will look deeper into the structure of a physics engine plugin, for example, the available -[DART](https://github.com/ignitionrobotics/ign-physics/tree/main/dartsim) +[DART](https://github.com/ignitionrobotics/ign-physics/tree/ign-physics4/dartsim) physics engine in `ign-physics` repository and how to define a custom \ref ignition::physics::Feature "Feature" for the plugin. @@ -135,7 +135,7 @@ will not run at the same time when requested. With the requirements and restrictions above, we will implement an example custom `Feature` that retrieves a simulation world from `dartsim` physics engine. -For example, we name it as [World.hh](https://github.com/ignitionrobotics/ign-physics/blob/main/dartsim/include/ignition/physics/dartsim/World.hh) +For example, we name it as [World.hh](https://github.com/ignitionrobotics/ign-physics/blob/ign-physics4/dartsim/include/ignition/physics/dartsim/World.hh) and the its content is as follow: ```cpp @@ -209,8 +209,8 @@ convenience function for querying the feature `Implementation` object. After defining the custom feature, please look into where it is added to a \ref ignition::physics::FeatureList "FeatureList" in -[CustomFeatures.hh](https://github.com/ignitionrobotics/ign-physics/blob/main/dartsim/src/CustomFeatures.hh) -and implemented in [CustomFeatures.cc](https://github.com/ignitionrobotics/ign-physics/blob/main/dartsim/src/CustomFeatures.cc). +[CustomFeatures.hh](https://github.com/ignitionrobotics/ign-physics/blob/ign-physics4/dartsim/src/CustomFeatures.hh) +and implemented in [CustomFeatures.cc](https://github.com/ignitionrobotics/ign-physics/blob/ign-physics4/dartsim/src/CustomFeatures.cc). These files are place as follows: ``` dartsim @@ -257,7 +257,7 @@ class CustomFeatures : The custom feature `RetrieveWorld` is added to `CustomFeatureList`, other custom features could also be added here. The `CustomFeatures` "FeatureList" here inherits from: -- [Base](https://github.com/ignitionrobotics/ign-physics/blob/main/dartsim/src/Base.hh) +- [Base](https://github.com/ignitionrobotics/ign-physics/blob/ign-physics4/dartsim/src/Base.hh) class for foundation definitions of Models, Joints, Links, and Shapes objects of `dartsim` interfacing to Ignition Physics API. - \ref ignition::physics::Implements3d "Implements3d" for implementing the @@ -292,10 +292,10 @@ dart::simulation::WorldPtr CustomFeatures::GetDartsimWorld( Here we simply implement the actual behavior of `GetDartsimWorld` to return the world pointer from `EntityStorage` object storing world pointers of `dartsim` in -[Base](https://github.com/ignitionrobotics/ign-physics/blob/main/dartsim/src/Base.hh) class. +[Base](https://github.com/ignitionrobotics/ign-physics/blob/ign-physics4/dartsim/src/Base.hh) class. Finally, we add the implemented `CustomFeatures` "FeatureList" together with other \ref ignition::physics::FeatureList "FeatureList" to final `DartsimFeatures` -"FeatureList" as in [dartsim/src/plugin.cc](https://github.com/ignitionrobotics/ign-physics/blob/main/dartsim/src/plugin.cc) +"FeatureList" as in [dartsim/src/plugin.cc](https://github.com/ignitionrobotics/ign-physics/blob/ign-physics4/dartsim/src/plugin.cc) (please see \ref createphysicsplugin "Implement a physics plugin" for registering the plugin to Ignition Physics).