diff --git a/CMakeLists.txt b/CMakeLists.txt index 064598270..86da572d2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -13,7 +13,9 @@ find_package(ignition-cmake3 REQUIRED) #============================================================================ # Configure the project #============================================================================ -ign_configure_project(VERSION_SUFFIX pre1) +ign_configure_project( + REPLACE_IGNITION_INCLUDE_PATH gz/physics + VERSION_SUFFIX pre1) #============================================================================ # Set project-specific options diff --git a/bullet/CMakeLists.txt b/bullet/CMakeLists.txt index ad1af1d55..90c1e08eb 100644 --- a/bullet/CMakeLists.txt +++ b/bullet/CMakeLists.txt @@ -27,6 +27,11 @@ target_link_libraries(${bullet_plugin} # Note that plugins are currently being installed in 2 places: /lib and the engine-plugins dir install(TARGETS ${bullet_plugin} DESTINATION ${IGNITION_PHYSICS_ENGINE_INSTALL_DIR}) +# Install redirection headers +install( + DIRECTORY include/ + DESTINATION "${IGN_INCLUDE_INSTALL_DIR_FULL}") + # The library created by `ign_add_component` includes the ign-physics version # (i.e. libignition-physics1-name-plugin.so), but for portability, # we also install an unversioned symlink into the same versioned folder. diff --git a/bullet/include/ignition/physics/bullet-plugin/Export.hh b/bullet/include/ignition/physics/bullet-plugin/Export.hh new file mode 100644 index 000000000..5bfa5216f --- /dev/null +++ b/bullet/include/ignition/physics/bullet-plugin/Export.hh @@ -0,0 +1,18 @@ +/* + * Copyright (C) 2022 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 diff --git a/dartsim/include/gz/physics/dartsim/World.hh b/dartsim/include/gz/physics/dartsim/World.hh new file mode 100644 index 000000000..57c7b703f --- /dev/null +++ b/dartsim/include/gz/physics/dartsim/World.hh @@ -0,0 +1,62 @@ +/* + * Copyright (C) 2018 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 GZ_PHYSICS_DARTSIM_WORLD_HH_ +#define GZ_PHYSICS_DARTSIM_WORLD_HH_ + +#include + +#include + +namespace ignition { +namespace physics { +namespace dartsim { + +///////////////////////////////////////////////// +class RetrieveWorld : public virtual Feature +{ + public: template + class World : public virtual Feature::World + { + /// \brief Get the underlying dartsim world for this World object. + public: dart::simulation::WorldPtr GetDartsimWorld(); + }; + + public: template + class Implementation : public virtual Feature::Implementation + { + public: virtual dart::simulation::WorldPtr GetDartsimWorld( + const Identity &_worldID) = 0; + }; +}; + +///////////////////////////////////////////////// +//! [feature template] +template +dart::simulation::WorldPtr RetrieveWorld::World +::GetDartsimWorld() +{ + return this->template Interface() + ->GetDartsimWorld(this->identity); +} +//! [feature template] + +} +} +} + +#endif diff --git a/dartsim/include/ignition/physics/dartsim-plugin/Export.hh b/dartsim/include/ignition/physics/dartsim-plugin/Export.hh new file mode 100644 index 000000000..f2587cf0e --- /dev/null +++ b/dartsim/include/ignition/physics/dartsim-plugin/Export.hh @@ -0,0 +1,18 @@ +/* + * Copyright (C) 2022 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 diff --git a/dartsim/include/ignition/physics/dartsim/World.hh b/dartsim/include/ignition/physics/dartsim/World.hh index c1eb6c957..1c8acf9f0 100644 --- a/dartsim/include/ignition/physics/dartsim/World.hh +++ b/dartsim/include/ignition/physics/dartsim/World.hh @@ -13,50 +13,6 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ + */ -#ifndef IGNITION_PHYSICS_DARTSIM_WORLD_HH_ -#define IGNITION_PHYSICS_DARTSIM_WORLD_HH_ - -#include - -#include - -namespace ignition { -namespace physics { -namespace dartsim { - -///////////////////////////////////////////////// -class RetrieveWorld : public virtual Feature -{ - public: template - class World : public virtual Feature::World - { - /// \brief Get the underlying dartsim world for this World object. - public: dart::simulation::WorldPtr GetDartsimWorld(); - }; - - public: template - class Implementation : public virtual Feature::Implementation - { - public: virtual dart::simulation::WorldPtr GetDartsimWorld( - const Identity &_worldID) = 0; - }; -}; - -///////////////////////////////////////////////// -//! [feature template] -template -dart::simulation::WorldPtr RetrieveWorld::World -::GetDartsimWorld() -{ - return this->template Interface() - ->GetDartsimWorld(this->identity); -} -//! [feature template] - -} -} -} - -#endif +#include diff --git a/examples/hello_world_loader/hello_world_loader.cc b/examples/hello_world_loader/hello_world_loader.cc index 7b185a495..2d89c55c3 100644 --- a/examples/hello_world_loader/hello_world_loader.cc +++ b/examples/hello_world_loader/hello_world_loader.cc @@ -19,12 +19,12 @@ //! [include statements] #include -#include -#include +#include +#include -#include -#include -#include +#include +#include +#include // The features that an engine must have to be loaded by this loader. using Features = ignition::physics::FeatureList< diff --git a/examples/hello_world_plugin/HelloWorldPlugin.cc b/examples/hello_world_plugin/HelloWorldPlugin.cc index ac616a10d..71e9a22e6 100644 --- a/examples/hello_world_plugin/HelloWorldPlugin.cc +++ b/examples/hello_world_plugin/HelloWorldPlugin.cc @@ -17,10 +17,10 @@ //////////////////////////////////////////////////////////// //! [include statements] -#include -#include -#include -#include +#include +#include +#include +#include //! [include statements] namespace mock diff --git a/examples/simple_plugin/EntityManagementFeatures.hh b/examples/simple_plugin/EntityManagementFeatures.hh index 86aaecbf8..fb027d033 100644 --- a/examples/simple_plugin/EntityManagementFeatures.hh +++ b/examples/simple_plugin/EntityManagementFeatures.hh @@ -17,11 +17,11 @@ //! [basic include] #include -#include +#include //! [basic include] //! [include feature] -#include +#include namespace ignition { namespace physics { diff --git a/examples/simple_plugin/EntityManagementFeatures_TEST.cc b/examples/simple_plugin/EntityManagementFeatures_TEST.cc index 2cfbe88f7..ddfdd9068 100644 --- a/examples/simple_plugin/EntityManagementFeatures_TEST.cc +++ b/examples/simple_plugin/EntityManagementFeatures_TEST.cc @@ -17,8 +17,8 @@ #include -#include -#include +#include +#include #include "EntityManagementFeatures.hh" // Simple executable that loads the simple plugin and constructs a world. diff --git a/examples/simple_plugin/plugin.cc b/examples/simple_plugin/plugin.cc index 673f9b34f..b89370e26 100644 --- a/examples/simple_plugin/plugin.cc +++ b/examples/simple_plugin/plugin.cc @@ -15,9 +15,9 @@ * */ -#include -#include -#include +#include +#include +#include #include "EntityManagementFeatures.hh" diff --git a/heightmap/include/gz/physics/heightmap/HeightmapShape.hh b/heightmap/include/gz/physics/heightmap/HeightmapShape.hh new file mode 100644 index 000000000..e13abe668 --- /dev/null +++ b/heightmap/include/gz/physics/heightmap/HeightmapShape.hh @@ -0,0 +1,116 @@ +/* + * Copyright (C) 2021 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 GZ_PHYSICS_HEIGHTMAP_HEIGHTMAPSHAPE_HH_ +#define GZ_PHYSICS_HEIGHTMAP_HEIGHTMAPSHAPE_HH_ + +#include + +#include + +#include +#include + +namespace ignition +{ +namespace physics +{ +namespace heightmap +{ + IGN_PHYSICS_DECLARE_SHAPE_TYPE(HeightmapShape) + + ///////////////////////////////////////////////// + class GetHeightmapShapeProperties + : public virtual FeatureWithRequirements + { + public: template + class HeightmapShape : public virtual Entity + { + public: using Dimensions = + typename FromPolicy::template Use; + + /// \brief Get the size of the heightmap in meters. + /// \returns The size of the heightmap. + public: Dimensions GetSize() const; + }; + + public: template + class Implementation : public virtual Feature::Implementation + { + public: using Dimensions = + typename FromPolicy::template Use; + + public: virtual Dimensions GetHeightmapShapeSize( + const Identity &_heightmapID) const = 0; + }; + }; + + ///////////////////////////////////////////////// + /// \brief Attach a heightmap shape to a link. + class AttachHeightmapShapeFeature + : public virtual FeatureWithRequirements + { + public: template + class Link : public virtual Feature::Link + { + public: using PoseType = + typename FromPolicy::template Use; + + public: using Dimensions = + typename FromPolicy::template Use; + + public: using ShapePtrType = HeightmapShapePtr; + + /// \brief Attach a heightmap shape to a link. + /// \param[in] _name Shape's name. + /// \param[in] _heightmapData Contains the 3D data for the heigthtmap. + /// \param[in] _pose Position in the world. + /// \param[in] _size Heightmap total size in meters. + /// \param[in] _subSampling Increase sampling to improve resolution. + public: ShapePtrType AttachHeightmapShape( + const std::string &_name, + const common::HeightmapData &_heightmapData, + const PoseType &_pose, + const Dimensions &_size, + int _subSampling = 1); + }; + + public: template + class Implementation : public virtual Feature::Implementation + { + public: using PoseType = + typename FromPolicy::template Use; + + public: using Dimensions = + typename FromPolicy::template Use; + + public: virtual Identity AttachHeightmapShape( + const Identity &_linkID, + const std::string &_name, + const common::HeightmapData &_heightmapData, + const PoseType &_pose, + const Dimensions &_size, + int _subSampling) = 0; + }; + }; +} +} +} + +#include + +#endif // GZ_PHYSICS_HEIGHTMAP_HEIGHTMAPSHAPE_HH_ diff --git a/heightmap/include/ignition/physics/heightmap/detail/HeightmapShape.hh b/heightmap/include/gz/physics/heightmap/detail/HeightmapShape.hh similarity index 87% rename from heightmap/include/ignition/physics/heightmap/detail/HeightmapShape.hh rename to heightmap/include/gz/physics/heightmap/detail/HeightmapShape.hh index 022c863e7..961a3e2c8 100644 --- a/heightmap/include/ignition/physics/heightmap/detail/HeightmapShape.hh +++ b/heightmap/include/gz/physics/heightmap/detail/HeightmapShape.hh @@ -15,12 +15,12 @@ * */ -#ifndef IGNITION_PHYSICS_HEIGHTMAP_DETAIL_HEIGHTMAPSHAPE_HH_ -#define IGNITION_PHYSICS_HEIGHTMAP_DETAIL_HEIGHTMAPSHAPE_HH_ +#ifndef GZ_PHYSICS_HEIGHTMAP_DETAIL_HEIGHTMAPSHAPE_HH_ +#define GZ_PHYSICS_HEIGHTMAP_DETAIL_HEIGHTMAPSHAPE_HH_ #include -#include +#include namespace ignition { @@ -56,4 +56,4 @@ namespace heightmap } } -#endif // IGNITION_PHYSICS_MESH_DETAIL_MESHSHAPE_HH_ +#endif // GZ_PHYSICS_MESH_DETAIL_MESHSHAPE_HH_ diff --git a/heightmap/include/ignition/physics/heightmap/HeightmapShape.hh b/heightmap/include/ignition/physics/heightmap/HeightmapShape.hh index 294c8d193..2074d72d0 100644 --- a/heightmap/include/ignition/physics/heightmap/HeightmapShape.hh +++ b/heightmap/include/ignition/physics/heightmap/HeightmapShape.hh @@ -13,104 +13,6 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ + */ -#ifndef IGNITION_PHYSICS_HEIGHTMAP_HEIGHTMAPSHAPE_HH_ -#define IGNITION_PHYSICS_HEIGHTMAP_HEIGHTMAPSHAPE_HH_ - -#include - -#include - -#include -#include - -namespace ignition -{ -namespace physics -{ -namespace heightmap -{ - IGN_PHYSICS_DECLARE_SHAPE_TYPE(HeightmapShape) - - ///////////////////////////////////////////////// - class GetHeightmapShapeProperties - : public virtual FeatureWithRequirements - { - public: template - class HeightmapShape : public virtual Entity - { - public: using Dimensions = - typename FromPolicy::template Use; - - /// \brief Get the size of the heightmap in meters. - /// \returns The size of the heightmap. - public: Dimensions GetSize() const; - }; - - public: template - class Implementation : public virtual Feature::Implementation - { - public: using Dimensions = - typename FromPolicy::template Use; - - public: virtual Dimensions GetHeightmapShapeSize( - const Identity &_heightmapID) const = 0; - }; - }; - - ///////////////////////////////////////////////// - /// \brief Attach a heightmap shape to a link. - class AttachHeightmapShapeFeature - : public virtual FeatureWithRequirements - { - public: template - class Link : public virtual Feature::Link - { - public: using PoseType = - typename FromPolicy::template Use; - - public: using Dimensions = - typename FromPolicy::template Use; - - public: using ShapePtrType = HeightmapShapePtr; - - /// \brief Attach a heightmap shape to a link. - /// \param[in] _name Shape's name. - /// \param[in] _heightmapData Contains the 3D data for the heigthtmap. - /// \param[in] _pose Position in the world. - /// \param[in] _size Heightmap total size in meters. - /// \param[in] _subSampling Increase sampling to improve resolution. - public: ShapePtrType AttachHeightmapShape( - const std::string &_name, - const common::HeightmapData &_heightmapData, - const PoseType &_pose, - const Dimensions &_size, - int _subSampling = 1); - }; - - public: template - class Implementation : public virtual Feature::Implementation - { - public: using PoseType = - typename FromPolicy::template Use; - - public: using Dimensions = - typename FromPolicy::template Use; - - public: virtual Identity AttachHeightmapShape( - const Identity &_linkID, - const std::string &_name, - const common::HeightmapData &_heightmapData, - const PoseType &_pose, - const Dimensions &_size, - int _subSampling) = 0; - }; - }; -} -} -} - -#include - -#endif // IGNITION_PHYSICS_HEIGHTMAP_HEIGHTMAPSHAPE_HH_ +#include diff --git a/include/CMakeLists.txt b/include/CMakeLists.txt index 54ca556c0..4b2bdd7bb 100644 --- a/include/CMakeLists.txt +++ b/include/CMakeLists.txt @@ -1 +1,2 @@ -add_subdirectory(ignition/physics) +add_subdirectory(gz) +install(DIRECTORY ignition DESTINATION ${IGN_INCLUDE_INSTALL_DIR_FULL}) diff --git a/include/gz/CMakeLists.txt b/include/gz/CMakeLists.txt new file mode 100644 index 000000000..8b03bd731 --- /dev/null +++ b/include/gz/CMakeLists.txt @@ -0,0 +1 @@ +add_subdirectory(physics) diff --git a/include/gz/physics/BoxShape.hh b/include/gz/physics/BoxShape.hh new file mode 100644 index 000000000..9270e59c2 --- /dev/null +++ b/include/gz/physics/BoxShape.hh @@ -0,0 +1,136 @@ +/* + * Copyright (C) 2018 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 GZ_PHYSICS_BOXSHAPE_HH_ +#define GZ_PHYSICS_BOXSHAPE_HH_ + +#include + +#include +#include + +namespace ignition +{ + namespace physics + { + IGN_PHYSICS_DECLARE_SHAPE_TYPE(BoxShape) + + class IGNITION_PHYSICS_VISIBLE GetBoxShapeProperties + : public virtual FeatureWithRequirements + { + public: template + class BoxShape : public virtual Entity + { + public: using Dimensions = + typename FromPolicy::template Use; + + /// \brief Get the dimensions of this BoxShape + /// \return the dimensions of this BoxShape + public: Dimensions GetSize() const; + }; + + public: template + class Implementation : public virtual Feature::Implementation + { + public: using Dimensions = + typename FromPolicy::template Use; + + public: virtual Dimensions GetBoxShapeSize( + const Identity &_boxID) const = 0; + }; + }; + + class IGNITION_PHYSICS_VISIBLE SetBoxShapeProperties + : public virtual FeatureWithRequirements + { + public: template + class BoxShape : public virtual Entity + { + public: using Dimensions = + typename FromPolicy::template Use; + + /// \brief Set the dimensions of this BoxShape + /// \param[in] _size + /// The desired dimensions of this BoxShape + public: void SetSize(const Dimensions &_size); + }; + + public: template + class Implementation : public virtual Feature::Implementation + { + public: using Dimensions = + typename FromPolicy::template Use; + + public: virtual void SetBoxShapeSize( + const Identity &_boxID, const Dimensions &_size) = 0; + }; + }; + + ///////////////////////////////////////////////// + /// \brief This feature constructs a new box shape and attaches the + /// desired pose in the link frame. The pose could be defined to be the box + /// center point or any corner in actual implementation. + class IGNITION_PHYSICS_VISIBLE AttachBoxShapeFeature + : public virtual FeatureWithRequirements + { + public: template + class Link : public virtual Feature::Link + { + public: using Dimensions = + typename FromPolicy::template Use; + + public: using PoseType = + typename FromPolicy::template Use; + + public: using ShapePtrType = BoxShapePtr; + + /// \brief Rigidly attach a BoxShape to this link. + /// \param[in] _size + /// The size of the BoxShape's dimensions. + /// \param[in] _pose + /// The desired pose of the BoxShape relative to the Link frame. + /// \returns a reference to the newly constructed BoxShape + // TODO(MXG): Create a struct that contains all the relevant properties, + // and pass that in here. + public: ShapePtrType AttachBoxShape( + const std::string &_name = "box", + const Dimensions &_size = Dimensions::Constant(1.0), + const PoseType &_pose = PoseType::Identity()); + }; + + public: template + class Implementation : public virtual Feature::Implementation + { + public: using Dimensions = + typename FromPolicy::template Use; + + public: using PoseType = + typename FromPolicy::template Use; + + public: virtual Identity AttachBoxShape( + const Identity &_linkID, + const std::string &_name, + const Dimensions &_size, + const PoseType &_pose) = 0; + }; + }; + } +} + +#include + +#endif diff --git a/include/ignition/physics/CMakeLists.txt b/include/gz/physics/CMakeLists.txt similarity index 100% rename from include/ignition/physics/CMakeLists.txt rename to include/gz/physics/CMakeLists.txt diff --git a/include/gz/physics/CanReadData.hh b/include/gz/physics/CanReadData.hh new file mode 100644 index 000000000..c2f4e9dc3 --- /dev/null +++ b/include/gz/physics/CanReadData.hh @@ -0,0 +1,147 @@ +/* + * Copyright (C) 2017 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 GZ_PHYSICS_CANREADDATA_HH_ +#define GZ_PHYSICS_CANREADDATA_HH_ + + +#include "gz/physics/OperateOnSpecifiedData.hh" + +namespace ignition +{ + namespace physics + { + /// \brief ReadOptions provides customization for the ReadRequiredData + /// and ReadExpectedData functions provided by CanReadRequiredData + /// and CanReadExpectedData. + /// \sa CanReadExpectedData::ReadExpectedData() + /// \sa CanReadRequiredData::ReadRequiredData() + struct IGNITION_PHYSICS_VISIBLE ReadOptions + { + /// \brief If a type has already been queried, do not perform the read + /// operation on it. + public: bool onlyReadUnqueriedData; + + /// \brief Default constructor. + /// \param[in] _onlyUnqueried Whether only unqueried data will be read, + /// default to true. + public: explicit ReadOptions(const bool _onlyUnqueried = true); + }; + + /// \brief CanReadRequiredData provides compile-time static analysis to + /// ensure that the inheriting class provides a Read(~) function overload + /// for each of the data types that are listed as required in the + /// Specification. It also provides a function that will invoke Read(~) on + /// each of the required data types (you may indicate whether or not it + /// should only invoke them on unqueried data). + /// + /// Note that you must pass the name of your class in as the first template + /// argument when inheriting it (Curiously Recurring Template Pattern CRTP), + /// e.g.: + /// + /// \code + /// class MyClass : public CanReadRequiredData + /// { + /// // ... define my class ... + /// }; + /// \endcode + /// + /// You may also use CanReadExpectedData if you want to further guarantee + /// that your class is able to read all the expected data. This is + /// recommended, because otherwise it might be easy to quietly overlook data + /// types that you assumed were being read, resulting in incorrect behavior. + /// + /// Note that you are allowed to inherit both CanReadRequiredData and + /// CanReadExpectedData while passing different Specifications to each, but + /// you should be aware that ReadRequiredData will only read the data + /// required by the Specification that is given to CanReadRequiredData. + /// Likewise, ReadExpectedData will only read the data expected by + /// the Specification that is given to CanReadExpectedData. + /// + /// While you can technically inherit CanReadRequiredData multiple times + /// and provide each base with a different Specification, this is strongly + /// discouraged because then you will be left with multiple ambiguous + /// versions of ReadRequiredData(~). Instead, it is beter to inherit it once + /// and combine the Specifications using SpecifyData. For example: + /// + /// \code + /// class MyClass : public CanReadRequiredData< + /// MyClass, + /// SpecifyData > + /// { + /// // ... define my class ... + /// }; + /// \endcode + /// + /// This class is designed to cause a compilation error if the inheriting + /// class does not provide all the necessary Read(~) functions. See the page + /// \ref ReadCompilationFail for more information. + template + class CanReadRequiredData + { + /// The ability to compile this constructor ensures that an inherited + /// class has functions that can read each of the data types required by + /// the Specification. + public: CanReadRequiredData(); + + /// Call this function to read all the types in _data that are listed as + /// required in the Specification. Setting _options.onlyReadUnqueriedData + /// to true will make it so that only data entries that have not been + /// queried will be passed to the Read(~) function. + /// If _options.onlyReadUnqueriedData is false, then all data that the + /// Specification lists as required will be read. + /// \param[in] _data CompositeData instance to read from. + /// \param[in] _options ReadOptions for customizing the read operation. + public: template + void ReadRequiredData( + const CompositeType &_data, + const ReadOptions &_options = ReadOptions()); + }; + + /// This class is the same as CanReadRequiredData, except it operates on all + /// "expected" data (which is a superset of "required" data) instead of only + /// the "required" data. + template + class CanReadExpectedData + { + /// The ability to compile this constructor ensures that an inherited + /// class has functions that can read each of the data types expected by + /// the Specification. + public: CanReadExpectedData(); + + /// Call this function to read all the types in _data that are listed as + /// expected in the Specification. Setting _options.onlyReadUnqueriedData + /// to true will make it so that only data entries that have not been + /// queried will be passed to the Read(~) function. If + /// _options.onlyReadUnqueriedData is false, + /// then all data that the Specification lists as expected will be read. + /// \param[in] _data CompositeData instance to read from. + /// \param[in] _options ReadOptions for customizing the read operation. + public: template + void ReadExpectedData( + const CompositeType &_data, + const ReadOptions &_options = ReadOptions()); + }; + } +} + +#include "gz/physics/detail/CanReadData.hh" + +#endif diff --git a/include/gz/physics/CanWriteData.hh b/include/gz/physics/CanWriteData.hh new file mode 100644 index 000000000..69ac65b5f --- /dev/null +++ b/include/gz/physics/CanWriteData.hh @@ -0,0 +1,148 @@ +/* + * Copyright (C) 2017 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 GZ_PHYSICS_CANWRITEDATA_HH_ +#define GZ_PHYSICS_CANWRITEDATA_HH_ + +#include "gz/physics/OperateOnSpecifiedData.hh" + +namespace ignition +{ + namespace physics + { + /// \brief A struct that defines options for writing data to a CompositeData + /// object. + /// \sa CanWriteExpectedData::WriteExpectedData() + /// \sa CanWriteRequiredData::WriteRequiredData() + struct IGNITION_PHYSICS_VISIBLE WriteOptions + { + /// \brief If a data type is not already part of the CompositeData, then + /// skip it instead of writing to it when this is true. When this is + /// false, create a new instance using the default constructor for any + /// data type that is missing, and then hand it over for writing. + public: bool skipMissingData; + + /// \brief If a data type has already been queried, do not perform the + /// write operation on it. + public: bool onlyWriteUnqueriedData; + + /// \brief Default constructor. + /// \param[in] _skipMissing Whether to skip writing fields that aren't + /// already present in the output CompositeData object, default to false. + /// \param[in] _onlyUnqueried Whether only unqueried data will be written, + /// default to true. + public: explicit WriteOptions(const bool _skipMissing = false, + const bool _onlyUnqueried = true); + }; + + /// \brief CanWriteRequiredData provides compile-time static analysis to + /// ensure that the inheriting class provides a Write(~) function overload + /// for each of the data types that are listed as required in the + /// Specification. It also provides a function that will invoke Write(~) on + /// each of the required data types (you may indicate whether or not it + /// should only invoke them on unqueried data). + /// + /// Note that you must pass the name of your class in as the first template + /// argument when inheriting it (Curiously Recurring Template Pattern CRTP), + /// e.g.: + /// + /// \code + /// class MyClass : public CanWriteRequiredData + /// { + /// // ... define my class ... + /// }; + /// \endcode + /// + /// You may also use CanWriteExpectedData if you want to further guarantee + /// that your class is able to write all the expected data (recommended). + /// + /// Note that you are allowed to inherit both CanWriteRequiredData and + /// CanWriteExpectedData while passing different Specifications to each, but + /// you should be aware that WriteRequiredData will only handle the data + /// required by the Specification that is given to CanWriteRequiredData. + /// Likewise, WriteExpectedData will only handle the data expected by + /// the Specification that is given to CanWriteExpectedData. + /// + /// While you can technically inherit CanWriteRequiredData multiple times + /// and provide each base with a different Specification, this is strongly + /// discouraged because then you will be left with multiple ambiguous + /// versions of WriteRequiredData(~). Instead, it is beter to inherit it + /// once and combine the Specifications using SpecifyData. For example: + /// + /// \code + /// class MyClass : public CanWriteRequiredData< + /// MyClass, + /// SpecifyData > + /// { + /// // ... define my class ... + /// }; + /// \endcode + /// + /// This class is designed to cause a compilation error if the inheriting + /// class does not provide all the necessary Write(~) functions. See the + /// page \ref WriteCompilationFail for more information. + template + class CanWriteRequiredData + { + /// \brief The ability to compile this constructor ensures that an + /// inherited class has functions that can write each of the data types + /// required by the Specification. + public: CanWriteRequiredData(); + + /// \brief Call this function to write all the types in _data that are + /// listed as required in the Specification. + /// \param[out] _data CompositeData instance to write to. + /// \param[in] _options WriteOptions for customizing the write operation. + public: template + void WriteRequiredData( + CompositeType &_data, + const WriteOptions &_options = WriteOptions()) const; + }; + + /// This class is the same as CanWriteRequiredData, except it operates on + /// all "expected" data (which is a superset of "required" data) instead of + /// only the "required" data. + template + class CanWriteExpectedData + { + /// The ability to compile this constructor ensures that an inherited + /// class has functions that can write each of the data types expected by + /// the Specification. + public: CanWriteExpectedData(); + + /// Call this function to write all the types in _data that are listed as + /// expected in the Specification. Setting _onlyWriteUnqueriedData to true + /// will make it so that only data entries that have not been queried will + /// be passed to the Write(~) function. If _onlyWriteUnqueriedData is + /// false, then all data that the Specification lists as expected will be + /// written. + /// \param[out] _data CompositeData instance to write to. + /// \param[in] _options WriteOptions for customizing the write operation. + public: template + void WriteExpectedData( + CompositeType &_data, + const WriteOptions &_options = WriteOptions()) const; + }; + } +} + +#include "gz/physics/detail/CanWriteData.hh" + +#endif diff --git a/include/gz/physics/CapsuleShape.hh b/include/gz/physics/CapsuleShape.hh new file mode 100644 index 000000000..0bba14b6f --- /dev/null +++ b/include/gz/physics/CapsuleShape.hh @@ -0,0 +1,151 @@ +/* + * Copyright (C) 2021 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 GZ_PHYSICS_CAPSULESHAPE_HH_ +#define GZ_PHYSICS_CAPSULESHAPE_HH_ + +#include + +#include +#include + +namespace ignition +{ + namespace physics + { + IGN_PHYSICS_DECLARE_SHAPE_TYPE(CapsuleShape) + + class IGNITION_PHYSICS_VISIBLE GetCapsuleShapeProperties + : public virtual FeatureWithRequirements + { + public: template + class CapsuleShape : public virtual Entity + { + public: using Scalar = typename PolicyT::Scalar; + + /// \brief Get the radius of this CapsuleShape + /// \return the radius of this CapsuleShape + public: Scalar GetRadius() const; + + /// \brief Get the length along the local z-axis of this + /// CapsuleShape's cylinder. + /// \return the length of this CapsuleShape + public: Scalar GetLength() const; + }; + + public: template + class Implementation : public virtual Feature::Implementation + { + public: using Scalar = typename PolicyT::Scalar; + + public: virtual Scalar GetCapsuleShapeRadius( + const Identity &_capsuleID) const = 0; + + public: virtual Scalar GetCapsuleShapeLength( + const Identity &_capsuleID) const = 0; + }; + }; + + ///////////////////////////////////////////////// + /// \brief This feature sets the CapsuleShape properties such as + /// the capsule radius and length. + class IGNITION_PHYSICS_VISIBLE SetCapsuleShapeProperties + : public virtual FeatureWithRequirements + { + public: template + class CapsuleShape : public virtual Entity + { + public: using Scalar = typename PolicyT::Scalar; + + /// \brief Set the radius of this CapsuleShape + /// \param[in] _radius + /// The desired radius of this CapsuleShape + public: void SetRadius(Scalar _radius); + + /// \brief Set the length of this CapsuleShape + /// \param[in] _length + /// The desired length of this CapsuleShape + public: void SetLength(Scalar length); + }; + + public: template + class Implementation : public virtual Feature::Implementation + { + public: using Scalar = typename PolicyT::Scalar; + + public: virtual void SetCapsuleShapeRadius( + const Identity &_capsuleID, Scalar _radius) = 0; + + public: virtual void SetCapsuleShapeLength( + const Identity &_capsuleID, Scalar _length) = 0; + }; + }; + + ///////////////////////////////////////////////// + /// \brief This feature constructs a new capsule shape and attaches the + /// desired pose in the link frame. The pose could be defined to be the + /// capsule center point in actual implementation. + class IGNITION_PHYSICS_VISIBLE AttachCapsuleShapeFeature + : public virtual FeatureWithRequirements + { + public: template + class Link : public virtual Feature::Link + { + public: using Scalar = typename PolicyT::Scalar; + + public: using PoseType = + typename FromPolicy::template Use; + + public: using ShapePtrType = CapsuleShapePtr; + + /// \brief Rigidly attach a CapsuleShape to this link. + /// \param[in] _radius + /// The radius of the capsule. + /// \param[in] _length + /// The length of the capsule. + /// \param[in] _pose + /// The desired pose of the CapsuleShape relative to the Link frame. + /// \returns a ShapePtrType to the newly constructed CapsuleShape + public: ShapePtrType AttachCapsuleShape( + const std::string &_name = "capsule", + Scalar _radius = 0.5, + Scalar _length = 1.0, + const PoseType &_pose = PoseType::Identity()); + }; + + public: template + class Implementation : public virtual Feature::Implementation + { + public: using Scalar = typename PolicyT::Scalar; + + public: using PoseType = + typename FromPolicy::template Use; + + public: virtual Identity AttachCapsuleShape( + const Identity &_linkID, + const std::string &_name, + Scalar _radius, + Scalar _length, + const PoseType &_pose) = 0; + }; + }; + } +} + +#include + +#endif diff --git a/include/gz/physics/Cloneable.hh b/include/gz/physics/Cloneable.hh new file mode 100644 index 000000000..4de4c0c6a --- /dev/null +++ b/include/gz/physics/Cloneable.hh @@ -0,0 +1,123 @@ +/* + * Copyright (C) 2018 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 GZ_PHYSICS_CLONEABLE_HH_ +#define GZ_PHYSICS_CLONEABLE_HH_ + +#include + +namespace ignition +{ + namespace physics + { + /// \brief This class allows us to effectively perform type erasure while + /// still being able to copy, move, and clone the values of objects. + /// \private + class Cloneable + { + /// \brief Default constructor + public: Cloneable() = default; + + /// \brief Virtual destructor + public: virtual ~Cloneable() = default; + + /// \brief Do not copy this class directly, use Clone() or Copy() instead. + /// \param[in] _doNotCopy Not used. + public: Cloneable(const Cloneable &_doNotCopy) = delete; + + /// \brief Do not copy this class directly, use Clone() or Copy() instead. + /// \param[in] _doNotCopy Not used. + public: Cloneable& operator=(const Cloneable &_doNotCopy) = delete; + + /// \brief Implement this function to allow your Cloneable type to be + /// cloned safely. + /// \return Return a pointer to a cloned version of this object. + public: virtual std::unique_ptr Clone() const = 0; + + /// \brief Implement this function to allow your Cloneable type to copy + /// another Cloneable object. Assume that the fully-derived type of other + /// is the same as the fully-derived type of this object (i.e. you may use + /// static_cast instead of dynamic_cast). + /// \param[in] _other Instance to copy into this object. + public: virtual void Copy(const Cloneable &_other) = 0; + + /// \brief Implement this function to allow your Cloneable type to move + /// data from another Cloneable object. Assume that the fully-derived + /// type of other is the same as the fully-derived type of this object. + /// \param[in] _other Instance to move into this object. + public: virtual void Copy(Cloneable &&_other) = 0; + }; + + /// \brief Assuming the type T follows the Rule of Five or the Rule of Zero + /// (see http://en.cppreference.com/w/cpp/language/rule_of_three), this + /// class creates a type that will implement the Copy and Clone functions + /// for it. + /// + /// \warning In order to minimize overhead, this class does not do any type + /// safety checks. It should only be used by a class like CompositeData + /// which takes responsibility for ensuring type safety. + /// \private + template + class MakeCloneable final : public T, public Cloneable + { + /// \brief Perfect-forwarding constructor + /// \param[in] _args + /// Parameters which will be perfectly forwarded to the constructor of T + public: template + // This constructor is not marked as explicit because we want it to + // allow implicit conversions by design + MakeCloneable(Args&&... _args); // NOLINT + + /// \brief Copy constructor + /// \param[in] _other + /// Another MakeCloneable object which we will copy + public: MakeCloneable(const MakeCloneable &_other); + + /// \brief Move constructor + /// \param[in] _other + /// An rvalue-reference to a MakeCloneable object which we will + /// consume + public: MakeCloneable(MakeCloneable &&_other); + + /// \brief Copy operator + /// \param[in] _other + /// Another MakeCloneable object which we will copy + /// \return A reference to this object + public: MakeCloneable &operator=(const MakeCloneable &_other); + + /// \brief Move operator + /// \param[in] _other + /// An rvalue-reference to a MakeCloneable object which we will + /// consume + /// \return A reference to this object + public: MakeCloneable& operator=(MakeCloneable &&_other); + + // Documentation inherited + public: std::unique_ptr Clone() const final; + + // Documentation inherited + public: void Copy(const Cloneable &_other) final; + + // Documentation inherited + public: void Copy(Cloneable &&_other) final; + }; + } +} + +#include "gz/physics/detail/Cloneable.hh" + +#endif diff --git a/include/gz/physics/CompositeData.hh b/include/gz/physics/CompositeData.hh new file mode 100644 index 000000000..a40a88571 --- /dev/null +++ b/include/gz/physics/CompositeData.hh @@ -0,0 +1,991 @@ +/* + * Copyright (C) 2018 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 GZ_PHYSICS_COMPOSITEDATA_HH_ +#define GZ_PHYSICS_COMPOSITEDATA_HH_ + +#include +#include +#include + +#include + +#include "gz/physics/Cloneable.hh" +#include "gz/physics/Export.hh" + +namespace ignition +{ + namespace physics + { + // Forward declarations + namespace detail + { + template class PrivateExpectData; + template class PrivateRequireData; + } + + /// \brief The CompositeData class allows arbitrary data structures to be + /// composed together, copied, and moved with type erasure. + class IGNITION_PHYSICS_VISIBLE CompositeData + { + /// \brief Default constructor. Creates an empty CompositeData object. + public: CompositeData(); + + /// \brief Virtual destructor + public: virtual ~CompositeData() = default; + + /// \brief Get a reference to a Data object. If an object of the Data type + /// does not already exist in this CompositeData, then create one using + /// its default constructor. This function will fail to compile if a + /// default constructor is not available for the Data type. + /// + /// When you have a `const CompositeData` object, you must use the + /// function CompositeData::query() in order to retrieve objects. + /// + /// Example usage: + /// + /// \code + /// #include + /// #include + /// + /// using namespace ignition::physics; + /// + /// // Create a data structure called MyData + /// struct MyData + /// { + /// std::string myString; + /// }; + /// + /// int main() + /// { + /// CompositeData composite; + /// + /// // An object of type MyData is created using the default + /// // constructor of MyData. The MyData object will be stored + /// // inside of the object called "composite", and we can grab a + /// // mutable reference to it. + /// MyData &data = composite.Get(); + /// + /// // We can modify the MyData object inside of "composite" using + /// // the mutable reference that we grabbed. + /// data.myString = "I modified this data"; + /// + /// // This will print out "I modified this data", because + /// // Get() will retrieve the instance of MyData that was + /// // created by the first call of Get(). + /// std::cout << composite.Get().myString << std::endl; + /// } + /// \endcode + /// + /// \tparam Data + /// The type of data entry to get + /// + /// \return a reference to to a Data-type object that is stored within + /// this CompositeData. + public: template + Data &Get(); + + /// \brief This struct is the return type of the various Insert...() + /// functions. It returns a reference to the data entry for the Data type, + /// and it indicates whether the insertion operation actually occurred. + public: template + struct InsertResult + { + /// \brief A reference to the Data entry within the CompositeData + /// object. + public: Data &data; + + /// \brief True if the operation resulted in inserting a new data entry. + /// + /// For Insert(), false means that nothing was inserted. + /// + /// For InsertOrAssign(), false means that the Data entry which + /// used to be in the CompositeData has been assigned the new value. + public: const bool inserted; + + // We explicitly delete the copy assignment operator, because we don't + // want someone to inadvertently assign a value to the data that's being + // referenced (users must do so explicitly by calling on the `data` + // member variable). Note that this operator is already implicitly + // deleted by having the const bool member variable, but we are also + // deleting it explicitly for clarity. + public: InsertResult &operator=(const InsertResult&) = delete; + }; + + /// \brief This will attempt to insert a new Data entry into the + /// CompositeData object, forwarding _args to the constructor of the + /// entry. If an entry already exists for this Data type, then nothing + /// will be inserted. + /// + /// Example usage: + /// + /// \code + /// #include + /// #include + /// #include + /// + /// using namespace ignition::physics; + /// + /// // Create a data structure called MyData + /// struct MyData + /// { + /// MyData(const std::string &_arg = "") + /// : myString(_arg) + /// { + /// // Intentionally blank + /// } + /// + /// std::string myString; + /// }; + /// + /// int main() + /// { + /// CompositeData composite; + /// + /// // Create an object of type MyData and store it in "composite". + /// // A reference to the newly created object is returned. + /// MyData &data = composite.Insert("some argument").data; + /// + /// // This will print out "some argument", because the string in + /// // MyData was initialized with that argument. + /// std::cout << composite.Get().myString << std::endl; + /// + /// // Modify the object + /// data.myString = "I modified this data"; + /// + /// // This will print out "I modified this data" because we used a + /// // mutable reference to change the value of MyData::myString. + /// std::cout << composite.Get().myString << std::endl; + /// + /// // This will not modify the MyData instance that's being held by + /// // "composite", because the instance already exists. It will + /// // just return the instance as a reference and ignore the + /// // argument that has been passed in. + /// MyData &altData = composite.Insert( + /// "another argument").data; + /// + /// // The reference to "data" is still perfectly valid, and in fact + /// // its underlying pointer is equal to the underlying pointer of + /// // "altData". + /// assert((&data) == (&altData)); + /// + /// // This will print out "I modified this data" because there have + /// // not been any function calls that can alter the value of + /// // myString in the time since that modification was made. + /// std::cout << composite.Insert().data.myString + /// << std::endl; + /// } + /// \endcode + /// + /// \tparam Data + /// The type name for the data entry + /// \tparam Args + /// This will be inferred from _args; you should not typically set this + /// explicitly. + /// + /// \param[in] _args + /// The arguments to use for construction. These will get wrapped in a + /// Data(...) constructor. If _args is left blank, the default + /// constructor will be used. + /// + /// \return An InsertResult which contains a reference to the data + /// entry (either the one that is newly inserted or the one that already + /// existed). InsertResult::inserted will be true if the entry was + /// inserted by this function, or false if the entry already existed. + /// + /// \sa InsertOrAssign + public: template + InsertResult Insert(Args &&..._args); + + /// \brief Attempt to insert a Data-type entry. If a Data-type entry did + /// not already exist, it will be constructed by copying (or moving) + /// the given arguments. If a Data-type entry already existed, the + /// existing entry will be assigned the value of Data(_args...). + /// + /// Any previously existing valid references to the Data entry will remain + /// valid, even after this function is called. + /// + /// Example usage: + /// + /// \code + /// #include + /// #include + /// + /// using namespace ignition::physics; + /// + /// // Create a data structure called MyData + /// struct MyData + /// { + /// MyData(const std::string &_arg = "") + /// : myString(_arg) + /// { + /// // Intentionally blank + /// } + /// + /// std::string myString; + /// }; + /// + /// int main() + /// { + /// CompositeData composite; + /// + /// // Create an object of type MyData and store it in "composite". + /// // A reference to the newly created object is returned. + /// MyData &data = composite.InsertOrAssign( + /// "some argument").data; + /// + /// // This will print out "some argument", because the string in + /// // MyData was initialized with that argument. + /// std::cout << composite.Get().myString << std::endl; + /// + /// // Modify the object + /// data.myString = "I modified this data"; + /// + /// // This will print out "I modified this data" because we used a + /// // mutable reference to change the value of MyData::myString. + /// std::cout << composite.Get().myString << std::endl; + /// + /// // Assign the MyData entry a new, default-constructed value. + /// composite.InsertOrAssign(); + /// + /// // This will print out nothing but a newline. + /// std::cout << composite.Get().myString << std::endl; + /// } + /// \endcode + /// + /// \tparam Data + /// The type name for the data entry + /// \tparam Args + /// This will be inferred from _args; you should not typically set this + /// explicitly. + /// + /// \param[in] _args + /// The arguments to use for construction or assignment. These will get + /// wrapped in a Data(...) constructor. If _args is left blank, the + /// default constructor will be used. + /// + /// \return an InsertResult which contains a reference to the + /// Data-type entry of this CompositeData. InsertResult::inserted + /// will be true iff a Data-type entry did not already exist in this + /// CompositeData. If the value was instead assigned, + /// InsertResult::inserted will be false. + /// + /// \sa Insert + public: template + InsertResult InsertOrAssign(Args &&... _args); + + /// \brief This will remove a Data-type object from this CompositeData and + /// delete it if one is present. Otherwise, it will do nothing. Data-types + /// that are marked as required will not (and cannot) be removed. + /// + /// If the data was successfully removed or did not exist to begin with, + /// this returns true. If the data was marked as required and therefore + /// not removed, this returns false. + /// + /// \warning Calling this function will permanently invalidate all + /// existing references to the Data-type entry of this CompositeData, i.e. + /// the references that get returned by Get(), Insert(~), + /// InsertOrAssign(~), or Query(). + /// + /// Example usage: + /// + /// \code + /// #include + /// #include + /// + /// using namespace ignition::physics; + /// + /// // Create a data structure called MyData + /// struct MyData + /// { + /// MyData(const std::string &_arg = "") + /// : myString(_arg) + /// { + /// // Intentionally blank + /// } + /// + /// std::string myString; + /// }; + /// + /// int main() + /// { + /// CompositeData composite; + /// + /// // Create an object of type MyData and store it in "composite". + /// // Append .data to get a reference to the new data. + /// MyData &data = composite.Insert("some argument").data; + /// + /// // Print out "some argument" + /// std::cout + /// << composite.Insert("another argument").data.myString + /// << std::endl; + /// + /// // Remove the MyData object. Note that "data" is now INVALID and + /// // must never be used again after this function call. + /// composite.Remove(); + /// + /// // Print out "another argument" + /// std::cout + /// << composite.Insert("another argument").data.myString + /// << std::endl; + /// } + /// \endcode + /// + /// \tparam Data + /// The type of data entry to remove + /// + /// \return true iff the CompositeData no longer contains this Data type. + public: template + bool Remove(); + + /// \brief Use these flags in Query(), Has(), and StatusOf() to change + /// their effects on the meta info of the data being queried. + /// + /// See UnqueriedEntries() for more on the "queried" flag. + enum class QueryMode : int + { + /// \brief Performing the operation will cause an unqueried Data's + /// status to flip to queried. Data that is already marked as queried + /// will be unaffected. + NORMAL = 0, + + /// \brief Performing the operation has no effect on whether data is + /// marked as queried. + SILENT + }; + + /// \brief Query this CompositeData for a Data-type entry. If it contains + /// a Data-type object, it gets returned as a Data*. Otherwise, a nullptr + /// is returned. + /// + /// If _mode is set to QueryMode::SILENT, then calling this function will + /// not cause the "queried" flag to change (see UnqueriedEntries() for + /// more on the "queried" flag). + /// + /// Example usage: + /// + /// \code + /// #include + /// #include + /// + /// using namespace ignition::physics; + /// + /// struct MyDataWithoutDefault + /// { + /// // This struct does not allow us to use the default constructor + /// MyDataWithoutDefault() = delete; + /// + /// // We must call this constructor if we want to make an object of + /// // this type. + /// MyDataWithoutDefault(const int inputValue) + /// : myValue(inputValue) + /// { + /// // Intentionally blank + /// } + /// + /// int myValue; + /// }; + /// + /// void SetValueIfAvailable(const int _value, + /// CompositeData &_composite) + /// { + /// // This CANNOT compile because MyDataWithoutDefault does not + /// // provide a default constructor + /// // MyDataWithoutDefault &data = + /// // composite.Get(); + /// + /// // Get a pointer to a MyDataWithoutDefault instance if one is + /// // available, otherwise we get a nullptr. + /// MyDataWithoutDefault *data = + /// _composite.Query(); + /// + /// if(data) + /// data->myValue = _value; + /// } + /// + /// int main() + /// { + /// CompositeData composite; + /// + /// // This will do nothing, because "composite" does not contain + /// // an object of type MyDataWithoutDefault. + /// SetValueIfAvailable(320, composite); + /// + /// // We can create a MyDataWithoutDefault object by calling the + /// // Insert function and passing it an argument for the object's + /// // constructor. + /// composite.Insert(123); + /// + /// // This will print out "123" because the object will not be + /// // re-created by Insert. We can call Insert because it + /// // can use the argument we pass in to create an instance of + /// // MyDataWithoutDefault if an instance of it did not already + /// // exist, unlike Get which can only call the default + /// // constructor. + /// std::cout + /// << composite.Insert(1).data.myValue + /// << std::endl; + /// + /// // This will set myValue to 5 because "composite" contains an + /// // instance of MyDataWithoutDefault. + /// SetValueIfAvailable(5, composite); + /// + /// // This will print out "5" because that was the value set by + /// // SetValueIfAvailable. + /// std::cout + /// << composite.Insert(3).data.myValue + /// << std::endl; + /// } + /// \endcode + /// + /// \tparam Data + /// The type of data entry to query for + /// + /// \param[in] _mode + /// Specify how this call should affect the query flag of the entry. + /// The default behavior is strongly recommended, unless you know what + /// you are doing. See QueryMode for more info. + /// + /// \return a pointer to the Data entry if this CompositeData has one. + /// Otherwise, this returns a nullptr. + public: template + Data *Query(const QueryMode _mode = QueryMode::NORMAL); + + /// \brief Const-qualified version of Query. This can be used to retrieve + /// data from a `const CompositeData`. + /// + /// If "mode" is set to QueryMode::SILENT, then calling this function will + /// not cause the "queried" flag to change (see UnqueriedEntries() for + /// more on the "queried" flag). + /// + /// Example usage: + /// + /// \code + /// #include + /// #include + /// + /// using namespace ignition::physics; + /// + /// struct MyData + /// { + /// std::string myString; + /// }; + /// + /// void PrintStringIfAvailable(const CompositeData &_composite) + /// { + /// // The following Get() CANNOT compile because "composite" is + /// // a const-qualified reference, but Get() is NOT a + /// // const-qualified member function. Get() cannot be + /// // const-qualified because it needs to create an instance of + /// // type T if an instance of that type was not already available + /// // in the CompositeData object, and that would mean modifying + /// // the CompositeData object, which violates const-correctness. + /// // This is similarly the case for Insert(~) and + /// // InsertOrAssign(~). + /// // + /// // MyData &data = _composite.Get(); // error! + /// + /// // Instead, we use the const-qualified Query() function which + /// // can return a const-qualified pointer to the data instance. + /// const MyData *data = _composite.Query(); + /// + /// if(data) + /// std::cout << data->myString << std::endl; + /// else + /// std::cout << "No string available" << std::endl; + /// } + /// + /// int main() + /// { + /// CompositeData composite; + /// + /// // This will print "No string available" because "composite" + /// // does not have a MyData instance yet. + /// PrintStringIfAvailable(composite); + /// + /// // Use the default constructor to create MyData, and use the + /// // reference it returns to set the value of MyData::myString. + /// composite.Get().myString = "here is a string"; + /// + /// // This will print "here is a string". + /// PrintStringIfAvailable(composite); + /// } + /// \endcode + /// + /// \tparam Data + /// The type of data entry to query for + /// + /// \param[in] _mode + /// Specify how this call should affect the query flag of the entry. + /// The default behavior is strongly recommended, unless you know what + /// you are doing. See QueryMode for more info. + /// + /// \return a const-qualified pointer to the Data entry if this + /// CompositeData has one. Otherwise, this returns a nullptr. + public: template + const Data *Query(const QueryMode mode = QueryMode::NORMAL) const; + + /// \brief Returns true if this CompositeData has an object of type Data, + /// otherwise returns false. This is literally equivalent to + /// (nullptr != Query(QueryMode::SILENT)). + /// + /// \tparam Data + /// The type of data entry to check for + /// + /// \return true if this CompositeData contains a Data-type entry. + public: template + bool Has() const; + + /// \brief Struct that describes the status of data. + struct IGNITION_PHYSICS_VISIBLE DataStatus + { + /// \brief If the data exists in the CompositeData, this will be true, + /// otherwise it is false. + public: bool exists; + + /// \brief If the data was marked as queried BEFORE calling StatusOf + /// (regardless of what QueryMode is used), this will be true, otherwise + /// it is false. See UnqueriedEntries() for more on the "queried" flag. + public: bool queried; + + /// \brief If the data is marked as required, this will be true, + /// otherwise it is false. + public: bool required; + + /// \brief Default constructor. Initializes everything to false. + DataStatus(); + }; + + /// \brief Returns a DataStatus object that describes the status of the + /// requested data type. + /// + /// \tparam Data + /// The type of data entry to check the status of + /// + /// \return a DataStatus for the requested entry. + public: template + DataStatus StatusOf() const; + + /// \brief Returns true if this CompositeData has a Data-type object + /// which was marked as queried, and that object is now marked as + /// unqueried. If an object of that type does not exist or it was already + /// unqueried, this returns false. + /// + /// This function is const-qualified because the query flags are declared + /// as mutable. + /// + /// Example usage: + /// + /// \code + /// #include + /// #include + /// + /// using namespace ignition::physics; + /// + /// struct MyData + /// { + /// std::string myString; + /// }; + /// + /// int main() + /// { + /// CompositeData composite; + /// + /// CompositeData::DataStatus status = composite.StatusOf(); + /// + /// // An instance of MyData should not exist, be required, or be + /// // queried. + /// assert(!status.exists); + /// assert(!status.queried); + /// assert(!status.required); + /// + /// composite.Insert(); + /// status = composite.StatusOf(); + /// + /// // MyData should now be queried, because explicitly creating an + /// // object will mark it as queried. + /// assert(status.exists); + /// assert(status.queried); + /// + /// // Turn off the "queried" flag for MyData. + /// composite.Unquery(); + /// + /// status = composite.StatusOf(); + /// assert(status.exists); + /// // It should be unqueried because we turned off its query flag + /// // using Unquery(). + /// assert(!status.queried); + /// } + /// \endcode + /// + /// \tparam Data + /// The type of data entry whose query flag should be cleared + /// + /// \return true if the query flag is changing from queried to unqueried, + /// otherwise false. + public: template + bool Unquery() const; + + /// \brief Marks the specified type of Data as required, creates one with + /// the given arguments if it did not exist, and returns a reference to + /// it. + /// + /// Warning: This cannot be undone. Once a Data type is marked as + /// required, it will continue to be required for this object throughout + /// the rest of the CompositeData object's lifespan. + /// + /// Example usage: + /// + /// \code + /// #include + /// #include + /// + /// using namespace ignition::physics; + /// + /// struct MyData + /// { + /// MyData(const std::string &arg = "") + /// : myString(arg) + /// { + /// // Intentionally blank + /// } + /// + /// std::string myString; + /// }; + /// + /// int main() + /// { + /// CompositeData composite; + /// + /// // Create an instance of MyData and mark it as required + /// composite.MakeRequired("this is required"); + /// + /// // Now it cannot be removed from "composite" + /// assert(!composite.Remove()); + /// + /// // Once "composite" goes out of scope, the MyData instance will + /// // be deleted as normal, because its lifecycle is still tied to + /// // the CompositeData object. + /// } + /// \endcode + /// + /// \tparam Data + /// The type of data entry that should be marked as required + /// \tparam Args + /// This will be inferred from _args; you should not typically set this + /// explicitly. + /// + /// \param[in] _args + /// The arguments to use for construction, if a Data-type entry did not + /// already exist within this CompositeData. + /// + /// \return a reference to the Data-type entry + public: template + Data &MakeRequired(Args &&..._args); + + /// \brief Returns true if the specified Data type is required by this + /// CompositeData object. Otherwise, returns false. + /// + /// For more on required data, see MakeRequired(). + /// + /// \tparam Data + /// The type of data entry whose requirements are being checked + /// + /// \return true iff the specified Data type is required by this + /// CompositeData + public: template + bool Requires() const; + + /// \brief When called from a generic CompositeData type, this always + /// returns false. More highly specified CompositeData types that use + /// ExpectData or RequireData may return true if the type of Data is + /// expected. + /// + /// \tparam Data + /// The type of data whose expectation is being checked + /// + /// \return When called on a basic CompositeData object, this is always + /// false. + public: template + static constexpr bool Expects(); + + /// \brief When called from a generic CompositeData type, this always + /// returns false. Static (Always) requirements are determined at + /// compile-time and cannot be changed at runtime. Using the RequireData + /// class can make this return true for more highly specified + /// CompositeData types. + /// + /// \warning This should never be used to check whether Data is required + /// on a specific instance, because the requirements that are placed on an + /// instance can be changed at runtime. This should only be used to check + /// whether a certain data type is always required for a certain + /// specification of CompositeData. + /// + /// \tparam Data + /// The type of data whose requirement we are checking at compile time + /// + /// \return When called on a basic CompositeData object, this is always + /// false. + public: template + static constexpr bool AlwaysRequires(); + + /// \brief Check how many data entries are in this CompositeData. Runs + /// with O(1) complexity. + /// + /// \return the number of data entries currently contained in this + /// CompositeData. + public: std::size_t EntryCount() const; + + /// \brief Check how many data entries in this CompositeData have not been + /// queried. See UnqueriedEntries() for more information about the + /// "queried" flag. Runs with O(1) complexity. + /// + /// \return the number of entries in this CompositeData which have + /// not been queried. + public: std::size_t UnqueriedEntryCount() const; + + /// \brief Reset the query flags on all data entries. This will make it + /// appear as though no entries have ever been queried. See + /// UnqueriedEntries() for more information about the "queried" flag. + /// + /// \attention It is good practice to call this function before returning + /// a CompositeData from a function and handing it off to another segment + /// of a pipeline, because sometimes the compiler inappropriately elides + /// the copy/move constructor/operators and passes along the state of the + /// queries, even though it should not. + public: void ResetQueries() const; + + /// \brief Get an ordered set of all data entries in this CompositeData. + /// Runs with O(N) complexity. + /// + /// Example usage: + /// + /// \code + /// #include + /// #include + /// + /// using namespace ignition::physics; + /// + /// struct MyData1 + /// { + /// /* ... put some data here ... */ + /// }; + /// + /// struct MyData2 + /// { + /// /* ... put some data here ... */ + /// }; + /// + /// struct MyData3 + /// { + /// /* ... put some data here ... */ + /// }; + /// + /// void PrintStuff(const CompositeData &composite) + /// { + /// std::cout << "Entry types:" << std::endl; + /// for (const std::string &label : composite.AllEntries()) + /// { + /// std::cout << " " << label << std::endl; + /// } + /// } + /// + /// int main() + /// { + /// CompositeData composite; + /// composite.Insert(); + /// composite.Insert(); + /// composite.Insert(); + /// + /// PrintStuff(composite); + /// + /// // The function PrintStuff will print the names of each struct. + /// } + /// \endcode + /// + /// \return an ordered (alphabetical) set of all data entries in this + /// CompositeData. + public: std::set AllEntries() const; + + /// \brief Get an ordered (alphabetical) set of the data entries in + /// this CompositeData which have not been queried (Get, Insert, + /// InsertOrAssign, Query, and MakeRequired all perform querying) since + /// the data was implicitly created (e.g. by a copy or move operation) or + /// since the last call to ResetQueries(), whichever is more recent. Runs + /// with O(N) complexity. + /// + /// Unqueried data entries might be created by copy/move construction, + /// copy/move assignment operation, or the Copy(~) function. Using the + /// copy/move operator or the Copy(~) function will reset the query flag + /// on any data that gets copied or moved over. + /// + /// \note This function can be useful for reporting runtime warnings about + /// any unsupported data types that have been given to you. + /// + /// Example usage: + /// + /// \code + /// #include + /// #include + /// + /// using namespace ignition::physics; + /// + /// struct MyData1 + /// { + /// /* ... put some data here ... */ + /// }; + /// + /// struct MyData2 + /// { + /// /* ... put some data here ... */ + /// }; + /// + /// struct MyData3 + /// { + /// /* ... put some data here ... */ + /// }; + /// + /// void DoStuff(CompositeData &composite) + /// { + /// MyData1 &data1 = composite.Get(); + /// /* ... do something with data1 ... */ + /// + /// MyData2 &data2 = composite.Get(); + /// /* ... do something with data2 ... */ + /// + /// + /// const std::set &unqueried = + /// composite.UnqueriedEntries(); + /// if(unqueried.size() > 0) + /// { + /// std::cout << "I don't know what to do with " + /// << "the following type(s) of data:\n"; + /// for(const std::string &label : unqueried) + /// std::cout << " -- " << label << "\n"; + /// std::cout << std::endl; + /// } + /// } + /// + /// int main() + /// { + /// CompositeData composite; + /// composite.Insert(); + /// composite.Insert(); + /// composite.Insert(); + /// + /// composite.ResetQueries(); + /// DoStuff(composite); + /// + /// // The function DoStuff will print out that it does not know + /// // what to do with MyData3. + /// } + /// \endcode + /// + /// \return an ordered set of the names of unqueried data entries in this + /// CompositeData. + public: std::set UnqueriedEntries() const; + + /// \brief Make this CompositeData a copy of _other. However, any data + /// entries in this CompositeData which are marked as required will not be + /// removed. + /// + /// \param[in] _other Another CompositeData + /// \param[in] _mergeRequirements If true, this object will also take on + /// the requirements specified by _other. Any objects that are already + /// marked as required in this CompositeData will remain required. If + /// false, the requirements of this CompositeData object are unaffected. + /// \return A reference to this object + /// + /// \sa Copy(CompositeData &&, bool) + /// \sa Merge() + /// \sa operator=() + public: CompositeData &Copy(const CompositeData &_other, + const bool _mergeRequirements = false); + + /// \brief An alternative to Copy(const CompositeData &, bool) that takes + /// advantage of move semantics. + public: CompositeData &Copy(CompositeData &&_other, + const bool _mergeRequirements = false); + + /// \brief Merge the data from _other into this CompositeData. If there + /// are any conflicting data entries, the entry from _other will take + /// precedence. + /// + /// \param[in] _other Another CompositeData + /// \param[in] _mergeRequirements If true, this object will also take on + /// the requirements specified by _other. Any objects that are already + /// marked as required in this CompositeData will remain required. If + /// false, the requirements of this CompositeData object are unaffected. + /// \return A reference to this object + /// + /// \sa Merge(CompositeData &&) + /// \sa Copy() + public: CompositeData &Merge(const CompositeData &_other, + const bool _mergeRequirements = false); + + /// \brief An alternative to Merge(const CompositeData &, bool) that takes + /// advantage of move semantics. + public: CompositeData &Merge(CompositeData &&_other, + const bool _mergeRequirements = false); + + /// \brief Copy constructor. Same as Copy(_other). + public: CompositeData(const CompositeData &_other); + + /// \brief Move constructor. Same as Copy(_other). + public: CompositeData(CompositeData &&_other); + + /// \brief Copy operator. Same as Copy(_other). + public: CompositeData &operator=(const CompositeData &_other); + + /// \brief Move operator. Same as Copy(_other). + public: CompositeData &operator=(CompositeData &&_other); + + /// \brief Struct which contains information about a data type within the + /// CompositeData. See gz/physics/detail/CompositeData.hh for the + /// definition. This class is public so that helper functions can use it + /// without being friends of the class. + /// \private + public: struct DataEntry; + + // We make this typedef public so that helper functions can use it without + // being friends of the class. + public: using MapOfData = std::map; + + IGN_UTILS_WARN_IGNORE__DLL_INTERFACE_MISSING + /// \brief Map from the label of a data object type to its entry + protected: MapOfData dataMap; + IGN_UTILS_WARN_RESUME__DLL_INTERFACE_MISSING + + /// \brief Total number of data entries currently in this CompositeData. + /// Note that this may differ from the size of dataMap, because some + /// entries in dataMap will be referring to nullptrs. + protected: std::size_t numEntries; + + /// \brief Total number of unique queries which have been performed since + /// either construction or the last call to ResetQueries(). + protected: mutable std::size_t numQueries; + + // Declare friendship + template friend class detail::PrivateExpectData; + template friend class detail::PrivateRequireData; + }; + } +} + +#include "gz/physics/detail/CompositeData.hh" + +#endif diff --git a/include/gz/physics/ConstructEmpty.hh b/include/gz/physics/ConstructEmpty.hh new file mode 100644 index 000000000..7ed6d503d --- /dev/null +++ b/include/gz/physics/ConstructEmpty.hh @@ -0,0 +1,139 @@ +/* + * Copyright (C) 2018 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 GZ_PHYSICS_CONSTRUCTEMPTY_HH_ +#define GZ_PHYSICS_CONSTRUCTEMPTY_HH_ + +#include + +#include + +namespace ignition { +namespace physics { + +///////////////////////////////////////////////// +//! [ConstructEmptyWorld] +/// \brief This feature constructs an empty world and returns its pointer +/// from the current physics engine in use. +class ConstructEmptyWorldFeature : public virtual Feature +{ + public: template + class Engine : public virtual Feature::Engine + { + public: using WorldPtrType = WorldPtr; + + /// \brief Construct an empty world and attach a given name to it. + /// \param[in] _name + /// Name of the world. + /// \return + /// The WorldPtrType of the constructed world. + public: WorldPtrType ConstructEmptyWorld(const std::string &_name); + }; + + public: template + class Implementation : public virtual Feature::Implementation + { + public: virtual Identity ConstructEmptyWorld( + const Identity &_engineID, const std::string &_name) = 0; + }; +}; +//! [ConstructEmptyWorld] + +///////////////////////////////////////////////// +/// \brief This feature constructs an empty model and returns its pointer +/// from the given world. +class ConstructEmptyModelFeature : public virtual Feature +{ + public: template + class World : public virtual Feature::World + { + public: using ModelPtrType = ModelPtr; + + /// \brief Construct an empty model and attach a given name to it. + /// \param[in] _name + /// Name of the model. + /// \return + /// The ModelPtrType of the constructed model. + public: ModelPtrType ConstructEmptyModel(const std::string &_name); + }; + + public: template + class Implementation : public virtual Feature::Implementation + { + public: virtual Identity ConstructEmptyModel( + const Identity &_worldID, const std::string &_name) = 0; + }; +}; + +///////////////////////////////////////////////// +/// \brief This feature constructs an empty nested model and returns its pointer +/// from the given world. +class ConstructEmptyNestedModelFeature : public virtual Feature +{ + public: template + class Model : public virtual Feature::Model + { + public: using ModelPtrType = ModelPtr; + + /// \brief Construct an empty model and attach a given name to it. + /// \param[in] _name + /// Name of the nested model. + /// \return + /// The ModelPtrType of the constructed model. + public: ModelPtrType ConstructEmptyNestedModel(const std::string &_name); + }; + + public: template + class Implementation : public virtual Feature::Implementation + { + public: virtual Identity ConstructEmptyNestedModel( + const Identity &_modelID, const std::string &_name) = 0; + }; +}; + +///////////////////////////////////////////////// +/// \brief This feature constructs an empty link and returns its pointer +/// from the given model. +class ConstructEmptyLinkFeature : public virtual Feature +{ + public: template + class Model : public virtual Feature::Model + { + public: using LinkPtrType = LinkPtr; + + /// \brief Construct an empty link and attach a given name to it. + /// \param[in] _name + /// Name of the link. + /// \return + /// The LinkPtrType of the constructed link. + public: LinkPtrType ConstructEmptyLink(const std::string &_name); + }; + + public: template + class Implementation : public virtual Feature::Implementation + { + public: virtual Identity ConstructEmptyLink( + const Identity &_modelID, const std::string &_name) = 0; + }; +}; + +} +} + +#include + +#endif diff --git a/include/gz/physics/ContactProperties.hh b/include/gz/physics/ContactProperties.hh new file mode 100644 index 000000000..2b097e448 --- /dev/null +++ b/include/gz/physics/ContactProperties.hh @@ -0,0 +1,169 @@ +/* + * Copyright (C) 2021 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 GZ_PHYSICS_CONTACTPROPERTIES_HH_ +#define GZ_PHYSICS_CONTACTPROPERTIES_HH_ + +#include +#include + +#include +#include +#include +#include +#include + +namespace ignition +{ +namespace physics +{ +/// \brief SetContactPropertiesCallbackFeature is a feature for setting the +/// properties of a contact after it is created but before it affects the +/// forward step. +class IGNITION_PHYSICS_VISIBLE SetContactPropertiesCallbackFeature + : public virtual FeatureWithRequirements +{ + /// \brief This struct gets filled by the simulator and contains various + /// properties of a contact joint (surface, constraint). All of the values + /// are optional, which means that they are only filled if the physics engine + /// supports them. Some originally unfilled values may still be processed by + /// the physics engine if they are set - this just means there is no default + /// value for them. + public: template struct ContactSurfaceParams + { + /// \brief Coefficient of friction along the 1st friction direction. + std::optional frictionCoeff; + + /// \brief Coefficient of friction along the 2nd friction direction. + std::optional secondaryFrictionCoeff; + + /// \brief Coefficient of rolling friction along the 1st friction direction. + std::optional rollingFrictionCoeff; + + /// \brief Coefficient of rolling friction along the 2nd friction direction. + std::optional secondaryRollingFrictionCoeff; + + /// \brief Coefficient of torsional friction. + std::optional torsionalFrictionCoeff; + + /// \brief Force-dependent slip coefficient along the 1st friction + /// direction. + std::optional slipCompliance; + + /// \brief Force-dependent slip coefficient along the 2nd friction + /// direction. + std::optional secondarySlipCompliance; + + /// \brief Defines the bounciness of the contact. 0 is not bouncy. Values + /// between 0 and 1 are allowed. + std::optional restitutionCoeff; + + /// \brief The first frictional direction. It should be perpendicular to the + /// contact normal. The second frictional direction can be computed as a + /// vector perpendicular both to the normal and to the first direction. + std::optional::template Use> + firstFrictionalDirection; + + /// \brief Desired velocity of the colliding bodies in the contact point. + /// Setting this to non-zero asks the physics engine to add such forces + /// that can achieve that the colliding bodies have the specified velocity. + /// The X component specifies velocity along 1st friction direction. + /// The Y component specifies velocity along 2nd friction direction. + /// The Z component specifies velocity along the contact normal. + std::optional::template Use> + contactSurfaceMotionVelocity; + + /// \brief Joint error reduction parameter. This is the fraction of the + /// joint error that will be attempted to be corrected in each simulation + /// step. Allowed values are 0 to 1. Default is usually somewhere between. + std::optional errorReductionParameter; + + /// \brief Maximum velocity that can be used to reduce joint error. + std::optional maxErrorReductionVelocity; + + /// \brief Maximum joint error for which no error reduction is performed. + std::optional maxErrorAllowance; + + /// \brief Constraint force mixing. This should be a non-negative number. + /// If greater than 0, this number is added to the diagonal of the system + /// matrix making the contact softer and the solution more stable. + std::optional constraintForceMixing; + }; + + public: template + class World : public virtual Feature::World + { + public: using ShapePtrType = typename GetContactsFromLastStepFeature + ::World::ShapePtrType; + + /// \brief This callback is called for every detected contact point and + /// allows customizing properties of the contact surface. + /// \param _contact[in] The contact object containing contact point, + /// normal, force etc. Please note that the force will + /// be always zero because the forward step has not yet + /// been run to compute the force. + /// \param _numContactsOnCollision[in] Number of contact points on the same + /// collision object. This can be used + /// e.g. for force normalization. + /// \param _surfaceParams[in,out] Parameters of the contact surface. They + /// are pre-filled by the physics engine and + /// the callback can alter them. + public: typedef std::function< + void( + const typename GetContactsFromLastStepFeature:: + World::Contact& /*_contact*/, + size_t /*_numContactsOnCollision*/, + ContactSurfaceParams& /*_surfaceParams*/) + > SurfaceParamsCallback; + + /// \brief Add the callback. + public: void AddContactPropertiesCallback( + const std::string &_callbackID, SurfaceParamsCallback _callback); + + /// \brief Remove the callback. + public: bool RemoveContactPropertiesCallback( + const std::string &_callbackID); + }; + + public: template + class Implementation : public virtual Feature::Implementation + { + public: using ContactImpl = typename GetContactsFromLastStepFeature + ::Implementation::ContactInternal; + + public: typedef std::function< + void(const ContactImpl&, size_t, ContactSurfaceParams&) + > SurfaceParamsCallback; + + /// \brief Add the callback. + public: virtual void AddContactPropertiesCallback( + const Identity &_worldID, + const std::string &_callbackID, + SurfaceParamsCallback _callback) = 0; + + /// \brief Remove the callback. + public: virtual bool RemoveContactPropertiesCallback( + const Identity &_worldID, const std::string &_callbackID) = 0; + }; +}; + +} +} + +#include "gz/physics/detail/ContactProperties.hh" + +#endif /* end of include guard: GZ_PHYSICS_CONTACTPROPERTIES_HH_ */ diff --git a/include/gz/physics/CylinderShape.hh b/include/gz/physics/CylinderShape.hh new file mode 100644 index 000000000..8ab6e73eb --- /dev/null +++ b/include/gz/physics/CylinderShape.hh @@ -0,0 +1,151 @@ +/* + * Copyright (C) 2018 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 GZ_PHYSICS_CYLINDERSHAPE_HH_ +#define GZ_PHYSICS_CYLINDERSHAPE_HH_ + +#include + +#include +#include + +namespace ignition +{ + namespace physics + { + IGN_PHYSICS_DECLARE_SHAPE_TYPE(CylinderShape) + + class IGNITION_PHYSICS_VISIBLE GetCylinderShapeProperties + : public virtual FeatureWithRequirements + { + public: template + class CylinderShape : public virtual Entity + { + public: using Scalar = typename PolicyT::Scalar; + + /// \brief Get the radius of this CylinderShape + /// \return the radius of this CylinderShape + public: Scalar GetRadius() const; + + /// \brief Get the height (length along the local z-axis) of this + /// CylinderShape. + /// \return the height of this CylinderShape + public: Scalar GetHeight() const; + }; + + public: template + class Implementation : public virtual Feature::Implementation + { + public: using Scalar = typename PolicyT::Scalar; + + public: virtual Scalar GetCylinderShapeRadius( + const Identity &_cylinderID) const = 0; + + public: virtual Scalar GetCylinderShapeHeight( + const Identity &_cylinderID) const = 0; + }; + }; + + ///////////////////////////////////////////////// + /// \brief This feature sets the CylinderShape properties such as + /// the cylinder radius and height. + class IGNITION_PHYSICS_VISIBLE SetCylinderShapeProperties + : public virtual FeatureWithRequirements + { + public: template + class CylinderShape : public virtual Entity + { + public: using Scalar = typename PolicyT::Scalar; + + /// \brief Set the radius of this CylinderShape + /// \param[in] _radius + /// The desired radius of this CylinderShape + public: void SetRadius(Scalar _radius); + + /// \brief Set the height of this CylinderShape + /// \param[in] _height + /// The desired height of this CylinderShape + public: void SetHeight(Scalar _height); + }; + + public: template + class Implementation : public virtual Feature::Implementation + { + public: using Scalar = typename PolicyT::Scalar; + + public: virtual void SetCylinderShapeRadius( + const Identity &_cylinderID, Scalar _radius) = 0; + + public: virtual void SetCylinderShapeHeight( + const Identity &_cylinderID, Scalar _height) = 0; + }; + }; + + ///////////////////////////////////////////////// + /// \brief This feature constructs a new cylinder shape and attaches the + /// desired pose in the link frame. The pose could be defined to be the + /// cylinder center point in actual implementation. + class IGNITION_PHYSICS_VISIBLE AttachCylinderShapeFeature + : public virtual FeatureWithRequirements + { + public: template + class Link : public virtual Feature::Link + { + public: using Scalar = typename PolicyT::Scalar; + + public: using PoseType = + typename FromPolicy::template Use; + + public: using ShapePtrType = CylinderShapePtr; + + /// \brief Rigidly attach a CylinderShape to this link. + /// \param[in] _radius + /// The radius of the cylinder. + /// \param[in] _height + /// The height of the cylinder. + /// \param[in] _pose + /// The desired pose of the CylinderShape relative to the Link frame. + /// \returns a ShapePtrType to the newly constructed CylinderShape + public: ShapePtrType AttachCylinderShape( + const std::string &_name = "cylinder", + Scalar _radius = 1.0, + Scalar _height = 1.0, + const PoseType &_pose = PoseType::Identity()); + }; + + public: template + class Implementation : public virtual Feature::Implementation + { + public: using Scalar = typename PolicyT::Scalar; + + public: using PoseType = + typename FromPolicy::template Use; + + public: virtual Identity AttachCylinderShape( + const Identity &_linkID, + const std::string &_name, + Scalar _radius, + Scalar _height, + const PoseType &_pose) = 0; + }; + }; + } +} + +#include + +#endif diff --git a/include/gz/physics/DataStatusMask.hh b/include/gz/physics/DataStatusMask.hh new file mode 100644 index 000000000..e563d9c0d --- /dev/null +++ b/include/gz/physics/DataStatusMask.hh @@ -0,0 +1,99 @@ +/* + * Copyright (C) 2017 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 GZ_PHYSICS_DATASTATUSMASK_HH_ +#define GZ_PHYSICS_DATASTATUSMASK_HH_ + +#include "gz/physics/CompositeData.hh" + +namespace ignition +{ + namespace physics + { + /// \brief This struct encodes criteria for CompositeData::DataStatus + /// so that Read and Write operations can be done for specific types + /// of data (ie. required and unqueried or only existing data). + /// It is used by OperateOnSpecifiedData at run-time. + /// \sa CompositeData::DataStatus + /// \sa OperateOnSpecifiedData + struct IGNITION_PHYSICS_VISIBLE DataStatusMask + { + /// \brief Specify a condition as MUST be true, MUST_NOT be true, + /// or EITHER. + enum Condition + { + /// \brief MUST be true + MUST = 0, + /// \brief MUST_NOT be true (ie. must be false) + MUST_NOT, + /// \brief EITHER true or false + EITHER + }; + + /// \brief MUST means the type must exist in the CompositeData in order + /// to be operated on. MUST_NOT means it must not exist (this can be + /// used to decide whether to create the object). + public: Condition exist; + + /// \brief MUST means the type must be queried. MUST_NOT means it must + /// not be queried. + public: Condition queried; + + /// \brief MUST means the type must be required by the CompositeData. + /// MUST_NOT means it must not be required. + /// + /// Superficially, this might appear to overlap with the role of + /// FindRequired and FindExpected, but those templates are filtering + /// data types at compile-time whereas this will filter them during + /// runtime. Requirements can be promoted during run-time, so they won't + /// necessarily match the static compile-time requirements, and a user + /// might want to dynamically choose which requirement level to operate + /// on during run-time. + public: Condition required; + + /// \brief Default constructor. Everything is set to EITHER so that + /// nothing is masked. + /// \param[in] _e Exist condition. + /// \param[in] _q Queried condition. + /// \param[in] _r Required condition. + public: DataStatusMask(const Condition _e = EITHER, + const Condition _q = EITHER, + const Condition _r = EITHER); + + /// \brief Test whether a single condition is satisfied: + /// - true if _condition == EITHER + /// - true if _condition == MUST and boolean _value == true + /// - true if _condition == MUST_NOT and boolean _value == false + /// - false otherwise + /// \param[in] _condition Condition requirement for variable check. + /// \param[in] _value Boolean value to check. + /// \return True if condition is satisfied, otherwise false. + public: static bool ConditionSatisfied( + const DataStatusMask::Condition _condition, + const bool _value); + + /// \brief Test whether all conditions of this DataStatusMask are + /// satisfied by the DataStatus. + /// \param[in] _status DataStatus to evaluate. + /// \return True if _status satisfies these conditions. + public: bool Satisfied(const CompositeData::DataStatus &_status) const; + }; + } +} + + +#endif diff --git a/include/gz/physics/DeclareJointType.hh b/include/gz/physics/DeclareJointType.hh new file mode 100644 index 000000000..84c2aa1f0 --- /dev/null +++ b/include/gz/physics/DeclareJointType.hh @@ -0,0 +1,55 @@ +/* + * Copyright (C) 2018 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 GZ_PHYSICS_DECLAREJOINTTYPE_HH_ +#define GZ_PHYSICS_DECLAREJOINTTYPE_HH_ + +#include +#include + +/// \brief Given a joint type named CustomJointType, this macro creates the +/// following classes: +/// +/// class CustomJointTypeCast +/// - A Feature class that allows plain Joint objects to downcast themselves to +/// CustomJointType, as long as it is truly an instance of a CustomJointType. +/// This class provides the function Joint::CastToCustomJointType() when +/// added to a Joint's FeatureList. +/// +/// template class CustomJointType +/// - An Entity class that includes the API of both the plain Joint class and +/// the CustomJointType, as defined by FeaturePolicy P and FeatureList F. +/// +/// template class CustomJointType3d +/// template class CustomJointType2d +/// template class CustomJointType3f +/// template class CustomJointType2f +/// - Similar to CustomJointType, except P is replaced with the predefined +/// Feature Policies. +/// +/// Physics engine plugin developers must implement the virtual function +/// +/// \code +/// ignition::physics::Identity CastToCustomJointType(const Identity &_id) const +/// \endcode +/// +/// if their physics engine plugin wants to be able to provide CustomJointType +/// features. +#define IGN_PHYSICS_DECLARE_JOINT_TYPE(CustomJointType) \ + DETAIL_IGN_PHYSICS_DECLARE_DERIVED_TYPE(Joint, CustomJointType) + +#endif diff --git a/include/gz/physics/DeclareShapeType.hh b/include/gz/physics/DeclareShapeType.hh new file mode 100644 index 000000000..a02356c36 --- /dev/null +++ b/include/gz/physics/DeclareShapeType.hh @@ -0,0 +1,55 @@ +/* + * Copyright (C) 2018 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 GZ_PHYSICS_DECLARESHAPETYPE_HH_ +#define GZ_PHYSICS_DECLARESHAPETYPE_HH_ + +#include +#include + +/// \brief Given a shape type named CustomShapeType, this macro creates the +/// following classes: +/// +/// class CustomShapeTypeCast +/// - A Feature class that allows plain Shape objects to downcast themselves to +/// CustomShapeType, as long as it is truly an instance of a CustomShapeType. +/// This class provides the function Shape::CastToCustomShapeType() when +/// added to a Shape's FeatureList. +/// +/// template class CustomShapeType +/// - An Entity class that includes the API of both the plain Shape class and +/// the CustomShapeType, as defined by FeaturePolicy P and FeatureList F. +/// +/// template class CustomShapeType3d +/// template class CustomShapeType2d +/// template class CustomShapeType3f +/// template class CustomShapeType2f +/// - Similar to CustomShapeType, except P is replaced with the predefined +/// Feature Policies. +/// +/// Physics engine plugin developers must implement the virtual function +/// +/// \code +/// ignition::physics::Identity CastToCustomShapeType(const Identity &_id) const +/// \endcode +/// +/// if their physics engine plugin wants to be able to provide CustomShapeType +/// features. +#define IGN_PHYSICS_DECLARE_SHAPE_TYPE(CustomShapeType) \ + DETAIL_IGN_PHYSICS_DECLARE_DERIVED_TYPE(Shape, CustomShapeType) + +#endif diff --git a/include/gz/physics/EllipsoidShape.hh b/include/gz/physics/EllipsoidShape.hh new file mode 100644 index 000000000..240ea3234 --- /dev/null +++ b/include/gz/physics/EllipsoidShape.hh @@ -0,0 +1,137 @@ +/* + * Copyright (C) 2021 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 GZ_PHYSICS_ELLIPSOIDSHAPE_HH_ +#define GZ_PHYSICS_ELLIPSOIDSHAPE_HH_ + +#include + +#include +#include + +namespace ignition +{ + namespace physics + { + IGN_PHYSICS_DECLARE_SHAPE_TYPE(EllipsoidShape) + + class IGNITION_PHYSICS_VISIBLE GetEllipsoidShapeProperties + : public virtual FeatureWithRequirements + { + public: template + class EllipsoidShape : public virtual Entity + { + public: using Dimensions = + typename FromPolicy::template Use; + + /// \brief Get the radius of this EllipsoidShape + /// \return the radius of this EllipsoidShape + public: Dimensions GetRadii() const; + }; + + public: template + class Implementation : public virtual Feature::Implementation + { + public: using Dimensions = + typename FromPolicy::template Use; + + public: virtual Dimensions GetEllipsoidShapeRadii( + const Identity &_ellipsoidID) const = 0; + }; + }; + + ///////////////////////////////////////////////// + /// \brief This feature sets the EllipsoidShape properties such as + /// the ellipsoid radii. + class IGNITION_PHYSICS_VISIBLE SetEllipsoidShapeProperties + : public virtual FeatureWithRequirements + { + public: template + class EllipsoidShape : public virtual Entity + { + public: using Dimensions = + typename FromPolicy::template Use; + + /// \brief Set the radius of this EllipsoidShape + /// \param[in] _radii + /// The desired radius of this EllipsoidShape + public: void SetRadii(Dimensions _radii); + }; + + public: template + class Implementation : public virtual Feature::Implementation + { + public: using Dimensions = + typename FromPolicy::template Use; + + public: virtual void SetEllipsoidShapeRadii( + const Identity &_ellipsoidID, Dimensions _radii) = 0; + }; + }; + + ///////////////////////////////////////////////// + /// \brief This feature constructs a new ellipsoid shape and attaches the + /// desired pose in the link frame. The pose could be defined to be the + /// ellipsoid center point in actual implementation. + class IGNITION_PHYSICS_VISIBLE AttachEllipsoidShapeFeature + : public virtual FeatureWithRequirements + { + public: template + class Link : public virtual Feature::Link + { + public: using Dimensions = + typename FromPolicy::template Use; + + public: using PoseType = + typename FromPolicy::template Use; + + public: using ShapePtrType = EllipsoidShapePtr; + + /// \brief Rigidly attach a EllipsoidShape to this link. + /// \param[in] _radii + /// The radius of the ellipsoid. + /// \param[in] _pose + /// The desired pose of the EllipsoidShape relative to the Link frame. + /// \returns a ShapePtrType to the newly constructed EllipsoidShape + public: ShapePtrType AttachEllipsoidShape( + const std::string &_name = "ellipsoid", + Dimensions _radii = Dimensions::Constant(1.0), + const PoseType &_pose = PoseType::Identity()); + }; + + public: template + class Implementation : public virtual Feature::Implementation + { + public: using Dimensions = + typename FromPolicy::template Use; + + public: using PoseType = + typename FromPolicy::template Use; + + public: virtual Identity AttachEllipsoidShape( + const Identity &_linkID, + const std::string &_name, + Dimensions _radii, + const PoseType &_pose) = 0; + }; + }; + } +} + +#include + +#endif diff --git a/include/gz/physics/Entity.hh b/include/gz/physics/Entity.hh new file mode 100644 index 000000000..5334fea0d --- /dev/null +++ b/include/gz/physics/Entity.hh @@ -0,0 +1,306 @@ +/* + * Copyright (C) 2017 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 GZ_PHYSICS_ENTITY_HH_ +#define GZ_PHYSICS_ENTITY_HH_ + +#include +#include +#include + +#include +#include + +namespace ignition +{ + namespace physics + { + // Forward declaration + namespace detail { template struct DeterminePlugin; } + template struct RequestFeatures; + + /// \brief This constant-value should be used to indicate that an Entity ID + /// is invalid (i.e. does not refer to a real entity). + const std::size_t INVALID_ENTITY_ID = + std::numeric_limits::max(); + + template + class EntityPtr + { + public: EntityPtr() = default; + public: EntityPtr(EntityPtr&&) = default; + public: EntityPtr &operator=(EntityPtr&&) = default; + public: ~EntityPtr() = default; + // Special handling for copy construction and copy assignment + public: EntityPtr(const EntityPtr&); + public: EntityPtr &operator=(const EntityPtr&); + + /// \brief Create an EntityPtr that points to an invalid Entity + public: EntityPtr(std::nullptr_t); + + /// \brief Create an EntityPtr that points to an invalid Entity + public: EntityPtr(std::nullopt_t); + + /// \brief Assign this to point to an invalid Entity. + public: EntityPtr &operator=(std::nullptr_t); + + /// \brief Assign this to point to an invalid Entity. + public: EntityPtr &operator=(std::nullopt_t); + + /// \brief Create an EntityPtr that points to the Entity tied to _identity + /// \param[in] _pimpl + /// Pointer to the implementation for this entity + /// \param[in] _identity + /// The identity of the Entity that this should point to + public: template + EntityPtr(const std::shared_ptr &_pimpl, + const Identity &_identity); + + /// \brief Create a new reference to another entity + /// \param[in] _other + /// Another entity reference with a compatible type and compatible set + /// of features + public: template + EntityPtr(const EntityPtr &_other); + + /// \brief Assign this to point at another compatible Entity + /// \param[in] _other + /// Another entity reference with a compatible type and compatible set + /// of features. + public: template + EntityPtr &operator=(const EntityPtr &_other); + + /// \brief Drill operator. Access members of the Entity being pointed to. + /// This does NOT check whether the Entity is valid before trying to use + /// it. If the validity of the Entity is in doubt, check Valid() or simply + /// operator bool() before attempting to use this operator. + /// \return The ability to call a member function on the underlying Entity + public: EntityT * operator->() const; + + /// \brief Dereference operator. Access a reference to the Entity being + /// pointed to. This does NOT check whether the Entity is valid before + /// trying to provide it. If the validity of the Entity is in doubt, check + /// IsEmpty() or simply operator bool() before attempting to use this + /// operator. + /// \return A reference to the underlying entity + public: EntityT & operator*() const; + + /// \brief Check whether this is pointing at a valid Entity. + /// \return True if this is pointing to a valid Entity, otherwise false. + public: bool Valid() const; + + /// \brief Implicitly cast this EntityPtr to a boolean. + /// \return True if this is pointing to a valid Entity, otherwise false. + public: operator bool() const; + + /// \brief Produces a hash for the Entity that this EntityPtr is referring + /// to. This function allows EntityPtr instances to be used as values in a + /// std::unordered_set or keys in a std::unordered_map. Using this + /// function directly should not normally be necessary. + /// \return A hash of the underlying Entity. + public: std::size_t Hash() const; + + /// \brief Comparison operator + /// \param[in] _other + /// Entity to compare to. + /// \remark Entities are uniquely identified by their underlying ID which + /// is assigned by the physics engine. This may produce unintuitive + /// results depending on how the physics engine organizes its Entities. + /// \returns True if the ID of this Entity is equal to the ID of _other, + /// otherwise returns false. + public: template + bool operator ==(const EntityPtr &_other) const; + + /// \brief Comparison operator + /// \param[in] _other + /// Entity to compare to. + /// \remark Entities are uniquely identified by their underlying ID which + /// is assigned by the physics engine. This may produce unintuitive + /// results depending on how the physics engine organizes its Entities. + /// \returns True if the ID of this Entity is less than the ID of _other, + /// otherwise returns false. + public: template + bool operator <(const EntityPtr &_other) const; + + /// \brief Comparison operator + /// \param[in] _other + /// Entity to compare to. + /// \remark Entities are uniquely identified by their underlying ID which + /// is assigned by the physics engine. This may produce unintuitive + /// results depending on how the physics engine organizes its Entities. + /// \returns True if the ID of this Entity is greater than the ID of + /// _other, otherwise returns false. + public: template + bool operator >(const EntityPtr &_other) const; + + /// \brief Comparison operator + /// \param[in] _other + /// Entity to compare to. + /// \remark Entities are uniquely identified by their underlying ID which + /// is assigned by the physics engine. This may produce unintuitive + /// results depending on how the physics engine organizes its Entities. + /// \returns True if the ID of this Entity is not equal to the ID of + /// _other, otherwise returns false. + public: template + bool operator !=(const EntityPtr &_other) const; + + /// \brief Comparison operator + /// \param[in] _other + /// Entity to compare to. + /// \remark Entities are uniquely identified by their underlying ID which + /// is assigned by the physics engine. This may produce unintuitive + /// results depending on how the physics engine organizes its Entities. + /// \returns True if the ID of this Entity is less than or equal to the ID + /// of _other, otherwise returns false. + public: template + bool operator <=(const EntityPtr &_other) const; + + /// \brief Comparison operator + /// \param[in] _other + /// Entity to compare to. + /// \remark Entities are uniquely identified by their underlying ID which + /// is assigned by the physics engine. This may produce unintuitive + /// results depending on how the physics engine organizes its Entities. + /// \returns True if the ID of this Entity is greater than or equal to the + /// ID of _other, otherwise returns false. + public: template + bool operator >=(const EntityPtr &_other) const; + + /// \brief If we are pointing to a valid entity, it will be stored here. + /// Otherwise, this is a nullopt. + /// + /// We make this mutable so that we get logical const-correctness. We are + /// not concerned with physical const-correctness here. + /// + /// When passing the type into the std::optional, we remove the + /// const-qualifier because otherwise a ConstEntityPtr object cannot be + /// modified to point to a different entity, as the assignment operator + /// of std::optional gets deleted when it is given a const-qualified type. + /// The dereference operations * and -> will still use the original + /// const-qualifications of EntityT, so logical constness is still + /// preserved, because the user cannot access an const-unqualified + /// reference to EntityT. + private: mutable std::optional> entity; + + + // Declare these friendships so we can cast between different Entity types + template friend class EntityPtr; + template friend struct RequestFeatures; + }; + + /// \brief This is the base class of all "proxy objects". The "proxy + /// objects" are essentially interfaces into the actual objects which exist + /// inside of the various physics engine implementations. The proxy objects + /// contain the minimal amount of data (e.g. a unique identifier, + /// a reference-counter for the implementation object, and a reference to + /// the implementation interface that it needs) necessary to interface with + /// the object inside of the implementation that it refers to. + /// + /// Examples of entities are the Link class, Joint class, and Model + /// class. + template + class Entity + { + public: using Policy = PolicyT; + public: using Features = FeaturesT; + public: using Pimpl = + typename detail::DeterminePlugin::type; + + /// \brief Get the Identity object of this Entity. + public: const Identity &FullIdentity() const; + + /// \brief Get the unique ID value of this Entity. + public: std::size_t EntityID() const; + + /// \brief Get a reference-counting std::shared_ptr to the object inside + /// the implementation that this object provides an abstraction for. + public: const std::shared_ptr &EntityReference() const; + + /// \brief Constructor for the Entity. + /// + /// \param[in] _pimpl + /// Pointer to the implementation plugin for this entity + /// \param[in] _identity + /// The identity of this entity + /// + // Notes for developers: + // - We provide a default constructor for this class so that the feature + // entity classes (which get virtually inherited) do not each need to + // call on the constructor of Entity. That would make it difficult + // to implement and maintain all the constructors of the different object + // feature classes. + // - Since all the features are virtually inherited, only the "final" + // inheriting class constructor needs to actually call this constructor. + // - The default argument for the identity will have an INVALID_ENTITY_ID + // value (which is the result of default-constructing Identity). If the + // Identity of an Entity is invalid, that implies that there is a bug in + // the construction of that Entity. + protected: Entity( + const std::shared_ptr &_pimpl = nullptr, + const Identity &_identity = Identity()); + + /// \brief Constructor that allows the pimpl to be moved instead of copied + /// + /// \param[in] _pimpl + /// Pointer to the implementation plugin for this entity + /// \param[in] _identity + /// The identity of this entity + /// + /// \sa Entity(const std::shared_ptr&, const Identity&) + protected: Entity( + std::shared_ptr &&_pimpl, + const Identity &_identity); + + /// \brief Get a pointer to the implementation of FeatureT. + /// + /// This is a convenience function so that entities don't have to query + /// for interfaces on their pimpl object. + /// + /// \return A pointer to the implementation of FeatureT, or a nullptr if + /// FeatureT is not available. + protected: template + typename FeatureT::template Implementation *Interface(); + + /// \brief Same as Interface(), but const-qualified so that const entities + /// can request const-qualified interfaces from the implementation. + /// \return A pointer to a const-qualified implementation of F, or a + /// nullptr if F is not available. + protected: template + const typename FeatureT::template Implementation *Interface() + const; + + /// \brief This is a pointer to the physics engine implementation, and it + /// can be used by the object features to find the interfaces that they + /// need in order to function. + protected: std::shared_ptr pimpl; + + /// \brief This field contains information to identify the entity. + protected: Identity identity; + + /// \brief Virtual destructor + public: virtual ~Entity() = default; + + // Allow EntityPtr to cast between EntityTypes + template friend class EntityPtr; + template friend struct RequestFeatures; + }; + } +} + +#include + +#endif diff --git a/include/gz/physics/Feature.hh b/include/gz/physics/Feature.hh new file mode 100644 index 000000000..cb98a69e8 --- /dev/null +++ b/include/gz/physics/Feature.hh @@ -0,0 +1,132 @@ +/* + * Copyright (C) 2017 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 GZ_PHYSICS_FEATURE_HH_ +#define GZ_PHYSICS_FEATURE_HH_ + +#include +#include +#include + +#include +#include + +namespace ignition +{ + namespace physics + { + ///////////////////////////////////////////////// + /// \brief This class defines the concept of a Feature. It should be + /// inherited by classes that define some plugin feature. + class Feature + { + /// \brief Placeholder class for the Engine API. Every Engine feature + /// MUST inherit this class. + public: template + class Engine : public virtual Entity + { + /// \brief Virtual destructor + public: virtual ~Engine() = default; + }; + + /// \brief Placeholder class in case a Feature does not define its own + /// World API + public: template + class World : public virtual Entity + { + /// \brief Virtual destructor + public: virtual ~World() = default; + }; + + /// \brief Placeholder class in case a Feature does not define its own + /// Model API + public: template + class Model : public virtual Entity + { + /// \brief Virtual destructor + public: virtual ~Model() = default; + }; + + /// \brief Placeholder class in case a Feature does not define its own + /// Link API + public: template + class Link : public virtual Entity + { + /// \brief Virtual destructor + public: virtual ~Link() = default; + }; + + /// \brief Placeholder class in case a Feature does not define its own + /// Joint API + public: template + class Joint : public virtual Entity + { + /// \brief Virtual destructor + public: virtual ~Joint() = default; + }; + + /// \brief Placeholder class in case a Feature does not define its own + /// Shape API + public: template + class Shape : public virtual Entity + { + /// \brief Virtual destructor + public: virtual ~Shape() = default; + }; + + public: template + class Implementation : public detail::Implementation + { + /// \brief Tell the physics plugin to initiate a physics engine. + /// + /// Some physics plugins might be able to provide multiple simultaneous + /// physics engines, in which case engineID might vary. The meaning of + /// engineID is implementation-defined, but every physics plugins must + /// support at least engineID==0 to be well-formed. If a physics plugin + /// only supports having one engine at a time, then it may only support + /// engineID==0. + /// + /// \return The Identity of the physics engine. For engineID==0 this + /// should also return an Identity whose id is 0. In the event of an + /// error (or an invalid engineID), the Identity id should be + /// INVALID_ENTITY_ID. + public: virtual Identity InitiateEngine(std::size_t engineID = 0) = 0; + + /// \brief Virtual destructor + public: virtual ~Implementation() = default; + }; + + /// \brief By default, a blank feature will not conflict with any other + /// features. If your feature does conflict with some other set of + /// features, then you should inherit the FeatureWithConflicts<...> class, + /// and provide it a list of those conflicting features. + template + static constexpr bool ConflictsWith() + { + return false; + } + + /// \brief By default, a blank feature will not require any other + /// features. If your feature does require some other set of features, + /// then you should inherit the FeatureWithRequirements class, and provide + /// it with a list of the Features that you require. + using RequiredFeatures = void; + }; + } +} + +#endif diff --git a/include/gz/physics/FeatureList.hh b/include/gz/physics/FeatureList.hh new file mode 100644 index 000000000..67aa7dcc4 --- /dev/null +++ b/include/gz/physics/FeatureList.hh @@ -0,0 +1,114 @@ +/* + * Copyright (C) 2018 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 GZ_PHYSICS_FEATURELIST_HH_ +#define GZ_PHYSICS_FEATURELIST_HH_ + +#include + +#include + +namespace ignition +{ + namespace physics + { + namespace detail + { + // Forward declarations + template struct CombineLists; + template struct SelfConflict; + template struct IterateTuple; + } + + ///////////////////////////////////////////////// + /// \brief Use a FeatureList to aggregate a list of Features. + /// + /// FeatureLists can be constructed in hierarchies, e.g. a FeatureList can + /// be passed into another FeatureList, and the set of all features in the + /// new list will be the sum. + /// + /// \code + /// // FeatureA, FeatureB, AdvancedA, and AdvancedB are all feature classes. + /// + /// using BasicList = FeatureList; + /// using AdvancedList = FeatureList; + /// \endcode + template + struct FeatureList : detail::IterateTuple> + { + /// Features is a std::tuple containing all the feature classes that are + /// bundled in this list. This list is fully seralialized; any hierarchy + /// that was used to construct this FeatureList will be collapsed in this + /// member. + public: using Features = + typename detail::CombineLists::Result; + + public: using FeatureTuple = std::tuple; + + /// \brief A static constexpr function which indicates whether a given + /// Feature, F, is contained in this list. + /// \tparam F + /// The feature class to check for in this FeatureList + /// \return true if F is in this FeatureList; false otherwise. + public: template + static constexpr bool HasFeature(); + + /// \brief A static constexpr function which indicates whether any + /// features in SomeFeatureList conflict with any features in + /// SomeFeatureList. + /// + /// \tparam SomeFeatureList + /// The list to compare against for conflicts. + /// \tparam AssertNoConflict + /// Setting this to true will result in a static_assert if a conflict is + /// found. That way, if a conflict exists, you will get a compilation + /// error, and the compilation error will tell you which feature is + /// conflicting. + /// \return true if any features in SomeFeatureList conflict with this + /// list or vice versa. + public: template + static constexpr bool ConflictsWith(); + + /// \brief All the features required by this FeatureList will be included + /// in CombineLists. + public: using RequiredFeatures = void; + + // Check that this FeatureList does not contain any self-conflicts. + static_assert(!detail::SelfConflict::value, + "FeatureList ERROR: YOUR LIST CONTAINS CONFLICTING FEATURES!"); + }; + + ///////////////////////////////////////////////// + /// \brief If your feature is known to conflict with any other feature, then + /// you should have your feature class inherit FeatureWithConflicts<...>, + /// and pass it a list of the features that it conflicts with. + template + struct FeatureWithConflicts; + + ///////////////////////////////////////////////// + /// \brief If your feature is known to require any other features, then you + /// should have your feature class inherit FeatureWithRequirements<...>, + /// and pass it a list of the features that it requires. + template + struct FeatureWithRequirements; + } +} + +#include + +#endif diff --git a/include/gz/physics/FeaturePolicy.hh b/include/gz/physics/FeaturePolicy.hh new file mode 100644 index 000000000..3c30b1a51 --- /dev/null +++ b/include/gz/physics/FeaturePolicy.hh @@ -0,0 +1,75 @@ +/* + * Copyright (C) 2018 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 GZ_PHYSICS_FEATUREPOLICY_HH_ +#define GZ_PHYSICS_FEATUREPOLICY_HH_ + +#include + +namespace ignition +{ + namespace physics + { + ///////////////////////////////////////////////// + /// \brief FeaturePolicy is a "policy class" used to provide metadata to + /// features about what kind of simulation engine they are going to be used + /// in. + /// + /// Currently, the information provided by the native FeaturePolicy includes + /// + /// - Scalar: double or float. Determines the numerical precision used + /// by the simulation. + /// + /// - Dim: 2 or 3. Determines whether the simulation is a 2D simulation + /// or a 3D simulation. + /// + /// Custom features may require additional metadata, which can be encoded + /// into a custom FeaturePolicy. However, keep in mind that most features + /// require at least the Scalar and Dim fields, so be sure to provide those + /// in your feature policy or else you are likely to encounter compilation + /// errors. + /// + /// Feature policies are typically composable, but you must be careful about + /// resolving ambiguously defined fields. If two parent policies are each + /// defining a field with the same name, then the child policy must + /// explicitly define that field itself. Preferably, the child would define + /// the field based on one of its parent's definitions, e.g.: + /// + /// \code + /// struct Child : public Parent1, public Parent2 + /// { + /// using AmbiguousField = typename Parent1::AmbiguousField; + /// }; + /// \endcode + /// + /// This design pattern is known as "Policy-based design". For more + /// information, see: https://en.wikipedia.org/wiki/Policy-based_design + template + struct FeaturePolicy + { + public: using Scalar = _Scalar; + public: enum { Dim = _Dim }; + }; + + using FeaturePolicy3d = FeaturePolicy; + using FeaturePolicy2d = FeaturePolicy; + using FeaturePolicy3f = FeaturePolicy; + using FeaturePolicy2f = FeaturePolicy; + } +} + +#endif diff --git a/include/gz/physics/FindFeatures.hh b/include/gz/physics/FindFeatures.hh new file mode 100644 index 000000000..61cdd9336 --- /dev/null +++ b/include/gz/physics/FindFeatures.hh @@ -0,0 +1,71 @@ +/* + * Copyright (C) 2018 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 GZ_PHYSICS_FINDFEATURES_HH_ +#define GZ_PHYSICS_FINDFEATURES_HH_ + +#include +#include + +#include + +namespace ignition +{ + namespace physics + { + template + struct FindFeatures + { + // Tuple of features that are being requested + using Features = FeatureListT; + + /// \brief Find a set of plugins that satisfy the requested list of + /// features. + /// + /// \tparam LoaderT + /// The type of plugin loader to use. Typically this will be an + /// ignition::plugin::Loader object, but the function can be used on any + /// type that has a function `C PluginsImplementing()` + /// and `C AllPlugins()` where `T` is an interface class + /// type and `C` is a container of `std::strings`. + /// + /// \param[in] _loader + /// The loader object to search for plugins. + /// + /// \return The names of plugins in _loader that provide the requested set + /// of features. + template + static std::set From(const LoaderT &_loader); + }; + + template + using FindFeatures3d = FindFeatures; + + template + using FindFeatures2d = FindFeatures; + + template + using FindFeatures3f = FindFeatures; + + template + using FindFeatures2f = FindFeatures; + } +} + +#include + +#endif diff --git a/include/gz/physics/FixedJoint.hh b/include/gz/physics/FixedJoint.hh new file mode 100644 index 000000000..8e90a1998 --- /dev/null +++ b/include/gz/physics/FixedJoint.hh @@ -0,0 +1,69 @@ +/* + * Copyright (C) 2018 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 GZ_PHYSICS_FIXEDJOINT_HH_ +#define GZ_PHYSICS_FIXEDJOINT_HH_ + +#include + +#include + +namespace ignition +{ + namespace physics + { + IGN_PHYSICS_DECLARE_JOINT_TYPE(FixedJoint) + + class IGNITION_PHYSICS_VISIBLE AttachFixedJointFeature + : public virtual FeatureWithRequirements + { + public: template + class Link : public virtual Feature::Link + { + public: using JointPtrType = FixedJointPtr; + + /// \brief Attach this link to another link using a FixedJoint. + /// \param[in] _parent + /// The parent link for the joint. Pass in a nullptr to attach the + /// link to the world. + /// \param[in] _name + /// The name for this joint. + /// \return A reference to the newly constructed FixedJoint. You can use + /// the SetJointTransformFromParentFeature and + /// SetJointTransformToChildFeature on this JointPtr to set the relative + /// transform between the parent and child if your physics engine offers + /// those features. + public: JointPtrType AttachFixedJoint( + const BaseLinkPtr &_parent, + const std::string &_name = "fixed"); + }; + + public: template + class Implementation : public virtual Feature::Implementation + { + public: virtual Identity AttachFixedJoint( + const Identity &_childID, + const BaseLinkPtr &_parent, + const std::string &_name) = 0; + }; + }; + } +} + +#include + +#endif diff --git a/include/gz/physics/ForwardStep.hh b/include/gz/physics/ForwardStep.hh new file mode 100644 index 000000000..620a13e9a --- /dev/null +++ b/include/gz/physics/ForwardStep.hh @@ -0,0 +1,199 @@ +/* + * Copyright (C) 2017 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 GZ_PHYSICS_FORWARDSTEP_HH_ +#define GZ_PHYSICS_FORWARDSTEP_HH_ + +#include +#include + +#include + +#include +#include + +namespace ignition +{ + namespace physics + { + // ---------------- Output Data Structures ----------------- + // In the long-term, these data structures should be defined in some kind of + // meta-file, and our build system should generate these definitions during + // compilation. These are being defined here in this header until we know + // more about how the ECS will work. + + struct WorldPose + { + ignition::math::Pose3d pose; + + std::size_t body; + }; + + struct WorldPoses + { + std::vector entries; + std::string annotation; + }; + + /// \brief ChangedWorldPoses has the same definition as WorldPoses. + /// This type provides a way to keep track of which poses have changed in a + /// simulation step. + struct ChangedWorldPoses + { + std::vector entries; + std::string annotation; + }; + + struct Point + { + ignition::math::Vector3d point; + + std::size_t relativeTo; + std::size_t inCoordinatesOf; + }; + + struct FreeVector + { + ignition::math::Vector3d vec; + + std::size_t inCoordinatesOf; + }; + + struct JointPositions + { + std::vector dofs; + std::vector positions; + std::string annotation; + }; + + struct Contacts + { + std::vector entries; + std::string annotation; + }; + + // ---------------- Input Data Structures ----------------- + // Same note as for Output Data Structures. Eventually, these should be + // defined in some kind of meta files. + + struct TimeStep + { + double dt; + }; + + struct ForceTorque + { + std::size_t body; + Point location; + + FreeVector force; + FreeVector torque; + + std::string annotation; + }; + + struct GeneralizedParameters + { + std::vector dofs; + std::vector forces; + std::string annotation; + }; + + struct PIDValues + { + double P; + double I; + double D; + }; + + struct ApplyExternalForceTorques + { + std::vector entries; + std::string annotation; + }; + + struct ApplyGeneralizedForces + { + std::vector forces; + std::string annotation; + }; + + struct VelocityControlCommands + { + std::vector commands; + std::string annotation; + }; + + struct ServoControlCommands + { + std::vector commands; + std::vector gains; + + std::string annotation; + }; + + ///////////////////////////////////////////////// + /// \brief ForwardStep is a feature that allows a simulation of a world to + /// take one step forward in time. + class ForwardStep : public virtual Feature + { + public: using Input = ExpectData< + ApplyExternalForceTorques, + ApplyGeneralizedForces, + VelocityControlCommands, + ServoControlCommands>; + + public: using Output = SpecifyData< + RequireData, + ExpectData >; + + public: using State = CompositeData; + + public: template + class World : public virtual Feature::World + { + public: void Step(Output &_h, State &_x, const Input &_u) + { + this->template Interface()-> + WorldForwardStep(this->identity, _h, _x, _u); + } + }; + + public: template + class Implementation : public virtual Feature::Implementation + { + public: virtual void WorldForwardStep( + const Identity &_worldID, + Output &_h, + State &_x, + const Input &_u) = 0; + }; + }; + + // ---------------- SetState Interface ----------------- + // class SetState + // { + // public: using State = CompositeData; + + // public: virtual void SetStateTo(const State &x) = 0; + + // public: virtual ~SetState() = default; + // }; + } +} + +#endif diff --git a/include/gz/physics/FrameData.hh b/include/gz/physics/FrameData.hh new file mode 100644 index 000000000..9c1744208 --- /dev/null +++ b/include/gz/physics/FrameData.hh @@ -0,0 +1,91 @@ +/* + * Copyright (C) 2017 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 GZ_PHYSICS_FRAMEDATA_HH_ +#define GZ_PHYSICS_FRAMEDATA_HH_ + +#include + +namespace ignition +{ + namespace physics + { + + /// \brief The FrameData struct fully describes the kinematic state of a + /// Frame with "Dim" dimensions and "Scalar" precision. Dim is allowed to be + /// 2 or 3, and Scalar is allowed to be double or float. We provide the + /// following fully qualified types: + /// + /// FrameData2d -- 2 dimensional frame with double precision + /// FrameData2f -- 2 dimensional frame with float precision + /// FrameData3d -- 3 dimensional frame with double precision + /// FrameData3f -- 3 dimensional frame with float precision + /// + /// The frame of reference for this data is dependent on the context in + /// which it is used. For FrameData which explicitly expresses its frame of + /// reference, see RelativeFrameData3. + template + struct FrameData + { + using Pose = ignition::physics::Pose; + using LinearVector = ignition::physics::LinearVector; + using AngularVector = ignition::physics::AngularVector; + + /// \brief Constructor. This will initialize the transform with identity + /// and all velocity and acceleration vectors to zero. + public: FrameData(); + + /// \brief The current SE3 transformation of the frame. + public: Pose pose; + + /// \brief The current linear velocity of the frame. + public: LinearVector linearVelocity; + + /// \brief The current angular velocity of the frame. + public: AngularVector angularVelocity; + + /// \brief The current linear acceleration of the frame. + public: LinearVector linearAcceleration; + + /// \brief The current angular acceleration of the frame. + public: AngularVector angularAcceleration; + + /// \brief Set the transform to identity and all velocity and acceleration + /// vectors to zero. + public: void SetToZero(); + }; + IGN_PHYSICS_MAKE_ALL_TYPE_COMBOS(FrameData) + + template + std::ostream& operator <<(std::ostream& stream, + const FrameData &_frame) + { + stream + << "Pose:\n" << _frame.pose.matrix() + << "\nLinear Velocity: " << _frame.linearVelocity.transpose() + << "\nAngular Velocity: " << _frame.angularVelocity.transpose() + << "\nLinear Acceleration: " << _frame.linearAcceleration.transpose() + << "\nAngular Acceleration: " << _frame.angularAcceleration.transpose(); + + return stream; + } + } +} + +#include + +#endif diff --git a/include/gz/physics/FrameID.hh b/include/gz/physics/FrameID.hh new file mode 100644 index 000000000..e9a436dd5 --- /dev/null +++ b/include/gz/physics/FrameID.hh @@ -0,0 +1,118 @@ +/* + * Copyright (C) 2017 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 GZ_PHYSICS_FRAMEID_HH_ +#define GZ_PHYSICS_FRAMEID_HH_ + +#include + +#include +#include +#include + +namespace ignition +{ + namespace physics + { + // Forward declaration + class FrameSemantics; + + ///////////////////////////////////////////////// + /// \brief Container for specifying Frame IDs. We do not want to use a + /// generic integer type for this, because it may lead to bugs where a plain + /// integer is mistaken for a FrameID. This also allows the compiler to + /// always perform argument deduction successfully. + /// + /// Physics engines may embed a std::shared_ptr which can be used for + /// reference counting. That way, a FrameID will remain valid for as long as + /// the user is referencing it. The member function IsReferenceCounted() can + /// be used to check whether the physics engine is performing reference + /// counting. The behavior of a frame which has been removed from simulation + /// but not deleted is undefined. + class IGNITION_PHYSICS_VISIBLE FrameID final + { + // Comparison operators + public: bool operator ==(const FrameID &_other) const; + public: bool operator <(const FrameID &_other) const; + public: bool operator >(const FrameID &_other) const; + public: bool operator <=(const FrameID &_other) const; + public: bool operator >=(const FrameID &_other) const; + public: bool operator !=(const FrameID &_other) const; + + // Explicitly allow default copy constructors and assignment operators + public: FrameID(const FrameID&) = default; + public: FrameID(FrameID&&) = default; + public: FrameID &operator =(const FrameID&) = default; + public: FrameID &operator =(FrameID&&) = default; + + /// \brief Get a reference to the world Frame. + public: static const FrameID &World(); + + /// \brief The numerical value of this FrameID's ID number. + public: std::size_t ID() const; + + /// \brief Returns true if this is the world frame. + public: bool IsWorld() const; + + /// \brief This will return true if this Frame is being reference-counted, + /// and false otherwise. + /// + /// FrameIDs that are not reference-counted may become invalid before the + /// end of their lifetime. + /// + /// FrameIDs that are reference-counted will remain valid throughout their + /// lifetime, but if the frames that they correspond to are removed from + /// the simulation, then the behavior of those frames will depend on the + /// physics engine implementation. Physics engines are allowed to refuse + /// to remove any frames which are actively being referenced. + /// + /// Note that physics engines may choose to reference count some of the + /// FrameIDs and not others, so this property must be accounted for per + /// FrameID. For example, some engines may support reference-counting for + /// Link frames but not Joint frames. Engine features can be used to + /// ensure that your physics engine of choice will behave as desired. + public: bool IsReferenceCounted() const; + + /// \brief Private constructor. Object types that express frame semantics + /// will be able to produce a FrameID by passing their own identity into + /// this constructor. They can also implicitly cast themselves into a + /// FrameID object. + private: explicit FrameID(const Identity &_identity); + + /// \brief Special enum for constructing the singleton World FrameID + private: enum WorldConstructorArg { ConstructWorld }; + + /// \brief Private constructor for creating the singleton World FrameID + private: explicit FrameID(WorldConstructorArg); + + // Friendship declaration + friend class FrameSemantics; + + /// \brief Integer value of the entity that this FrameID is tied to + private: std::size_t id; + + /// \brief Reference counter for the entity that this FrameID is tied to. + /// Objects that don't support reference counting will leave this as a + /// nullptr. + IGN_UTILS_WARN_IGNORE__DLL_INTERFACE_MISSING + private: std::shared_ptr ref; + IGN_UTILS_WARN_RESUME__DLL_INTERFACE_MISSING + }; + } +} + +#endif diff --git a/include/gz/physics/FrameSemantics.hh b/include/gz/physics/FrameSemantics.hh new file mode 100644 index 000000000..eeff3a4da --- /dev/null +++ b/include/gz/physics/FrameSemantics.hh @@ -0,0 +1,214 @@ +/* + * Copyright (C) 2017 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 GZ_PHYSICS_FRAMESEMANTICS_HH_ +#define GZ_PHYSICS_FRAMESEMANTICS_HH_ + +#include + +#include +#include +#include +#include +#include + +namespace ignition +{ + namespace physics + { + ///////////////////////////////////////////////// + /// \brief FrameSemantics is an Interface that can be provided by + /// ignition-physics engines to provide users with easy ways to express + /// kinematic quantities in terms of frames and compute their values in + /// terms of arbitrary frames of reference. + class IGNITION_PHYSICS_VISIBLE FrameSemantics : public virtual Feature + { + // Forward declaration + public: template class Frame; + + /// \brief This class defines the engine interface that provides the + /// FrameSemantics feature. + public: template + class Engine : public virtual Feature::Engine + { + public: using FrameData = + ignition::physics::FrameData< + typename PolicyT::Scalar, PolicyT::Dim>; + + /// \brief Resolve can take a RelativeQuantity (RQ) and compute its + /// values in terms of other reference frames. The argument `relativeTo` + /// indicates a frame that the quantity should be compared against (e.g. + /// the velocity of Frame A relative to Frame B where both A and B may + /// be moving). The argument `inCoordinatesOf` indicates the coordinate + /// frame that the values should be expressed in (this is usually just a + /// change in rotation). + public: template + typename RQ::Quantity Resolve( + const RQ &_quantity, + const FrameID &_relativeTo, + const FrameID &_inCoordinatesOf) const; + + /// \brief This overload causes the World Frame to be used as the + /// default frame when relativeTo is not specified. It also causes the + /// frame specified for relativeTo to be used as the frame for + /// inCoordinatesOf. + /// + /// In other words: + /// + /// -- Get the value of v in terms of the World Frame + /// Resolve(v) + /// + /// -- Get the value of v relative to frame A, in coordinates of frame A + /// Resolve(v, A) + /// + /// -- Get the value of v relative to frame A, in coordinates of frame B + /// Resolve(v, A, B) + /// + public: template + typename RQ::Quantity Resolve( + const RQ &_quantity, + const FrameID &_relativeTo = FrameID::World()) const; + + /// \brief Create a new RelativeQuantity which expresses the input + /// quantity in terms of a new parent frame. Note that the returned + /// RelativeQuantity will behave as though it has a constant value + /// within its new parent frame. + public: template + RQ Reframe(const RQ &_quantity, + const FrameID &_withRespectTo = FrameID::World()) const; + + template friend class FrameSemantics::Frame; + }; + + /// \brief Base class for the API of a Frame. This will be inherited by + /// any objects that are able to express Frame Semantics. + public: template + class Frame : public virtual Entity + { + public: using FrameData = + ignition::physics::FrameData; + + /// \brief Get a FrameID for this object + public: FrameID GetFrameID() const; + + /// \brief Get the FrameData of this object with respect to the world. + public: FrameData FrameDataRelativeToWorld() const; + + /// \brief Get the FrameData of this object with respect to another + /// frame. The data will also be expressed in the coordinates of the + /// _relativeTo frame. + public: FrameData FrameDataRelativeTo( + const FrameID &_relativeTo) const; + + /// \brief Get the FrameData of this object relative to another frame, + /// expressed in the coordinates of a third frame. + public: FrameData FrameDataRelativeTo( + const FrameID &_relativeTo, + const FrameID &_inCoordinatesOf) const; + + /// \brief Implicit conversion to a FrameID is provided. This way, a + /// reference to the Object can be treated as a FrameID. + public: operator FrameID() const; + + /// \brief Virtual destructor + public: virtual ~Frame() = default; + }; + + /// \brief This class is inherited by physics plugin classes that want to + /// provide this feature. + template + class Implementation : public virtual Feature::Implementation + { + public: using FrameData = + ignition::physics::FrameData; + + /// \brief Get the current 3D transformation of the specified frame with + /// respect to the WorldFrame. + /// + /// Engine developers only need to provide an implementation for this + /// function in order to provide FrameSemantics. + public: virtual FrameData FrameDataRelativeToWorld( + const FrameID &_id) const = 0; + + /// \brief Physics engines can use this function to generate a FrameID + /// using an existing Identity. + /// + /// This function is part of a design that ensures that FrameID objects + /// can only be instantiated by "authoritative" sources, like a physics + /// engine. + /// + /// \param[in] _identity + /// The underlying identity of the frame object. + /// \return A FrameID object that corresponds to _identity. + protected: virtual FrameID GenerateFrameID( + const Identity &_identity) const; + }; + }; + + ///////////////////////////////////////////////// + /// \brief This feature will apply frame semantics to Link objects. + class IGNITION_PHYSICS_VISIBLE LinkFrameSemantics + : public virtual FrameSemantics + { + public: template + using Link = FrameSemantics::Frame; + }; + + ///////////////////////////////////////////////// + /// \brief This feature will apply frame semantics to Joint objects. + class IGNITION_PHYSICS_VISIBLE JointFrameSemantics + : public virtual FrameSemantics + { + public: template + using Joint = FrameSemantics::Frame; + }; + + ///////////////////////////////////////////////// + class IGNITION_PHYSICS_VISIBLE ShapeFrameSemantics + : public virtual FrameSemantics + { + public: template + using Shape = FrameSemantics::Frame; + }; + + ///////////////////////////////////////////////// + /// \brief This feature will apply frame semantics to Model objects. + class IGNITION_PHYSICS_VISIBLE ModelFrameSemantics + : public virtual FrameSemantics + { + public: template + using Model = FrameSemantics::Frame; + }; + + ///////////////////////////////////////////////// + /// \brief This feature will apply frame semantics to all objects. + class IGNITION_PHYSICS_VISIBLE CompleteFrameSemantics + : public virtual LinkFrameSemantics, + public virtual JointFrameSemantics, + public virtual ModelFrameSemantics + { + // This alias is needed in order to disambiguate which Engine class to use + // from the base classes. + public: template + using Engine = FrameSemantics::Engine; + }; + } +} + +#include + +#endif diff --git a/include/gz/physics/FreeGroup.hh b/include/gz/physics/FreeGroup.hh new file mode 100644 index 000000000..7035df228 --- /dev/null +++ b/include/gz/physics/FreeGroup.hh @@ -0,0 +1,216 @@ +/* + * 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 GZ_PHYSICS_FREEGROUP_HH_ +#define GZ_PHYSICS_FREEGROUP_HH_ + +#include +#include +#include + +namespace ignition +{ + namespace physics + { + DETAIL_IGN_PHYSICS_DEFINE_ENTITY(FreeGroup) + + ///////////////////////////////////////////////// + /// \brief This feature provides an interface between the Model and Link + /// classes and the FreeGroup class, which represents a group of links + /// that are not connected to the world with any kinematic constraints. + /// A FreeGroup can represent a single connected group of links that + /// forms a tree with the root of the tree connected to the world with + /// a FreeJoint, but it can also represent a group of other FreeGroups. + /// Each FreeGroup has 1 canonical link, whose frame is used for getting + /// and setting properties like pose and velocity. + /// If the FreeGroup is a single tree of connected links, the canonical + /// link should be the root of that tree. + /// If the FreeGroup contains multiple FreeGroups, the canonical link + /// should be selected from one of the component FreeGroups. + class IGNITION_PHYSICS_VISIBLE FindFreeGroupFeature : public virtual Feature + { + public: template + class Model : public virtual Feature::Model + { + using FreeGroupPtrType = FreeGroupPtr; + using ConstFreeGroupPtrType = ConstFreeGroupPtr; + + /// \brief Find a FreeGroup that includes all the links in this model. + /// \return a FreeGroup that envelops all links in the model if such a + /// group is available. Otherwise a nullptr is returned. + public: FreeGroupPtrType FindFreeGroup(); + + /// \brief const version of FindFreeGroup() + public: ConstFreeGroupPtrType FindFreeGroup() const; + }; + + public: template + class Link : public virtual Feature::Link + { + using FreeGroupPtrType = FreeGroupPtr; + using ConstFreeGroupPtrType = ConstFreeGroupPtr; + + /// \brief Find the smallest FreeGroup that includes this Link. + /// \return a FreeGroup that includes this link and any connected links. + /// If this link is constrained to the world in some way, then a nullptr + /// is returned. + public: FreeGroupPtrType FindFreeGroup(); + + /// \brief const version of FindFreeGroup() + public: ConstFreeGroupPtrType FindFreeGroup() const; + }; + + public: template + class FreeGroup : public virtual Entity + { + /// \brief The root link of this FreeGroup. This link is the root of one + /// of the kinematic trees represented by this FreeGroup. Getting and + /// setting properties (like poses and velocities) on the group will be + /// done in terms of this link. + public: LinkPtr RootLink(); + + /// \brief const version of RootLink() + public: ConstLinkPtr RootLink() const; + + /// \brief The root link of this FreeGroup. This link is the root of one + /// of the kinematic trees represented by this FreeGroup. Getting and + /// setting properties (like poses and velocities) on the group will be + /// done in terms of this link. + /// DEPRECATED. Please use RootLink() + public: LinkPtr IGN_DEPRECATED(4.0) CanonicalLink(); + + /// \brief const version of CanonicalLink() + /// DEPRECATED. Please use RootLink() + public: ConstLinkPtr IGN_DEPRECATED(4.0) + CanonicalLink() const; + }; + + public: template + class Implementation : public virtual Feature::Implementation + { + public: virtual Identity FindFreeGroupForModel( + const Identity &_modelID) const = 0; + + public: virtual Identity FindFreeGroupForLink( + const Identity &_linkID) const = 0; + + public: virtual Identity GetFreeGroupRootLink( + const Identity &_groupID) const = 0; + }; + }; + + ///////////////////////////////////////////////// + class IGNITION_PHYSICS_VISIBLE FreeGroupFrameSemantics + : public virtual FeatureWithRequirements< + FindFreeGroupFeature, FrameSemantics> + { + public: template + using FreeGroup = FrameSemantics::Frame; + }; + + ///////////////////////////////////////////////// + /// \brief This features sets the FreeGroup pose in world frame. However, + /// while a physics engine with maximal coordinates can provide + /// Link::SetWorldPose and similar functions for setting velocity + /// regardless of the kinematic constraints on that link, this behavior + /// for FreeGroup is not well defined and difficult to implement + /// with generalized coordinates. The FreeGroup::SetWorldPose function + /// should provide an analog to both Link::SetWorldPose and + /// Model::SetWorldPose. + class IGNITION_PHYSICS_VISIBLE SetFreeGroupWorldPose + : public virtual FeatureWithRequirements + { + /// \brief This class defines the FreeGroup concept, which represents a + /// group of links that are not connected to the world with any kinematic + /// constraints. This class also provides a rough definition of this + /// FreeGroup pose in world frame. See FindFreeGroupFeature class + /// documentation for more detail. + public: template + class FreeGroup : public virtual Entity + { + public: using PoseType = + typename FromPolicy::template Use; + + /// \brief Set this FreeGroup pose in world frame. + public: void SetWorldPose(const PoseType &_pose); + }; + + public: template + class Implementation : public virtual Feature::Implementation + { + public: using PoseType = + typename FromPolicy::template Use; + + public: virtual void SetFreeGroupWorldPose( + const Identity &_groupID, + const PoseType &_pose) = 0; + }; + }; + + ///////////////////////////////////////////////// + /// \brief This features sets the FreeGroup linear and angular velocity in + /// world frame. + class IGNITION_PHYSICS_VISIBLE SetFreeGroupWorldVelocity + : public virtual FeatureWithRequirements + { + /// \brief This class defines the FreeGroup concept, which represents a + /// group of links that are not connected to the world with any kinematic + /// constraints. This class also provides a rough definition of this + /// FreeGroup linear and angular velocity in world frame. See + /// FindFreeGroupFeature class documentation for more detail. + public: template + class FreeGroup : public virtual Entity + { + public: using LinearVelocity = + typename FromPolicy::template Use; + + public: using AngularVelocity = + typename FromPolicy::template Use; + + /// \brief Set this FreeGroup linear velocity in world frame. + public: void SetWorldLinearVelocity( + const LinearVelocity &_linearVelocity); + + /// \brief Set this FreeGroup angular velocity in world frame. + public: void SetWorldAngularVelocity( + const AngularVelocity &_angularVelocity); + }; + + public: template + class Implementation : public virtual Feature::Implementation + { + public: using LinearVelocity = + typename FromPolicy::template Use; + + public: using AngularVelocity = + typename FromPolicy::template Use; + + public: virtual void SetFreeGroupWorldLinearVelocity( + const Identity &_groupID, + const LinearVelocity &_linearVelocity) = 0; + + public: virtual void SetFreeGroupWorldAngularVelocity( + const Identity &_groupID, + const AngularVelocity &_angularVelocity) = 0; + }; + }; + } +} + +#include + +#endif diff --git a/include/gz/physics/FreeJoint.hh b/include/gz/physics/FreeJoint.hh new file mode 100644 index 000000000..a67876270 --- /dev/null +++ b/include/gz/physics/FreeJoint.hh @@ -0,0 +1,62 @@ +/* + * Copyright (C) 2018 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 GZ_PHYSICS_FREEJOINT_HH_ +#define GZ_PHYSICS_FREEJOINT_HH_ + +#include +#include + +namespace ignition +{ + namespace physics + { + IGN_PHYSICS_DECLARE_JOINT_TYPE(FreeJoint) + + class IGNITION_PHYSICS_VISIBLE SetFreeJointRelativeTransformFeature + : public virtual FeatureWithRequirements + { + public: template + class FreeJoint : public virtual Feature::Link + { + public: using PoseType = + typename FromPolicy::template Use; + + /// \brief Set the transform from the joint's parent link to its child + /// link by changing the generalized positions of this joint. + /// \param[in] _pose + /// The desired transformation matrix from the joint's parent + public: void SetRelativeTransform(const PoseType &_pose); + }; + + /// \private The implementation API for setting a free joint transform + public: template + class Implementation : public virtual Feature::Implementation + { + public: using PoseType = + typename FromPolicy::template Use; + + public: virtual void SetFreeJointRelativeTransform( + const Identity &_id, const PoseType &_pose) = 0; + }; + }; + } +} + +#include + +#endif diff --git a/include/gz/physics/Geometry.hh b/include/gz/physics/Geometry.hh new file mode 100644 index 000000000..5934ab39b --- /dev/null +++ b/include/gz/physics/Geometry.hh @@ -0,0 +1,104 @@ +/* + * Copyright (C) 2018 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 GZ_PHYSICS_GEOMETRY_HH_ +#define GZ_PHYSICS_GEOMETRY_HH_ + +#include + +#define DETAIL_IGN_PHYSICS_MAKE_BOTH_PRECISIONS(Type, Dim) \ + using Type ## Dim ## d = Type; \ + using Type ## Dim ## f = Type; + +/// \brief This macro defines the following types: +/// Type2d // 2-dimensional version of Type with double precision +/// Type2f // 2-dimensional version of Type with float precision +/// Type3d // 3-dimensional version of Type with double precision +/// Type3f // 3-dimensional version of Type with float precision +#define IGN_PHYSICS_MAKE_ALL_TYPE_COMBOS(Type) \ + DETAIL_IGN_PHYSICS_MAKE_BOTH_PRECISIONS(Type, 2) \ + DETAIL_IGN_PHYSICS_MAKE_BOTH_PRECISIONS(Type, 3) + +namespace ignition +{ + namespace physics + { + /// \brief This is used by ignition-physics to represent rigid body + /// transforms in 2D or 3D simulations. The precision can be chosen as + /// float or scalar. + template + using Pose = Eigen::Transform; + IGN_PHYSICS_MAKE_ALL_TYPE_COMBOS(Pose) + + template + using Vector = Eigen::Matrix; + IGN_PHYSICS_MAKE_ALL_TYPE_COMBOS(Vector) + + template + using LinearVector = Vector; + IGN_PHYSICS_MAKE_ALL_TYPE_COMBOS(LinearVector) + + template + using AngularVector = Vector; + IGN_PHYSICS_MAKE_ALL_TYPE_COMBOS(AngularVector) + + template + struct Wrench + { + AngularVector torque; + LinearVector force; + }; + IGN_PHYSICS_MAKE_ALL_TYPE_COMBOS(Wrench) + + template + using AlignedBox = Eigen::AlignedBox; + IGN_PHYSICS_MAKE_ALL_TYPE_COMBOS(AlignedBox) + + ///////////////////////////////////////////////// + /// \brief This struct is used to conveniently convert from a policy to a + /// geometric type. Example usage: + /// + /// using AngularVector = FromPolicy::To; + template + struct FromPolicy + { + using Scalar = typename PolicyT::Scalar; + enum { Dim = PolicyT::Dim }; + + template