From 7a763b631f052f634bd3c34cbad2143936c5b5fc Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Mon, 14 Jan 2019 12:30:07 +0800 Subject: [PATCH 01/23] Introducing AttachPlaneShape feature --- CMakeLists.txt | 6 +- dartsim/src/EntityManagementFeatures.cc | 4 +- dartsim/src/ShapeFeatures.hh | 6 +- dartsim/src/SimulationFeatures_TEST.cc | 15 +- include/ignition/physics/PlaneShape.hh | 158 ++++++++++++++++++ include/ignition/physics/detail/PlaneShape.hh | 79 +++++++++ 6 files changed, 261 insertions(+), 7 deletions(-) create mode 100644 include/ignition/physics/PlaneShape.hh create mode 100644 include/ignition/physics/detail/PlaneShape.hh diff --git a/CMakeLists.txt b/CMakeLists.txt index a2dcc7ebb..18ced0ee8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -55,9 +55,9 @@ ign_find_package(sdformat8 REQUIRED_BY sdf dartsim) # Find dartsim for the dartsim plugin wrapper ign_find_package(DART COMPONENTS - collision-bullet - utils - utils-urdf + collision-ode +# utils +# utils-urdf CONFIG VERSION 6 REQUIRED_BY dartsim diff --git a/dartsim/src/EntityManagementFeatures.cc b/dartsim/src/EntityManagementFeatures.cc index 995cc8dd0..5dcd84c50 100644 --- a/dartsim/src/EntityManagementFeatures.cc +++ b/dartsim/src/EntityManagementFeatures.cc @@ -18,7 +18,7 @@ #include "EntityManagementFeatures.hh" #include -#include +#include #include #include @@ -297,7 +297,7 @@ Identity EntityManagementFeatures::ConstructEmptyWorld( const auto &world = std::make_shared(_name); world->getConstraintSolver()->setCollisionDetector( - dart::collision::BulletCollisionDetector::create()); + dart::collision::OdeCollisionDetector::create()); return this->GenerateIdentity(this->AddWorld(world, _name), world); } diff --git a/dartsim/src/ShapeFeatures.hh b/dartsim/src/ShapeFeatures.hh index c72577f6c..0f74942f1 100644 --- a/dartsim/src/ShapeFeatures.hh +++ b/dartsim/src/ShapeFeatures.hh @@ -24,6 +24,7 @@ #include #include #include +#include #include #include "Base.hh" @@ -48,7 +49,10 @@ using ShapeFeatureList = FeatureList< AttachSphereShapeFeature, mesh::GetMeshShapeProperties, // mesh::SetMeshShapeProperties, - mesh::AttachMeshShapeFeature + mesh::AttachMeshShapeFeature, + GetPlaneShapeProperties, +// SetPlaneShapeProperties, + AttachPlaneShapeFeature >; class ShapeFeatures : diff --git a/dartsim/src/SimulationFeatures_TEST.cc b/dartsim/src/SimulationFeatures_TEST.cc index d28478767..2078dcb4a 100644 --- a/dartsim/src/SimulationFeatures_TEST.cc +++ b/dartsim/src/SimulationFeatures_TEST.cc @@ -28,6 +28,7 @@ #include #include #include +#include #include #include @@ -38,7 +39,8 @@ using TestFeatureList = ignition::physics::FeatureList< ignition::physics::LinkFrameSemantics, ignition::physics::ForwardStep, ignition::physics::GetEntities, - ignition::physics::sdf::ConstructSdfWorld + ignition::physics::sdf::ConstructSdfWorld/*, + ignition::physics::*/ >; using TestWorldPtr = ignition::physics::World3dPtr; @@ -110,6 +112,17 @@ TEST_P(SimulationFeatures_TEST, Falling) } } +TEST_P(SimulationFeatures_TEST, CollisionBetweenMeshAndPlane) +{ + const std::string library = GetParam(); + if (library.empty()) + return; + + std::cout << "Testing library " << library << std::endl; + auto worlds = LoadWorlds(library, TEST_WORLD_DIR "/mesh_plane_falling.world"); + +} + int main(int argc, char *argv[]) { ::testing::InitGoogleTest(&argc, argv); diff --git a/include/ignition/physics/PlaneShape.hh b/include/ignition/physics/PlaneShape.hh new file mode 100644 index 000000000..a892ac248 --- /dev/null +++ b/include/ignition/physics/PlaneShape.hh @@ -0,0 +1,158 @@ +/* + * Copyright (C) 2019 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#ifndef IGNITION_PHYSICS_PLANESHAPE_HH_ +#define IGNITION_PHYSICS_PLANESHAPE_HH_ + +#include + +#include +#include + +namespace ignition +{ +namespace physics +{ +namespace mesh +{ + IGN_PHYSICS_DECLARE_SHAPE_TYPE(PlaneShape); + + ///////////////////////////////////////////////// + template + struct PlaneTypes + { + public: using Normal = + typename FromPolicy::template Use; + + public: using Point = + typename FromPolicy::template Use; + }; + + ///////////////////////////////////////////////// + class GetPlaneShapeProperties + : public virtual FeatureWithRequirements + { + public: template + class PlaneShape + : public virtual Entity, + public PlaneTypes + { + /// \brief Get the normal vector for this plane. For a 2D simulation, this + /// will be a scalar quantity. + /// \returns the normal vector for this plane. + public: Normal GetNormal() const; + + /// \brief Get a point on the plane. For a 2D simulation, this value + /// is irrelevant, because all (x, y) locations will be on the plane. + /// \returns the offset of the plane. + public: Point GetPoint() const; + }; + + public: template + class Implementation + : public virtual Feature::Implementation, + public PlaneTypes + { + // See PlaneShape::GetNormal() + public: virtual Normal GetPlaneShapeNormal( + std::size_t _planeID) const = 0; + + // See PlaneShape::GetPoint() + public: virtual Point GetPlaneShapePoint( + std::size_t _planeID) const = 0; + }; + }; + + ///////////////////////////////////////////////// + class SetPlaneShapeProperties + : public virtual FeatureWithRequirements + { + public: template + class PlaneShape + : public virtual Entity, + public PlaneTypes + { + /// \brief Set the normal vector of this plane + /// \param[in] _normal + /// The new normal vector for this plane + public: void SetNormal(const Normal &_normal); + + /// \brief Specify a point on this plane. The normal vector will remain + /// fixed. + /// \param[in] _point + /// A point which needs to be on the plane. + public: void SetPoint(const Point &_point); + }; + + public: template + class Implementation + : public virtual Feature::Implementation, + public PlaneTypes + { + public: virtual void SetPlaneShapeNormal( + std::size_t _planeID, + const Normal &_normal) = 0; + + public: virtual void SetPlaneShapePoint( + std::size_t _planeID, + const Point &_normal) = 0; + }; + }; + + ///////////////////////////////////////////////// + class AttachPlaneShapeFeature + : public virtual FeatureWithRequirements + { + public: template + class Link + : public virtual Feature::Link, + public PlaneTypes + { + /// \brief Attach a PlaneShape to this link + /// \param[in] _name + /// Name to give to the PlaneShape + /// \param[in] _normal + /// Normal vector for the plane + /// \param[in] _offset + /// Offset of the plane + /// \returns the PlaneShapePtr that was just created. + public: PlaneShapePtr AttachPlaneShape( + const std::string &_name, + const Normal &_normal, + const Point &_offset = Point::Zero()); + }; + + public: template + class Implementation + : public virtual Feature::Implementation, + public PlaneTypes + { + public: virtual Identity AttachPlaneShape( + std::size_t _linkID, + const std::string &_name, + const Normal &_normal, + const Point &_offset) = 0; + }; + }; + +} +} +} + +#include + +#endif // IGNITION_PHYSICS_PLANESHAPE_HH_ diff --git a/include/ignition/physics/detail/PlaneShape.hh b/include/ignition/physics/detail/PlaneShape.hh new file mode 100644 index 000000000..c3eb2e587 --- /dev/null +++ b/include/ignition/physics/detail/PlaneShape.hh @@ -0,0 +1,79 @@ +/* + * Copyright (C) 2019 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#ifndef IGNITION_PHYSICS_DETAIL_PLANESHAPE_HH_ +#define IGNITION_PHYSICS_DETAIL_PLANESHAPE_HH_ + +#include + +#include + +namespace ignition +{ +namespace physics +{ +namespace mesh +{ + ///////////////////////////////////////////////// + template + auto GetPlaneShapeProperties::PlaneShape::GetNormal() const -> Normal + { + return this->template Interface() + ->GetPlaneShapeNormal(this->identity); + } + + ///////////////////////////////////////////////// + template + auto GetPlaneShapeProperties::PlaneShape::GetPoint() const -> Point + { + return this->template Interface() + ->GetPlaneShapePoint(this->identity); + } + + ///////////////////////////////////////////////// + template + void SetPlaneShapeProperties::PlaneShape::SetNormal( + const Normal &_normal) + { + return this->template Interface() + ->SetPlaneShapeNormal(this->identity, _normal); + } + + ///////////////////////////////////////////////// + template + void SetPlaneShapeProperties::PlaneShape::SetPoint( + const Point &_point) + { + return this->template Interface() + ->SetPlaneShapePoint(this->identity, _point); + } + + ///////////////////////////////////////////////// + template + PlaneShapePtr AttachPlaneShapeFeature::Link::AttachPlaneShape( + const std::string &_name, + const Normal &_normal, + const Point &_offset) + { + return this->template Interface() + ->AttachPlaneShape(this->identity, _name, _normal, _offset); + } +} +} +} + +#endif // IGNITION_PHYSICS_DETAIL_PLANESHAPE_HH_ From 36b82b69cfd7d73652ff24a1a702f4911be97528 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Mon, 4 Feb 2019 17:30:52 +0800 Subject: [PATCH 02/23] Syntax corrections --- include/ignition/physics/PlaneShape.hh | 77 +++++++++++-------- include/ignition/physics/detail/PlaneShape.hh | 3 - 2 files changed, 43 insertions(+), 37 deletions(-) diff --git a/include/ignition/physics/PlaneShape.hh b/include/ignition/physics/PlaneShape.hh index a892ac248..7f9e8fc12 100644 --- a/include/ignition/physics/PlaneShape.hh +++ b/include/ignition/physics/PlaneShape.hh @@ -27,30 +27,21 @@ namespace ignition { namespace physics { -namespace mesh -{ - IGN_PHYSICS_DECLARE_SHAPE_TYPE(PlaneShape); - - ///////////////////////////////////////////////// - template - struct PlaneTypes - { - public: using Normal = - typename FromPolicy::template Use; - - public: using Point = - typename FromPolicy::template Use; - }; + IGN_PHYSICS_DECLARE_SHAPE_TYPE(PlaneShape) ///////////////////////////////////////////////// class GetPlaneShapeProperties : public virtual FeatureWithRequirements { public: template - class PlaneShape - : public virtual Entity, - public PlaneTypes + class PlaneShape : public virtual Entity { + public: using Normal = + typename FromPolicy::template Use; + + public: using Point = + typename FromPolicy::template Use; + /// \brief Get the normal vector for this plane. For a 2D simulation, this /// will be a scalar quantity. /// \returns the normal vector for this plane. @@ -63,10 +54,14 @@ namespace mesh }; public: template - class Implementation - : public virtual Feature::Implementation, - public PlaneTypes + class Implementation : public virtual Feature::Implementation { + public: using Normal = + typename FromPolicy::template Use; + + public: using Point = + typename FromPolicy::template Use; + // See PlaneShape::GetNormal() public: virtual Normal GetPlaneShapeNormal( std::size_t _planeID) const = 0; @@ -82,10 +77,14 @@ namespace mesh : public virtual FeatureWithRequirements { public: template - class PlaneShape - : public virtual Entity, - public PlaneTypes + class PlaneShape : public virtual Entity { + public: using Normal = + typename FromPolicy::template Use; + + public: using Point = + typename FromPolicy::template Use; + /// \brief Set the normal vector of this plane /// \param[in] _normal /// The new normal vector for this plane @@ -99,10 +98,14 @@ namespace mesh }; public: template - class Implementation - : public virtual Feature::Implementation, - public PlaneTypes + class Implementation : public virtual Feature::Implementation { + public: using Normal = + typename FromPolicy::template Use; + + public: using Point = + typename FromPolicy::template Use; + public: virtual void SetPlaneShapeNormal( std::size_t _planeID, const Normal &_normal) = 0; @@ -118,10 +121,14 @@ namespace mesh : public virtual FeatureWithRequirements { public: template - class Link - : public virtual Feature::Link, - public PlaneTypes + class Link : public virtual Feature::Link { + public: using Normal = + typename FromPolicy::template Use; + + public: using Point = + typename FromPolicy::template Use; + /// \brief Attach a PlaneShape to this link /// \param[in] _name /// Name to give to the PlaneShape @@ -137,10 +144,14 @@ namespace mesh }; public: template - class Implementation - : public virtual Feature::Implementation, - public PlaneTypes + class Implementation : public virtual Feature::Implementation { + public: using Normal = + typename FromPolicy::template Use; + + public: using Point = + typename FromPolicy::template Use; + public: virtual Identity AttachPlaneShape( std::size_t _linkID, const std::string &_name, @@ -148,8 +159,6 @@ namespace mesh const Point &_offset) = 0; }; }; - -} } } diff --git a/include/ignition/physics/detail/PlaneShape.hh b/include/ignition/physics/detail/PlaneShape.hh index c3eb2e587..dcc353b6c 100644 --- a/include/ignition/physics/detail/PlaneShape.hh +++ b/include/ignition/physics/detail/PlaneShape.hh @@ -25,8 +25,6 @@ namespace ignition { namespace physics -{ -namespace mesh { ///////////////////////////////////////////////// template @@ -74,6 +72,5 @@ namespace mesh } } } -} #endif // IGNITION_PHYSICS_DETAIL_PLANESHAPE_HH_ From e98802fa78c86050a1c5e9604003e2849757db83 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Thu, 7 Feb 2019 18:37:20 +0800 Subject: [PATCH 03/23] Adding test for mesh-plane collisions --- dartsim/src/Collisions_TEST.cc | 148 ++++++++++++++++++ dartsim/src/SimulationFeatures_TEST.cc | 14 +- include/ignition/physics/detail/FreeJoint.hh | 2 +- include/ignition/physics/detail/PlaneShape.hh | 7 +- 4 files changed, 154 insertions(+), 17 deletions(-) create mode 100644 dartsim/src/Collisions_TEST.cc diff --git a/dartsim/src/Collisions_TEST.cc b/dartsim/src/Collisions_TEST.cc new file mode 100644 index 000000000..c16365174 --- /dev/null +++ b/dartsim/src/Collisions_TEST.cc @@ -0,0 +1,148 @@ +/* + * Copyright (C) 2019 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include + +#include + +#include +#include +#include + +// Features +#include +#include +#include +#include +#include +#include +#include + +#include + +using TestFeatureList = ignition::physics::FeatureList< + ignition::physics::LinkFrameSemantics, + ignition::physics::ForwardStep, + ignition::physics::GetEntities, + ignition::physics::ConstructEmptyWorldFeature, + ignition::physics::ConstructEmptyModelFeature, + ignition::physics::ConstructEmptyLinkFeature, + ignition::physics::mesh::AttachMeshShapeFeature, + ignition::physics::AttachPlaneShapeFeature, + ignition::physics::SetFreeJointRelativeTransformFeature +>; + +using TestWorldPtr = ignition::physics::World3dPtr; +using TestEnginePtr = ignition::physics::Engine3dPtr; + +using WorldConstructor = std::function; + +std::unordered_set LoadWorlds( + const std::string &_library, + const WorldConstructor &_constructor) +{ + ignition::plugin::Loader loader; + loader.LoadLib(_library); + + const std::set pluginNames = + ignition::physics::FindFeatures3d::From(loader); + + std::unordered_set worlds; + for (const std::string &name : pluginNames) + { + ignition::plugin::PluginPtr plugin = loader.Instantiate(name); + + std::cout << " -- Plugin name: " << name << std::endl; + + auto engine = + ignition::physics::RequestEngine3d::From(plugin); + EXPECT_NE(nullptr, engine); + + worlds.insert(_constructor(engine)); + } + + return worlds; +} + +class Collisions_TEST + : public ::testing::Test, + public ::testing::WithParamInterface +{}; + +INSTANTIATE_TEST_CASE_P(PhysicsPlugins, Collisions_TEST, + ::testing::ValuesIn(ignition::physics::test::g_PhysicsPluginLibraries),); // NOLINT + +TestWorldPtr ConstructMeshPlaneWorld( + const ignition::physics::Engine3dPtr &_engine, + const ignition::common::Mesh &_mesh) +{ + auto world = _engine->ConstructEmptyWorld("world"); + + auto model = world->ConstructEmptyModel("mesh"); + auto link = model->ConstructEmptyLink("link"); + link->AttachMeshShape("shape", _mesh, ignition::physics::Pose3d::Identity()); + + model = world->ConstructEmptyModel("plane"); + link = model->ConstructEmptyLink("link"); + link->AttachPlaneShape("shape", ignition::physics::AngularVector3d::UnitZ(), + 5.0 * ignition::physics::LinearVector3d::UnitZ()); + + return world; +} + +TEST_P(Collisions_TEST, MeshAndPlane) +{ + const std::string library = GetParam(); + if (library.empty()) + return; + + const std::string meshFilename = IGNITION_PHYSICS_RESOURCE_DIR "/chassis.dae"; + auto &meshManager = *ignition::common::MeshManager::Instance(); + auto *mesh = meshManager.Load(meshFilename); + + std::cout << "Testing library " << library << std::endl; + auto worlds = LoadWorlds(library, [&](const TestEnginePtr &_engine) + { + return ConstructMeshPlaneWorld(_engine, *mesh); + }); + + for (const auto &world : worlds) + { + const auto link = world->GetModel(0)->GetLink(0); + + std::cout << link->FrameDataRelativeToWorld() + .pose.translation().transpose() << std::endl; + + ignition::physics::ForwardStep::Output output; + ignition::physics::ForwardStep::State state; + ignition::physics::ForwardStep::Input input; + for (std::size_t i = 0; i < 500; ++i) + { + // TODO(MXG): This is running suspiciously slow + world->Step(output, state, input); + } + + std::cout << link->FrameDataRelativeToWorld() + .pose.translation().transpose() << std::endl; + } +} + +int main(int argc, char *argv[]) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/dartsim/src/SimulationFeatures_TEST.cc b/dartsim/src/SimulationFeatures_TEST.cc index 2078dcb4a..575681f62 100644 --- a/dartsim/src/SimulationFeatures_TEST.cc +++ b/dartsim/src/SimulationFeatures_TEST.cc @@ -39,8 +39,7 @@ using TestFeatureList = ignition::physics::FeatureList< ignition::physics::LinkFrameSemantics, ignition::physics::ForwardStep, ignition::physics::GetEntities, - ignition::physics::sdf::ConstructSdfWorld/*, - ignition::physics::*/ + ignition::physics::sdf::ConstructSdfWorld >; using TestWorldPtr = ignition::physics::World3dPtr; @@ -112,17 +111,6 @@ TEST_P(SimulationFeatures_TEST, Falling) } } -TEST_P(SimulationFeatures_TEST, CollisionBetweenMeshAndPlane) -{ - const std::string library = GetParam(); - if (library.empty()) - return; - - std::cout << "Testing library " << library << std::endl; - auto worlds = LoadWorlds(library, TEST_WORLD_DIR "/mesh_plane_falling.world"); - -} - int main(int argc, char *argv[]) { ::testing::InitGoogleTest(&argc, argv); diff --git a/include/ignition/physics/detail/FreeJoint.hh b/include/ignition/physics/detail/FreeJoint.hh index 08df68142..5a3922d13 100644 --- a/include/ignition/physics/detail/FreeJoint.hh +++ b/include/ignition/physics/detail/FreeJoint.hh @@ -30,7 +30,7 @@ namespace ignition SetRelativeTransform(const PoseType &_pose) { this->template Interface()-> - SetFreeJointTransform(this->identity, _pose); + SetFreeJointRelativeTransform(this->identity, _pose); } } } diff --git a/include/ignition/physics/detail/PlaneShape.hh b/include/ignition/physics/detail/PlaneShape.hh index dcc353b6c..d2746a25e 100644 --- a/include/ignition/physics/detail/PlaneShape.hh +++ b/include/ignition/physics/detail/PlaneShape.hh @@ -65,10 +65,11 @@ namespace physics PlaneShapePtr AttachPlaneShapeFeature::Link::AttachPlaneShape( const std::string &_name, const Normal &_normal, - const Point &_offset) + const Point &_point) { - return this->template Interface() - ->AttachPlaneShape(this->identity, _name, _normal, _offset); + return PlaneShapePtr(this->pimpl, + this->template Interface() + ->AttachPlaneShape(this->identity, _name, _normal, _point)); } } } From 06ac26929a960abaeeb0f3675aa02869fad770ff Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Fri, 8 Feb 2019 17:37:07 +0800 Subject: [PATCH 04/23] Finish test for plane features --- dartsim/src/Collisions_TEST.cc | 35 ++++++++++++++++++++++++---------- 1 file changed, 25 insertions(+), 10 deletions(-) diff --git a/dartsim/src/Collisions_TEST.cc b/dartsim/src/Collisions_TEST.cc index c16365174..c7e7ab4e9 100644 --- a/dartsim/src/Collisions_TEST.cc +++ b/dartsim/src/Collisions_TEST.cc @@ -31,6 +31,7 @@ #include #include #include +#include #include @@ -43,7 +44,8 @@ using TestFeatureList = ignition::physics::FeatureList< ignition::physics::ConstructEmptyLinkFeature, ignition::physics::mesh::AttachMeshShapeFeature, ignition::physics::AttachPlaneShapeFeature, - ignition::physics::SetFreeJointRelativeTransformFeature + ignition::physics::SetFreeJointRelativeTransformFeature, + ignition::physics::AttachFixedJointFeature >; using TestWorldPtr = ignition::physics::World3dPtr; @@ -92,14 +94,23 @@ TestWorldPtr ConstructMeshPlaneWorld( { auto world = _engine->ConstructEmptyWorld("world"); + Eigen::Isometry3d tf = Eigen::Isometry3d::Identity(); + tf.translation()[2] = 2.0; + auto model = world->ConstructEmptyModel("mesh"); auto link = model->ConstructEmptyLink("link"); - link->AttachMeshShape("shape", _mesh, ignition::physics::Pose3d::Identity()); + // TODO(anyone): This test is somewhat awkward because we lift up the mesh + // from the center of the link instead of lifting up the link or the model. + // We're doing this because we don't currently have an API for moving models + // or links around. See the conversation here for more: + // https://bitbucket.org/ignitionrobotics/ign-physics/pull-requests/46/wip-add-link-features/diff#comment-87592809 + link->AttachMeshShape("mesh", _mesh, tf); model = world->ConstructEmptyModel("plane"); link = model->ConstructEmptyLink("link"); - link->AttachPlaneShape("shape", ignition::physics::AngularVector3d::UnitZ(), - 5.0 * ignition::physics::LinearVector3d::UnitZ()); + + link->AttachPlaneShape("plane", ignition::physics::AngularVector3d::UnitZ()); + link->AttachFixedJoint(nullptr); return world; } @@ -124,20 +135,24 @@ TEST_P(Collisions_TEST, MeshAndPlane) { const auto link = world->GetModel(0)->GetLink(0); - std::cout << link->FrameDataRelativeToWorld() - .pose.translation().transpose() << std::endl; + EXPECT_NEAR( + 0.0, link->FrameDataRelativeToWorld().pose.translation()[2], 1e-6); ignition::physics::ForwardStep::Output output; ignition::physics::ForwardStep::State state; ignition::physics::ForwardStep::Input input; - for (std::size_t i = 0; i < 500; ++i) + for (std::size_t i = 0; i < 1000; ++i) { - // TODO(MXG): This is running suspiciously slow world->Step(output, state, input); } - std::cout << link->FrameDataRelativeToWorld() - .pose.translation().transpose() << std::endl; + // Make sure the mesh was stopped by the plane. In 2000 time steps at the + // default step size of 0.001, a free falling body should drop approximately + // 19.6 meters. As long as the body is somewhere near 1.91, then it has been + // stopped by the plane (the exact value might vary because the body might + // be rocking side-to-side after falling). + EXPECT_NEAR( + -1.91, link->FrameDataRelativeToWorld().pose.translation()[2], 0.05); } } From 4aa9cefee4e326e24d538e19b11d708540971983 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Fri, 8 Feb 2019 17:40:51 +0800 Subject: [PATCH 05/23] Fix conflict --- CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 1400ebe0d..79cd36889 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -56,8 +56,8 @@ ign_find_package(sdformat8 REQUIRED_BY sdf dartsim) ign_find_package(DART COMPONENTS collision-ode -# utils -# utils-urdf + utils + utils-urdf CONFIG VERSION 6.7.2 REQUIRED_BY dartsim From 33701d771ec7e7a2c60034ded46de7d177d13f27 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Thu, 14 Feb 2019 10:24:11 -0800 Subject: [PATCH 06/23] fix whitespace --- include/ignition/physics/PlaneShape.hh | 2 +- include/ignition/physics/detail/PlaneShape.hh | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/include/ignition/physics/PlaneShape.hh b/include/ignition/physics/PlaneShape.hh index 31736ab60..9de30066d 100644 --- a/include/ignition/physics/PlaneShape.hh +++ b/include/ignition/physics/PlaneShape.hh @@ -164,4 +164,4 @@ namespace physics #include -#endif // IGNITION_PHYSICS_PLANESHAPE_HH_ +#endif // IGNITION_PHYSICS_PLANESHAPE_HH_ diff --git a/include/ignition/physics/detail/PlaneShape.hh b/include/ignition/physics/detail/PlaneShape.hh index d2746a25e..36d07e15e 100644 --- a/include/ignition/physics/detail/PlaneShape.hh +++ b/include/ignition/physics/detail/PlaneShape.hh @@ -74,4 +74,4 @@ namespace physics } } -#endif // IGNITION_PHYSICS_DETAIL_PLANESHAPE_HH_ +#endif // IGNITION_PHYSICS_DETAIL_PLANESHAPE_HH_ From e79690586e5ba40ecc4d847e41e4babd8e9e039d Mon Sep 17 00:00:00 2001 From: chapulina Date: Mon, 13 Apr 2020 13:20:58 -0700 Subject: [PATCH 07/23] Bump to 1.7.0 --- CMakeLists.txt | 2 +- Changelog.md | 2 ++ 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 3738abb15..d5083f0ee 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,7 +3,7 @@ cmake_minimum_required(VERSION 3.5.1 FATAL_ERROR) #============================================================================ # Initialize the project #============================================================================ -project(ignition-physics1 VERSION 1.6.0) +project(ignition-physics1 VERSION 1.7.0) #============================================================================ # Find ignition-cmake diff --git a/Changelog.md b/Changelog.md index 4788f5f60..659eb6a57 100644 --- a/Changelog.md +++ b/Changelog.md @@ -2,6 +2,8 @@ ### Ignition Physics 1.x.x (20XX-XX-XX) +### Ignition Physics 1.7.0 (2020-04-13) + 1. Add RequestFeatures API for casting the features of an entity to a new feature set when possible. * [Pull request 130](https://bitbucket.org/ignitionrobotics/ign-physics/pull-requests/130) From 45055e83d6f8f300c6a4357fe408e557dd6b4c0d Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Mon, 13 Apr 2020 20:31:31 +0000 Subject: [PATCH 08/23] Close branch bump_1.7.0 From ac4843ae96cb79841f81e0a83991ae0d5c9ca817 Mon Sep 17 00:00:00 2001 From: chapulina Date: Mon, 13 Apr 2020 13:33:50 -0700 Subject: [PATCH 09/23] Added tag ignition-physics_1.7.0 for changeset 90e58cd7ba26 From 79acf719668d94617c5c78e4299b6de1ffdef2b8 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Mon, 13 Apr 2020 17:24:12 -0500 Subject: [PATCH 10/23] Use LinearVector for the normal vector of planes --- dartsim/src/Collisions_TEST.cc | 2 +- dartsim/src/ShapeFeatures.cc | 7 ++++--- dartsim/src/ShapeFeatures.hh | 4 ++-- include/ignition/physics/PlaneShape.hh | 15 +++++++-------- 4 files changed, 14 insertions(+), 14 deletions(-) diff --git a/dartsim/src/Collisions_TEST.cc b/dartsim/src/Collisions_TEST.cc index c7e7ab4e9..d7c28f39c 100644 --- a/dartsim/src/Collisions_TEST.cc +++ b/dartsim/src/Collisions_TEST.cc @@ -109,7 +109,7 @@ TestWorldPtr ConstructMeshPlaneWorld( model = world->ConstructEmptyModel("plane"); link = model->ConstructEmptyLink("link"); - link->AttachPlaneShape("plane", ignition::physics::AngularVector3d::UnitZ()); + link->AttachPlaneShape("plane", ignition::physics::LinearVector3d::UnitZ()); link->AttachFixedJoint(nullptr); return world; diff --git a/dartsim/src/ShapeFeatures.cc b/dartsim/src/ShapeFeatures.cc index 7c54843b3..3264d2e60 100644 --- a/dartsim/src/ShapeFeatures.cc +++ b/dartsim/src/ShapeFeatures.cc @@ -275,7 +275,7 @@ Identity ShapeFeatures::CastToPlaneShape(const Identity &_shapeID) const } ///////////////////////////////////////////////// -AngularVector3d ShapeFeatures::GetPlaneShapeNormal( +LinearVector3d ShapeFeatures::GetPlaneShapeNormal( const Identity &_planeID) const { const auto *shapeInfo = this->ReferenceInterface(_planeID); @@ -311,8 +311,9 @@ Identity ShapeFeatures::AttachPlaneShape( DartBodyNode *bn = this->ReferenceInterface(_linkID)->link.get(); dart::dynamics::ShapeNode *sn = - bn->createShapeNodeWith( - plane, bn->getName() + ":" + _name); + bn->createShapeNodeWith( + plane, bn->getName() + ":" + _name); const std::size_t shapeID = this->AddShape({sn, _name}); return this->GenerateIdentity(shapeID, this->shapes.at(shapeID)); diff --git a/dartsim/src/ShapeFeatures.hh b/dartsim/src/ShapeFeatures.hh index 3ea658aec..33d3df826 100644 --- a/dartsim/src/ShapeFeatures.hh +++ b/dartsim/src/ShapeFeatures.hh @@ -139,7 +139,7 @@ class ShapeFeatures : public: Identity CastToPlaneShape( const Identity &_shapeID) const override; - public: AngularVector3d GetPlaneShapeNormal( + public: LinearVector3d GetPlaneShapeNormal( const Identity &_planeID) const override; public: LinearVector3d GetPlaneShapePoint( @@ -148,7 +148,7 @@ class ShapeFeatures : public: Identity AttachPlaneShape( const Identity &_linkID, const std::string &_name, - const AngularVector3d &_normal, + const LinearVector3d &_normal, const LinearVector3d &_point) override; }; diff --git a/include/ignition/physics/PlaneShape.hh b/include/ignition/physics/PlaneShape.hh index 9de30066d..28f9d4dcf 100644 --- a/include/ignition/physics/PlaneShape.hh +++ b/include/ignition/physics/PlaneShape.hh @@ -37,13 +37,12 @@ namespace physics class PlaneShape : public virtual Entity { public: using Normal = - typename FromPolicy::template Use; + typename FromPolicy::template Use; public: using Point = typename FromPolicy::template Use; - /// \brief Get the normal vector for this plane. For a 2D simulation, this - /// will be a scalar quantity. + /// \brief Get the normal vector for this plane. /// \returns the normal vector for this plane. public: Normal GetNormal() const; @@ -57,7 +56,7 @@ namespace physics class Implementation : public virtual Feature::Implementation { public: using Normal = - typename FromPolicy::template Use; + typename FromPolicy::template Use; public: using Point = typename FromPolicy::template Use; @@ -80,7 +79,7 @@ namespace physics class PlaneShape : public virtual Entity { public: using Normal = - typename FromPolicy::template Use; + typename FromPolicy::template Use; public: using Point = typename FromPolicy::template Use; @@ -101,7 +100,7 @@ namespace physics class Implementation : public virtual Feature::Implementation { public: using Normal = - typename FromPolicy::template Use; + typename FromPolicy::template Use; public: using Point = typename FromPolicy::template Use; @@ -124,7 +123,7 @@ namespace physics class Link : public virtual Feature::Link { public: using Normal = - typename FromPolicy::template Use; + typename FromPolicy::template Use; public: using Point = typename FromPolicy::template Use; @@ -147,7 +146,7 @@ namespace physics class Implementation : public virtual Feature::Implementation { public: using Normal = - typename FromPolicy::template Use; + typename FromPolicy::template Use; public: using Point = typename FromPolicy::template Use; From f5e20fe00ce9914ff2997c3eb2e0f078ea9bdd23 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Mon, 13 Apr 2020 17:31:15 -0500 Subject: [PATCH 11/23] Update function documentation, remove unnecessary include --- dartsim/src/SimulationFeatures_TEST.cc | 1 - include/ignition/physics/PlaneShape.hh | 3 +-- 2 files changed, 1 insertion(+), 3 deletions(-) diff --git a/dartsim/src/SimulationFeatures_TEST.cc b/dartsim/src/SimulationFeatures_TEST.cc index 77fd99569..cdfed1877 100644 --- a/dartsim/src/SimulationFeatures_TEST.cc +++ b/dartsim/src/SimulationFeatures_TEST.cc @@ -34,7 +34,6 @@ #include #include #include -#include #include #include diff --git a/include/ignition/physics/PlaneShape.hh b/include/ignition/physics/PlaneShape.hh index 28f9d4dcf..29db226e0 100644 --- a/include/ignition/physics/PlaneShape.hh +++ b/include/ignition/physics/PlaneShape.hh @@ -46,8 +46,7 @@ namespace physics /// \returns the normal vector for this plane. public: Normal GetNormal() const; - /// \brief Get a point on the plane. For a 2D simulation, this value - /// is irrelevant, because all (x, y) locations will be on the plane. + /// \brief Get a point on the plane. /// \returns the offset of the plane. public: Point GetPoint() const; }; From 5d260abc62de67134e12bb9893de36d48e333d0f Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Tue, 14 Apr 2020 17:59:32 +0000 Subject: [PATCH 12/23] Close branch use_ode_collision_detection From 0a47f8ac4f81b07810b32b1869da2e508a13daee Mon Sep 17 00:00:00 2001 From: claireyywang Date: Fri, 17 Apr 2020 16:42:49 -0700 Subject: [PATCH 13/23] add .gitignore from .hgignore Signed-off-by: claireyywang --- .hgignore => .gitignore | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename .hgignore => .gitignore (100%) diff --git a/.hgignore b/.gitignore similarity index 100% rename from .hgignore rename to .gitignore From 30b00489ce566fd7fd4e4fe036b22f4bc1be93b4 Mon Sep 17 00:00:00 2001 From: claireyywang Date: Fri, 17 Apr 2020 16:43:47 -0700 Subject: [PATCH 14/23] remove hg syntax and add comment Signed-off-by: claireyywang --- .gitignore | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/.gitignore b/.gitignore index ebd4abe1b..62b9898cc 100644 --- a/.gitignore +++ b/.gitignore @@ -1,6 +1,7 @@ -syntax: glob - +# CMake folders build build_* + +# OS generated files .DS_Store *.swp From 4496acdd5f039feac702afdda5cf957396948f83 Mon Sep 17 00:00:00 2001 From: chapulina Date: Tue, 21 Apr 2020 14:13:19 -0700 Subject: [PATCH 15/23] [ign-physics1] Update BitBucket links (#33) * [ign-physics1] Update BitBucket links Signed-off-by: Louise Poubel --- CONTRIBUTING.md | 2 +- Changelog.md | 47 +++++++++++---------- README.md | 54 ++++++++++++------------- bitbucket-pipelines.yml | 2 +- dartsim/src/Collisions_TEST.cc | 2 +- dartsim/src/EntityManagementFeatures.cc | 2 +- tutorials/02_installation.md | 8 ++-- 7 files changed, 58 insertions(+), 59 deletions(-) diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md index 6877dd263..147239ce5 100644 --- a/CONTRIBUTING.md +++ b/CONTRIBUTING.md @@ -1 +1 @@ -See the [Ignition Robotics contributing guide](https://bitbucket.org/ignitionrobotics/ign-gazebo/src/default/CONTRIBUTING.md). +See the [Ignition Robotics contributing guide](https://ignitionrobotics.org/docs/all/contributing). diff --git a/Changelog.md b/Changelog.md index 659eb6a57..6ab65e9c1 100644 --- a/Changelog.md +++ b/Changelog.md @@ -2,47 +2,50 @@ ### Ignition Physics 1.x.x (20XX-XX-XX) +1. Add PlaneShape feature and implement in dartsim with test. + * [BitBucket pull request 66](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-physics/pull-requests/66) + ### Ignition Physics 1.7.0 (2020-04-13) 1. Add RequestFeatures API for casting the features of an entity to a new feature set when possible. - * [Pull request 130](https://bitbucket.org/ignitionrobotics/ign-physics/pull-requests/130) + * [BitBucket pull request 130](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-physics/pull-requests/130) ### Ignition Physics 1.6.0 (2020-03-18) 1. Add Get Bounding Box features - * [Pull request 122](https://bitbucket.org/ignitionrobotics/ign-physics/pull-requests/122) + * [BitBucket pull request 122](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-physics/pull-requests/122) 1. Install plugins to unversioned files - * [Pull request 121](https://bitbucket.org/ignitionrobotics/ign-physics/pull-requests/121) + * [BitBucket pull request 121](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-physics/pull-requests/121) ### Ignition Physics 1.5.0 (2020-02-10) 1. Clean up internal resources when a model gets removed - * [Pull request 115](https://bitbucket.org/ignitionrobotics/ign-physics/pull-requests/115) + * [BitBucket pull request 115](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-physics/pull-requests/115) 1. Add DetachJointFeature feature. - * [Pull request 102](https://bitbucket.org/ignitionrobotics/ign-physics/pull-requests/102) + * [BitBucket pull request 102](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-physics/pull-requests/102) ### Ignition Physics 1.4.0 (2019-08-27) 1. Add SetJointVelocityCommand feature. - * [Pull request 100](https://bitbucket.org/ignitionrobotics/ign-physics/pull-requests/100) + * [BitBucket pull request 100](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-physics/pull-requests/100) 1. Add `IGN_PROFILER_ENABLE` cmake option for enabling the ign-common profiler. - * [Pull request 96](https://bitbucket.org/ignitionrobotics/ign-physics/pull-requests/96) + * [BitBucket pull request 96](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-physics/pull-requests/96) ### Ignition Physics 1.3.1 (2019-07-19) 1. Set the time step from ForwardStep::Input in dartsim. - * [Pull request 95](https://bitbucket.org/ignitionrobotics/ign-physics/pull-requests/95) + * [BitBucket pull request 95](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-physics/pull-requests/95) ### Ignition Physics 1.3.0 (2019-07-18) 1. Support for more friction pyramid parameters in dartsim. - * [Pull request 94](https://bitbucket.org/ignitionrobotics/ign-physics/pull-requests/94) + * [BitBucket pull request 94](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-physics/pull-requests/94) 1. Skip compilation of test plugins if `BUILD_TESTING` is false - * [Pull request 92](https://bitbucket.org/ignitionrobotics/ign-physics/pull-requests/92) + * [BitBucket pull request 92](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-physics/pull-requests/92) ### Ignition Physics 1.2.0 (2019-05-29) @@ -51,34 +54,34 @@ ### Ignition Physics 1.1.0 (2019-05-20) 1. Simple port of existing PERFORMANCE test as BENCHMARK - * [Pull request 84](https://bitbucket.org/ignitionrobotics/ign-physics/pull-requests/84) + * [BitBucket pull request 84](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-physics/pull-requests/84) 1. Add prototype of FreeGroup features - * [Pull request 85](https://bitbucket.org/ignitionrobotics/ign-physics/pull-requests/85) + * [BitBucket pull request 85](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-physics/pull-requests/85) 1. Feature for adding external forces and torques to a link - * [Pull request 79](https://bitbucket.org/ignitionrobotics/ign-physics/pull-requests/79) + * [BitBucket pull request 79](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-physics/pull-requests/79) 1. Assign friction coefficients from collision elements - * [Pull request 80](https://bitbucket.org/ignitionrobotics/ign-physics/pull-requests/80) + * [BitBucket pull request 80](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-physics/pull-requests/80) 1. Added axis-aligned bounding box feature - * [Pull request 68](https://bitbucket.org/ignitionrobotics/ign-physics/pull-requests/68) - * [Pull request 69](https://bitbucket.org/ignitionrobotics/ign-physics/pull-requests/69) - * [Pull request 71](https://bitbucket.org/ignitionrobotics/ign-physics/pull-requests/71) - * [Pull request 78](https://bitbucket.org/ignitionrobotics/ign-physics/pull-requests/78) + * [BitBucket pull request 68](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-physics/pull-requests/68) + * [BitBucket pull request 69](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-physics/pull-requests/69) + * [BitBucket pull request 71](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-physics/pull-requests/71) + * [BitBucket pull request 78](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-physics/pull-requests/78) 1. Add introduction and installation tutorials - * [Pull request 76](https://bitbucket.org/ignitionrobotics/ign-physics/pull-requests/76) - * [Pull request 77](https://bitbucket.org/ignitionrobotics/ign-physics/pull-requests/77) + * [BitBucket pull request 76](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-physics/pull-requests/76) + * [BitBucket pull request 77](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-physics/pull-requests/77) 1. Remove workaround for console\_bridge linking on macOS - * [Pull request 75](https://bitbucket.org/ignitionrobotics/ign-physics/pull-requests/75) + * [BitBucket pull request 75](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-physics/pull-requests/75) ### Ignition Physics 1.0.1 (2019-03-05) 1. Don't link core to ignition-common3, just the test plugin that uses it - * [Pull request 74](https://bitbucket.org/ignitionrobotics/ign-physics/pull-requests/74) + * [BitBucket pull request 74](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-physics/pull-requests/74) ### Ignition Physics 1.0.0 (2019-03-01) diff --git a/README.md b/README.md index 9f9fbd822..25148bdee 100644 --- a/README.md +++ b/README.md @@ -2,8 +2,8 @@ **Maintainer:** scpeters AT openrobotics DOT org -[![Bitbucket open issues](https://img.shields.io/bitbucket/issues-raw/ignitionrobotics/ign-physics.svg)](https://bitbucket.org/ignitionrobotics/ign-physics/issues) -[![Bitbucket open pull requests](https://img.shields.io/bitbucket/pr-raw/ignitionrobotics/ign-physics.svg)](https://bitbucket.org/ignitionrobotics/ign-physics/pull-requests) +[![GitHub open issues](https://img.shields.io/github/issues-raw/ignitionrobotics/ign-physics.svg)](https://github.com/ignitionrobotics/ign-physics/issues) +[![GitHub open pull requests](https://img.shields.io/github/issues-pr-raw/ignitionrobotics/ign-physics.svg)](https://github.com/ignitionrobotics/ign-physics/pulls) [![Discourse topics](https://img.shields.io/discourse/https/community.gazebosim.org/topics.svg)](https://community.gazebosim.org) [![Hex.pm](https://img.shields.io/hexpm/l/plug.svg)](https://www.apache.org/licenses/LICENSE-2.0) @@ -20,35 +20,35 @@ designed to support simulation and rapid development of robot applications. # Table of Contents -[Motivation](#markdown-header-motivation) +[Motivation](#motivation) -[Features](#markdown-header-features) +[Features](#features) -[Install](#markdown-header-install) +[Install](#install) -* [Binary Install](#markdown-header-binary-install) +* [Binary Install](#binary-install) -* [Source Install](#markdown-header-source-install) +* [Source Install](#source-install) - * [Prerequisites](#markdown-header-prerequisites) + * [Prerequisites](#prerequisites) - * [Building from Source](#markdown-header-building-from-source) + * [Building from Source](#building-from-source) -[Usage](#markdown-header-usage) +[Usage](#usage) -[Documentation](#markdown-header-documentation) +[Documentation](#documentation) -[Testing](#markdown-header-testing) +[Testing](#testing) -[Folder Structure](#markdown-header-folder-structure) +[Folder Structure](#folder-structure) -[Code of Conduct](#markdown-header-code-of-conduct) +[Code of Conduct](#code-of-conduct) -[Contributing](#markdown-header-code-of-contributing) +[Contributing](#code-of-contributing) -[Versioning](#markdown-header-versioning) +[Versioning](#versioning) -[License](#markdown-header-license) +[License](#license) # Motivation @@ -80,9 +80,9 @@ Ignition Physics provides the following functionality: # Install -We recommend following the [Binary Install](#markdown-header-binary-install) instructions to get up and running as quickly and painlessly as possible. +We recommend following the [Binary Install](#binary-install) instructions to get up and running as quickly and painlessly as possible. -The [Source Install](#markdown-header-source-install) instructions should be used if you need the very latest software improvements, you need to modify the code, or you plan to make a contribution. +The [Source Install](#source-install) instructions should be used if you need the very latest software improvements, you need to modify the code, or you plan to make a contribution. ## Binary Install @@ -140,7 +140,7 @@ sudo update-alternatives --install \ 1. Clone the repository ``` - hg clone https://bitbucket.org/ignitionrobotics/ign-physics + git clone https://github.com/ignitionrobotics/ign-physics ``` 2. Configure and build @@ -155,10 +155,6 @@ sudo update-alternatives --install \ sudo make install ``` -# Usage - -Please refer to the [examples directory](https://bitbucket.org/ignitionrobotics/ign-physics/raw/default/examples/?at=default). - # Documentation API and tutorials can be found at [https://ignitionrobotics.org/libs/physics](https://ignitionrobotics.org/libs/physics). @@ -174,7 +170,7 @@ You can also generate the documentation from a clone of this repository by follo 2. Clone the repository ``` - hg clone https://bitbucket.org/ignitionrobotics/ign-physics + git clone https://github.com/ignitionrobotics/ign-physics ``` 3. Configure and build the documentation. @@ -193,7 +189,7 @@ You can also generate the documentation from a clone of this repository by follo Follow these steps to run tests and static code analysis in your clone of this repository. -1. Follow the [source install instruction](#markdown-header-source-install). +1. Follow the [source install instruction](#source-install). 2. Run tests. @@ -233,12 +229,12 @@ ign-physics # Contributing Please see -[CONTRIBUTING.md](https://bitbucket.org/ignitionrobotics/ign-gazebo/src/406665896aa40bb42f14cf61d48b3d94f2fc5dd8/CONTRIBUTING.md?at=default&fileviewer=file-view-default). +[CONTRIBUTING.md](https://github.com/ignitionrobotics/ign-gazebo/blob/master/CONTRIBUTING.md). # Code of Conduct Please see -[CODE\_OF\_CONDUCT.md](https://bitbucket.org/ignitionrobotics/ign-gazebo/src/406665896aa40bb42f14cf61d48b3d94f2fc5dd8/CODE_OF_CONDUCT.md?at=default&fileviewer=file-view-default). +[CODE\_OF\_CONDUCT.md](https://github.com/ignitionrobotics/ign-gazebo/blob/master/CODE_OF_CONDUCT.md). # Versioning @@ -246,4 +242,4 @@ This library uses [Semantic Versioning](https://semver.org/). Additionally, this # License -This library is licensed under [Apache 2.0](https://www.apache.org/licenses/LICENSE-2.0). See also the [LICENSE](https://bitbucket.org/ignitionrobotics/ign-physics/src/default/LICENSE) file. +This library is licensed under [Apache 2.0](https://www.apache.org/licenses/LICENSE-2.0). See also the [LICENSE](https://github.com/ignitionrobotics/ign-physics/blob/master/LICENSE) file. diff --git a/bitbucket-pipelines.yml b/bitbucket-pipelines.yml index fdf4b2840..286d061dd 100644 --- a/bitbucket-pipelines.yml +++ b/bitbucket-pipelines.yml @@ -5,7 +5,7 @@ pipelines: - step: script: - apt-get update - - apt-get -y install cmake build-essential curl git mercurial cppcheck software-properties-common g++-8 ruby-dev + - apt-get -y install cmake build-essential curl git cppcheck software-properties-common g++-8 ruby-dev - update-alternatives --install /usr/bin/gcc gcc /usr/bin/gcc-8 800 --slave /usr/bin/g++ g++ /usr/bin/g++-8 --slave /usr/bin/gcov gcov /usr/bin/gcov-8 - gcc -v - g++ -v diff --git a/dartsim/src/Collisions_TEST.cc b/dartsim/src/Collisions_TEST.cc index d7c28f39c..33e4d5ca8 100644 --- a/dartsim/src/Collisions_TEST.cc +++ b/dartsim/src/Collisions_TEST.cc @@ -103,7 +103,7 @@ TestWorldPtr ConstructMeshPlaneWorld( // from the center of the link instead of lifting up the link or the model. // We're doing this because we don't currently have an API for moving models // or links around. See the conversation here for more: - // https://bitbucket.org/ignitionrobotics/ign-physics/pull-requests/46/wip-add-link-features/diff#comment-87592809 + // https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-physics/pull-requests/46/page/1#comment-87592809 link->AttachMeshShape("mesh", _mesh, tf); model = world->ConstructEmptyModel("plane"); diff --git a/dartsim/src/EntityManagementFeatures.cc b/dartsim/src/EntityManagementFeatures.cc index 230a9039b..0295adb0b 100644 --- a/dartsim/src/EntityManagementFeatures.cc +++ b/dartsim/src/EntityManagementFeatures.cc @@ -33,7 +33,7 @@ namespace dartsim { ///////////////////////////////////////////////// /// This class helps to resolve an issue with excessive contacts being computed: -/// https://bitbucket.org/ignitionrobotics/ign-physics/issues/11/ +/// https://github.com/ignitionrobotics/ign-physics/issues/11/ /// /// TODO(MXG): Delete this class when we switch to using dartsim-6.8: /// https://github.com/dartsim/dart/pull/1232 diff --git a/tutorials/02_installation.md b/tutorials/02_installation.md index 716d77598..b1e6d9296 100644 --- a/tutorials/02_installation.md +++ b/tutorials/02_installation.md @@ -50,9 +50,9 @@ sudo apt-get install libignition-cmake2-dev \ libsdformat8-dev ``` -Clone the ign-physics repository from bitbucket +Clone the ign-physics repository from GitHub ```{.sh} -hg clone https://bitbucket.org/ignitionrobotics/ign-physics +git clone https://github.com/ignitionrobotics/ign-physics ``` Then build using CMake @@ -98,9 +98,9 @@ brew tap osrf/simulation brew install ignition-physics1 --only-dependencies ``` -Clone the ign-physics repository from bitbucket +Clone the ign-physics repository from GitHub ```{.sh} -hg clone https://bitbucket.org/ignitionrobotics/ign-physics +git clone https://github.com/ignitionrobotics/ign-physics ``` Then build using CMake From 6f03a085e7916b9411e5d945af7f06c767c27f29 Mon Sep 17 00:00:00 2001 From: Addisu Taddese Date: Wed, 22 Apr 2020 20:35:54 -0500 Subject: [PATCH 16/23] Fix collision issue with detachable joints (#31) Fix collision issue with detachable joints The issue occurs if a model is spawned and is attached to a parent model without a call to `step()` in between. This should be fixed in DART, but in the meantime, we can just call `incrementVersion` after `moveTo` is called on a DART BodyNode. --- Changelog.md | 3 + dartsim/src/JointFeatures.cc | 12 +++ dartsim/src/JointFeatures_TEST.cc | 150 ++++++++++++++++++++++++++++++ dartsim/worlds/ground.sdf | 35 +++++++ 4 files changed, 200 insertions(+) create mode 100644 dartsim/worlds/ground.sdf diff --git a/Changelog.md b/Changelog.md index 6ab65e9c1..b99305fa5 100644 --- a/Changelog.md +++ b/Changelog.md @@ -2,6 +2,9 @@ ### Ignition Physics 1.x.x (20XX-XX-XX) +1. Fix collision issue with detachable joints + * [Pull request 31](https://github.com/ignitionrobotics/ign-physics/pull/31) + 1. Add PlaneShape feature and implement in dartsim with test. * [BitBucket pull request 66](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-physics/pull-requests/66) diff --git a/dartsim/src/JointFeatures.cc b/dartsim/src/JointFeatures.cc index 6252ea595..ef9d8798e 100644 --- a/dartsim/src/JointFeatures.cc +++ b/dartsim/src/JointFeatures.cc @@ -161,6 +161,9 @@ void JointFeatures::DetachJoint(const Identity &_jointId) freeJoint->setSpatialVelocity(spatialVelocity, dart::dynamics::Frame::World(), dart::dynamics::Frame::World()); + // TODO(addisu) Remove incrementVersion once DART has been updated to + // internally increment the BodyNode's version after moveTo. + child->incrementVersion(); } ///////////////////////////////////////////////// @@ -200,6 +203,9 @@ Identity JointFeatures::AttachFixedJoint( const std::size_t jointID = this->AddJoint( bn->moveTo(parentBn, properties)); + // TODO(addisu) Remove incrementVersion once DART has been updated to + // internally increment the BodyNode's version after moveTo. + bn->incrementVersion(); return this->GenerateIdentity(jointID, this->joints.at(jointID)); } @@ -282,6 +288,9 @@ Identity JointFeatures::AttachRevoluteJoint( const std::size_t jointID = this->AddJoint( bn->moveTo(parentBn, properties)); + // TODO(addisu) Remove incrementVersion once DART has been updated to + // internally increment the BodyNode's version after moveTo. + bn->incrementVersion(); return this->GenerateIdentity(jointID, this->joints.at(jointID)); } @@ -341,6 +350,9 @@ Identity JointFeatures::AttachPrismaticJoint( const std::size_t jointID = this->AddJoint( bn->moveTo(parentBn, properties)); + // TODO(addisu) Remove incrementVersion once DART has been updated to + // internally increment the BodyNode's version after moveTo. + bn->incrementVersion(); return this->GenerateIdentity(jointID, this->joints.at(jointID)); } diff --git a/dartsim/src/JointFeatures_TEST.cc b/dartsim/src/JointFeatures_TEST.cc index db1bc7cb0..996014920 100644 --- a/dartsim/src/JointFeatures_TEST.cc +++ b/dartsim/src/JointFeatures_TEST.cc @@ -37,8 +37,10 @@ #include #include #include +#include #include +#include #include #include @@ -50,12 +52,14 @@ using TestFeatureList = ignition::physics::FeatureList< physics::dartsim::RetrieveWorld, physics::AttachFixedJointFeature, physics::DetachJointFeature, + physics::SetJointTransformFromParentFeature, physics::ForwardStep, physics::FreeJointCast, physics::GetBasicJointState, physics::GetEntities, physics::RevoluteJointCast, physics::SetJointVelocityCommandFeature, + physics::sdf::ConstructSdfModel, physics::sdf::ConstructSdfWorld >; @@ -317,8 +321,17 @@ TEST_F(JointFeaturesFixture, JointAttachDetach) EXPECT_GT(0.0, body2LinearVelocity.Z()); } + const auto poseParent = dartBody1->getTransform(); + const auto poseChild = dartBody2->getTransform(); + auto poseParentChild = poseParent.inverse() * poseChild; + auto fixedJoint = model2Body->AttachFixedJoint(model1Body); + // AttachFixedJoint snaps the child body to the origin of the parent, so we + // set a transform on the joint to keep the transform between the two bodies + // the same as it was before they were attached + fixedJoint->SetTransformFromParent(poseParentChild); + for (std::size_t i = 0; i < numSteps; ++i) { world->Step(output, state, input); @@ -350,6 +363,14 @@ TEST_F(JointFeaturesFixture, JointAttachDetach) // Negative z velocity EXPECT_GT(0.0, body2LinearVelocity.Z()); } + + // After a while, body2 should reach the ground and come to a stop + for (std::size_t i = 0; i < 1000; ++i) + { + world->Step(output, state, input); + } + + EXPECT_NEAR(0.0, dartBody2->getLinearVelocity().z(), 1e-3); } ///////////////////////////////////////////////// @@ -397,6 +418,135 @@ TEST_F(JointFeaturesFixture, LinkCountsInJointAttachDetach) EXPECT_EQ(0u, model2->GetLinkCount()); } +///////////////////////////////////////////////// +// Attach a fixed joint between links that belong to different models where one +// of the models is created after a step is called +TEST_F(JointFeaturesFixture, JointAttachDetachSpawnedModel) +{ + std::string model1Str = R"( + + + 0 0 0.1 0 0 0 + + + + + 0.2 0.2 0.2 + + + + + + )"; + + std::string model2Str = R"( + + + 1 0 0.1 0 0 0 + + + + + 0.1 + + + + + + )"; + + physics::ForwardStep::Output output; + physics::ForwardStep::State state; + physics::ForwardStep::Input input; + + physics::World3dPtr world; + { + sdf::Root root; + const sdf::Errors errors = root.Load(TEST_WORLD_DIR "ground.sdf"); + ASSERT_TRUE(errors.empty()) << errors.front(); + world = this->engine->ConstructWorld(*root.WorldByIndex(0)); + ASSERT_NE(nullptr, world); + } + + { + sdf::Root root; + sdf::Errors errors = root.LoadSdfString(model1Str); + ASSERT_TRUE(errors.empty()) << errors.front(); + ASSERT_NE(nullptr, root.ModelByIndex(0)); + world->ConstructModel(*root.ModelByIndex(0)); + } + + world->Step(output, state, input); + + { + sdf::Root root; + sdf::Errors errors = root.LoadSdfString(model2Str); + ASSERT_TRUE(errors.empty()) << errors.front(); + ASSERT_NE(nullptr, root.ModelByIndex(0)); + world->ConstructModel(*root.ModelByIndex(0)); + } + + const std::string modelName1{"M1"}; + const std::string modelName2{"M2"}; + const std::string bodyName1{"body"}; + const std::string bodyName2{"chassis"}; + + auto model1 = world->GetModel(modelName1); + auto model2 = world->GetModel(modelName2); + auto model1Body = model1->GetLink(bodyName1); + auto model2Body = model2->GetLink(bodyName2); + + dart::simulation::WorldPtr dartWorld = world->GetDartsimWorld(); + ASSERT_NE(nullptr, dartWorld); + + const auto skeleton1 = dartWorld->getSkeleton(modelName1); + const auto skeleton2 = dartWorld->getSkeleton(modelName2); + ASSERT_NE(nullptr, skeleton1); + ASSERT_NE(nullptr, skeleton2); + + auto *dartBody1 = skeleton1->getBodyNode(bodyName1); + auto *dartBody2 = skeleton2->getBodyNode(bodyName2); + + ASSERT_NE(nullptr, dartBody1); + ASSERT_NE(nullptr, dartBody2); + + const auto poseParent = dartBody1->getTransform(); + const auto poseChild = dartBody2->getTransform(); + + // Before ign-physics PR #31, uncommenting the following `step` call makes + // this test pass, but commenting it out makes it fail. + // world->Step(output, state, input); + auto fixedJoint = model2Body->AttachFixedJoint(model1Body); + + // Pose of child relative to parent + auto poseParentChild = poseParent.inverse() * poseChild; + + // We let the joint be at the origin of the child link. + fixedJoint->SetTransformFromParent(poseParentChild); + + const std::size_t numSteps = 100; + + for (std::size_t i = 0; i < numSteps; ++i) + { + world->Step(output, state, input); + } + + // Expect both bodies to hit the ground and stop + EXPECT_NEAR(0.0, dartBody1->getLinearVelocity().z(), 1e-3); + EXPECT_NEAR(0.0, dartBody2->getLinearVelocity().z(), 1e-3); + + fixedJoint->Detach(); + + for (std::size_t i = 0; i < numSteps; ++i) + { + world->Step(output, state, input); + } + + // Expect both bodies to remain in contact with the ground with zero velocity. + EXPECT_NEAR(0.0, dartBody1->getLinearVelocity().z(), 1e-3); + EXPECT_NEAR(0.0, dartBody2->getLinearVelocity().z(), 1e-3); +} + ///////////////////////////////////////////////// int main(int argc, char *argv[]) { diff --git a/dartsim/worlds/ground.sdf b/dartsim/worlds/ground.sdf new file mode 100644 index 000000000..016f8befb --- /dev/null +++ b/dartsim/worlds/ground.sdf @@ -0,0 +1,35 @@ + + + + + true + + + + + 0 0 1 + 100 100 + + + + + + 100 + 50 + + + + + + + + 0 0 1 + 100 100 + + + + + + + + From 520f6f0b50c66b6fe59571a615807d67d4dc442e Mon Sep 17 00:00:00 2001 From: Steven Peters Date: Thu, 30 Apr 2020 10:05:48 -0700 Subject: [PATCH 17/23] Add CODEOWNERS (cherry-pick of #43) (#48) Signed-off-by: Steven Peters --- .github/CODEOWNERS | 7 +++++++ 1 file changed, 7 insertions(+) create mode 100644 .github/CODEOWNERS diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS new file mode 100644 index 000000000..c2b7b40cc --- /dev/null +++ b/.github/CODEOWNERS @@ -0,0 +1,7 @@ +# More info: +# https://help.github.com/en/github/creating-cloning-and-archiving-repositories/about-code-owners + +* @mxgrey +include/* @scpeters +tpe/* @claireyywang +tutorials/* @maryaB-osr From a051f0762b4d30c0e4529aa13e0cdd87578cff7f Mon Sep 17 00:00:00 2001 From: chapulina Date: Mon, 4 May 2020 07:21:33 -0700 Subject: [PATCH 18/23] [ign-physics1] Workflow updates (#51) Signed-off-by: Louise Poubel --- .github/workflows/ci-bionic.yml | 17 +++++++++++++++++ .github/workflows/pr-collection-labeler.yml | 13 +++++++++++++ .github/workflows/triage.yml | 19 +++++++++++++++++++ README.md | 6 +++--- 4 files changed, 52 insertions(+), 3 deletions(-) create mode 100644 .github/workflows/ci-bionic.yml create mode 100644 .github/workflows/pr-collection-labeler.yml create mode 100644 .github/workflows/triage.yml diff --git a/.github/workflows/ci-bionic.yml b/.github/workflows/ci-bionic.yml new file mode 100644 index 000000000..dae3605d7 --- /dev/null +++ b/.github/workflows/ci-bionic.yml @@ -0,0 +1,17 @@ +name: Ubuntu Bionic CI + +on: [push, pull_request] + +jobs: + bionic-ci: + runs-on: ubuntu-latest + name: Ubuntu Bionic CI + steps: + - name: Checkout + uses: actions/checkout@v2 + - name: Bionic CI + id: ci + uses: ignition-tooling/ubuntu-bionic-ci-action@master + with: + apt-dependencies: 'software-properties-common libignition-cmake2-dev libignition-common3-dev libignition-math6-dev libignition-math6-eigen3-dev libignition-plugin-dev libeigen3-dev libsdformat8-dev dart6-data libdart6-collision-ode-dev libdart6-dev libdart6-utils-urdf-dev libbenchmark-dev' + codecov-token: ${{ secrets.CODECOV_TOKEN }} diff --git a/.github/workflows/pr-collection-labeler.yml b/.github/workflows/pr-collection-labeler.yml new file mode 100644 index 000000000..99e9730bc --- /dev/null +++ b/.github/workflows/pr-collection-labeler.yml @@ -0,0 +1,13 @@ +name: PR Collection Labeler + +on: pull_request + +jobs: + pr_collection_labeler: + runs-on: ubuntu-latest + steps: + - name: Add collection labels + if: github.event.action == 'opened' + uses: ignition-tooling/pr-collection-labeler@v1 + with: + github-token: ${{ secrets.GITHUB_TOKEN }} diff --git a/.github/workflows/triage.yml b/.github/workflows/triage.yml new file mode 100644 index 000000000..69c16ac81 --- /dev/null +++ b/.github/workflows/triage.yml @@ -0,0 +1,19 @@ +on: + issues: + types: [opened] + pull_request: + types: [opened] +name: Ticket opened +jobs: + assign: + name: Add ticket to inbox + runs-on: ubuntu-latest + steps: + - name: Add ticket to inbox + uses: technote-space/create-project-card-action@v1 + with: + PROJECT: Core development + COLUMN: Inbox + GITHUB_TOKEN: ${{ secrets.TRIAGE_TOKEN }} + CHECK_ORG_PROJECT: true + diff --git a/README.md b/README.md index 25148bdee..05c272e1b 100644 --- a/README.md +++ b/README.md @@ -9,9 +9,9 @@ Build | Status -- | -- -Test coverage | [![codecov](https://codecov.io/bb/ignitionrobotics/ign-physics/branch/default/graph/badge.svg)](https://codecov.io/bb/ignitionrobotics/ign-physics) -Ubuntu Bionic | [![Build Status](https://build.osrfoundation.org/buildStatus/icon?job=ignition_physics-ci-default-bionic-amd64)](https://build.osrfoundation.org/job/ignition_physics-ci-default-bionic-amd64) -Homebrew | [![Build Status](https://build.osrfoundation.org/buildStatus/icon?job=ignition_physics-ci-default-homebrew-amd64)](https://build.osrfoundation.org/job/ignition_physics-ci-default-homebrew-amd64) +Test coverage | [![codecov](https://codecov.io/gh/ignitionrobotics/ign-physics/branch/master/graph/badge.svg)](https://codecov.io/gh/ignitionrobotics/ign-physics) +Ubuntu Bionic | [![Build Status](https://build.osrfoundation.org/buildStatus/icon?job=ignition_physics-ci-master-bionic-amd64)](https://build.osrfoundation.org/job/ignition_physics-ci-master-bionic-amd64) +Homebrew | [![Build Status](https://build.osrfoundation.org/buildStatus/icon?job=ignition_physics-ci-master-homebrew-amd64)](https://build.osrfoundation.org/job/ignition_physics-ci-master-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 From 56adc6dc3ec2dd0db9ecabd984b81d6c90da7ce1 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Thu, 7 May 2020 08:03:38 -0700 Subject: [PATCH 19/23] Version and changelog Signed-off-by: Louise Poubel --- CMakeLists.txt | 2 +- Changelog.md | 26 ++++++++++++++++++++++++++ 2 files changed, 27 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index da1c57665..8a53b8262 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,7 +3,7 @@ cmake_minimum_required(VERSION 3.5.1 FATAL_ERROR) #============================================================================ # Initialize the project #============================================================================ -project(ignition-physics2 VERSION 2.0.0) +project(ignition-physics2 VERSION 2.1.0) #============================================================================ # Find ignition-cmake diff --git a/Changelog.md b/Changelog.md index 67365fc65..a06a5365c 100644 --- a/Changelog.md +++ b/Changelog.md @@ -2,6 +2,8 @@ ### Ignition Physics 2.x.x (20XX-XX-XX) +### Ignition Physics 2.1.0 (2020-05-07) + 1. Add RequestFeatures API for casting the features of an entity to a new feature set when possible. * [BitBucket pull request 130](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-physics/pull-requests/130) @@ -17,6 +19,30 @@ 1. Added support for collision bitmasks for collision filtering * [BitBucket pull request 116](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-physics/pull-requests/116) +1. Clean up internal resources when a model gets removed + * [BitBucket pull request 115](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-physics/pull-requests/115) + +1. Trivial Physics Engine - partial implementation + * [BitBucket pull request 125](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-physics/pull-requests/125) + * [BitBucket pull request 126](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-physics/pull-requests/126) + * [Pull request 30](https://github.com/ignitionrobotics/ign-physics/pull/30) + * [Pull request 45](https://github.com/ignitionrobotics/ign-physics/pull/45) + +1. Add simple example of physics plugin and loader + * [BitBucket pull request 115](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-physics/pull-requests/115) + +1. Update BitBucket links, add .gitignore, CODEOWNERS, workflow + * [Pull request 34](https://github.com/ignitionrobotics/ign-physics/pull/34) + * [Pull request 39](https://github.com/ignitionrobotics/ign-physics/pull/39) + * [Pull request 47](https://github.com/ignitionrobotics/ign-physics/pull/47) + * [Pull request 52](https://github.com/ignitionrobotics/ign-physics/pull/52) + +1. Physics Plugin Documentation + * [Pull request 36](https://github.com/ignitionrobotics/ign-physics/pull/36) + +1. Reduce the symbol load caused by feature templates + * [Pull request 41](https://github.com/ignitionrobotics/ign-physics/pull/41) + ### Ignition Physics 2.0.0 (2019-12-10) 1. Support compiling against dart 6.9. From 956aa47e3219747bd7417d120850c3086924d060 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Thu, 7 May 2020 08:04:45 -0700 Subject: [PATCH 20/23] more changelog Signed-off-by: Louise Poubel --- Changelog.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/Changelog.md b/Changelog.md index a06a5365c..4151cac0b 100644 --- a/Changelog.md +++ b/Changelog.md @@ -43,6 +43,9 @@ 1. Reduce the symbol load caused by feature templates * [Pull request 41](https://github.com/ignitionrobotics/ign-physics/pull/41) +1. Add PlaneShape feature and implement in dartsim with test. + * [BitBucket pull request 66](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-physics/pull-requests/66) + ### Ignition Physics 2.0.0 (2019-12-10) 1. Support compiling against dart 6.9. From c28c24f43c6c7c823aa9b8167e599fde2f9d0200 Mon Sep 17 00:00:00 2001 From: Steven Peters Date: Thu, 7 May 2020 09:10:52 -0700 Subject: [PATCH 21/23] Add extra changelog entry merged from 1.x.x Signed-off-by: Steven Peters --- Changelog.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/Changelog.md b/Changelog.md index 4151cac0b..923f86db7 100644 --- a/Changelog.md +++ b/Changelog.md @@ -43,6 +43,9 @@ 1. Reduce the symbol load caused by feature templates * [Pull request 41](https://github.com/ignitionrobotics/ign-physics/pull/41) +1. Fix collision issue with detachable joints + * [Pull request 31](https://github.com/ignitionrobotics/ign-physics/pull/31) + 1. Add PlaneShape feature and implement in dartsim with test. * [BitBucket pull request 66](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-physics/pull-requests/66) From 9936e3a2c103e952638fbce8e1d6d253772bcd2f Mon Sep 17 00:00:00 2001 From: Addisu Taddese Date: Thu, 7 May 2020 18:30:01 -0500 Subject: [PATCH 22/23] Restore detached BodyNodes to original skeleton (#42) * dartsim: add prefix 'modelName/' to child BodyNode before calling moveTo in Attach*Joint, then try to restore BodyNode to its original skeleton if / is detected in child BodyNode's name during DetachJoint * Store the original name of links for use in moving BodyNodes between skeletons * Failing test demonstrating problem when a model and link have the same name * Handle the case when model and link names are the same * Update changelog Signed-off-by: Addisu Z. Taddese Co-authored-by: Steve Peters --- Changelog.md | 3 + dartsim/src/Base.hh | 17 +++++ dartsim/src/EntityManagementFeatures.cc | 2 +- dartsim/src/JointFeatures.cc | 84 ++++++++++++++++++++++--- dartsim/src/JointFeatures_TEST.cc | 30 +++++++-- dartsim/worlds/joint_across_models.sdf | 36 +++++++++++ 6 files changed, 158 insertions(+), 14 deletions(-) diff --git a/Changelog.md b/Changelog.md index b99305fa5..6e61953a8 100644 --- a/Changelog.md +++ b/Changelog.md @@ -2,6 +2,9 @@ ### Ignition Physics 1.x.x (20XX-XX-XX) +1. Restore detached BodyNodes to original skeleton + * [Pull request 42](https://github.com/ignitionrobotics/ign-physics/pull/42) + 1. Fix collision issue with detachable joints * [Pull request 31](https://github.com/ignitionrobotics/ign-physics/pull/31) diff --git a/dartsim/src/Base.hh b/dartsim/src/Base.hh index 2d299018e..c626aa70a 100644 --- a/dartsim/src/Base.hh +++ b/dartsim/src/Base.hh @@ -56,6 +56,10 @@ struct ModelInfo struct LinkInfo { dart::dynamics::BodyNodePtr link; + /// \brief It may be necessary for dartsim to rename a BodyNode (eg. when + /// moving the BodyNode to a new skeleton), so we store the Gazebo-specified + /// name of the Link here. + std::string name; }; struct JointInfo @@ -124,6 +128,16 @@ struct EntityStorage return idToObject.at(_id); } + Value1 &at(const Key2 &_key) + { + return idToObject.at(objectToID.at(_key)); + } + + const Value1 &at(const Key2 &_key) const + { + return idToObject.at(objectToID.at(_key)); + } + std::size_t size() const { return idToObject.size(); @@ -274,6 +288,9 @@ class Base : public Implements3d> const std::size_t id = this->GetNextEntity(); this->links.idToObject[id] = std::make_shared(); this->links.idToObject[id]->link = _bn; + // The name of the BodyNode during creation is assumed to be the + // Gazebo-specified name. + this->links.idToObject[id]->name = _bn->getName(); this->links.objectToID[_bn] = id; this->frames[id] = _bn; diff --git a/dartsim/src/EntityManagementFeatures.cc b/dartsim/src/EntityManagementFeatures.cc index 0295adb0b..10122b268 100644 --- a/dartsim/src/EntityManagementFeatures.cc +++ b/dartsim/src/EntityManagementFeatures.cc @@ -321,7 +321,7 @@ Identity EntityManagementFeatures::GetJoint( const std::string &EntityManagementFeatures::GetLinkName( const Identity &_linkID) const { - return this->ReferenceInterface(_linkID)->link->getName(); + return this->ReferenceInterface(_linkID)->name; } ///////////////////////////////////////////////// diff --git a/dartsim/src/JointFeatures.cc b/dartsim/src/JointFeatures.cc index ef9d8798e..0f862371c 100644 --- a/dartsim/src/JointFeatures.cc +++ b/dartsim/src/JointFeatures.cc @@ -156,7 +156,56 @@ void JointFeatures::DetachJoint(const Identity &_jointId) child->getSpatialVelocity( dart::dynamics::Frame::World(), dart::dynamics::Frame::World()); - auto freeJoint = child->moveTo(nullptr); + + if (!this->links.HasEntity(child)) + { + return; + } + + auto childLinkInfo = this->links.at(child); + + dart::dynamics::SkeletonPtr skeleton; + { + // Find the original skeleton the child BodyNode belonged to + std::string oldName = child->getName(); + if (oldName != childLinkInfo->name) + { + std::size_t originalNameIndex = oldName.rfind(childLinkInfo->name); + if (originalNameIndex > 1 && originalNameIndex != std::string::npos && + this->models.HasEntity(joint->getSkeleton())) + { + // Assume that the original and the current skeletons are in the same + // world. + auto worldId = + this->models + .idToContainerID[this->models.IdentityOf(joint->getSkeleton())]; + auto dartWorld = this->worlds.at(worldId); + std::string modelName = oldName.substr(0, originalNameIndex - 1); + skeleton = dartWorld->getSkeleton(modelName); + if (skeleton) + { + child->setName(childLinkInfo->name); + } + } + if (nullptr == skeleton) + { + ignerr << "Could not find the original skeleton of BodyNode " + << "[" << oldName << "] when detaching joint " + << "[" << joint->getName() << "]. Detached links may not work " + << "as expected.\n"; + } + } + } + + dart::dynamics::FreeJoint *freeJoint; + if (skeleton) + { + freeJoint = child->moveTo(skeleton, nullptr); + } + else + { + freeJoint = child->moveTo(nullptr); + } freeJoint->setTransform(transform); freeJoint->setSpatialVelocity(spatialVelocity, dart::dynamics::Frame::World(), @@ -186,8 +235,8 @@ Identity JointFeatures::AttachFixedJoint( const BaseLink3dPtr &_parent, const std::string &_name) { - DartBodyNode *const bn = - this->ReferenceInterface(_childID)->link.get(); + auto linkInfo = this->ReferenceInterface(_childID); + DartBodyNode *const bn = linkInfo->link.get(); dart::dynamics::WeldJoint::Properties properties; properties.mName = _name; @@ -201,6 +250,13 @@ Identity JointFeatures::AttachFixedJoint( return this->GenerateInvalidId(); } + { + auto skeleton = bn->getSkeleton(); + if (skeleton) + { + bn->setName(skeleton->getName() + '/' + linkInfo->name); + } + } const std::size_t jointID = this->AddJoint( bn->moveTo(parentBn, properties)); // TODO(addisu) Remove incrementVersion once DART has been updated to @@ -270,8 +326,8 @@ Identity JointFeatures::AttachRevoluteJoint( const std::string &_name, const AngularVector3d &_axis) { - DartBodyNode *const bn = - this->ReferenceInterface(_childID)->link.get(); + auto linkInfo = this->ReferenceInterface(_childID); + DartBodyNode *const bn = linkInfo->link.get(); dart::dynamics::RevoluteJoint::Properties properties; properties.mName = _name; properties.mAxis = _axis; @@ -286,6 +342,13 @@ Identity JointFeatures::AttachRevoluteJoint( return this->GenerateInvalidId(); } + { + auto skeleton = bn->getSkeleton(); + if (skeleton) + { + bn->setName(skeleton->getName() + '/' + linkInfo->name); + } + } const std::size_t jointID = this->AddJoint( bn->moveTo(parentBn, properties)); // TODO(addisu) Remove incrementVersion once DART has been updated to @@ -332,8 +395,8 @@ Identity JointFeatures::AttachPrismaticJoint( const std::string &_name, const LinearVector3d &_axis) { - DartBodyNode *const bn = - this->ReferenceInterface(_childID)->link.get(); + auto linkInfo = this->ReferenceInterface(_childID); + DartBodyNode *const bn = linkInfo->link.get(); dart::dynamics::PrismaticJoint::Properties properties; properties.mName = _name; properties.mAxis = _axis; @@ -348,6 +411,13 @@ Identity JointFeatures::AttachPrismaticJoint( return this->GenerateInvalidId(); } + { + auto skeleton = bn->getSkeleton(); + if (skeleton) + { + bn->setName(skeleton->getName() + '/' + linkInfo->name); + } + } const std::size_t jointID = this->AddJoint( bn->moveTo(parentBn, properties)); // TODO(addisu) Remove incrementVersion once DART has been updated to diff --git a/dartsim/src/JointFeatures_TEST.cc b/dartsim/src/JointFeatures_TEST.cc index 996014920..f64e9aa7f 100644 --- a/dartsim/src/JointFeatures_TEST.cc +++ b/dartsim/src/JointFeatures_TEST.cc @@ -332,6 +332,10 @@ TEST_F(JointFeaturesFixture, JointAttachDetach) // the same as it was before they were attached fixedJoint->SetTransformFromParent(poseParentChild); + // The name of the link obtained using the ign-physics API should remain the + // same even though AttachFixedJoint renames the associated BodyNode. + EXPECT_EQ(bodyName, model2Body->GetName()); + for (std::size_t i = 0; i < numSteps; ++i) { world->Step(output, state, input); @@ -349,6 +353,10 @@ TEST_F(JointFeaturesFixture, JointAttachDetach) // now detach joint and expect model2 to start moving again fixedJoint->Detach(); + // The name of the link obtained using the ign-physics API should remain the + // same even though Detach renames the associated BodyNode. + EXPECT_EQ(bodyName, model2Body->GetName()); + for (std::size_t i = 0; i < numSteps; ++i) { world->Step(output, state, input); @@ -410,12 +418,22 @@ TEST_F(JointFeaturesFixture, LinkCountsInJointAttachDetach) // now detach joint and expect model2 to start moving again fixedJoint->Detach(); - // After detaching we expect each model to have 1 link, but the current - // behavior is that there are 2 links in model1 and 0 in model2 - // EXPECT_EQ(1u, model1->GetLinkCount()); - // EXPECT_EQ(1u, model2->GetLinkCount()); - EXPECT_EQ(2u, model1->GetLinkCount()); - EXPECT_EQ(0u, model2->GetLinkCount()); + // After detaching we expect each model to have 1 link + EXPECT_EQ(1u, model1->GetLinkCount()); + EXPECT_EQ(1u, model2->GetLinkCount()); + + // Test that a model with the same name as a link doesn't cause problems + const std::string modelName3{"body"}; + auto model3 = world->GetModel(modelName3); + EXPECT_EQ(1u, model3->GetLinkCount()); + + auto model3Body = model3->GetLink(bodyName); + auto fixedJoint2 = model3Body->AttachFixedJoint(model2Body); + EXPECT_EQ(2u, model2->GetLinkCount()); + fixedJoint2->Detach(); + // After detaching we expect each model to have 1 link + EXPECT_EQ(1u, model2->GetLinkCount()); + EXPECT_EQ(1u, model3->GetLinkCount()); } ///////////////////////////////////////////////// diff --git a/dartsim/worlds/joint_across_models.sdf b/dartsim/worlds/joint_across_models.sdf index 8ad7b79ac..a4308581d 100644 --- a/dartsim/worlds/joint_across_models.sdf +++ b/dartsim/worlds/joint_across_models.sdf @@ -86,6 +86,42 @@ + + + + 1 1 0.5 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 10 0 3.0 0 0.0 0 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 1.0 + + + + + 1 1 0.5 + + + + From 975aad38c19b5cbe345e0b8efeba17390aecb456 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Thu, 7 May 2020 17:09:01 -0700 Subject: [PATCH 23/23] changelog Signed-off-by: Louise Poubel --- Changelog.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/Changelog.md b/Changelog.md index 0ad62a78a..1f58edc6c 100644 --- a/Changelog.md +++ b/Changelog.md @@ -49,6 +49,9 @@ 1. Add PlaneShape feature and implement in dartsim with test. * [BitBucket pull request 66](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-physics/pull-requests/66) +1. Restore detached BodyNodes to original skeleton + * [Pull request 42](https://github.com/ignitionrobotics/ign-physics/pull/42) + ### Ignition Physics 2.0.0 (2019-12-10) 1. Support compiling against dart 6.9.