From b084f662c9a44f6d7b7fb2021355dea7ec6b4ceb Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Fri, 6 Nov 2020 17:36:55 -0800 Subject: [PATCH] Add Capsule class with inertia calculation method (#163) This adds a class for a capsule shape (a cylinder with hemispheres capping its flat faces) with an inertia calculation method using the Inertial addition operator to combine the inertias of a cylinder and two hemispheres. The class is similar to the Cylinder class but with several differences: * There is no Quaternion parameter, which is rarely used in the other shape classes. * DensityFromMass method returns NaN if density is invalid instead of -1 * MassMatrix() returns std::optional instead passing a reference and returning bool. With the prior API that passes a reference to a MassMatrix3, it is unclear whether the object is mutated when the parameters are invalid. Using std::optional removes the ambiguity and ensures that invalid values are not returned. Other changes include: * Fix typos in Cylinder.hh, MassMatrix3.hh * Bump version to 6.7.0~pre1 * cpplint.py: allow c++14/17 headers Signed-off-by: Steve Peters --- CMakeLists.txt | 4 +- include/ignition/math/Capsule.hh | 151 +++++++++++++++++++++ include/ignition/math/Cylinder.hh | 2 +- include/ignition/math/MassMatrix3.hh | 10 +- include/ignition/math/detail/Capsule.hh | 166 ++++++++++++++++++++++++ src/Capsule_TEST.cc | 130 +++++++++++++++++++ tools/cpplint.py | 13 ++ 7 files changed, 468 insertions(+), 8 deletions(-) create mode 100644 include/ignition/math/Capsule.hh create mode 100644 include/ignition/math/detail/Capsule.hh create mode 100644 src/Capsule_TEST.cc diff --git a/CMakeLists.txt b/CMakeLists.txt index 6c3ab8458..1046beecc 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,7 +3,7 @@ cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) #============================================================================ # Initialize the project #============================================================================ -project(ignition-math6 VERSION 6.6.0) +project(ignition-math6 VERSION 6.7.0) #============================================================================ # Find ignition-cmake @@ -15,7 +15,7 @@ find_package(ignition-cmake2 2.0.0 REQUIRED) # Configure the project #============================================================================ set (c++standard 17) -ign_configure_project() +ign_configure_project(VERSION_SUFFIX pre1) #============================================================================ # Set project-specific options diff --git a/include/ignition/math/Capsule.hh b/include/ignition/math/Capsule.hh new file mode 100644 index 000000000..cbf4ae198 --- /dev/null +++ b/include/ignition/math/Capsule.hh @@ -0,0 +1,151 @@ +/* + * Copyright (C) 2020 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +#ifndef IGNITION_MATH_CAPSULE_HH_ +#define IGNITION_MATH_CAPSULE_HH_ + +#include +#include "ignition/math/MassMatrix3.hh" +#include "ignition/math/Material.hh" + +namespace ignition +{ + namespace math + { + // Foward declarations + class CapsulePrivate; + + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_MATH_VERSION_NAMESPACE { + // + /// \class Capsule Capsule.hh ignition/math/Capsule.hh + /// \brief A representation of a capsule or sphere-capped cylinder. + /// + /// The capsule class supports defining a capsule with a radius, + /// length, and material properties. The shape is equivalent to a cylinder + /// aligned with the Z-axis and capped with hemispheres. Radius and + /// length are in meters. See Material for more on material properties. + /// \tparam Precision Scalar numeric type. + template + class Capsule + { + /// \brief Default constructor. The default radius and length are both + /// zero. + public: Capsule() = default; + + /// \brief Construct a capsule with a length and radius. + /// \param[in] _length Length of the capsule. + /// \param[in] _radius Radius of the capsule. + public: Capsule(const Precision _length, const Precision _radius); + + /// \brief Construct a capsule with a length, radius, and material. + /// \param[in] _length Length of the capsule. + /// \param[in] _radius Radius of the capsule. + /// \param[in] _mat Material property for the capsule. + public: Capsule(const Precision _length, const Precision _radius, + const Material &_mat); + + /// \brief Get the radius in meters. + /// \return The radius of the capsule in meters. + public: Precision Radius() const; + + /// \brief Set the radius in meters. + /// \param[in] _radius The radius of the capsule in meters. + public: void SetRadius(const Precision _radius); + + /// \brief Get the length in meters. + /// \return The length of the capsule in meters. + public: Precision Length() const; + + /// \brief Set the length in meters. + /// \param[in] _length The length of the capsule in meters. + public: void SetLength(const Precision _length); + + /// \brief Get the material associated with this capsule. + /// \return The material assigned to this capsule + public: const Material &Mat() const; + + /// \brief Set the material associated with this capsule. + /// \param[in] _mat The material assigned to this capsule + public: void SetMat(const Material &_mat); + + /// \brief Get the mass matrix for this capsule. This function + /// is only meaningful if the capsule's radius, length, and material + /// have been set. + /// \return The computed mass matrix if parameters are valid + /// (radius > 0), (length > 0), and (density > 0). Otherwise + /// std::nullopt is returned. + public: std::optional< MassMatrix3 > MassMatrix() const; + + /// \brief Check if this capsule is equal to the provided capsule. + /// Radius, length, and material properties will be checked. + public: bool operator==(const Capsule &_capsule) const; + + /// \brief Get the volume of the capsule in m^3. + /// \return Volume of the capsule in m^3. + public: Precision Volume() const; + + /// \brief Compute the capsule's density given a mass value. The + /// capsule is assumed to be solid with uniform density. This + /// function requires the capsule's radius and length to be set to + /// values greater than zero. The Material of the capsule is ignored. + /// \param[in] _mass Mass of the capsule, in kg. This value should be + /// greater than zero. + /// \return Density of the capsule in kg/m^3. A NaN is returned + /// if radius, length or _mass is <= 0. + public: Precision DensityFromMass(const Precision _mass) const; + + /// \brief Set the density of this capsule based on a mass value. + /// Density is computed using + /// Precision DensityFromMass(const Precision _mass) const. The + /// capsule is assumed to be solid with uniform density. This + /// function requires the capsule's radius and length to be set to + /// values greater than zero. The existing Material density value is + /// overwritten only if the return value from this true. + /// \param[in] _mass Mass of the capsule, in kg. This value should be + /// greater than zero. + /// \return True if the density was set. False is returned if the + /// capsule's radius, length, or the _mass value are <= 0. + /// \sa Precision DensityFromMass(const Precision _mass) const + public: bool SetDensityFromMass(const Precision _mass); + + /// \brief Radius of the capsule. + private: Precision radius = 0.0; + + /// \brief Length of the capsule. + private: Precision length = 0.0; + + /// \brief the capsule's material. + private: Material material; + }; + + /// \typedef Capsule Capsulei + /// \brief Capsule with integer precision. + typedef Capsule Capsulei; + + /// \typedef Capsule Capsuled + /// \brief Capsule with double precision. + typedef Capsule Capsuled; + + /// \typedef Capsule Capsulef + /// \brief Capsule with float precision. + typedef Capsule Capsulef; + } + } +} +#include "ignition/math/detail/Capsule.hh" + +#endif diff --git a/include/ignition/math/Cylinder.hh b/include/ignition/math/Cylinder.hh index 9a5802e18..31a2eacca 100644 --- a/include/ignition/math/Cylinder.hh +++ b/include/ignition/math/Cylinder.hh @@ -32,7 +32,7 @@ namespace ignition inline namespace IGNITION_MATH_VERSION_NAMESPACE { // /// \class Cylinder Cylinder.hh ignition/math/Cylinder.hh - /// \brief A represntation of a cylinder. + /// \brief A representation of a cylinder. /// /// The cylinder class supports defining a cylinder with a radius, /// length, rotational offset, and material properties. Radius and diff --git a/include/ignition/math/MassMatrix3.hh b/include/ignition/math/MassMatrix3.hh index ddc5b58b1..9d3f56847 100644 --- a/include/ignition/math/MassMatrix3.hh +++ b/include/ignition/math/MassMatrix3.hh @@ -1069,7 +1069,7 @@ namespace ignition const Quaternion &_rot = Quaternion::Identity) { // Check that _mass and _size are strictly positive - // and that quatenion is valid + // and that quaternion is valid if (_mass <= 0 || _size.Min() <= 0 || _rot == Quaternion::Zero) { return false; @@ -1087,7 +1087,7 @@ namespace ignition const Quaternion &_rot = Quaternion::Identity) { // Check that _mass and _size are strictly positive - // and that quatenion is valid + // and that quaternion is valid if (this->Mass() <= 0 || _size.Min() <= 0 || _rot == Quaternion::Zero) { @@ -1120,7 +1120,7 @@ namespace ignition const Quaternion &_rot = Quaternion::Identity) { // Check that density, _radius and _length are strictly positive - // and that quatenion is valid + // and that quaternion is valid if (_mat.Density() <= 0 || _length <= 0 || _radius <= 0 || _rot == Quaternion::Zero) { @@ -1144,7 +1144,7 @@ namespace ignition const Quaternion &_rot = Quaternion::Identity) { // Check that _mass, _radius and _length are strictly positive - // and that quatenion is valid + // and that quaternion is valid if (_mass <= 0 || _length <= 0 || _radius <= 0 || _rot == Quaternion::Zero) { @@ -1165,7 +1165,7 @@ namespace ignition const Quaternion &_rot) { // Check that _mass and _size are strictly positive - // and that quatenion is valid + // and that quaternion is valid if (this->Mass() <= 0 || _length <= 0 || _radius <= 0 || _rot == Quaternion::Zero) { diff --git a/include/ignition/math/detail/Capsule.hh b/include/ignition/math/detail/Capsule.hh new file mode 100644 index 000000000..ebf52b616 --- /dev/null +++ b/include/ignition/math/detail/Capsule.hh @@ -0,0 +1,166 @@ +/* + * Copyright (C) 2020 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +#ifndef IGNITION_MATH_DETAIL_CAPSULE_HH_ +#define IGNITION_MATH_DETAIL_CAPSULE_HH_ + +#include +#include +#include +#include + +namespace ignition +{ +namespace math +{ + +////////////////////////////////////////////////// +template +Capsule::Capsule(const T _length, const T _radius) +{ + this->length = _length; + this->radius = _radius; +} + +////////////////////////////////////////////////// +template +Capsule::Capsule(const T _length, const T _radius, const Material &_mat) +{ + this->length = _length; + this->radius = _radius; + this->material = _mat; +} + +////////////////////////////////////////////////// +template +T Capsule::Radius() const +{ + return this->radius; +} + +////////////////////////////////////////////////// +template +void Capsule::SetRadius(const T _radius) +{ + this->radius = _radius; +} + +////////////////////////////////////////////////// +template +T Capsule::Length() const +{ + return this->length; +} + +////////////////////////////////////////////////// +template +void Capsule::SetLength(const T _length) +{ + this->length = _length; +} + +////////////////////////////////////////////////// +template +const Material &Capsule::Mat() const +{ + return this->material; +} + +////////////////////////////////////////////////// +template +void Capsule::SetMat(const Material &_mat) +{ + this->material = _mat; +} + +////////////////////////////////////////////////// +template +bool Capsule::operator==(const Capsule &_capsule) const +{ + return equal(this->radius, _capsule.Radius()) && + equal(this->length, _capsule.Length()) && + this->material == _capsule.Mat(); +} + +////////////////////////////////////////////////// +template +std::optional< MassMatrix3 > Capsule::MassMatrix() const +{ + // mass and moment of inertia of cylinder about centroid + MassMatrix3 cylinder; + cylinder.SetFromCylinderZ(this->material, this->length, this->radius); + + // mass and moment of inertia of hemisphere about centroid + const T r2 = this->radius * this->radius; + const T hemisphereMass = this->material.Density() * + 2. / 3. * IGN_PI * r2 * this->radius; + // efunda.com/math/solids/solids_display.cfm?SolidName=Hemisphere + const T ixx = 83. / 320. * hemisphereMass * r2; + const T izz = 2. / 5. * hemisphereMass * r2; + MassMatrix3 hemisphere(hemisphereMass, Vector3(ixx, ixx, izz), + Vector3::Zero);; + + // Distance from centroid of cylinder to centroid of hemisphere, + // since centroid of hemisphere is 3/8 radius from its flat base + const T dz = this->length / 2. + this->radius * 3. / 8.; + Inertial upperCap(hemisphere, Pose3(0, 0, dz, 0, 0, 0)); + Inertial lowerCap(hemisphere, Pose3(0, 0, -dz, 0, 0, 0)); + + // Use Inertial class to add MassMatrix3 objects at different poses + Inertial capsule = + Inertial(cylinder, Pose3::Zero) + upperCap + lowerCap; + + if (!capsule.MassMatrix().IsValid()) + { + return std::nullopt; + } + + return std::make_optional(capsule.MassMatrix()); +} + +////////////////////////////////////////////////// +template +T Capsule::Volume() const +{ + return IGN_PI * std::pow(this->radius, 2) * + (this->length + 4. / 3. * this->radius); +} + +////////////////////////////////////////////////// +template +bool Capsule::SetDensityFromMass(const T _mass) +{ + T newDensity = this->DensityFromMass(_mass); + if (isnan(newDensity)) + return false; + + this->material.SetDensity(newDensity); + return true; +} + +////////////////////////////////////////////////// +template +T Capsule::DensityFromMass(const T _mass) const +{ + if (this->radius <= 0 || this->length <=0 || _mass <= 0) + return std::numeric_limits::quiet_NaN(); + + return _mass / this->Volume(); +} + +} +} +#endif diff --git a/src/Capsule_TEST.cc b/src/Capsule_TEST.cc new file mode 100644 index 000000000..bb09828d9 --- /dev/null +++ b/src/Capsule_TEST.cc @@ -0,0 +1,130 @@ +/* + * Copyright (C) 2020 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +#include +#include +#include + +#include "ignition/math/Capsule.hh" +#include "ignition/math/Helpers.hh" + +using namespace ignition; + +///////////////////////////////////////////////// +TEST(CapsuleTest, Constructor) +{ + // Default constructor + { + math::Capsuled capsule; + EXPECT_DOUBLE_EQ(0.0, capsule.Length()); + EXPECT_DOUBLE_EQ(0.0, capsule.Radius()); + EXPECT_EQ(math::Material(), capsule.Mat()); + + math::Capsuled capsule2; + EXPECT_EQ(capsule, capsule2); + } + + // Length and radius constructor + { + math::Capsuled capsule(1.0, 2.0); + EXPECT_DOUBLE_EQ(1.0, capsule.Length()); + EXPECT_DOUBLE_EQ(2.0, capsule.Radius()); + EXPECT_EQ(math::Material(), capsule.Mat()); + + math::Capsuled capsule2(1.0, 2.0); + EXPECT_EQ(capsule, capsule2); + } + + // Length, radius, mat + { + math::Capsuled capsule(1.0, 2.0, + math::Material(math::MaterialType::WOOD)); + EXPECT_DOUBLE_EQ(1.0, capsule.Length()); + EXPECT_DOUBLE_EQ(2.0, capsule.Radius()); + EXPECT_EQ(math::Material(math::MaterialType::WOOD), capsule.Mat()); + + math::Capsuled capsule2(1.0, 2.0, + math::Material(math::MaterialType::WOOD)); + EXPECT_EQ(capsule, capsule2); + } +} + +////////////////////////////////////////////////// +TEST(CapsuleTest, Mutators) +{ + math::Capsuled capsule; + EXPECT_DOUBLE_EQ(0.0, capsule.Length()); + EXPECT_DOUBLE_EQ(0.0, capsule.Radius()); + EXPECT_EQ(math::Material(), capsule.Mat()); + + capsule.SetLength(100.1); + capsule.SetRadius(.123); + capsule.SetMat(math::Material(math::MaterialType::PINE)); + + EXPECT_DOUBLE_EQ(100.1, capsule.Length()); + EXPECT_DOUBLE_EQ(.123, capsule.Radius()); + EXPECT_EQ(math::Material(math::MaterialType::PINE), capsule.Mat()); +} + +////////////////////////////////////////////////// +TEST(CapsuleTest, VolumeAndDensity) +{ + double mass = 1.0; + math::Capsuled capsule(1.0, 0.001); + double expectedVolume = (IGN_PI * std::pow(0.001, 2) * (1.0 + 4./3. * 0.001)); + EXPECT_DOUBLE_EQ(expectedVolume, capsule.Volume()); + + double expectedDensity = mass / expectedVolume; + EXPECT_DOUBLE_EQ(expectedDensity, capsule.DensityFromMass(mass)); + + // Bad density + math::Capsuled capsule2; + EXPECT_TRUE(math::isnan(capsule2.DensityFromMass(mass))); +} + +////////////////////////////////////////////////// +TEST(CapsuleTest, Mass) +{ + double mass = 2.0; + double l = 2.0; + double r = 0.1; + math::Capsuled capsule(l, r); + capsule.SetDensityFromMass(mass); + + const double cylinderVolume = IGN_PI * r*r * l; + const double sphereVolume = IGN_PI * 4. / 3. * r*r*r; + const double volume = cylinderVolume + sphereVolume; + const double cylinderMass = mass * cylinderVolume / volume; + const double sphereMass = mass * sphereVolume / volume; + + // expected values based on formula used in Open Dynamics Engine + // https://bitbucket.org/odedevs/ode/src/0.16.2/ode/src/mass.cpp#lines-148:153 + // and the following article: + // https://www.gamedev.net/tutorials/_/technical/math-and-physics/capsule-inertia-tensor-r3856/ + double ixxIyy = (1/12.0) * cylinderMass * (3*r*r + l*l) + + sphereMass * (0.4*r*r + 0.375*r*l + 0.25*l*l); + double izz = r*r * (0.5 * cylinderMass + 0.4 * sphereMass); + + math::MassMatrix3d expectedMassMat; + expectedMassMat.SetInertiaMatrix(ixxIyy, ixxIyy, izz, 0.0, 0.0, 0.0); + expectedMassMat.SetMass(mass); + + auto massMat = capsule.MassMatrix(); + ASSERT_NE(std::nullopt, massMat); + EXPECT_EQ(expectedMassMat, *massMat); + EXPECT_EQ(expectedMassMat.DiagonalMoments(), massMat->DiagonalMoments()); + EXPECT_DOUBLE_EQ(expectedMassMat.Mass(), massMat->Mass()); +} diff --git a/tools/cpplint.py b/tools/cpplint.py index 04365fa6f..34493e040 100644 --- a/tools/cpplint.py +++ b/tools/cpplint.py @@ -303,6 +303,7 @@ 'random', 'ratio', 'regex', + 'scoped_allocator', 'set', 'sstream', 'stack', @@ -321,6 +322,18 @@ 'utility', 'valarray', 'vector', + # 17.6.1.2 C++14 headers + 'shared_mutex', + # 17.6.1.2 C++17 headers + 'any', + 'charconv', + 'codecvt', + 'execution', + 'filesystem', + 'memory_resource', + 'optional', + 'string_view', + 'variant', # 17.6.1.2 C++ headers for C library facilities 'cassert', 'ccomplex',