From 1fd7435256d78364d2ec901f4fd261f0f8ce69f2 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Thu, 4 Jul 2024 03:49:32 -0700 Subject: [PATCH] Remove APIs deprecated in gz-math7 (#601) Signed-off-by: Steve Peters --- Migration.md | 2 + include/gz/math/Angle.hh | 10 --- include/gz/math/Matrix3.hh | 45 ---------- include/gz/math/Pose3.hh | 63 -------------- include/gz/math/Quaternion.hh | 111 ------------------------ include/gz/math/SphericalCoordinates.hh | 16 ---- src/Angle.cc | 12 --- src/Pose_TEST.cc | 32 ------- src/Quaternion_TEST.cc | 10 --- src/SphericalCoordinates.cc | 13 --- src/python_pybind11/src/Pose3.hh | 5 -- src/python_pybind11/test/Pose3_TEST.py | 54 ------------ 12 files changed, 2 insertions(+), 371 deletions(-) diff --git a/Migration.md b/Migration.md index 1adfc0fdd..e5444ebd5 100644 --- a/Migration.md +++ b/Migration.md @@ -13,6 +13,8 @@ release will remove the deprecated code. header files, the `ignition::` namespaces, and `IGN_*` and `IGNITION_*` macros. +1. Deprecated functionality from version 7 has been removed. + ### Deprecations 1. **Stopwatch.hh** diff --git a/include/gz/math/Angle.hh b/include/gz/math/Angle.hh index 635894581..114e1e870 100644 --- a/include/gz/math/Angle.hh +++ b/include/gz/math/Angle.hh @@ -96,20 +96,10 @@ namespace gz { } - /// \brief Set the value from an angle in radians. - /// \param[in] _radian Radian value. - /// \deprecated Use void SetRadian(double) - public: void GZ_DEPRECATED(7) Radian(double _radian); - /// \brief Set the value from an angle in radians. /// \param[in] _radian Radian value. public: void SetRadian(double _radian); - /// \brief Set the value from an angle in degrees - /// \param[in] _degree Degree value - /// \deprecated Use void SetDegree(double) - public: void GZ_DEPRECATED(7) Degree(double _degree); - /// \brief Set the value from an angle in degrees /// \param[in] _degree Degree value public: void SetDegree(double _degree); diff --git a/include/gz/math/Matrix3.hh b/include/gz/math/Matrix3.hh index 761f387a1..92bcb6d40 100644 --- a/include/gz/math/Matrix3.hh +++ b/include/gz/math/Matrix3.hh @@ -185,19 +185,6 @@ namespace gz this->data[2][2] = _v22; } - /// \brief Set the matrix from three axis (1 per column). - /// \param[in] _xAxis The x axis, the first column of the matrix. - /// \param[in] _yAxis The y axis, the second column of the matrix. - /// \param[in] _zAxis The z axis, the third column of the matrix. - /// \deprecated Use SetAxes(const Vector3 &, const Vector3 &, - /// const Vector3 &,) - public: void GZ_DEPRECATED(7) Axes(const Vector3 &_xAxis, - const Vector3 &_yAxis, - const Vector3 &_zAxis) - { - this->SetAxes(_xAxis, _yAxis, _zAxis); - } - /// \brief Set the matrix from three axis (1 per column). /// \param[in] _xAxis The x axis, the first column of the matrix. /// \param[in] _yAxis The y axis, the second column of the matrix. @@ -211,15 +198,6 @@ namespace gz this->SetCol(2, _zAxis); } - /// \brief Set as a rotation matrix from an axis and angle. - /// \param[in] _axis the axis - /// \param[in] _angle ccw rotation around the axis in radians - /// \deprecated Use SetFromAxisAngle(const Vector3 &, T) - public: void GZ_DEPRECATED(7) Axis(const Vector3 &_axis, T _angle) - { - this->SetFromAxisAngle(_axis, _angle); - } - /// \brief Set as a rotation matrix from an axis and angle. /// \param[in] _axis the axis /// \param[in] _angle ccw rotation around the axis in radians @@ -242,19 +220,6 @@ namespace gz this->data[2][2] = _axis.Z()*_axis.Z()*C + c; } - /// \brief Set as a rotation matrix to represent rotation from - /// vector _v1 to vector _v2, so that - /// _v2.Normalize() == this * _v1.Normalize() holds. - /// - /// \param[in] _v1 The first vector - /// \param[in] _v2 The second vector - /// \deprecated Use SetFrom2Axes(const Vector3 &, const Vector3 &) - public: void GZ_DEPRECATED(7) From2Axes( - const Vector3 &_v1, const Vector3 &_v2) - { - this->SetFrom2Axes(_v1, _v2); - } - /// \brief Set as a rotation matrix to represent rotation from /// vector _v1 to vector _v2, so that /// _v2.Normalize() == this * _v1.Normalize() holds. @@ -298,16 +263,6 @@ namespace gz this->SetFromAxisAngle(cross, acos(dot)); } - /// \brief Set a column. - /// \param[in] _c The colum index [0, 1, 2]. _col is clamped to the - /// range [0, 2]. - /// \param[in] _v The value to set in each row of the column. - /// \deprecated Use SetCol(unsigned int _c, const Vector3 &_v) - public: void GZ_DEPRECATED(7) Col(unsigned int _c, const Vector3 &_v) - { - this->SetCol(_c, _v); - } - /// \brief Set a column. /// \param[in] _c The colum index [0, 1, 2]. _col is clamped to the /// range [0, 2]. diff --git a/include/gz/math/Pose3.hh b/include/gz/math/Pose3.hh index 31cb748f7..c6e8935e3 100644 --- a/include/gz/math/Pose3.hh +++ b/include/gz/math/Pose3.hh @@ -177,69 +177,6 @@ namespace gz return Pose3(inv * (this->p*-1), inv); } - /// \brief Addition operator. - /// A is the transform from O to P specified in frame O - /// B is the transform from P to Q specified in frame P - /// then, B + A is the transform from O to Q specified in frame O - /// \param[in] _pose Pose3 to add to this pose. - /// \return The resulting pose. - public: GZ_DEPRECATED(7) Pose3 operator+(const Pose3 &_pose) const - { - Pose3 result; - - result.p = this->CoordPositionAdd(_pose); - result.q = this->CoordRotationAdd(_pose.q); - - return result; - } - - /// \brief Addition assignment operator. - /// \param[in] _pose Pose3 to add to this pose. - /// \sa operator+(const Pose3 &_pose) const. - /// \return The resulting pose. - public: GZ_DEPRECATED(7) const Pose3 & - operator+=(const Pose3 &_pose) - { - this->p = this->CoordPositionAdd(_pose); - this->q = this->CoordRotationAdd(_pose.q); - - return *this; - } - - /// \brief Negation operator. - /// A is the transform from O to P in frame O - /// then -A is transform from P to O specified in frame P - /// \return The resulting pose. - public: GZ_DEPRECATED(7) Pose3 operator-() const - { - return this->Inverse(); - } - - /// \brief Subtraction operator. - /// A is the transform from O to P in frame O - /// B is the transform from O to Q in frame O - /// B - A is the transform from P to Q in frame P - /// \param[in] _pose Pose3 to subtract from this one. - /// \return The resulting pose. - public: GZ_DEPRECATED(7) Pose3 operator-(const Pose3 &_pose) const - { - return Pose3(this->CoordPositionSub(_pose), - this->CoordRotationSub(_pose.q)); - } - - /// \brief Subtraction assignment operator. - /// \param[in] _pose Pose3 to subtract from this one - /// \sa operator-(const Pose3 &_pose) const. - /// \return The resulting pose - public: GZ_DEPRECATED(7) const Pose3 & - operator-=(const Pose3 &_pose) - { - this->p = this->CoordPositionSub(_pose); - this->q = this->CoordRotationSub(_pose.q); - - return *this; - } - /// \brief Equality operator. /// \param[in] _pose Pose3 for comparison. /// \return True if this pose is equal to the given pose. diff --git a/include/gz/math/Quaternion.hh b/include/gz/math/Quaternion.hh index 635e75deb..724d9afcd 100644 --- a/include/gz/math/Quaternion.hh +++ b/include/gz/math/Quaternion.hh @@ -295,17 +295,6 @@ namespace gz return result; } - /// \brief Set the quaternion from an axis and angle - /// \param[in] _ax X axis - /// \param[in] _ay Y axis - /// \param[in] _az Z axis - /// \param[in] _aa Angle in radians - /// \deprecated Use SetFromAxisAngle(T, T, T, T) - public: void GZ_DEPRECATED(7) Axis(T _ax, T _ay, T _az, T _aa) - { - this->SetFromAxisAngle(_ax, _ay, _az, _aa); - } - /// \brief Set the quaternion from an axis and angle. /// \param[in] _ax X axis /// \param[in] _ay Y axis @@ -337,15 +326,6 @@ namespace gz this->Normalize(); } - /// \brief Set the quaternion from an axis and angle - /// \param[in] _axis Axis - /// \param[in] _a Angle in radians - /// \deprecated Use SetFromAxisAngle(const Vector3 &_axis, T _a) - public: void GZ_DEPRECATED(7) Axis(const Vector3 &_axis, T _a) - { - this->SetFromAxisAngle(_axis, _a); - } - /// \brief Set the quaternion from an axis and angle /// \param[in] _axis Axis /// \param[in] _a Angle in radians @@ -367,17 +347,6 @@ namespace gz this->qz = _z; } - /// \brief Set the quaternion from Euler angles. The order of operations - /// is roll, pitch, yaw around a fixed body frame axis - /// (the original frame of the object before rotation is applied). - /// Roll is a rotation about x, pitch is about y, yaw is about z. - /// \param[in] _vec Euler angle - /// \deprecated Use SetFromEuler(const Vector3 &) - public: void GZ_DEPRECATED(7) Euler(const Vector3 &_vec) - { - this->SetFromEuler(_vec); - } - /// \brief Set the quaternion from Euler angles. The order of operations /// is roll, pitch, yaw around a fixed body frame axis /// (the original frame of the object before rotation is applied). @@ -388,16 +357,6 @@ namespace gz this->SetFromEuler(_vec.X(), _vec.Y(), _vec.Z()); } - /// \brief Set the quaternion from Euler angles. - /// \param[in] _roll Roll angle (radians). - /// \param[in] _pitch Pitch angle (radians). - /// \param[in] _yaw Yaw angle (radians). - /// \deprecated Use SetFromEuler(T, T, T) - public: void GZ_DEPRECATED(7) Euler(T _roll, T _pitch, T _yaw) - { - this->SetFromEuler(_roll, _pitch, _yaw); - } - /// \brief Set the quaternion from Euler angles. /// \param[in] _roll Roll angle in radians. /// \param[in] _pitch Pitch angle in radians. @@ -531,15 +490,6 @@ namespace gz return this->Euler().Z(); } - /// \brief Return rotation as axis and angle - /// \param[out] _axis rotation axis - /// \param[out] _angle ccw angle in radians - /// \deprecated Use AxisAngle(Vector3 &_axis, T &_angle) const - public: void GZ_DEPRECATED(7) ToAxis(Vector3 &_axis, T &_angle) const - { - this->AxisAngle(_axis, _angle); - } - /// \brief Convert this quaternion to an axis and angle. /// \param[out] _axis Rotation axis. /// \param[out] _angle CCW angle in radians. @@ -559,19 +509,6 @@ namespace gz } } - /// \brief Set from a rotation matrix. - /// \param[in] _mat rotation matrix (must be orthogonal, the function - /// doesn't check it) - /// - /// Implementation inspired by - /// http://www.euclideanspace.com/maths/geometry/rotations/ - /// conversions/matrixToQuaternion/ - /// \deprecated Use SetFromMatrix(const Matrix3&) - public: void GZ_DEPRECATED(7) Matrix(const Matrix3 &_mat) - { - this->SetFromMatrix(_mat); - } - /// \brief Set from a rotation matrix. /// \param[in] _mat Rotation matrix (must be orthogonal, the function /// doesn't check it). @@ -616,22 +553,6 @@ namespace gz } } - /// \brief Set this quaternion to represent rotation from - /// vector _v1 to vector _v2, so that - /// _v2.Normalize() == this * _v1.Normalize() holds. - /// - /// \param[in] _v1 The first vector. - /// \param[in] _v2 The second vector. - /// - /// Implementation inspired by - /// http://stackoverflow.com/a/11741520/1076564 - /// \deprecated Use SetFrom2Axes(const Vector3 &, const Vector3 &) - public: void GZ_DEPRECATED(7) From2Axes( - const Vector3 &_v1, const Vector3 &_v2) - { - this->SetFrom2Axes(_v1, _v2); - } - /// \brief Set this quaternion to represent rotation from /// vector _v1 to vector _v2, so that /// _v2.Normalize() == this * _v1.Normalize() holds. @@ -1131,14 +1052,6 @@ namespace gz return this->qz; } - /// \brief Set the x component. - /// \param[in] _v The new value for the x quaternion component. - /// \deprecated Use SetX(T) - public: inline void GZ_DEPRECATED(7) X(T _v) - { - this->SetX(_v); - } - /// \brief Set the x component. /// \param[in] _v The new value for the x quaternion component. public: inline void SetX(T _v) @@ -1146,14 +1059,6 @@ namespace gz this->qx = _v; } - /// \brief Set the y component. - /// \param[in] _v The new value for the y quaternion component. - /// \deprecated Use SetY(T) - public: inline void GZ_DEPRECATED(7) Y(T _v) - { - this->SetY(_v); - } - /// \brief Set the y component. /// \param[in] _v The new value for the y quaternion component. public: inline void SetY(T _v) @@ -1162,14 +1067,6 @@ namespace gz } - /// \brief Set the z component. - /// \param[in] _v The new value for the z quaternion component. - /// \deprecated Use SetZ(T) - public: inline void GZ_DEPRECATED(7) Z(T _v) - { - this->SetZ(_v); - } - /// \brief Set the z component. /// \param[in] _v The new value for the z quaternion component. public: inline void SetZ(T _v) @@ -1177,14 +1074,6 @@ namespace gz this->qz = _v; } - /// \brief Set the w component. - /// \param[in] _v The new value for the w quaternion component. - /// \deprecated Use SetW(T) - public: inline void GZ_DEPRECATED(7) W(T _v) - { - this->SetW(_v); - } - /// \brief Set the w component. /// \param[in] _v The new value for the w quaternion component. public: inline void SetW(T _v) diff --git a/include/gz/math/SphericalCoordinates.hh b/include/gz/math/SphericalCoordinates.hh index 2808c738f..b8fba584a 100644 --- a/include/gz/math/SphericalCoordinates.hh +++ b/include/gz/math/SphericalCoordinates.hh @@ -144,22 +144,6 @@ namespace gz /// \return Type as string public: static std::string Convert(SurfaceType _type); - /// \brief Get the distance between two points expressed in geographic - /// latitude and longitude. It assumes that both points are at sea level. - /// Example: _latA = 38.0016667 and _lonA = -123.0016667) represents - /// the point with latitude 38d 0'6.00"N and longitude 123d 0'6.00"W. - /// \param[in] _latA Latitude of point A. - /// \param[in] _lonA Longitude of point A. - /// \param[in] _latB Latitude of point B. - /// \param[in] _lonB Longitude of point B. - /// \return Distance in meters. - /// \deprecated Use DistanceWGS84 instead. - public: GZ_DEPRECATED(7) static double Distance( - const gz::math::Angle &_latA, - const gz::math::Angle &_lonA, - const gz::math::Angle &_latB, - const gz::math::Angle &_lonB); - /// \brief Get the distance between two points expressed in geographic /// latitude and longitude. It assumes that both points are at sea level. /// Example: _latA = 38.0016667 and _lonA = -123.0016667) represents diff --git a/src/Angle.cc b/src/Angle.cc index 30c1e856c..fc566d03a 100644 --- a/src/Angle.cc +++ b/src/Angle.cc @@ -35,24 +35,12 @@ const Angle &Angle::Pi = gPi; const Angle &Angle::HalfPi = gHalfPi; const Angle &Angle::TwoPi = gTwoPi; -////////////////////////////////////////////////// -void Angle::Radian(double _radian) -{ - this->value = _radian; -} - ////////////////////////////////////////////////// void Angle::SetRadian(double _radian) { this->value = _radian; } -////////////////////////////////////////////////// -void Angle::Degree(double _degree) -{ - this->value = _degree * GZ_PI / 180.0; -} - ////////////////////////////////////////////////// void Angle::SetDegree(double _degree) { diff --git a/src/Pose_TEST.cc b/src/Pose_TEST.cc index 44bb4e8c5..5e8a55247 100644 --- a/src/Pose_TEST.cc +++ b/src/Pose_TEST.cc @@ -73,17 +73,6 @@ TEST(PoseTest, Pose) EXPECT_TRUE(math::equal((A * B).Rot().Euler().X(), 0.0)); EXPECT_TRUE(math::equal((A * B).Rot().Euler().Y(), 0.0)); EXPECT_TRUE(math::equal((A * B).Rot().Euler().Z(), 3.0*GZ_PI/4.0)); - - GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION - // Coverage for + operator - EXPECT_EQ(A * B, B + A); - EXPECT_NE(A * B, A + B); - - // Coverage for += operator - math::Pose3d C(B); - C += A; - EXPECT_EQ(C, A * B); - GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION } { // If: @@ -104,17 +93,6 @@ TEST(PoseTest, Pose) (A.Inverse() * math::Pose3d()).Rot().Euler().Y(), 0.0)); EXPECT_TRUE(math::equal( (A.Inverse() * math::Pose3d()).Rot().Euler().Z(), -GZ_PI/4)); - - GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION - // Coverage for unitary - operator - // test negation operator - EXPECT_TRUE(math::equal((-A).Pos().X(), -1.0/sqrt(2))); - EXPECT_TRUE(math::equal((-A).Pos().Y(), 1.0/sqrt(2))); - EXPECT_TRUE(math::equal((-A).Pos().Z(), 0.0)); - EXPECT_TRUE(math::equal((-A).Rot().Euler().X(), 0.0)); - EXPECT_TRUE(math::equal((-A).Rot().Euler().Y(), 0.0)); - EXPECT_TRUE(math::equal((-A).Rot().Euler().Z(), -GZ_PI/4.0)); - GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION } { // If: @@ -131,16 +109,6 @@ TEST(PoseTest, Pose) EXPECT_TRUE(math::equal((A.Inverse() * B).Rot().Euler().X(), 0.0)); EXPECT_TRUE(math::equal((A.Inverse() * B).Rot().Euler().Y(), 0.0)); EXPECT_TRUE(math::equal((A.Inverse() * B).Rot().Euler().Z(), GZ_PI/4.0)); - - GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION - // Coverage for - operator - EXPECT_EQ(A.Inverse() * B, B - A); - - // Coverage for -= operator - math::Pose3d C(B); - C -= A; - EXPECT_EQ(C, A.Inverse() * B); - GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION } { math::Pose3d pose; diff --git a/src/Quaternion_TEST.cc b/src/Quaternion_TEST.cc index 865e4bdc6..03db3f2c4 100644 --- a/src/Quaternion_TEST.cc +++ b/src/Quaternion_TEST.cc @@ -325,16 +325,6 @@ TEST(QuaternionTest, MathAxis) { math::Quaterniond q(GZ_PI*0.1, GZ_PI*0.5, GZ_PI); -GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION - // Deprecated in gz-math7 - q.Axis(0, 1, 0, GZ_PI); - EXPECT_EQ(q, math::Quaterniond(6.12303e-17, 0, 1, 0)); - - // Deprecated in gz-math7 - q.Axis(1, 0, 0, GZ_PI); - EXPECT_EQ(q, math::Quaterniond(0, 1, 0, 0)); -GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION - q.SetFromAxisAngle(0, 1, 0, GZ_PI); EXPECT_EQ(q, math::Quaterniond(6.12303e-17, 0, 1, 0)); diff --git a/src/SphericalCoordinates.cc b/src/SphericalCoordinates.cc index ed7f77eef..38d2989f2 100644 --- a/src/SphericalCoordinates.cc +++ b/src/SphericalCoordinates.cc @@ -427,19 +427,6 @@ double SphericalCoordinates::DistanceWGS84(const Angle &_latA, return d; } -////////////////////////////////////////////////// -/// Based on Haversine formula (http://en.wikipedia.org/wiki/Haversine_formula). -double SphericalCoordinates::Distance(const Angle &_latA, - const Angle &_lonA, - const Angle &_latB, - const Angle &_lonB) -{ - // LCOV_EXCL_START - return SphericalCoordinates::DistanceWGS84( - _latA, _lonA, _latB, _lonB); - // LCOV_EXCL_STOP -} - ////////////////////////////////////////////////// /// Based on Haversine formula (http://en.wikipedia.org/wiki/Haversine_formula). /// This takes into account the surface type. diff --git a/src/python_pybind11/src/Pose3.hh b/src/python_pybind11/src/Pose3.hh index 334287fe4..2b29af3ff 100644 --- a/src/python_pybind11/src/Pose3.hh +++ b/src/python_pybind11/src/Pose3.hh @@ -67,11 +67,6 @@ void helpDefineMathPose3(py::module &m, const std::string &typestr) .def(py::init()) .def(py::init()) .def(py::init()) - .def(py::self + py::self) - .def(py::self += py::self) - .def(-py::self) - .def(py::self - py::self) - .def(py::self -= py::self) .def(py::self == py::self) .def(py::self != py::self) .def(py::self * py::self) diff --git a/src/python_pybind11/test/Pose3_TEST.py b/src/python_pybind11/test/Pose3_TEST.py index 9bd37d915..b4fcc29c3 100644 --- a/src/python_pybind11/test/Pose3_TEST.py +++ b/src/python_pybind11/test/Pose3_TEST.py @@ -37,49 +37,6 @@ def test_pose(self): B = Pose3d(Vector3d(1, 0, 0), Quaterniond(0, 0, math.pi/2.0)) - # test hypothesis that if - # A is the transform from O to P specified in frame O - # B is the transform from P to Q specified in frame P - # then, B + A is the transform from O to Q specified in frame O - self.assertAlmostEqual((B + A).x(), 1.0 + 1.0/math.sqrt(2)) - self.assertAlmostEqual((B + A).y(), 1.0/math.sqrt(2)) - self.assertAlmostEqual((B + A).z(), 0.0) - self.assertAlmostEqual((B + A).roll(), 0.0) - self.assertAlmostEqual((B + A).pitch(), 0.0) - self.assertAlmostEqual((B + A).yaw(), 3.0*math.pi/4.0) - - # If: - # A is the transform from O to P in frame O - # B is the transform from O to Q in frame O - # then -A is transform from P to O specified in frame P - self.assertAlmostEqual((Pose3d() - A).x(), -1.0/math.sqrt(2)) - self.assertAlmostEqual((Pose3d() - A).y(), 1.0/math.sqrt(2)) - self.assertAlmostEqual((Pose3d() - A).z(), 0.0) - self.assertAlmostEqual((Pose3d() - A).roll(), 0.0) - self.assertAlmostEqual((Pose3d() - A).pitch(), 0.0) - self.assertAlmostEqual((Pose3d() - A).yaw(), -math.pi/4) - - # test negation operator - self.assertAlmostEqual((-A).x(), -1.0/math.sqrt(2)) - self.assertAlmostEqual((-A).y(), 1.0/math.sqrt(2)) - self.assertAlmostEqual((-A).z(), 0.0) - self.assertAlmostEqual((-A).roll(), 0.0) - self.assertAlmostEqual((-A).pitch(), 0.0) - self.assertAlmostEqual((-A).yaw(), -math.pi/4.0) - - # If: - # A is the transform from O to P in frame O - # B is the transform from O to Q in frame O - # B - A is the transform from P to Q in frame P - B = Pose3d(Vector3d(1, 1, 0), - Quaterniond(0, 0, math.pi/2.0)) - self.assertAlmostEqual((B - A).x(), 1.0/math.sqrt(2)) - self.assertAlmostEqual((B - A).y(), 1.0/math.sqrt(2)) - self.assertAlmostEqual((B - A).z(), 0.0) - self.assertAlmostEqual((B - A).roll(), 0.0) - self.assertAlmostEqual((B - A).pitch(), 0.0) - self.assertAlmostEqual((B - A).yaw(), math.pi/4.0) - pose = Pose3d() self.assertTrue(pose.pos() == Vector3d(0, 0, 0)) self.assertTrue(pose.rot() == Quaterniond(0, 0, 0)) @@ -101,17 +58,6 @@ def test_pose(self): self.assertTrue(pose1.rot() == Quaterniond(0.946281, -0.0933066, -0.226566, -0.210984)) - pose = Pose3d(1, 2, 3, .1, .2, .3) + Pose3d(4, 5, 6, .4, .5, .6) - self.assertTrue(pose == Pose3d(5.74534, 7.01053, 8.62899, - 0.675732, 0.535753, 1.01174)) - - pose += pose - self.assertTrue(pose == Pose3d(11.314, 16.0487, 15.2559, - 1.49463, 0.184295, 2.13932)) - - pose -= Pose3d(pose) - self.assertTrue(pose == Pose3d(0, 0, 0, 0, 0, 0)) - pose.pos().set(5, 6, 7) pose.rot().set_from_euler(Vector3d(.4, .6, 0)) self.assertTrue(pose.coord_position_add(Vector3d(1, 2, 3)) ==