From d025776bda90df9cc533721b44133de48a8a607a Mon Sep 17 00:00:00 2001 From: Benjamin Perseghetti Date: Sun, 12 May 2024 03:52:56 -0400 Subject: [PATCH] Adding cone primitives. Squashing commits to make requested target of main with backports to harmonic. git cherry-pick conflict resolved with: a7613bd0e013aa309c2fd55d8a8fed9ee6f591f3 Signed-off-by: Benjamin Perseghetti --- include/gz/math/Cone.hh | 193 +++++++++++++++++++ include/gz/math/Helpers.hh | 5 + include/gz/math/MassMatrix3.hh | 77 ++++++++ include/gz/math/detail/Cone.hh | 172 +++++++++++++++++ src/Cone_TEST.cc | 147 ++++++++++++++ src/Helpers.i | 1 + src/Helpers_TEST.cc | 3 + src/MassMatrix3_TEST.cc | 51 +++++ src/python_pybind11/CMakeLists.txt | 1 + src/python_pybind11/src/Cone.hh | 125 ++++++++++++ src/python_pybind11/src/Helpers.cc | 12 ++ src/python_pybind11/src/MassMatrix3.hh | 29 +++ src/python_pybind11/src/_gz_math_pybind11.cc | 3 + src/python_pybind11/test/Cone_TEST.py | 126 ++++++++++++ src/python_pybind11/test/Helpers_TEST.py | 2 +- src/ruby/Cone.i | 80 ++++++++ src/ruby/Helpers.i | 3 + src/ruby/MassMatrix3.i | 13 +- 18 files changed, 1041 insertions(+), 2 deletions(-) create mode 100644 include/gz/math/Cone.hh create mode 100644 include/gz/math/detail/Cone.hh create mode 100644 src/Cone_TEST.cc create mode 100644 src/python_pybind11/src/Cone.hh create mode 100644 src/python_pybind11/test/Cone_TEST.py create mode 100644 src/ruby/Cone.i diff --git a/include/gz/math/Cone.hh b/include/gz/math/Cone.hh new file mode 100644 index 000000000..58d161f99 --- /dev/null +++ b/include/gz/math/Cone.hh @@ -0,0 +1,193 @@ +/* + * Copyright 2024 CogniPilot Foundation + * Copyright 2024 Open Source Robotics Foundation + * Copyright 2024 Rudis Laboratories + * + * 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_MATH_CONE_HH_ +#define GZ_MATH_CONE_HH_ + +#include +#include "gz/math/MassMatrix3.hh" +#include "gz/math/Material.hh" +#include "gz/math/Quaternion.hh" + +namespace gz +{ + namespace math + { + // Foward declarations + class ConePrivate; + + // Inline bracket to help doxygen filtering. + inline namespace GZ_MATH_VERSION_NAMESPACE { + // + /// \class Cone Cone.hh gz/math/Cone.hh + /// \brief A representation of a cone. + /// + /// The cone class supports defining a cone with a radius, + /// length, rotational offset, and material properties. Radius and + /// length are in meters. See Material for more on material properties. + /// By default, a cone's length is aligned with the Z axis where the base + /// of the cone is proximal to the origin and vertex points in positive Z. + /// The rotational offset encodes a rotation from the z axis. + template + class Cone + { + /// \brief Default constructor. The default radius and length are both + /// zero. The default rotational offset is + /// Quaternion::Identity. + public: Cone() = default; + + /// \brief Construct a cone with a length, radius, and optionally + /// a rotational offset. + /// \param[in] _length Length of the cone. + /// \param[in] _radius Radius of the cone. + /// \param[in] _rotOffset Rotational offset of the cone. + public: Cone(const Precision _length, const Precision _radius, + const Quaternion &_rotOffset = + Quaternion::Identity); + + /// \brief Construct a cone with a length, radius, material and + /// optionally a rotational offset. + /// \param[in] _length Length of the cone. + /// \param[in] _radius Radius of the cone. + /// \param[in] _mat Material property for the cone. + /// \param[in] _rotOffset Rotational offset of the cone. + public: Cone(const Precision _length, const Precision _radius, + const Material &_mat, + const Quaternion &_rotOffset = + Quaternion::Identity); + + /// \brief Get the radius in meters. + /// \return The radius of the cone in meters. + public: Precision Radius() const; + + /// \brief Set the radius in meters. + /// \param[in] _radius The radius of the cone in meters. + public: void SetRadius(const Precision _radius); + + /// \brief Get the length in meters. + /// \return The length of the cone in meters. + public: Precision Length() const; + + /// \brief Set the length in meters. + /// \param[in] _length The length of the cone in meters. + public: void SetLength(const Precision _length); + + /// \brief Get the rotational offset. By default, a cone's length + /// is aligned with the Z axis. The rotational offset encodes + /// a rotation from the z axis. + /// \return The cone's rotational offset. + /// \sa void SetRotationalOffset(const Quaternion &_rot) + public: Quaternion RotationalOffset() const; + + /// \brief Set the rotation offset. + /// \param[in] _rotOffset + /// See Quaternion RotationalOffset() for details on the + /// rotational offset. + /// \sa Quaternion RotationalOffset() const + public: void SetRotationalOffset( + const Quaternion &_rotOffset); + + /// \brief Get the material associated with this cone. + /// \return The material assigned to this cone + public: const Material &Mat() const; + + /// \brief Set the material associated with this cone. + /// \param[in] _mat The material assigned to this cone + public: void SetMat(const Material &_mat); + + /// \brief Get the mass matrix for this cone. This function + /// is only meaningful if the cone's radius, length, and material + /// have been set. Optionally, set the rotational offset. + /// \param[out] _massMat The computed mass matrix will be stored + /// here. + /// \return False if computation of the mass matrix failed, which + /// could be due to an invalid radius (<=0), length (<=0), or density + /// (<=0). + public: bool MassMatrix(MassMatrix3d &_massMat) const; + + /// \brief Get the mass matrix for this cone. This function + /// is only meaningful if the cone's radius, length, and material + /// have been set. Optionally, set the rotational offset. + /// \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 cone is equal to the provided cone. + /// Radius, length, and material properties will be checked. + public: bool operator==(const Cone &_cone) const; + + /// \brief Get the volume of the cone in m^3. + /// \return Volume of the cone in m^3. + public: Precision Volume() const; + + /// \brief Compute the cone's density given a mass value. The + /// cone is assumed to be solid with uniform density. This + /// function requires the cone's radius and length to be set to + /// values greater than zero. The Material of the cone is ignored. + /// \param[in] _mass Mass of the cone, in kg. This value should be + /// greater than zero. + /// \return Density of the cone in kg/m^3. A negative value is + /// returned if radius, length or _mass is <= 0. + public: Precision DensityFromMass(const Precision _mass) const; + + /// \brief Set the density of this cone based on a mass value. + /// Density is computed using + /// Precision DensityFromMass(const Precision _mass) const. The + /// cone is assumed to be solid with uniform density. This + /// function requires the cone'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 cone, in kg. This value should be + /// greater than zero. + /// \return True if the density was set. False is returned if the + /// cone'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 cone. + private: Precision radius = 0.0; + + /// \brief Length of the cone. + private: Precision length = 0.0; + + /// \brief the cone's material. + private: Material material; + + /// \brief Rotational offset. + private: Quaternion rotOffset = + Quaternion::Identity; + }; + + /// \typedef Cone Conei + /// \brief Cone with integer precision. + typedef Cone Conei; + + /// \typedef Cone Coned + /// \brief Cone with double precision. + typedef Cone Coned; + + /// \typedef Cone Conef + /// \brief Cone with float precision. + typedef Cone Conef; + } + } +} +#include "gz/math/detail/Cone.hh" + +#endif diff --git a/include/gz/math/Helpers.hh b/include/gz/math/Helpers.hh index 25be34fae..4d0f09d45 100644 --- a/include/gz/math/Helpers.hh +++ b/include/gz/math/Helpers.hh @@ -63,6 +63,11 @@ constexpr T GZ_MASSMATRIX3_DEFAULT_TOLERANCE = T(10); /// \param[in] _radius Sphere radius #define GZ_SPHERE_VOLUME(_radius) (4.0*GZ_PI*std::pow(_radius, 3)/3.0) +/// \brief Compute cone volume +/// \param[in] _r Cone base radius +/// \param[in] _l Cone length +#define GZ_CONE_VOLUME(_r, _l) (_l * GZ_PI * std::pow(_r, 2) / 3.0) + /// \brief Compute cylinder volume /// \param[in] _r Cylinder base radius /// \param[in] _l Cylinder length diff --git a/include/gz/math/MassMatrix3.hh b/include/gz/math/MassMatrix3.hh index bd60a2362..3d63eb701 100644 --- a/include/gz/math/MassMatrix3.hh +++ b/include/gz/math/MassMatrix3.hh @@ -930,6 +930,83 @@ namespace gz return this->SetMoi(R * L * R.Transposed()); } + /// \brief Set inertial properties based on a Material and equivalent + /// cone aligned with Z axis. + /// \param[in] _mat Material that specifies a density. Uniform density + /// is used. + /// \param[in] _length Length of cone along Z axis. + /// \param[in] _radius Radius of cone. + /// \param[in] _rot Rotational offset of equivalent cone. + /// \return True if inertial properties were set successfully. + public: bool SetFromConeZ(const Material &_mat, + const T _length, + const T _radius, + const Quaternion &_rot = Quaternion::Identity) + { + // Check that density, _radius and _length are strictly positive + // and that quaternion is valid + if (_mat.Density() <= 0 || _length <= 0 || _radius <= 0 || + _rot == Quaternion::Zero) + { + return false; + } + T volume = GZ_PI * _radius * _radius * _length; + return this->SetFromConeZ(_mat.Density() * volume, + _length, _radius, _rot); + } + + /// \brief Set inertial properties based on mass and equivalent cone + /// aligned with Z axis. + /// \param[in] _mass Mass to set. + /// \param[in] _length Length of cone along Z axis. + /// \param[in] _radius Radius of cone. + /// \param[in] _rot Rotational offset of equivalent cone. + /// \return True if inertial properties were set successfully. + public: bool SetFromConeZ(const T _mass, + const T _length, + const T _radius, + const Quaternion &_rot = Quaternion::Identity) + { + // Check that _mass, _radius and _length are strictly positive + // and that quaternion is valid + if (_mass <= 0 || _length <= 0 || _radius <= 0 || + _rot == Quaternion::Zero) + { + return false; + } + this->SetMass(_mass); + return this->SetFromConeZ(_length, _radius, _rot); + } + + /// \brief Set inertial properties based on equivalent cone + /// aligned with Z axis using the current mass value. + /// \param[in] _length Length of cone along Z axis. + /// \param[in] _radius Radius of cone. + /// \param[in] _rot Rotational offset of equivalent cone. + /// \return True if inertial properties were set successfully. + public: bool SetFromConeZ(const T _length, + const T _radius, + const Quaternion &_rot) + { + // Check that _mass and _size are strictly positive + // and that quaternion is valid + if (this->Mass() <= 0 || _length <= 0 || _radius <= 0 || + _rot == Quaternion::Zero) + { + return false; + } + + // Diagonal matrix L with principal moments + T radius2 = std::pow(_radius, 2); + Matrix3 L; + L(0, 0) = 3.0 * this->mass * (4.0 * radius2 + + std::pow(_length, 2)) / 80.0; + L(1, 1) = L(0, 0); + L(2, 2) = 3.0 * this->mass * radius2 / 10.0; + Matrix3 R(_rot); + return this->SetMoi(R * L * R.Transposed()); + } + /// \brief Set inertial properties based on a Material and equivalent /// cylinder aligned with Z axis. /// \param[in] _mat Material that specifies a density. Uniform density diff --git a/include/gz/math/detail/Cone.hh b/include/gz/math/detail/Cone.hh new file mode 100644 index 000000000..e1ccd8282 --- /dev/null +++ b/include/gz/math/detail/Cone.hh @@ -0,0 +1,172 @@ +/* + * Copyright 2024 CogniPilot Foundation + * Copyright 2024 Open Source Robotics Foundation + * Copyright 2024 Rudis Laboratories + * + * 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_MATH_DETAIL_CONE_HH_ +#define GZ_MATH_DETAIL_CONE_HH_ + +#include + +namespace gz +{ +namespace math +{ + +////////////////////////////////////////////////// +template +Cone::Cone(const T _length, const T _radius, + const Quaternion &_rotOffset) +{ + this->length = _length; + this->radius = _radius; + this->rotOffset = _rotOffset; +} + +////////////////////////////////////////////////// +template +Cone::Cone(const T _length, const T _radius, + const Material &_mat, const Quaternion &_rotOffset) +{ + this->length = _length; + this->radius = _radius; + this->material = _mat; + this->rotOffset = _rotOffset; +} + +////////////////////////////////////////////////// +template +T Cone::Radius() const +{ + return this->radius; +} + +////////////////////////////////////////////////// +template +void Cone::SetRadius(const T _radius) +{ + this->radius = _radius; +} + +////////////////////////////////////////////////// +template +T Cone::Length() const +{ + return this->length; +} + +////////////////////////////////////////////////// +template +void Cone::SetLength(const T _length) +{ + this->length = _length; +} + +////////////////////////////////////////////////// +template +Quaternion Cone::RotationalOffset() const +{ + return this->rotOffset; +} + +////////////////////////////////////////////////// +template +void Cone::SetRotationalOffset(const Quaternion &_rotOffset) +{ + this->rotOffset = _rotOffset; +} + +////////////////////////////////////////////////// +template +const Material &Cone::Mat() const +{ + return this->material; +} + +////////////////////////////////////////////////// +template +void Cone::SetMat(const Material &_mat) +{ + this->material = _mat; +} + +////////////////////////////////////////////////// +template +bool Cone::operator==(const Cone &_cone) const +{ + return equal(this->radius, _cone.Radius()) && + equal(this->length, _cone.Length()) && + this->material == _cone.Mat(); +} + +////////////////////////////////////////////////// +template +bool Cone::MassMatrix(MassMatrix3d &_massMat) const +{ + return _massMat.SetFromConeZ( + this->material, this->length, + this->radius, this->rotOffset); +} + +////////////////////////////////////////////////// +template +std::optional < MassMatrix3 > Cone::MassMatrix() const +{ + gz::math::MassMatrix3 _massMat; + + if(!_massMat.SetFromConeZ( + this->material, this->length, + this->radius, this->rotOffset)) + { + return std::nullopt; + } + else + { + return std::make_optional(_massMat); + } +} + +////////////////////////////////////////////////// +template +T Cone::Volume() const +{ + return GZ_PI * std::pow(this->radius, 2) * + this->length; +} + +////////////////////////////////////////////////// +template +bool Cone::SetDensityFromMass(const T _mass) +{ + T newDensity = this->DensityFromMass(_mass); + if (newDensity > 0) + this->material.SetDensity(newDensity); + return newDensity > 0; +} + +////////////////////////////////////////////////// +template +T Cone::DensityFromMass(const T _mass) const +{ + if (this->radius <= 0 || this->length <=0 || _mass <= 0) + return -1.0; + + return _mass / this->Volume(); +} + +} // namespace math +} // namespace gz +#endif diff --git a/src/Cone_TEST.cc b/src/Cone_TEST.cc new file mode 100644 index 000000000..3137fc966 --- /dev/null +++ b/src/Cone_TEST.cc @@ -0,0 +1,147 @@ +/* + * Copyright 2024 CogniPilot Foundation + * Copyright 2024 Open Source Robotics Foundation + * Copyright 2024 Rudis Laboratories + * + * 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 "gz/math/Cone.hh" + +using namespace gz; + +///////////////////////////////////////////////// +TEST(ConeTest, Constructor) +{ + // Default constructor + { + math::Coned cone; + EXPECT_DOUBLE_EQ(0.0, cone.Length()); + EXPECT_DOUBLE_EQ(0.0, cone.Radius()); + EXPECT_EQ(math::Quaterniond::Identity, cone.RotationalOffset()); + EXPECT_EQ(math::Material(), cone.Mat()); + + math::Coned cone2; + EXPECT_EQ(cone, cone2); + } + + // Length and radius constructor + { + math::Coned cone(1.0, 2.0); + EXPECT_DOUBLE_EQ(1.0, cone.Length()); + EXPECT_DOUBLE_EQ(2.0, cone.Radius()); + EXPECT_EQ(math::Quaterniond::Identity, cone.RotationalOffset()); + EXPECT_EQ(math::Material(), cone.Mat()); + + math::Coned cone2(1.0, 2.0); + EXPECT_EQ(cone, cone2); + } + + // Length, radius, and rot constructor + { + math::Coned cone(1.0, 2.0, math::Quaterniond(0.1, 0.2, 0.3)); + EXPECT_DOUBLE_EQ(1.0, cone.Length()); + EXPECT_DOUBLE_EQ(2.0, cone.Radius()); + EXPECT_EQ(math::Quaterniond(0.1, 0.2, 0.3), + cone.RotationalOffset()); + EXPECT_EQ(math::Material(), cone.Mat()); + + math::Coned cone2(1.0, 2.0, math::Quaterniond(0.1, 0.2, 0.3)); + EXPECT_EQ(cone, cone2); + } + + // Length, radius, mat and rot constructor + { + math::Coned cone(1.0, 2.0, + math::Material(math::MaterialType::WOOD), + math::Quaterniond(0.1, 0.2, 0.3)); + EXPECT_DOUBLE_EQ(1.0, cone.Length()); + EXPECT_DOUBLE_EQ(2.0, cone.Radius()); + EXPECT_EQ(math::Quaterniond(0.1, 0.2, 0.3), + cone.RotationalOffset()); + EXPECT_EQ(math::Material(math::MaterialType::WOOD), cone.Mat()); + + math::Coned cone2(1.0, 2.0, + math::Material(math::MaterialType::WOOD), + math::Quaterniond(0.1, 0.2, 0.3)); + EXPECT_EQ(cone, cone2); + } +} + +////////////////////////////////////////////////// +TEST(ConeTest, Mutators) +{ + math::Coned cone; + EXPECT_DOUBLE_EQ(0.0, cone.Length()); + EXPECT_DOUBLE_EQ(0.0, cone.Radius()); + EXPECT_EQ(math::Quaterniond::Identity, cone.RotationalOffset()); + EXPECT_EQ(math::Material(), cone.Mat()); + + cone.SetLength(100.1); + cone.SetRadius(.123); + cone.SetRotationalOffset(math::Quaterniond(1.2, 2.3, 3.4)); + cone.SetMat(math::Material(math::MaterialType::PINE)); + + EXPECT_DOUBLE_EQ(100.1, cone.Length()); + EXPECT_DOUBLE_EQ(.123, cone.Radius()); + EXPECT_EQ(math::Quaterniond(1.2, 2.3, 3.4), + cone.RotationalOffset()); + EXPECT_EQ(math::Material(math::MaterialType::PINE), cone.Mat()); +} + +////////////////////////////////////////////////// +TEST(ConeTest, VolumeAndDensity) +{ + double mass = 1.0; + math::Coned cone(1.0, 0.001); + double expectedVolume = (GZ_PI * std::pow(0.001, 2) * 1.0); + EXPECT_DOUBLE_EQ(expectedVolume, cone.Volume()); + + double expectedDensity = mass / expectedVolume; + EXPECT_DOUBLE_EQ(expectedDensity, cone.DensityFromMass(mass)); + + // Bad density + math::Coned cone2; + EXPECT_GT(0.0, cone2.DensityFromMass(mass)); +} + +////////////////////////////////////////////////// +TEST(ConeTest, Mass) +{ + double mass = 2.0; + double l = 2.0; + double r = 0.1; + math::Coned cone(l, r); + cone.SetDensityFromMass(mass); + + math::MassMatrix3d massMat; + double ixxIyy = (3.0/80.0) * mass * (4*r*r + l*l); + double izz = (3.0/10.0) * mass * r * r; + + math::MassMatrix3d expectedMassMat; + expectedMassMat.SetInertiaMatrix(ixxIyy, ixxIyy, izz, 0.0, 0.0, 0.0); + expectedMassMat.SetMass(mass); + + cone.MassMatrix(massMat); + EXPECT_EQ(expectedMassMat, massMat); + EXPECT_DOUBLE_EQ(expectedMassMat.Mass(), massMat.Mass()); + + auto massMatOpt = cone.MassMatrix(); + ASSERT_NE(std::nullopt, massMatOpt); + EXPECT_EQ(expectedMassMat, *massMatOpt); + EXPECT_EQ(expectedMassMat.DiagonalMoments(), massMatOpt->DiagonalMoments()); + EXPECT_DOUBLE_EQ(expectedMassMat.Mass(), massMatOpt->Mass()); +} diff --git a/src/Helpers.i b/src/Helpers.i index a15cc5584..65aabe680 100644 --- a/src/Helpers.i +++ b/src/Helpers.i @@ -34,6 +34,7 @@ constexpr T GZ_MASSMATRIX3_DEFAULT_TOLERANCE = T(10); #define GZ_SQRT2 1.41421356237309504880 #define GZ_SPHERE_VOLUME(_radius) (4.0 * GZ_PI * std::pow(_radius, 3)/3.0) +#define GZ_CONE_VOLUME(_r, _l) (_l * GZ_PI * std::pow(_r, 2) / 3.0) #define GZ_CYLINDER_VOLUME(_r, _l) (_l * GZ_PI * std::pow(_r, 2)) #define GZ_BOX_VOLUME(_x, _y, _z) (_x *_y * _z) #define GZ_BOX_VOLUME_V(_v) (_v.X() *_v.Y() * _v.Z()) diff --git a/src/Helpers_TEST.cc b/src/Helpers_TEST.cc index fe09adea0..c230889f0 100644 --- a/src/Helpers_TEST.cc +++ b/src/Helpers_TEST.cc @@ -429,6 +429,9 @@ TEST(HelpersTest, Volume) EXPECT_DOUBLE_EQ(GZ_SPHERE_VOLUME(0.1), 4.0*GZ_PI*std::pow(.1, 3)/3.0); EXPECT_DOUBLE_EQ(GZ_SPHERE_VOLUME(-1.1), 4.0*GZ_PI*std::pow(-1.1, 3)/3.0); + EXPECT_DOUBLE_EQ(GZ_CONE_VOLUME(0.5, 2.0), 2 * GZ_PI * std::pow(.5, 2) / 3.0); + EXPECT_DOUBLE_EQ(GZ_CONE_VOLUME(1, -1), -1 * GZ_PI * std::pow(1, 2) / 3.0); + EXPECT_DOUBLE_EQ(GZ_CYLINDER_VOLUME(0.5, 2.0), 2 * GZ_PI * std::pow(.5, 2)); EXPECT_DOUBLE_EQ(GZ_CYLINDER_VOLUME(1, -1), -1 * GZ_PI * std::pow(1, 2)); diff --git a/src/MassMatrix3_TEST.cc b/src/MassMatrix3_TEST.cc index b9c91e1b7..f51b48361 100644 --- a/src/MassMatrix3_TEST.cc +++ b/src/MassMatrix3_TEST.cc @@ -831,6 +831,57 @@ TEST(MassMatrix3dTest, EquivalentBox) } } +///////////////////////////////////////////////// +TEST(MassMatrix3dTest, SetFromConeZ) +{ + const math::Quaterniond q0 = math::Quaterniond::Identity; + + // Default mass matrix with non-positive inertia + { + math::MassMatrix3d m; + + // input is all zeros, so SetFromConeZ should fail + EXPECT_FALSE(m.SetFromConeZ(0, 0, 0, q0)); + EXPECT_FALSE(m.SetFromConeZ(0, 0, q0)); + + // even if some parameters are valid, none should be set if they + // are not all valid + EXPECT_FALSE(m.SetFromConeZ(1, 0, 0, q0)); + EXPECT_FALSE(m.SetFromConeZ(1, 1, 0, q0)); + EXPECT_FALSE(m.SetFromConeZ(1, 0, 1, q0)); + EXPECT_DOUBLE_EQ(m.Mass(), 0.0); + } + + // unit cone with mass 1.0 + { + const double mass = 1.0; + const double length = 1.0; + const double radius = 0.5; + math::MassMatrix3d m; + EXPECT_TRUE(m.SetFromConeZ(mass, length, radius, q0)); + + double ixx = (3.0 / 80.0) * mass * (4.0 * std::pow(radius, 2) + + std::pow(length, 2)); + double iyy = ixx; + double izz = (3.0 / 10.0) * mass * std::pow(radius, 2); + const math::Vector3d ixxyyzz(ixx, iyy, izz); + EXPECT_EQ(m.DiagonalMoments(), ixxyyzz); + EXPECT_EQ(m.OffDiagonalMoments(), math::Vector3d::Zero); + + double density = mass / (GZ_PI * radius * radius * length); + math::Material mat(density); + EXPECT_DOUBLE_EQ(density, mat.Density()); + math::MassMatrix3d m1; + EXPECT_FALSE(m1.SetFromConeZ(math::Material(0), length, radius)); + EXPECT_TRUE(m1.SetFromConeZ(mat, length, radius)); + EXPECT_EQ(m, m1); + + // double the length and radius + EXPECT_TRUE(m.SetFromConeZ(mass, 2*length, 2*radius, q0)); + EXPECT_EQ(m.DiagonalMoments(), 4*ixxyyzz); + } +} + ///////////////////////////////////////////////// TEST(MassMatrix3dTest, SetFromCylinderZ) { diff --git a/src/python_pybind11/CMakeLists.txt b/src/python_pybind11/CMakeLists.txt index 90d1dcacb..6f9fb755a 100644 --- a/src/python_pybind11/CMakeLists.txt +++ b/src/python_pybind11/CMakeLists.txt @@ -118,6 +118,7 @@ if (BUILD_TESTING) Box_TEST Capsule_TEST Color_TEST + Cone_TEST Cylinder_TEST DiffDriveOdometry_TEST Ellipsoid_TEST diff --git a/src/python_pybind11/src/Cone.hh b/src/python_pybind11/src/Cone.hh new file mode 100644 index 000000000..c2a11d6cb --- /dev/null +++ b/src/python_pybind11/src/Cone.hh @@ -0,0 +1,125 @@ +/* + * Copyright 2024 CogniPilot Foundation + * Copyright 2024 Open Source Robotics Foundation + * Copyright 2024 Rudis Laboratories + * + * 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_MATH_PYTHON__CONE_HH_ +#define GZ_MATH_PYTHON__CONE_HH_ + +#include + +#include +#include +#include + +#include + +namespace py = pybind11; +using namespace pybind11::literals; + +namespace gz +{ +namespace math +{ +namespace python +{ +/// Define a pybind11 wrapper for a gz::math::Cone +/** + * \param[in] module a pybind11 module to add the definition to + * \param[in] typestr name of the type used by Python + */ +template +void defineMathCone(py::module &m, const std::string &typestr) +{ + + using Class = gz::math::Cone; + std::string pyclass_name = typestr; + py::class_(m, + pyclass_name.c_str(), + py::buffer_protocol(), + py::dynamic_attr()) + .def(py::init<>()) + .def(py::init&>(), + py::arg("_length") = 0, + py::arg("_radius") = 0, + py::arg("_rotOffset") = gz::math::Quaternion::Identity) + .def(py::init&>(), + py::arg("_length") = 0, + py::arg("_radius") = 0, + py::arg("_material") = gz::math::Material(), + py::arg("_rotOffset") = gz::math::Quaternion::Identity) + .def(py::self == py::self) + .def("radius", + &Class::Radius, + "Get the radius in meters.") + .def("set_radius", + &Class::SetRadius, + "Set the radius in meters.") + .def("length", + &Class::Length, + "Get the length in meters.") + .def("set_length", + &Class::SetLength, + "Set the length in meters.") + .def("rotational_offset", + &Class::RotationalOffset, + "Get the rotation offset.") + .def("set_rotational_offset", + &Class::SetRotationalOffset, + "Set the rotation offset.") + .def("mat", + &Class::Mat, + "Get the material associated with this box.") + .def("set_mat", + &Class::SetMat, + "Set the material associated with this box.") + .def("volume", + &Class::Volume, + "Get the volume of the box in m^3.") + .def("density_from_mass", + &Class::DensityFromMass, + "Compute the box's density given a mass value.") + .def("set_density_from_mass", + &Class::SetDensityFromMass, + "Set the density of this box based on a mass value.") + .def("mass_matrix", + py::overload_cast<>(&Class::MassMatrix, py::const_), + "Get the mass matrix for this box. This function " + "is only meaningful if the box's size and material " + "have been set.") + .def("mass_matrix", + py::overload_cast&> + (&Class::MassMatrix, py::const_), + "Get the mass matrix for this box. This function " + "is only meaningful if the box's size and material " + "have been set.") + .def("__copy__", [](const Class &self) { + return Class(self); + }) + .def("__deepcopy__", [](const Class &self, py::dict) { + return Class(self); + }, "memo"_a); +} + +} // namespace python +} // namespace math +} // namespace gz + +#endif // GZ_MATH_PYTHON__CONE_HH_ diff --git a/src/python_pybind11/src/Helpers.cc b/src/python_pybind11/src/Helpers.cc index d012d0a60..0070e1eb4 100644 --- a/src/python_pybind11/src/Helpers.cc +++ b/src/python_pybind11/src/Helpers.cc @@ -41,6 +41,15 @@ float SphereVolume(const float _radius) return GZ_SPHERE_VOLUME(_radius); } +/// \brief Compute cone volume +/// \param[in] _r Cone base radius +/// \param[in] _l Cone length +/// \return cone volume +float ConeVolume(const float _r, const float _l) +{ + return GZ_CONE_VOLUME(_r, _l); +} + /// \brief Compute cylinder volume /// \param[in] _r Cylinder base radius /// \param[in] _l Cylinder length @@ -172,6 +181,9 @@ void defineMathHelpers(py::module &m) .def("gz_sphere_volume", &SphereVolume, "Compute sphere volume") + .def("gz_cone_volume", + &ConeVolume, + "Compute cone volume") .def("gz_cylinder_volume", &CylinderVolume, "Compute cylinder volume") diff --git a/src/python_pybind11/src/MassMatrix3.hh b/src/python_pybind11/src/MassMatrix3.hh index dab0a1ebf..afe17f71b 100644 --- a/src/python_pybind11/src/MassMatrix3.hh +++ b/src/python_pybind11/src/MassMatrix3.hh @@ -114,6 +114,35 @@ void helpDefineMathMassMatrix3(py::module &m, const std::string &typestr) py::arg("_size") = gz::math::Vector3::Zero, py::arg("_rot") = gz::math::Quaternion::Identity, "Set inertial properties based on a Material and equivalent box.") + .def("set_from_cone_z", + py::overload_cast&> + (&Class::SetFromConeZ), + py::arg("_mat") = gz::math::Material(), + py::arg("_length") = 0, + py::arg("_radius") = 0, + py::arg("_rot") = gz::math::Quaternion::Identity, + "Set inertial properties based on a Material and equivalent " + "cone aligned with Z axis.") + .def("set_from_cone_z", + py::overload_cast&> + (&Class::SetFromConeZ), + py::arg("_mass") = 0, + py::arg("_length") = 0, + py::arg("_radius") = 0, + py::arg("_rot") = gz::math::Quaternion::Identity, + "Set inertial properties based on a Material and equivalent " + "cone aligned with Z axis.") + .def("set_from_cone_z", + py::overload_cast&> + (&Class::SetFromConeZ), + py::arg("_length") = 0, + py::arg("_radius") = 0, + py::arg("_rot") = gz::math::Quaternion::Identity, + "Set inertial properties based on a Material and equivalent " + "cone aligned with Z axis.") .def("set_from_cylinder_z", py::overload_cast&> diff --git a/src/python_pybind11/src/_gz_math_pybind11.cc b/src/python_pybind11/src/_gz_math_pybind11.cc index 432b049bd..8a82494ca 100644 --- a/src/python_pybind11/src/_gz_math_pybind11.cc +++ b/src/python_pybind11/src/_gz_math_pybind11.cc @@ -19,6 +19,7 @@ #include "Box.hh" #include "Capsule.hh" #include "Color.hh" +#include "Cone.hh" #include "Cylinder.hh" #include "DiffDriveOdometry.hh" #include "Ellipsoid.hh" @@ -165,6 +166,8 @@ PYBIND11_MODULE(BINDINGS_MODULE_NAME, m) gz::math::python::defineMathSphere(m, "Sphered"); + gz::math::python::defineMathCone(m, "Coned"); + gz::math::python::defineMathCylinder(m, "Cylinderd"); gz::math::python::defineMathInertial(m, "Inertiald"); diff --git a/src/python_pybind11/test/Cone_TEST.py b/src/python_pybind11/test/Cone_TEST.py new file mode 100644 index 000000000..412952a69 --- /dev/null +++ b/src/python_pybind11/test/Cone_TEST.py @@ -0,0 +1,126 @@ +# Copyright 2024 CogniPilot Foundation +# Copyright 2024 Open Source Robotics Foundation +# Copyright 2024 Rudis Laboratories +# +# 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. + +import math +import unittest + +import gz +from gz.math7 import Coned, MassMatrix3d, Material, Quaterniond + + +class TestCone(unittest.TestCase): + + def test_constructor(self): + # Default constructor + cone = Coned() + self.assertEqual(0.0, cone.length()) + self.assertEqual(0.0, cone.radius()) + self.assertEqual(Quaterniond.IDENTITY, cone.rotational_offset()) + self.assertEqual(Material(), cone.mat()) + + cone2 = Coned() + self.assertEqual(cone, cone2) + + # Length and radius constructor + cone = Coned(1.0, 2.0) + self.assertEqual(1.0, cone.length()) + self.assertEqual(2.0, cone.radius()) + self.assertEqual(Quaterniond.IDENTITY, cone.rotational_offset()) + self.assertEqual(Material(), cone.mat()) + + cone2 = Coned(1.0, 2.0) + self.assertEqual(cone, cone2) + + # Length, radius, and rot constructor + cone = Coned(1.0, 2.0, Quaterniond(0.1, 0.2, 0.3)) + self.assertEqual(1.0, cone.length()) + self.assertEqual(2.0, cone.radius()) + self.assertEqual(Quaterniond(0.1, 0.2, 0.3), + cone.rotational_offset()) + self.assertEqual(Material(), cone.mat()) + + cone2 = Coned(1.0, 2.0, Quaterniond(0.1, 0.2, 0.3)) + self.assertEqual(cone, cone2) + + # Length, radius, mat and rot constructor + cone = Coned(1.0, 2.0, Material(gz.math7.MaterialType.WOOD), + Quaterniond(0.1, 0.2, 0.3)) + self.assertEqual(1.0, cone.length()) + self.assertEqual(2.0, cone.radius()) + self.assertEqual(Quaterniond(0.1, 0.2, 0.3), cone.rotational_offset()) + self.assertEqual(Material(gz.math7.MaterialType.WOOD), cone.mat()) + + cone2 = Coned(1.0, 2.0, Material(gz.math7.MaterialType.WOOD), + Quaterniond(0.1, 0.2, 0.3)) + self.assertEqual(cone, cone2) + + def test_mutators(self): + cone = Coned() + self.assertEqual(0.0, cone.length()) + self.assertEqual(0.0, cone.radius()) + self.assertEqual(Quaterniond.IDENTITY, cone.rotational_offset()) + self.assertEqual(Material(), cone.mat()) + + cone.set_length(100.1) + cone.set_radius(.123) + cone.set_rotational_offset(Quaterniond(1.2, 2.3, 3.4)) + cone.set_mat(Material(gz.math7.MaterialType.PINE)) + + self.assertEqual(100.1, cone.length()) + self.assertEqual(.123, cone.radius()) + self.assertEqual(Quaterniond(1.2, 2.3, 3.4), cone.rotational_offset()) + self.assertEqual(Material(gz.math7.MaterialType.PINE), cone.mat()) + + def test_volume_and_density(self): + mass = 1.0 + cone = Coned(1.0, 0.001) + expectedVolume = (math.pi * math.pow(0.001, 2) * 1.0) + self.assertEqual(expectedVolume, cone.volume()) + + expectedDensity = mass / expectedVolume + self.assertEqual(expectedDensity, cone.density_from_mass(mass)) + + # Bad density + cone2 = Coned() + self.assertGreater(0.0, cone2.density_from_mass(mass)) + + def test_mass(self): + mass = 2.0 + length = 2.0 + r = 0.1 + cone = Coned(length, r) + cone.set_density_from_mass(mass) + + massMat = MassMatrix3d() + ixxIyy = (3/80.0) * mass * (4*r*r + length*length) + izz = (3/10.0) * mass * r * r + + expectedMassMat = MassMatrix3d() + expectedMassMat.set_inertia_matrix(ixxIyy, ixxIyy, izz, 0.0, 0.0, 0.0) + expectedMassMat.set_mass(mass) + + cone.mass_matrix(massMat) + self.assertEqual(expectedMassMat, massMat) + self.assertEqual(expectedMassMat.mass(), massMat.mass()) + + massMat2 = cone.mass_matrix() + self.assertEqual(expectedMassMat, massMat2) + self.assertEqual(expectedMassMat.diagonal_moments(), massMat2.diagonal_moments()) + self.assertEqual(expectedMassMat.mass(), massMat2.mass()) + + +if __name__ == '__main__': + unittest.main() diff --git a/src/python_pybind11/test/Helpers_TEST.py b/src/python_pybind11/test/Helpers_TEST.py index 1836432bd..840c23570 100644 --- a/src/python_pybind11/test/Helpers_TEST.py +++ b/src/python_pybind11/test/Helpers_TEST.py @@ -16,7 +16,7 @@ import unittest from gz.math8 import (Helpers, gz_box_volume, gz_box_volume_v, gz_cylinder_volume, - gz_sphere_volume, Vector3d, equal, fixnan, + gz_cone_volume, gz_sphere_volume, Vector3d, equal, fixnan, greater_or_near_equal, is_even, is_odd, is_power_of_two, isnan, is_time_string, less_or_near_equal, max, mean, min, parse_float, parse_int, precision, round_up_multiple, diff --git a/src/ruby/Cone.i b/src/ruby/Cone.i new file mode 100644 index 000000000..caee3fa54 --- /dev/null +++ b/src/ruby/Cone.i @@ -0,0 +1,80 @@ +/* + * Copyright 2024 CogniPilot Foundation + * Copyright 2024 Open Source Robotics Foundation + * Copyright 2024 Rudis Laboratories + * + * 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. + * +*/ + +%module cone +%{ +#include +#include +#include +#include +#include +%} + +namespace gz +{ + namespace math + { + template + class Cone + { + %rename("%(undercase)s", %$isfunction, notregexmatch$name="^[A-Z]*$") ""; + + public: Cone() = default; + + public: Cone(const Precision _length, const Precision _radius, + const gz::math::Quaternion &_rotOffset = + gz::math::Quaternion::Identity); + + public: Cone(const Precision _length, const Precision _radius, + const gz::math::Material &_mat, + const gz::math::Quaternion &_rotOffset = + gz::math::Quaternion::Identity); + + public: ~Cone() = default; + + public: Precision Radius() const; + + public: void SetRadius(const Precision _radius); + + public: Precision Length() const; + + public: void SetLength(const Precision _length); + + public: gz::math::Quaternion RotationalOffset() const; + + public: void SetRotationalOffset( + const gz::math::Quaternion &_rotOffset); + + public: const gz::math::Material &Mat() const; + + public: void SetMat(const gz::math::Material &_mat); + + public: bool MassMatrix(gz::math::MassMatrix3 &_massMat) const; + + public: bool operator==(const Cone &_cone) const; + + public: Precision Volume() const; + + public: Precision DensityFromMass(const Precision _mass) const; + + public: bool SetDensityFromMass(const Precision _mass); + }; + %template(Coned) Cone; + } +} diff --git a/src/ruby/Helpers.i b/src/ruby/Helpers.i index 1921334f1..c61556402 100644 --- a/src/ruby/Helpers.i +++ b/src/ruby/Helpers.i @@ -47,6 +47,9 @@ import math def gz_sphere_volume(_radius): return (4.0*GZ_PI*math.pow(_radius, 3)/3.0) +def gz_cone_volume(_r, _l): + return (_l * GZ_PI * math.pow(_r, 2)/3.0) + def gz_cylinder_volume(_r, _l): return (_l * GZ_PI * math.pow(_r, 2)) diff --git a/src/ruby/MassMatrix3.i b/src/ruby/MassMatrix3.i index 4e13f035b..6ef205b4e 100644 --- a/src/ruby/MassMatrix3.i +++ b/src/ruby/MassMatrix3.i @@ -99,10 +99,21 @@ namespace gz public: bool SetFromCylinderZ(const T _mass, const T _length, const T _radius, - const Quaternion &_rot = Quaternion::Identity); + const Quaternion &_rot = Quaternion::Identity); public: bool SetFromCylinderZ(const T _length, const T _radius, const Quaternion &_rot); + public: bool SetFromConeZ(const Material &_mat, + const T _length, + const T _radius, + const Quaternion &_rot = Quaternion::Identity); + public: bool SetFromConeZ(const T _mass, + const T _length, + const T _radius, + const Quaternion &_rot = Quaternion::Identity); + public: bool SetFromConeZ(const T _length, + const T _radius, + const Quaternion &_rot); public: bool SetFromSphere(const Material &_mat, const T _radius); public: bool SetFromSphere(const T _mass, const T _radius); public: bool SetFromSphere(const T _radius);