From 0d94af7b615cd9ec5a1cde758fa610e62ef59ceb Mon Sep 17 00:00:00 2001 From: Silvio Traversaro Date: Sun, 17 Sep 2023 15:34:30 +0200 Subject: [PATCH] Remove warnings related to Raw classes --- bindings/iDynTree.i | 11 ++---- bindings/pybind11/idyntree_core.cpp | 39 +++++++------------ bindings/python/python.i | 20 +++++----- src/core/include/iDynTree/Direction.h | 5 +++ src/core/include/iDynTree/Position.h | 5 +++ src/core/include/iDynTree/Rotation.h | 1 + src/core/include/iDynTree/RotationalInertia.h | 3 +- src/core/include/iDynTree/SpatialAcc.h | 1 + .../include/iDynTree/SpatialForceVector.h | 10 ++--- src/core/include/iDynTree/SpatialMomentum.h | 1 + .../include/iDynTree/SpatialMotionVector.h | 10 ++--- src/core/include/iDynTree/SpatialVector.h | 9 +++++ src/core/include/iDynTree/Twist.h | 1 + src/core/include/iDynTree/Wrench.h | 1 + src/core/src/ClassicalAcc.cpp | 4 +- src/core/src/Direction.cpp | 7 ++++ .../src/InertiaNonLinearParametrization.cpp | 4 +- src/core/src/Position.cpp | 8 ++++ src/core/src/Rotation.cpp | 9 +++++ src/core/src/RotationalInertia.cpp | 12 ++++++ src/core/src/SpatialAcc.cpp | 7 ++++ src/core/src/SpatialMomentum.cpp | 7 ++++ src/core/src/TestUtils.cpp | 2 +- src/core/src/Transform.cpp | 4 +- src/core/src/Twist.cpp | 8 ++++ src/core/src/Wrench.cpp | 7 ++++ .../tests/ArticulatedBodyInertiaUnitTest.cpp | 2 +- src/core/tests/SpatialInertiaUnitTest.cpp | 2 +- .../tests/TransformFromMatrix4x4UnitTest.cpp | 10 ++--- .../ExternalWrenchesEstimationUnitTest.cpp | 4 +- src/high-level/src/KinDynComputations.cpp | 6 +-- src/model/include/iDynTree/ModelTestUtils.h | 2 +- src/model/include/iDynTree/SubModel.h | 1 - src/model/src/DenavitHartenberg.cpp | 2 +- src/model/tests/LinkUnitTest.cpp | 2 +- src/model/tests/ModelUnitTest.cpp | 4 +- .../codecs/include/private/InertialElement.h | 2 +- src/model_io/codecs/src/InertialElement.cpp | 2 +- src/model_io/codecs/src/URDFModelExport.cpp | 2 +- .../InertialParametersSolidShapesHelpers.cpp | 4 +- .../DynamicsLinearizationIntegrationTest.cpp | 2 +- ...etersSolidShapesHelpersIntegrationTest.cpp | 2 +- 42 files changed, 158 insertions(+), 87 deletions(-) diff --git a/bindings/iDynTree.i b/bindings/iDynTree.i index e206389d6d1..494bcb9f0c5 100644 --- a/bindings/iDynTree.i +++ b/bindings/iDynTree.i @@ -49,7 +49,6 @@ #include "iDynTree/VectorFixSize.h" // Basic Vectors: Point Vectors and Spatial Vectors -#include "iDynTree/PositionRaw.h" #include "iDynTree/Position.h" #include "iDynTree/SpatialForceVector.h" #include "iDynTree/SpatialMotionVector.h" @@ -62,14 +61,13 @@ #include "iDynTree/Axis.h" // Inertias -#include "iDynTree/RotationalInertiaRaw.h" -#include "iDynTree/SpatialInertiaRaw.h" +#include "iDynTree/RotationalInertia.h" +#include "iDynTree/SpatialInertia.h" #include "iDynTree/SpatialInertia.h" #include "iDynTree/ArticulatedBodyInertia.h" #include "iDynTree/InertiaNonLinearParametrization.h" // Transformations: Rotation and Transform -#include "iDynTree/RotationRaw.h" #include "iDynTree/Rotation.h" #include "iDynTree/Transform.h" #include "iDynTree/TransformDerivative.h" @@ -184,7 +182,6 @@ namespace std { %template(Vector16) iDynTree::VectorFixSize<16>; // Basic Vectors: Point Vectors and Spatial Vectors -%include "iDynTree/PositionRaw.h" %include "iDynTree/Position.h" %include "iDynTree/GeomVector3.h" @@ -216,14 +213,12 @@ namespace std { %include "iDynTree/Axis.h" // Inertias -%include "iDynTree/RotationalInertiaRaw.h" -%include "iDynTree/SpatialInertiaRaw.h" +%include "iDynTree/RotationalInertia.h" %include "iDynTree/SpatialInertia.h" %include "iDynTree/ArticulatedBodyInertia.h" %include "iDynTree/InertiaNonLinearParametrization.h" // Transformations: Rotation and Transform -%include "iDynTree/RotationRaw.h" %include "iDynTree/Rotation.h" %include "iDynTree/Transform.h" %include "iDynTree/TransformDerivative.h" diff --git a/bindings/pybind11/idyntree_core.cpp b/bindings/pybind11/idyntree_core.cpp index c48df152eb0..0bcfc7a5865 100644 --- a/bindings/pybind11/idyntree_core.cpp +++ b/bindings/pybind11/idyntree_core.cpp @@ -5,10 +5,8 @@ #include #include #include -#include #include -#include -#include +#include #include #include #include @@ -197,23 +195,17 @@ void iDynTreeCoreBindings(pybind11::module& module) { createFixSizeMatrix<6, 6>(module, "Matrix6x6"); // Positions, Rotations and Transforms. - py::class_>(module, "_PositionRaw") - // Do not expose constructor as we do not want users to use this class. - .def("__repr__", &PositionRaw::toString); - - py::class_(module, "Position") + py::class_>(module, "Position") .def(py::init()) .def(py::init()) .def(py::self + py::self) .def(py::self - py::self) .def(-py::self) + .def("__repr__", &Position::toString) .def_static("Zero", &Position::Zero); - py::class_>(module, "_RotationRaw") - // Do not expose constructor as we do not want users to use this class. - .def("__repr__", &RotationRaw::toString); - py::class_(module, "Rotation") + py::class_>(module, "Rotation") .def(py::init()) .def(py::init>(module, - "RotationalInertia") + py::class_>(module, + "RotationalInertia") .def(py::init()); - py::class_(module, "_SpatialInertiaRaw") + py::class_(module, "SpatialInertia") + .def(py::init()) + .def(py::init()) .def("from_rotational_inertia_wrt_center_of_mass", - &SpatialInertiaRaw::fromRotationalInertiaWrtCenterOfMass) - .def("get_mass", &SpatialInertiaRaw::getMass) - .def("get_center_of_mass", &SpatialInertiaRaw::getCenterOfMass) + &SpatialInertia::fromRotationalInertiaWrtCenterOfMass) + .def("get_mass", &SpatialInertia::getMass) + .def("get_center_of_mass", &SpatialInertia::getCenterOfMass) .def("get_rotational_inertia_wrt_frame_origin", - &SpatialInertiaRaw::getRotationalInertiaWrtFrameOrigin) + &SpatialInertia::getRotationalInertiaWrtFrameOrigin) .def("get_rotational_inertia_wrt_center_of_mass", - &SpatialInertiaRaw::getRotationalInertiaWrtCenterOfMass); - - py::class_(module, "SpatialInertia") - .def(py::init()) - .def(py::init()) + &SpatialInertia::getRotationalInertiaWrtCenterOfMass); .def_static("Zero", &SpatialInertia::Zero) .def("as_matrix", &SpatialInertia::asMatrix) .def(py::self + py::self) diff --git a/bindings/python/python.i b/bindings/python/python.i index 1e5033fec73..7cfa8beb8ae 100644 --- a/bindings/python/python.i +++ b/bindings/python/python.i @@ -324,13 +324,13 @@ import_array(); if (i != 3 || j != 3) throw std::runtime_error("Wrong size of input matrix"); - return iDynTree::RotationRaw(in, static_cast(i), static_cast(j)); + return iDynTree::Rotation(in, static_cast(i), static_cast(j)); } Rotation(const double* in_data, const std::ptrdiff_t in_rows, const std::ptrdiff_t in_cols) { - iDynTree::Rotation* rot = new iDynTree::Rotation(iDynTree::RotationRaw(in_data, - static_cast(in_rows), - static_cast(in_cols))); + iDynTree::Rotation* rot = new iDynTree::Rotation(in_data, + static_cast(in_rows), + static_cast(in_cols)); return rot; } }; @@ -340,11 +340,11 @@ import_array(); if (size != 3) throw std::runtime_error("Wrong size of input array"); - return iDynTree::PositionRaw(in, static_cast(size)); + return iDynTree::Position(in, static_cast(size)); } Position(const double* in_data, const unsigned in_size) { - iDynTree::Position* pos = new iDynTree::Position(iDynTree::PositionRaw(in_data, in_size)); + iDynTree::Position* pos = new iDynTree::Position(in_data, in_size); return pos; } }; @@ -352,15 +352,15 @@ import_array(); %extend iDynTree::Transform { void setPosition(const double* in_data, const unsigned in_size) { - $self->setPosition(iDynTree::PositionRaw(in_data, static_cast(in_size))); + $self->setPosition(iDynTree::Position(in_data, static_cast(in_size))); } void setRotation(const double* in_data, const std::ptrdiff_t in_rows, const std::ptrdiff_t in_cols) { - $self->setRotation(iDynTree::RotationRaw(in_data, - static_cast(in_rows), - static_cast(in_cols))); + $self->setRotation(iDynTree::Rotation(in_data, + static_cast(in_rows), + static_cast(in_cols))); } }; diff --git a/src/core/include/iDynTree/Direction.h b/src/core/include/iDynTree/Direction.h index fb50593b576..d9d4dc7d9b1 100644 --- a/src/core/include/iDynTree/Direction.h +++ b/src/core/include/iDynTree/Direction.h @@ -50,6 +50,11 @@ namespace iDynTree */ Direction(const double* in_data, const unsigned int in_size); + /** + * Assignment operator: assign a Direction from another Direction + */ + Direction& operator=(const Direction& other); + /** * Normalize the representation of the direction, useful if * the coordinates of the direction has been manually setted diff --git a/src/core/include/iDynTree/Position.h b/src/core/include/iDynTree/Position.h index df9a36f7b51..26ef89bde82 100644 --- a/src/core/include/iDynTree/Position.h +++ b/src/core/include/iDynTree/Position.h @@ -50,6 +50,11 @@ namespace iDynTree */ Position(const Position & other); + /** + * Assignment operator: assign a Position from another Position + */ + Position& operator=(const Position& other); + /** * Constructor from a raw buffer of 3 doubles. */ diff --git a/src/core/include/iDynTree/Rotation.h b/src/core/include/iDynTree/Rotation.h index 93d456586aa..7e5ac268e2e 100644 --- a/src/core/include/iDynTree/Rotation.h +++ b/src/core/include/iDynTree/Rotation.h @@ -110,6 +110,7 @@ namespace iDynTree /** * overloaded operators */ + Rotation& operator=(const Rotation & other); Rotation operator*(const Rotation & other) const; Rotation inverse() const; Position operator*(const Position & other) const; diff --git a/src/core/include/iDynTree/RotationalInertia.h b/src/core/include/iDynTree/RotationalInertia.h index 69f181ffbe9..6d614900287 100644 --- a/src/core/include/iDynTree/RotationalInertia.h +++ b/src/core/include/iDynTree/RotationalInertia.h @@ -11,7 +11,7 @@ namespace iDynTree class Position; /** - * Class providing the raw coordinates for a 3d inertia matrix. + * Class providing the coordinates for a 3d inertia matrix. * * \ingroup iDynTreeCore * @@ -35,6 +35,7 @@ namespace iDynTree RotationalInertia(); RotationalInertia(const double * in_data, const unsigned int in_rows, const unsigned int in_cols); RotationalInertia(const RotationalInertia & other); + RotationalInertia& operator=(const RotationalInertia& other); /** * Initializer helper: return a zero matrix. diff --git a/src/core/include/iDynTree/SpatialAcc.h b/src/core/include/iDynTree/SpatialAcc.h index aa385b34ed8..f7e10d9779c 100644 --- a/src/core/include/iDynTree/SpatialAcc.h +++ b/src/core/include/iDynTree/SpatialAcc.h @@ -32,6 +32,7 @@ namespace iDynTree SpatialAcc(const SpatialAcc& other); // overloaded operator + SpatialAcc& operator=(const SpatialAcc &other); SpatialAcc operator+(const SpatialAcc &other) const; SpatialAcc operator-(const SpatialAcc &other) const; SpatialAcc operator-() const; diff --git a/src/core/include/iDynTree/SpatialForceVector.h b/src/core/include/iDynTree/SpatialForceVector.h index 318c0858061..499cdce4ec1 100644 --- a/src/core/include/iDynTree/SpatialForceVector.h +++ b/src/core/include/iDynTree/SpatialForceVector.h @@ -1,8 +1,8 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef IDYNTREE_SPATIAL_FORCE_RAW_H -#define IDYNTREE_SPATIAL_FORCE_RAW_H +#ifndef IDYNTREE_SPATIAL_FORCE_VECTOR_H +#define IDYNTREE_SPATIAL_FORCE_VECTOR_H #include #include @@ -21,8 +21,8 @@ namespace iDynTree * * This is just a basic vector, used to implement the adjoint transformations in * a general way. The relative adjoint transformation is contained in - * TransformRaw::apply(SpatialForceRaw), - * for consistency with the iDynTree::PositionRaw class. + * Transform::apply(SpatialForce), + * for consistency with the iDynTree::Position class. * * \note in iDynTree, the spatial vector follows this serialization: the first three elements are * the linear part and the second three elements are the angular part. @@ -53,4 +53,4 @@ namespace iDynTree }; } -#endif /* IDYNTREE_SPATIAL_FORCE_RAW_H */ +#endif /* IDYNTREE_SPATIAL_FORCE_VECTOR_H */ diff --git a/src/core/include/iDynTree/SpatialMomentum.h b/src/core/include/iDynTree/SpatialMomentum.h index 4be72563529..25e42b7715e 100644 --- a/src/core/include/iDynTree/SpatialMomentum.h +++ b/src/core/include/iDynTree/SpatialMomentum.h @@ -30,6 +30,7 @@ namespace iDynTree SpatialMomentum(const SpatialMomentum & other); // overloaded operators + SpatialMomentum& operator=(const SpatialMomentum &other); SpatialMomentum operator+(const SpatialMomentum &other) const; SpatialMomentum operator-(const SpatialMomentum &other) const; SpatialMomentum operator-() const; diff --git a/src/core/include/iDynTree/SpatialMotionVector.h b/src/core/include/iDynTree/SpatialMotionVector.h index 17147e66f60..8688465fec8 100644 --- a/src/core/include/iDynTree/SpatialMotionVector.h +++ b/src/core/include/iDynTree/SpatialMotionVector.h @@ -1,8 +1,8 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef IDYNTREE_SPATIAL_MOTION_RAW_H -#define IDYNTREE_SPATIAL_MOTION_RAW_H +#ifndef IDYNTREE_SPATIAL_MOTION_VECTOR_H +#define IDYNTREE_SPATIAL_MOTION_VECTOR_H #include #include @@ -15,7 +15,7 @@ namespace iDynTree class Dummy {}; /** - * Class providing the raw coordinates for any motion spatial vector + * Class providing the coordinates for any motion spatial vector * (i.e. vector form of an element of se(3)). * * \ingroup iDynTreeCore @@ -28,8 +28,8 @@ namespace iDynTree * * This is just a basic vector, used to implement the adjoint transformations in * a general way. The relative adjoint transformation is contained in - * TransformRaw::apply(SpatialMotionRaw), - * for consistency with the iDynTree::PositionRaw class. + * Transform::apply(SpatialMotion), + * for consistency with the iDynTree::Position class. * * \note in iDynTree, the spatial vector follows this serialization: the first three elements are * the linear part and the second three elements are the angular part. diff --git a/src/core/include/iDynTree/SpatialVector.h b/src/core/include/iDynTree/SpatialVector.h index 32fe52c8e0b..7cb39795e73 100644 --- a/src/core/include/iDynTree/SpatialVector.h +++ b/src/core/include/iDynTree/SpatialVector.h @@ -132,6 +132,7 @@ SpatialVector /** * overloaded operators */ + DerivedSpatialVecT& operator=(const DerivedSpatialVecT &other); DerivedSpatialVecT operator+(const DerivedSpatialVecT &other) const; DerivedSpatialVecT operator-(const DerivedSpatialVecT &other) const; DerivedSpatialVecT operator-() const; @@ -334,6 +335,14 @@ SpatialVector } // overloaded operators + SPATIALVECTOR_TEMPLATE_HDR + DerivedSpatialVecT& SPATIALVECTOR_INSTANCE_HDR::operator=(const DerivedSpatialVecT &other) + { + this->getLinearVec3() = other.getLinearVec3(); + this->getAngularVec3() = other.getAngularVec3(); + return &this; + } + SPATIALVECTOR_TEMPLATE_HDR DerivedSpatialVecT SPATIALVECTOR_INSTANCE_HDR::operator+(const DerivedSpatialVecT &other) const { diff --git a/src/core/include/iDynTree/Twist.h b/src/core/include/iDynTree/Twist.h index 251ca80caf3..59820a176f3 100644 --- a/src/core/include/iDynTree/Twist.h +++ b/src/core/include/iDynTree/Twist.h @@ -28,6 +28,7 @@ namespace iDynTree Twist(const Twist& other); /** overloaded operators **/ + Twist& operator=(const Twist & other); Twist operator+(const Twist &other) const; Twist operator-(const Twist &other) const; Twist operator-() const; diff --git a/src/core/include/iDynTree/Wrench.h b/src/core/include/iDynTree/Wrench.h index 4763f7a46d3..2695b811439 100644 --- a/src/core/include/iDynTree/Wrench.h +++ b/src/core/include/iDynTree/Wrench.h @@ -23,6 +23,7 @@ namespace iDynTree Wrench(const Wrench & other); // overloaded operators + Wrench& operator=(const Wrench & other); Wrench operator+(const Wrench &other) const; Wrench operator-(const Wrench &other) const; Wrench operator-() const; diff --git a/src/core/src/ClassicalAcc.cpp b/src/core/src/ClassicalAcc.cpp index daa9f8993f2..fd2333e4d00 100644 --- a/src/core/src/ClassicalAcc.cpp +++ b/src/core/src/ClassicalAcc.cpp @@ -4,7 +4,7 @@ #include #include #include -#include +#include #include @@ -29,7 +29,7 @@ ClassicalAcc::ClassicalAcc(const ClassicalAcc& other): } -const ClassicalAcc& ClassicalAcc::changeCoordFrame(const RotationRaw& newCoordFrame) +const ClassicalAcc& ClassicalAcc::changeCoordFrame(const Rotation& newCoordFrame) { Eigen::Map thisData(this->data()); Eigen::Map rotData(newCoordFrame.data()); diff --git a/src/core/src/Direction.cpp b/src/core/src/Direction.cpp index c0dd07b323c..32f0ee12878 100644 --- a/src/core/src/Direction.cpp +++ b/src/core/src/Direction.cpp @@ -34,6 +34,13 @@ namespace iDynTree this->m_data[2] = other.m_data[2]; } + Direction& Direction::operator=(const Direction& other) + { + this->m_data[0] = other.m_data[0]; + this->m_data[1] = other.m_data[1]; + this->m_data[2] = other.m_data[2]; + return *this; + } void Direction::setToDefault() { diff --git a/src/core/src/InertiaNonLinearParametrization.cpp b/src/core/src/InertiaNonLinearParametrization.cpp index 1fa7584b0eb..f3192755920 100644 --- a/src/core/src/InertiaNonLinearParametrization.cpp +++ b/src/core/src/InertiaNonLinearParametrization.cpp @@ -126,7 +126,7 @@ void RigidBodyInertiaNonLinearParametrization::fromRigidBodyInertia(const Spatia this->com = inertia.getCenterOfMass(); // We get the inertia at the COM - RotationalInertiaRaw inertiaAtCOM = inertia.getRotationalInertiaWrtCenterOfMass(); + RotationalInertia inertiaAtCOM = inertia.getRotationalInertiaWrtCenterOfMass(); // We get the inertia at the principal axis using eigen JacobiSVD > eigenValuesSolver; @@ -163,7 +163,7 @@ void RigidBodyInertiaNonLinearParametrization::fromRigidBodyInertia(const Spatia SpatialInertia RigidBodyInertiaNonLinearParametrization::toRigidBodyInertia() const { // Compute the inertia wrt to the center of mass - RotationalInertiaRaw rotInertia; + RotationalInertia rotInertia; Eigen::Vector3d principalMomentOfInertiaAtCOM; diff --git a/src/core/src/Position.cpp b/src/core/src/Position.cpp index f2f54295628..33b222f727b 100644 --- a/src/core/src/Position.cpp +++ b/src/core/src/Position.cpp @@ -68,6 +68,14 @@ namespace iDynTree } + Position& Position::operator=(const Position& other) + { + this->m_data[0] = other.m_data[0]; + this->m_data[1] = other.m_data[1]; + this->m_data[2] = other.m_data[2]; + return *this; + } + Position::Position(Span other): VectorFixSize< 3 >(other) { diff --git a/src/core/src/Rotation.cpp b/src/core/src/Rotation.cpp index c602b68ad49..9752d1ed174 100644 --- a/src/core/src/Rotation.cpp +++ b/src/core/src/Rotation.cpp @@ -289,6 +289,15 @@ namespace iDynTree return inverse2(*this); } + Rotation& Rotation::operator=(const Rotation & other) + { + Eigen::Map thisData(m_data); + Eigen::Map otherData(other.data()); + thisData = otherData; + return *this; + } + + Rotation Rotation::operator*(const Rotation& other) const { return compose(*this,other); diff --git a/src/core/src/RotationalInertia.cpp b/src/core/src/RotationalInertia.cpp index 21bf51a32d2..2744ea7d68e 100644 --- a/src/core/src/RotationalInertia.cpp +++ b/src/core/src/RotationalInertia.cpp @@ -30,6 +30,18 @@ RotationalInertia::RotationalInertia(const RotationalInertia& other): Matrix3x3( { } +RotationalInertia& RotationalInertia::operator=(const RotationalInertia& other) +{ + for(std::size_t row=0; row < 3; row++ ) + { + for(std::size_t col=0; col < 3; col++ ) + { + this->m_data[row*3+col] = other.m_data[row*3+col]; + } + } + return *this; +} + RotationalInertia RotationalInertia::Zero() { RotationalInertia ret; diff --git a/src/core/src/SpatialAcc.cpp b/src/core/src/SpatialAcc.cpp index 36ac9b461d4..7b821cead2e 100644 --- a/src/core/src/SpatialAcc.cpp +++ b/src/core/src/SpatialAcc.cpp @@ -26,6 +26,13 @@ SpatialAcc::SpatialAcc(const SpatialAcc& other): } +SpatialAcc& SpatialAcc::operator=(const SpatialAcc &other) +{ + this->getLinearVec3() = other.getLinearVec3(); + this->getAngularVec3() = other.getAngularVec3(); + return *this; +} + SpatialAcc SpatialAcc::operator+(const SpatialAcc& other) const { return efficient6dSum(*this,other); diff --git a/src/core/src/SpatialMomentum.cpp b/src/core/src/SpatialMomentum.cpp index 175c3553723..580164b1aaf 100644 --- a/src/core/src/SpatialMomentum.cpp +++ b/src/core/src/SpatialMomentum.cpp @@ -26,6 +26,13 @@ SpatialMomentum::SpatialMomentum(const SpatialMomentum& other): } +SpatialMomentum& SpatialMomentum::operator=(const SpatialMomentum &other) +{ + this->getLinearVec3() = other.getLinearVec3(); + this->getAngularVec3() = other.getAngularVec3(); + return *this; +} + SpatialMomentum SpatialMomentum::operator+(const SpatialMomentum& other) const { return efficient6dSum(*this,other); diff --git a/src/core/src/TestUtils.cpp b/src/core/src/TestUtils.cpp index dc9221edd70..92b3de2b617 100644 --- a/src/core/src/TestUtils.cpp +++ b/src/core/src/TestUtils.cpp @@ -119,7 +119,7 @@ SpatialInertia getRandomInertia() SpatialInertia inertiaLink(getRandomDouble(0,4), Position(getRandomDouble(-2,2),getRandomDouble(-2,2),getRandomDouble(-2,2)), - rot*RotationalInertiaRaw(rotInertiaData,3,3)); + rot*RotationalInertia(rotInertiaData,3,3)); return inertiaLink; } diff --git a/src/core/src/Transform.cpp b/src/core/src/Transform.cpp index 119873ce8d7..792e525581e 100644 --- a/src/core/src/Transform.cpp +++ b/src/core/src/Transform.cpp @@ -329,9 +329,9 @@ std::string Transform::reservedToString() const // the rotational inertial is rotated and then // the parallel axis theorem applies - RotationalInertiaRaw newRotInertia; + RotationalInertia newRotInertia; Eigen::Map newI(newRotInertia.data()); - RotationalInertiaRaw oldRotInertiaWrtCom = op2.getRotationalInertiaWrtCenterOfMass(); + RotationalInertia oldRotInertiaWrtCom = op2.getRotationalInertiaWrtCenterOfMass(); Eigen::Map oldIWrtCom(oldRotInertiaWrtCom.data()); Eigen::Map R(op1.getRotation().data()); diff --git a/src/core/src/Twist.cpp b/src/core/src/Twist.cpp index 8d52d5203c8..14c9595cf35 100644 --- a/src/core/src/Twist.cpp +++ b/src/core/src/Twist.cpp @@ -35,6 +35,14 @@ Twist::Twist(const Twist& other): } +Twist& Twist::operator=(const Twist &other) +{ + this->getLinearVec3() = other.getLinearVec3(); + this->getAngularVec3() = other.getAngularVec3(); + return *this; +} + + Twist Twist::operator+(const Twist& other) const { return efficient6dSum(*this,other); diff --git a/src/core/src/Wrench.cpp b/src/core/src/Wrench.cpp index 91d384d6164..f9b1436e173 100644 --- a/src/core/src/Wrench.cpp +++ b/src/core/src/Wrench.cpp @@ -26,6 +26,13 @@ Wrench::Wrench(const Wrench& other): { } +Wrench& Wrench::operator=(const Wrench &other) +{ + this->getLinearVec3() = other.getLinearVec3(); + this->getAngularVec3() = other.getAngularVec3(); + return *this; +} + Wrench Wrench::operator+(const Wrench& other) const { return efficient6dSum(*this,other); diff --git a/src/core/tests/ArticulatedBodyInertiaUnitTest.cpp b/src/core/tests/ArticulatedBodyInertiaUnitTest.cpp index 82db7350492..c511d4a8ff3 100644 --- a/src/core/tests/ArticulatedBodyInertiaUnitTest.cpp +++ b/src/core/tests/ArticulatedBodyInertiaUnitTest.cpp @@ -125,7 +125,7 @@ int main() double rotInertiaData[3*3] = {10.0,0.5,0.8, 0.5,20.0,0.6, 0.8,0.6,25.0}; - SpatialInertia inertia(1.0,Position(0.3,0.6,0.2),RotationalInertiaRaw(rotInertiaData,3,3)); + SpatialInertia inertia(1.0,Position(0.3,0.6,0.2),RotationalInertia(rotInertiaData,3,3)); ArticulatedBodyInertia abi = ArticulatedBodyInertia(inertia); checkInertiaAccProduct(abi,twist); diff --git a/src/core/tests/SpatialInertiaUnitTest.cpp b/src/core/tests/SpatialInertiaUnitTest.cpp index 3513816e146..ed9ed230f47 100644 --- a/src/core/tests/SpatialInertiaUnitTest.cpp +++ b/src/core/tests/SpatialInertiaUnitTest.cpp @@ -444,7 +444,7 @@ int main() double rotInertiaData[3*3] = {10.0,0.04,0.04, 0.04,20.0,0.04, 0.04,0.04,24.0}; - SpatialInertia inertia(1.0,Position(100,-5,10),RotationalInertiaRaw(rotInertiaData,3,3)); + SpatialInertia inertia(1.0,Position(100,-5,10),RotationalInertia(rotInertiaData,3,3)); checkInertiaTwistProduct(inertia,twist); checkInertiaTransformation(trans,inertia); diff --git a/src/core/tests/TransformFromMatrix4x4UnitTest.cpp b/src/core/tests/TransformFromMatrix4x4UnitTest.cpp index 49b769c362d..03f8af51889 100644 --- a/src/core/tests/TransformFromMatrix4x4UnitTest.cpp +++ b/src/core/tests/TransformFromMatrix4x4UnitTest.cpp @@ -15,13 +15,10 @@ using namespace iDynTree; void checkFromMatrix4x4() { - RotationRaw rot_raw(-2.449, -1.224, -2.041, 0.408, -0.707, -0.707, 0.816, 0.409, -1.154); - PositionRaw pos_raw(1, 2, 3); + Rotation rot(-2.449, -1.224, -2.041, 0.408, -0.707, -0.707, 0.816, 0.409, -1.154); + Position pos(1, 2, 3); double transform[16] = {-2.449, -1.224, -2.041, 1, 0.408, -0.707, -0.707, 2, 0.816, 0.409, -1.154, 3, 0, 0, 0, 1}; - Rotation rot = Rotation(rot_raw); - Position pos = Position(pos_raw); - Matrix4x4 homogeneousMatrix(transform, 4, 4); Transform t_posrot(rot, pos); @@ -32,9 +29,8 @@ void checkFromMatrix4x4() void checkLogExp() { - PositionRaw pos_raw(0, 0, 0); + Position pos(0, 0, 0); Rotation rot = Rotation::RPY(0.0, 1.57, 0.0); - Position pos = Position(pos_raw); Transform t_posrot(rot, pos); SpatialMotionVector v; diff --git a/src/estimation/tests/ExternalWrenchesEstimationUnitTest.cpp b/src/estimation/tests/ExternalWrenchesEstimationUnitTest.cpp index dfb791de9d9..73f4326c141 100644 --- a/src/estimation/tests/ExternalWrenchesEstimationUnitTest.cpp +++ b/src/estimation/tests/ExternalWrenchesEstimationUnitTest.cpp @@ -37,7 +37,7 @@ inline Link getSimplestLink() SpatialInertia inertiaLink(7.0, Position(5.0,0.0,0.0), - rot*RotationalInertiaRaw(rotInertiaData,3,3)); + rot*RotationalInertia(rotInertiaData,3,3)); Link link; @@ -194,7 +194,7 @@ void checkSimpleModelExternalWrenchEstimationWithFTSensors() 0.0,1.0,0.0, 0.0,0.0,1.0}; - SpatialInertia inertia(1.0,iDynTree::Position::Zero(),RotationalInertiaRaw(rotInertiaData,3,3)); + SpatialInertia inertia(1.0,iDynTree::Position::Zero(),RotationalInertia(rotInertiaData,3,3)); Link link0, link1, link2, link3; link0.setInertia(inertia); link1.setInertia(inertia); diff --git a/src/high-level/src/KinDynComputations.cpp b/src/high-level/src/KinDynComputations.cpp index 342c2d57358..6eda6cbaf87 100644 --- a/src/high-level/src/KinDynComputations.cpp +++ b/src/high-level/src/KinDynComputations.cpp @@ -2205,7 +2205,7 @@ void KinDynComputations::KinDynComputationsPrivateAttributes::processOnLeftSideB assert(jac.cols() == m_robot_model.getNrOfDOFs()+6); // Get the center of mass in the base frame - Position vectorFromComToBaseWithRotationOfBase = PositionRaw::inverse(this->getRobotLockedInertia().getCenterOfMass()); + Position vectorFromComToBaseWithRotationOfBase = Position::inverse(this->getRobotLockedInertia().getCenterOfMass()); Transform newOutputFrame_X_oldOutputFrame; if (leftSideRepresentation == BODY_FIXED_REPRESENTATION) @@ -2239,7 +2239,7 @@ Twist KinDynComputations::getCentroidalAverageVelocity() Twist base_averageVelocity = base_lockedInertia.applyInverse(base_momentum); // Get the center of mass in the base frame - Position vectorFromComToBaseWithRotationOfBase = PositionRaw::inverse(pimpl->getRobotLockedInertia().getCenterOfMass()); + Position vectorFromComToBaseWithRotationOfBase = Position::inverse(pimpl->getRobotLockedInertia().getCenterOfMass()); Transform newOutputFrame_X_oldOutputFrame; if (pimpl->m_frameVelRepr == BODY_FIXED_REPRESENTATION) @@ -2387,7 +2387,7 @@ SpatialMomentum KinDynComputations::getCentroidalTotalMomentum() SpatialMomentum base_momentum = pimpl->m_pos.worldBasePos().inverse()*pimpl->m_totalMomentum; // Get the center of mass in the base frame - Position vectorFromComToBaseWithRotationOfBase = PositionRaw::inverse(pimpl->getRobotLockedInertia().getCenterOfMass()); + Position vectorFromComToBaseWithRotationOfBase = Position::inverse(pimpl->getRobotLockedInertia().getCenterOfMass()); Transform newOutputFrame_X_oldOutputFrame; if (pimpl->m_frameVelRepr == BODY_FIXED_REPRESENTATION) diff --git a/src/model/include/iDynTree/ModelTestUtils.h b/src/model/include/iDynTree/ModelTestUtils.h index 9af0de0740f..c1e092accfa 100644 --- a/src/model/include/iDynTree/ModelTestUtils.h +++ b/src/model/include/iDynTree/ModelTestUtils.h @@ -33,7 +33,7 @@ inline Link getRandomLink() SpatialInertia inertiaLink(getRandomDouble(0,4), Position(getRandomDouble(-2,2),getRandomDouble(-2,2),getRandomDouble(-2,2)), - rot*RotationalInertiaRaw(rotInertiaData,3,3)); + rot*RotationalInertia(rotInertiaData,3,3)); Link link; diff --git a/src/model/include/iDynTree/SubModel.h b/src/model/include/iDynTree/SubModel.h index 69834284878..fc69adb4d3a 100644 --- a/src/model/include/iDynTree/SubModel.h +++ b/src/model/include/iDynTree/SubModel.h @@ -16,7 +16,6 @@ namespace iDynTree class Traversal; class JointPosDoubleArray; class LinkPositions; - class IRawVector; /** * Class representing the decomposition in one model in several submodels. diff --git a/src/model/src/DenavitHartenberg.cpp b/src/model/src/DenavitHartenberg.cpp index f64d562c2cc..5647bf8d05f 100644 --- a/src/model/src/DenavitHartenberg.cpp +++ b/src/model/src/DenavitHartenberg.cpp @@ -738,7 +738,7 @@ bool CreateModelFromDHChain(const DHChain& inputChain, outputModel = Model(); // All the inertial will be of one kg in the origin - iDynTree::SpatialInertia inertiaDefault(1.0,Position::Zero(),RotationalInertiaRaw::Zero()); + iDynTree::SpatialInertia inertiaDefault(1.0,Position::Zero(),RotationalInertia::Zero()); iDynTree::Link linkDefault; linkDefault.setInertia(inertiaDefault); diff --git a/src/model/tests/LinkUnitTest.cpp b/src/model/tests/LinkUnitTest.cpp index fbf84d636c3..573aad59b58 100644 --- a/src/model/tests/LinkUnitTest.cpp +++ b/src/model/tests/LinkUnitTest.cpp @@ -16,7 +16,7 @@ int main() double rotInertiaData[3*3] = {10.0,0.0,0.0, 0.0,20.0,0.0, 0.0,0.0,30.0}; - SpatialInertia settedInertia(1.0,Position(100,0,0),RotationalInertiaRaw(rotInertiaData,3,3)); + SpatialInertia settedInertia(1.0,Position(100,0,0),RotationalInertia(rotInertiaData,3,3)); Link link; diff --git a/src/model/tests/ModelUnitTest.cpp b/src/model/tests/ModelUnitTest.cpp index a6a1283614a..9b7d65e156c 100644 --- a/src/model/tests/ModelUnitTest.cpp +++ b/src/model/tests/ModelUnitTest.cpp @@ -305,7 +305,7 @@ void checkSimpleModel() 0.0,12.0,0.0, 0.0,0.0,10.0}; - SpatialInertia inertiaLink0(1.0,Position(100,0,0),RotationalInertiaRaw(rotInertiaData,3,3)); + SpatialInertia inertiaLink0(1.0,Position(100,0,0),RotationalInertia(rotInertiaData,3,3)); Link link0; link0.setInertia(inertiaLink0); @@ -354,7 +354,7 @@ void checkInsertJointAndLink() 0.0,12.0,0.0, 0.0,0.0,10.0}; - SpatialInertia inertiaLink0(1.0,Position(100,0,0),RotationalInertiaRaw(rotInertiaData,3,3)); + SpatialInertia inertiaLink0(1.0,Position(100,0,0),RotationalInertia(rotInertiaData,3,3)); Link link0; link0.setInertia(inertiaLink0); diff --git a/src/model_io/codecs/include/private/InertialElement.h b/src/model_io/codecs/include/private/InertialElement.h index 6ebce4dcb79..84bc6c890c5 100644 --- a/src/model_io/codecs/include/private/InertialElement.h +++ b/src/model_io/codecs/include/private/InertialElement.h @@ -17,7 +17,7 @@ namespace iDynTree { class iDynTree::InertialElement: public iDynTree::XMLElement { Transform m_centerOfMass; double m_mass; - RotationalInertiaRaw m_rotationalInertiaWRTCoM; + RotationalInertia m_rotationalInertiaWRTCoM; iDynTree::Link &m_link; public: diff --git a/src/model_io/codecs/src/InertialElement.cpp b/src/model_io/codecs/src/InertialElement.cpp index 4d8f5a56c06..e2c6d732e8c 100644 --- a/src/model_io/codecs/src/InertialElement.cpp +++ b/src/model_io/codecs/src/InertialElement.cpp @@ -79,7 +79,7 @@ namespace iDynTree { xy, yy, yz, xz, yz, zz }; - m_rotationalInertiaWRTCoM = RotationalInertiaRaw(buffer, 3, 3); + m_rotationalInertiaWRTCoM = RotationalInertia(buffer, 3, 3); return true; }); diff --git a/src/model_io/codecs/src/URDFModelExport.cpp b/src/model_io/codecs/src/URDFModelExport.cpp index c372933ea37..2637396f1ee 100644 --- a/src/model_io/codecs/src/URDFModelExport.cpp +++ b/src/model_io/codecs/src/URDFModelExport.cpp @@ -89,7 +89,7 @@ bool exportInertial(const SpatialInertia &inertia, xmlNodePtr parent_element) ok = ok && exportTransform(Transform(Rotation::Identity(), inertia.getCenterOfMass()), inertial); xmlNodePtr inertia_xml = xmlNewChild(inertial, NULL, BAD_CAST "inertia", NULL); - RotationalInertiaRaw rotInertia = inertia.getRotationalInertiaWrtCenterOfMass(); + RotationalInertia rotInertia = inertia.getRotationalInertiaWrtCenterOfMass(); ok = ok && doubleToStringWithClassicLocale(rotInertia(0, 0), bufStr); xmlNewProp(inertia_xml, BAD_CAST "ixx", BAD_CAST bufStr.c_str()); ok = ok && doubleToStringWithClassicLocale(rotInertia(0, 1), bufStr); diff --git a/src/solid-shapes/src/InertialParametersSolidShapesHelpers.cpp b/src/solid-shapes/src/InertialParametersSolidShapesHelpers.cpp index f00922117a0..a645ec585f3 100644 --- a/src/solid-shapes/src/InertialParametersSolidShapesHelpers.cpp +++ b/src/solid-shapes/src/InertialParametersSolidShapesHelpers.cpp @@ -27,10 +27,10 @@ SpatialInertia boxGet6DInertiaInLinkFrameFromDensity(const Box& box, double boxVolume = box.getX() * box.getY() * box.getZ(); double boxMass = density*boxVolume; // Assuming uniform density, the center of mass is coincident with the box center - PositionRaw comInGeomFrame; + Position comInGeomFrame; comInGeomFrame.zero(); // From http://scienceworld.wolfram.com/physics/MomentofInertiaRectangularParallelepiped.html - RotationalInertiaRaw rotInertiaInGeomFrame; + RotationalInertia rotInertiaInGeomFrame; rotInertiaInGeomFrame.zero(); double x2 = box.getX() * box.getX(); double y2 = box.getY() * box.getY(); diff --git a/src/tests/integration/DynamicsLinearizationIntegrationTest.cpp b/src/tests/integration/DynamicsLinearizationIntegrationTest.cpp index 00416e9aaec..0b6c6a3125a 100644 --- a/src/tests/integration/DynamicsLinearizationIntegrationTest.cpp +++ b/src/tests/integration/DynamicsLinearizationIntegrationTest.cpp @@ -472,7 +472,7 @@ int main() // First test some models used for debug Model doubleBodyModel; - RotationalInertiaRaw rotInertia = RotationalInertiaRaw::Zero(); + RotationalInertia rotInertia = RotationalInertia::Zero(); rotInertia(0,0) = rotInertia(1,1) = rotInertia(2,2) = 1.0; Position com = Position::Zero(); diff --git a/src/tests/integration/InertialParametersSolidShapesHelpersIntegrationTest.cpp b/src/tests/integration/InertialParametersSolidShapesHelpersIntegrationTest.cpp index 629a21a9bae..bf4ea1fcf97 100644 --- a/src/tests/integration/InertialParametersSolidShapesHelpersIntegrationTest.cpp +++ b/src/tests/integration/InertialParametersSolidShapesHelpersIntegrationTest.cpp @@ -33,7 +33,7 @@ int main() // http://scienceworld.wolfram.com/physics/MomentofInertiaRectangularParallelepiped.html double totalMass = 24; double expectedInertia = 16; - RotationalInertiaRaw rotInertia; + RotationalInertia rotInertia; rotInertia.zero(); rotInertia(0, 0) = rotInertia(1, 1) = rotInertia(2, 2) = expectedInertia;