diff --git a/CHANGELOG.md b/CHANGELOG.md index 80559fbc0d0..9e8abb647fe 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -17,6 +17,13 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 - The license of the library is changed to `BSD-3-Clause` (https://github.com/robotology/idyntree/pull/1089). - The `iDynTree::ModelExporterOptions` class was changed to be defined as a struct (https://github.com/robotology/idyntree/pull/1088). +- The header structure has been shortened, from `` to `` (https://github.com/robotology/idyntree/pull/1104). +- `iDynTree::idyntree-sensors` library has been merged in `iDynTree::idyntree-model` (https://github.com/robotology/idyntree/pull/1104). + +### Deprecated + +- Linking `iDynTree::idyntree-sensors` is deprecated, you can just link `iDynTree::idyntree-model` instead, or just search and replace `iDynTree::idyntree-sensors` with an empty string if you are already linking `iDynTree::idyntree-model` (https://github.com/robotology/idyntree/pull/1104). +- Including ``, ``, ``, ``, ``, `` is deprecated, just include `` . To perform this migration, just search and replace ` //Utils -#include "iDynTree/Core/Utils.h" +#include "iDynTree/Utils.h" // Basic math classes -#include "iDynTree/Core/MatrixDynSize.h" -#include "iDynTree/Core/MatrixFixSize.h" -#include "iDynTree/Core/SparseMatrix.h" +#include "iDynTree/MatrixDynSize.h" +#include "iDynTree/MatrixFixSize.h" +#include "iDynTree/SparseMatrix.h" -#include "iDynTree/Core/VectorDynSize.h" -#include "iDynTree/Core/VectorFixSize.h" +#include "iDynTree/VectorDynSize.h" +#include "iDynTree/VectorFixSize.h" // Basic Vectors: Point Vectors and Spatial Vectors -#include "iDynTree/Core/PositionRaw.h" -#include "iDynTree/Core/Position.h" -#include "iDynTree/Core/SpatialForceVector.h" -#include "iDynTree/Core/SpatialMotionVector.h" -#include "iDynTree/Core/Twist.h" -#include "iDynTree/Core/Wrench.h" -#include "iDynTree/Core/SpatialMomentum.h" -#include "iDynTree/Core/SpatialAcc.h" -#include "iDynTree/Core/ClassicalAcc.h" -#include "iDynTree/Core/Direction.h" -#include "iDynTree/Core/Axis.h" +#include "iDynTree/PositionRaw.h" +#include "iDynTree/Position.h" +#include "iDynTree/SpatialForceVector.h" +#include "iDynTree/SpatialMotionVector.h" +#include "iDynTree/Twist.h" +#include "iDynTree/Wrench.h" +#include "iDynTree/SpatialMomentum.h" +#include "iDynTree/SpatialAcc.h" +#include "iDynTree/ClassicalAcc.h" +#include "iDynTree/Direction.h" +#include "iDynTree/Axis.h" // Inertias -#include "iDynTree/Core/RotationalInertiaRaw.h" -#include "iDynTree/Core/SpatialInertiaRaw.h" -#include "iDynTree/Core/SpatialInertia.h" -#include "iDynTree/Core/ArticulatedBodyInertia.h" -#include "iDynTree/Core/InertiaNonLinearParametrization.h" +#include "iDynTree/RotationalInertiaRaw.h" +#include "iDynTree/SpatialInertiaRaw.h" +#include "iDynTree/SpatialInertia.h" +#include "iDynTree/ArticulatedBodyInertia.h" +#include "iDynTree/InertiaNonLinearParametrization.h" // Transformations: Rotation and Transform -#include "iDynTree/Core/RotationRaw.h" -#include "iDynTree/Core/Rotation.h" -#include "iDynTree/Core/Transform.h" -#include "iDynTree/Core/TransformDerivative.h" -#include "iDynTree/Core/Span.h" +#include "iDynTree/RotationRaw.h" +#include "iDynTree/Rotation.h" +#include "iDynTree/Transform.h" +#include "iDynTree/TransformDerivative.h" +#include "iDynTree/Span.h" // Model related data structures -#include "iDynTree/Model/Indices.h" -#include "iDynTree/Model/LinkState.h" -#include "iDynTree/Model/Link.h" -#include "iDynTree/Model/IJoint.h" -#include "iDynTree/Model/FixedJoint.h" -#include "iDynTree/Model/MovableJointImpl.h" -#include "iDynTree/Model/RevoluteJoint.h" -#include "iDynTree/Model/PrismaticJoint.h" -#include "iDynTree/Model/Traversal.h" -#include "iDynTree/Model/SolidShapes.h" -#include "iDynTree/Model/Model.h" -#include "iDynTree/Model/JointState.h" -#include "iDynTree/Model/FreeFloatingMatrices.h" -#include "iDynTree/Model/FreeFloatingState.h" -#include "iDynTree/Model/ContactWrench.h" -#include "iDynTree/Model/ModelTestUtils.h" -#include "iDynTree/Model/ModelTransformers.h" -#include "iDynTree/Model/SubModel.h" +#include "iDynTree/Indices.h" +#include "iDynTree/LinkState.h" +#include "iDynTree/Link.h" +#include "iDynTree/IJoint.h" +#include "iDynTree/FixedJoint.h" +#include "iDynTree/MovableJointImpl.h" +#include "iDynTree/RevoluteJoint.h" +#include "iDynTree/PrismaticJoint.h" +#include "iDynTree/Traversal.h" +#include "iDynTree/SolidShapes.h" +#include "iDynTree/Model.h" +#include "iDynTree/JointState.h" +#include "iDynTree/FreeFloatingMatrices.h" +#include "iDynTree/FreeFloatingState.h" +#include "iDynTree/ContactWrench.h" +#include "iDynTree/ModelTestUtils.h" +#include "iDynTree/ModelTransformers.h" +#include "iDynTree/SubModel.h" // Kinematics & Dynamics related functions -#include "iDynTree/Model/ForwardKinematics.h" -#include "iDynTree/Model/Dynamics.h" -#include "iDynTree/Model/DenavitHartenberg.h" +#include "iDynTree/ForwardKinematics.h" +#include "iDynTree/Dynamics.h" +#include "iDynTree/DenavitHartenberg.h" // Sensors related data structures -#include "iDynTree/Sensors/Sensors.h" -#include "iDynTree/Sensors/SixAxisForceTorqueSensor.h" -#include "iDynTree/Sensors/AccelerometerSensor.h" -#include "iDynTree/Sensors/GyroscopeSensor.h" -#include "iDynTree/Sensors/ThreeAxisAngularAccelerometerSensor.h" -#include "iDynTree/Sensors/ThreeAxisForceTorqueContactSensor.h" -#include "iDynTree/Sensors/PredictSensorsMeasurements.h" +#include "iDynTree/Sensors.h" +#include "iDynTree/SixAxisForceTorqueSensor.h" +#include "iDynTree/AccelerometerSensor.h" +#include "iDynTree/GyroscopeSensor.h" +#include "iDynTree/ThreeAxisAngularAccelerometerSensor.h" +#include "iDynTree/ThreeAxisForceTorqueContactSensor.h" +#include "iDynTree/PredictSensorsMeasurements.h" // Model loading from external formats -#include "iDynTree/ModelIO/URDFDofsImport.h" -#include "iDynTree/ModelIO/ModelLoader.h" -#include "iDynTree/ModelIO/ModelExporter.h" -#include "iDynTree/ModelIO/ModelCalibrationHelper.h" +#include "iDynTree/URDFDofsImport.h" +#include "iDynTree/ModelLoader.h" +#include "iDynTree/ModelExporter.h" +#include "iDynTree/ModelCalibrationHelper.h" // Estimation related classes -#include "iDynTree/Estimation/ExternalWrenchesEstimation.h" -#include "iDynTree/Estimation/ExtWrenchesAndJointTorquesEstimator.h" -#include "iDynTree/Estimation/SimpleLeggedOdometry.h" -#include "iDynTree/Estimation/BerdyHelper.h" -#include "iDynTree/Estimation/BerdySparseMAPSolver.h" -#include "iDynTree/Estimation/AttitudeEstimator.h" -#include "iDynTree/Estimation/AttitudeMahonyFilter.h" -#include "iDynTree/Estimation/ExtendedKalmanFilter.h" -#include "iDynTree/Estimation/AttitudeQuaternionEKF.h" +#include "iDynTree/ExternalWrenchesEstimation.h" +#include "iDynTree/ExtWrenchesAndJointTorquesEstimator.h" +#include "iDynTree/SimpleLeggedOdometry.h" +#include "iDynTree/BerdyHelper.h" +#include "iDynTree/BerdySparseMAPSolver.h" +#include "iDynTree/AttitudeEstimator.h" +#include "iDynTree/AttitudeMahonyFilter.h" +#include "iDynTree/ExtendedKalmanFilter.h" +#include "iDynTree/AttitudeQuaternionEKF.h" // SolidShapes related classes #include "iDynTree/InertialParametersSolidShapesHelpers.h" @@ -149,19 +149,19 @@ namespace std { } //Utils -%include "iDynTree/Core/Utils.h" +%include "iDynTree/Utils.h" /* Note : always include headers following the inheritance order */ // Basic math classes -%include "iDynTree/Core/MatrixDynSize.h" -%include "iDynTree/Core/MatrixFixSize.h" -%include "iDynTree/Core/SparseMatrix.h" +%include "iDynTree/MatrixDynSize.h" +%include "iDynTree/MatrixFixSize.h" +%include "iDynTree/SparseMatrix.h" %template(SparseMatrixRowMajor) iDynTree::SparseMatrix; %template(SparseMatrixColMajor) iDynTree::SparseMatrix; -%include "iDynTree/Core/VectorDynSize.h" -%include "iDynTree/Core/VectorFixSize.h" +%include "iDynTree/VectorDynSize.h" +%include "iDynTree/VectorFixSize.h" #ifdef SWIGMATLAB %include "./matlab/matlab_matvec.i" @@ -186,12 +186,12 @@ namespace std { %template(Vector16) iDynTree::VectorFixSize<16>; // Basic Vectors: Point Vectors and Spatial Vectors -%include "iDynTree/Core/PositionRaw.h" -%include "iDynTree/Core/Position.h" +%include "iDynTree/PositionRaw.h" +%include "iDynTree/Position.h" -%include "iDynTree/Core/GeomVector3.h" +%include "iDynTree/GeomVector3.h" -%include "iDynTree/Core/SpatialVector.h" +%include "iDynTree/SpatialVector.h" %template() iDynTree::DualSpace; %template() iDynTree::DualSpace; @@ -207,41 +207,41 @@ namespace std { %template(SpatialMotionVectorBase) iDynTree::SpatialVector; %template(SpatialForceVectorBase) iDynTree::SpatialVector; -%include "iDynTree/Core/SpatialMotionVector.h" -%include "iDynTree/Core/SpatialForceVector.h" -%include "iDynTree/Core/Twist.h" -%include "iDynTree/Core/Wrench.h" -%include "iDynTree/Core/SpatialMomentum.h" -%include "iDynTree/Core/SpatialAcc.h" -%include "iDynTree/Core/ClassicalAcc.h" -%include "iDynTree/Core/Direction.h" -%include "iDynTree/Core/Axis.h" +%include "iDynTree/SpatialMotionVector.h" +%include "iDynTree/SpatialForceVector.h" +%include "iDynTree/Twist.h" +%include "iDynTree/Wrench.h" +%include "iDynTree/SpatialMomentum.h" +%include "iDynTree/SpatialAcc.h" +%include "iDynTree/ClassicalAcc.h" +%include "iDynTree/Direction.h" +%include "iDynTree/Axis.h" // Inertias -%include "iDynTree/Core/RotationalInertiaRaw.h" -%include "iDynTree/Core/SpatialInertiaRaw.h" -%include "iDynTree/Core/SpatialInertia.h" -%include "iDynTree/Core/ArticulatedBodyInertia.h" -%include "iDynTree/Core/InertiaNonLinearParametrization.h" +%include "iDynTree/RotationalInertiaRaw.h" +%include "iDynTree/SpatialInertiaRaw.h" +%include "iDynTree/SpatialInertia.h" +%include "iDynTree/ArticulatedBodyInertia.h" +%include "iDynTree/InertiaNonLinearParametrization.h" // Transformations: Rotation and Transform -%include "iDynTree/Core/RotationRaw.h" -%include "iDynTree/Core/Rotation.h" -%include "iDynTree/Core/Transform.h" -%include "iDynTree/Core/TransformDerivative.h" -%include "iDynTree/Core/Span.h" -%include "iDynTree/Core/MatrixView.h" +%include "iDynTree/RotationRaw.h" +%include "iDynTree/Rotation.h" +%include "iDynTree/Transform.h" +%include "iDynTree/TransformDerivative.h" +%include "iDynTree/Span.h" +%include "iDynTree/MatrixView.h" %template(DynamicSpan) iDynTree::Span; %template(DynamicMatrixView) iDynTree::MatrixView; // Model related data structures -%include "iDynTree/Model/Indices.h" -%include "iDynTree/Model/LinkState.h" -%include "iDynTree/Model/Link.h" -%include "iDynTree/Model/IJoint.h" -%include "iDynTree/Model/FixedJoint.h" -%include "iDynTree/Model/MovableJointImpl.h" +%include "iDynTree/Indices.h" +%include "iDynTree/LinkState.h" +%include "iDynTree/Link.h" +%include "iDynTree/IJoint.h" +%include "iDynTree/FixedJoint.h" +%include "iDynTree/MovableJointImpl.h" %template(MovableJointImpl1) iDynTree::MovableJointImpl<1,1>; %template(MovableJointImpl2) iDynTree::MovableJointImpl<2,2>; @@ -250,18 +250,18 @@ namespace std { %template(MovableJointImpl5) iDynTree::MovableJointImpl<5,5>; %template(MovableJointImpl6) iDynTree::MovableJointImpl<6,6>; -%include "iDynTree/Model/RevoluteJoint.h" -%include "iDynTree/Model/PrismaticJoint.h" -%include "iDynTree/Model/Traversal.h" -%include "iDynTree/Model/SolidShapes.h" -%include "iDynTree/Model/Model.h" -%include "iDynTree/Model/JointState.h" -%include "iDynTree/Model/FreeFloatingMatrices.h" -%include "iDynTree/Model/FreeFloatingState.h" -%include "iDynTree/Model/ContactWrench.h" -%include "iDynTree/Model/ModelTestUtils.h" -%include "iDynTree/Model/ModelTransformers.h" -%include "iDynTree/Model/SubModel.h" +%include "iDynTree/RevoluteJoint.h" +%include "iDynTree/PrismaticJoint.h" +%include "iDynTree/Traversal.h" +%include "iDynTree/SolidShapes.h" +%include "iDynTree/Model.h" +%include "iDynTree/JointState.h" +%include "iDynTree/FreeFloatingMatrices.h" +%include "iDynTree/FreeFloatingState.h" +%include "iDynTree/ContactWrench.h" +%include "iDynTree/ModelTestUtils.h" +%include "iDynTree/ModelTransformers.h" +%include "iDynTree/SubModel.h" %include "joints.i" @@ -270,37 +270,37 @@ namespace std { // Kinematics & Dynamics related functions -%include "iDynTree/Model/ForwardKinematics.h" -%include "iDynTree/Model/Dynamics.h" -%include "iDynTree/Model/DenavitHartenberg.h" +%include "iDynTree/ForwardKinematics.h" +%include "iDynTree/Dynamics.h" +%include "iDynTree/DenavitHartenberg.h" // Sensors related data structures -%include "iDynTree/Sensors/Sensors.h" -%include "iDynTree/Sensors/SixAxisForceTorqueSensor.h" -%include "iDynTree/Sensors/AccelerometerSensor.h" -%include "iDynTree/Sensors/GyroscopeSensor.h" -%include "iDynTree/Sensors/ThreeAxisAngularAccelerometerSensor.h" -%include "iDynTree/Sensors/ThreeAxisForceTorqueContactSensor.h" -%include "iDynTree/Sensors/PredictSensorsMeasurements.h" +%include "iDynTree/Sensors.h" +%include "iDynTree/SixAxisForceTorqueSensor.h" +%include "iDynTree/AccelerometerSensor.h" +%include "iDynTree/GyroscopeSensor.h" +%include "iDynTree/ThreeAxisAngularAccelerometerSensor.h" +%include "iDynTree/ThreeAxisForceTorqueContactSensor.h" +%include "iDynTree/PredictSensorsMeasurements.h" %include "sensors.i" // Model loading from external formats -%include "iDynTree/ModelIO/URDFDofsImport.h" -%include "iDynTree/ModelIO/ModelLoader.h" -%include "iDynTree/ModelIO/ModelExporter.h" -%include "iDynTree/ModelIO/ModelCalibrationHelper.h" +%include "iDynTree/URDFDofsImport.h" +%include "iDynTree/ModelLoader.h" +%include "iDynTree/ModelExporter.h" +%include "iDynTree/ModelCalibrationHelper.h" // Estimation related classes -%include "iDynTree/Estimation/ExternalWrenchesEstimation.h" -%include "iDynTree/Estimation/ExtWrenchesAndJointTorquesEstimator.h" -%include "iDynTree/Estimation/SimpleLeggedOdometry.h" -%include "iDynTree/Estimation/BerdyHelper.h" -%include "iDynTree/Estimation/BerdySparseMAPSolver.h" -%include "iDynTree/Estimation/AttitudeEstimator.h" -%include "iDynTree/Estimation/AttitudeMahonyFilter.h" -%include "iDynTree/Estimation/ExtendedKalmanFilter.h" -%include "iDynTree/Estimation/AttitudeQuaternionEKF.h" +%include "iDynTree/ExternalWrenchesEstimation.h" +%include "iDynTree/ExtWrenchesAndJointTorquesEstimator.h" +%include "iDynTree/SimpleLeggedOdometry.h" +%include "iDynTree/BerdyHelper.h" +%include "iDynTree/BerdySparseMAPSolver.h" +%include "iDynTree/AttitudeEstimator.h" +%include "iDynTree/AttitudeMahonyFilter.h" +%include "iDynTree/ExtendedKalmanFilter.h" +%include "iDynTree/AttitudeQuaternionEKF.h" // SolidShapes related classes %include "iDynTree/InertialParametersSolidShapesHelpers.h" diff --git a/bindings/matlab/autogenerated/iDynTreeMATLAB_wrap.cxx b/bindings/matlab/autogenerated/iDynTreeMATLAB_wrap.cxx index 100e2818177..c2a81ebbccf 100644 --- a/bindings/matlab/autogenerated/iDynTreeMATLAB_wrap.cxx +++ b/bindings/matlab/autogenerated/iDynTreeMATLAB_wrap.cxx @@ -2974,94 +2974,94 @@ SWIGINTERN void std_vector_Sl_int_Sg__insert__SWIG_1(std::vector< int > *self,st #include //Utils -#include "iDynTree/Core/Utils.h" +#include "iDynTree/Utils.h" // Basic math classes -#include "iDynTree/Core/MatrixDynSize.h" -#include "iDynTree/Core/MatrixFixSize.h" -#include "iDynTree/Core/SparseMatrix.h" +#include "iDynTree/MatrixDynSize.h" +#include "iDynTree/MatrixFixSize.h" +#include "iDynTree/SparseMatrix.h" -#include "iDynTree/Core/VectorDynSize.h" -#include "iDynTree/Core/VectorFixSize.h" +#include "iDynTree/VectorDynSize.h" +#include "iDynTree/VectorFixSize.h" // Basic Vectors: Point Vectors and Spatial Vectors -#include "iDynTree/Core/PositionRaw.h" -#include "iDynTree/Core/Position.h" -#include "iDynTree/Core/SpatialForceVector.h" -#include "iDynTree/Core/SpatialMotionVector.h" -#include "iDynTree/Core/Twist.h" -#include "iDynTree/Core/Wrench.h" -#include "iDynTree/Core/SpatialMomentum.h" -#include "iDynTree/Core/SpatialAcc.h" -#include "iDynTree/Core/ClassicalAcc.h" -#include "iDynTree/Core/Direction.h" -#include "iDynTree/Core/Axis.h" +#include "iDynTree/PositionRaw.h" +#include "iDynTree/Position.h" +#include "iDynTree/SpatialForceVector.h" +#include "iDynTree/SpatialMotionVector.h" +#include "iDynTree/Twist.h" +#include "iDynTree/Wrench.h" +#include "iDynTree/SpatialMomentum.h" +#include "iDynTree/SpatialAcc.h" +#include "iDynTree/ClassicalAcc.h" +#include "iDynTree/Direction.h" +#include "iDynTree/Axis.h" // Inertias -#include "iDynTree/Core/RotationalInertiaRaw.h" -#include "iDynTree/Core/SpatialInertiaRaw.h" -#include "iDynTree/Core/SpatialInertia.h" -#include "iDynTree/Core/ArticulatedBodyInertia.h" -#include "iDynTree/Core/InertiaNonLinearParametrization.h" +#include "iDynTree/RotationalInertiaRaw.h" +#include "iDynTree/SpatialInertiaRaw.h" +#include "iDynTree/SpatialInertia.h" +#include "iDynTree/ArticulatedBodyInertia.h" +#include "iDynTree/InertiaNonLinearParametrization.h" // Transformations: Rotation and Transform -#include "iDynTree/Core/RotationRaw.h" -#include "iDynTree/Core/Rotation.h" -#include "iDynTree/Core/Transform.h" -#include "iDynTree/Core/TransformDerivative.h" -#include "iDynTree/Core/Span.h" +#include "iDynTree/RotationRaw.h" +#include "iDynTree/Rotation.h" +#include "iDynTree/Transform.h" +#include "iDynTree/TransformDerivative.h" +#include "iDynTree/Span.h" // Model related data structures -#include "iDynTree/Model/Indices.h" -#include "iDynTree/Model/LinkState.h" -#include "iDynTree/Model/Link.h" -#include "iDynTree/Model/IJoint.h" -#include "iDynTree/Model/FixedJoint.h" -#include "iDynTree/Model/MovableJointImpl.h" -#include "iDynTree/Model/RevoluteJoint.h" -#include "iDynTree/Model/PrismaticJoint.h" -#include "iDynTree/Model/Traversal.h" -#include "iDynTree/Model/SolidShapes.h" -#include "iDynTree/Model/Model.h" -#include "iDynTree/Model/JointState.h" -#include "iDynTree/Model/FreeFloatingMatrices.h" -#include "iDynTree/Model/FreeFloatingState.h" -#include "iDynTree/Model/ContactWrench.h" -#include "iDynTree/Model/ModelTestUtils.h" -#include "iDynTree/Model/ModelTransformers.h" -#include "iDynTree/Model/SubModel.h" +#include "iDynTree/Indices.h" +#include "iDynTree/LinkState.h" +#include "iDynTree/Link.h" +#include "iDynTree/IJoint.h" +#include "iDynTree/FixedJoint.h" +#include "iDynTree/MovableJointImpl.h" +#include "iDynTree/RevoluteJoint.h" +#include "iDynTree/PrismaticJoint.h" +#include "iDynTree/Traversal.h" +#include "iDynTree/SolidShapes.h" +#include "iDynTree/Model.h" +#include "iDynTree/JointState.h" +#include "iDynTree/FreeFloatingMatrices.h" +#include "iDynTree/FreeFloatingState.h" +#include "iDynTree/ContactWrench.h" +#include "iDynTree/ModelTestUtils.h" +#include "iDynTree/ModelTransformers.h" +#include "iDynTree/SubModel.h" // Kinematics & Dynamics related functions -#include "iDynTree/Model/ForwardKinematics.h" -#include "iDynTree/Model/Dynamics.h" -#include "iDynTree/Model/DenavitHartenberg.h" +#include "iDynTree/ForwardKinematics.h" +#include "iDynTree/Dynamics.h" +#include "iDynTree/DenavitHartenberg.h" // Sensors related data structures -#include "iDynTree/Sensors/Sensors.h" -#include "iDynTree/Sensors/SixAxisForceTorqueSensor.h" -#include "iDynTree/Sensors/AccelerometerSensor.h" -#include "iDynTree/Sensors/GyroscopeSensor.h" -#include "iDynTree/Sensors/ThreeAxisAngularAccelerometerSensor.h" -#include "iDynTree/Sensors/ThreeAxisForceTorqueContactSensor.h" -#include "iDynTree/Sensors/PredictSensorsMeasurements.h" +#include "iDynTree/Sensors.h" +#include "iDynTree/SixAxisForceTorqueSensor.h" +#include "iDynTree/AccelerometerSensor.h" +#include "iDynTree/GyroscopeSensor.h" +#include "iDynTree/ThreeAxisAngularAccelerometerSensor.h" +#include "iDynTree/ThreeAxisForceTorqueContactSensor.h" +#include "iDynTree/PredictSensorsMeasurements.h" // Model loading from external formats -#include "iDynTree/ModelIO/URDFDofsImport.h" -#include "iDynTree/ModelIO/ModelLoader.h" -#include "iDynTree/ModelIO/ModelExporter.h" -#include "iDynTree/ModelIO/ModelCalibrationHelper.h" +#include "iDynTree/URDFDofsImport.h" +#include "iDynTree/ModelLoader.h" +#include "iDynTree/ModelExporter.h" +#include "iDynTree/ModelCalibrationHelper.h" // Estimation related classes -#include "iDynTree/Estimation/ExternalWrenchesEstimation.h" -#include "iDynTree/Estimation/ExtWrenchesAndJointTorquesEstimator.h" -#include "iDynTree/Estimation/SimpleLeggedOdometry.h" -#include "iDynTree/Estimation/BerdyHelper.h" -#include "iDynTree/Estimation/BerdySparseMAPSolver.h" -#include "iDynTree/Estimation/AttitudeEstimator.h" -#include "iDynTree/Estimation/AttitudeMahonyFilter.h" -#include "iDynTree/Estimation/ExtendedKalmanFilter.h" -#include "iDynTree/Estimation/AttitudeQuaternionEKF.h" +#include "iDynTree/ExternalWrenchesEstimation.h" +#include "iDynTree/ExtWrenchesAndJointTorquesEstimator.h" +#include "iDynTree/SimpleLeggedOdometry.h" +#include "iDynTree/BerdyHelper.h" +#include "iDynTree/BerdySparseMAPSolver.h" +#include "iDynTree/AttitudeEstimator.h" +#include "iDynTree/AttitudeMahonyFilter.h" +#include "iDynTree/ExtendedKalmanFilter.h" +#include "iDynTree/AttitudeQuaternionEKF.h" // SolidShapes related classes #include "iDynTree/InertialParametersSolidShapesHelpers.h" diff --git a/bindings/pybind11/CMakeLists.txt b/bindings/pybind11/CMakeLists.txt index 3b2a5f09a74..7e4ef6adcbe 100644 --- a/bindings/pybind11/CMakeLists.txt +++ b/bindings/pybind11/CMakeLists.txt @@ -9,7 +9,6 @@ pybind11_add_module(pybind11_idyntree idyntree.cpp target_link_libraries(pybind11_idyntree PUBLIC idyntree-core idyntree-model - idyntree-sensors idyntree-modelio) # The generated Python dynamic module must have the same name as the pybind11 diff --git a/bindings/pybind11/idyntree_core.cpp b/bindings/pybind11/idyntree_core.cpp index aeb18f5de39..c48df152eb0 100644 --- a/bindings/pybind11/idyntree_core.cpp +++ b/bindings/pybind11/idyntree_core.cpp @@ -1,19 +1,19 @@ #include "idyntree_core.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include #include #include diff --git a/bindings/pybind11/idyntree_high_level.cpp b/bindings/pybind11/idyntree_high_level.cpp index c6abbfe0119..29258267ba5 100644 --- a/bindings/pybind11/idyntree_high_level.cpp +++ b/bindings/pybind11/idyntree_high_level.cpp @@ -1,9 +1,9 @@ -#include -#include -#include -#include +#include +#include +#include +#include #include -#include +#include #include #include #include diff --git a/bindings/pybind11/idyntree_model.cpp b/bindings/pybind11/idyntree_model.cpp index ae423f064ca..87ee7511e11 100644 --- a/bindings/pybind11/idyntree_model.cpp +++ b/bindings/pybind11/idyntree_model.cpp @@ -2,16 +2,16 @@ #include "error_utilities.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include #include diff --git a/bindings/pybind11/idyntree_modelio_urdf.cpp b/bindings/pybind11/idyntree_modelio_urdf.cpp index f4fe94257fa..91ebfec440e 100644 --- a/bindings/pybind11/idyntree_modelio_urdf.cpp +++ b/bindings/pybind11/idyntree_modelio_urdf.cpp @@ -4,10 +4,10 @@ #include #include -#include -#include -#include -#include +#include +#include +#include +#include #include #include diff --git a/bindings/pybind11/idyntree_sensors.cpp b/bindings/pybind11/idyntree_sensors.cpp index 2ade2842315..689e7b6d23d 100644 --- a/bindings/pybind11/idyntree_sensors.cpp +++ b/bindings/pybind11/idyntree_sensors.cpp @@ -5,9 +5,9 @@ #include #include -#include -#include -#include +#include +#include +#include namespace iDynTree { namespace bindings { diff --git a/bindings/pybind11/tests/test_idyntree_high_level.py b/bindings/pybind11/tests/test_idyntree_high_level.py index 508851aa4cc..0e1e450bdd7 100644 --- a/bindings/pybind11/tests/test_idyntree_high_level.py +++ b/bindings/pybind11/tests/test_idyntree_high_level.py @@ -1,4 +1,4 @@ -"""Tests for idyntree-sensors Python bindings.""" +"""Tests for idyntree-high-level Python bindings.""" import itertools import math import unittest diff --git a/bindings/pybind11/tests/test_idyntree_sensors.py b/bindings/pybind11/tests/test_idyntree_sensors.py index d4792cb1f07..f43bcd27cbc 100644 --- a/bindings/pybind11/tests/test_idyntree_sensors.py +++ b/bindings/pybind11/tests/test_idyntree_sensors.py @@ -1,4 +1,4 @@ -"""Tests for idyntree-sensors Python bindings.""" +"""Tests for sensors-related Python bindings.""" import itertools import unittest diff --git a/cmake/InstallBasicPackageFiles.cmake b/cmake/InstallBasicPackageFiles.cmake index 08ced4f9930..8a0f2c6e9d6 100644 --- a/cmake/InstallBasicPackageFiles.cmake +++ b/cmake/InstallBasicPackageFiles.cmake @@ -508,7 +508,9 @@ ${_IBPF_INCLUDE_CONTENT} unset(_set_include_dir_code) unset(_target_list) foreach(_target ${_targets}) - list(APPEND _target_list ${_IBPF_NAMESPACE}${_target}) + if(NOT ${_target} STREQUAL "idyntree-sensors") + list(APPEND _target_list ${_IBPF_NAMESPACE}${_target}) + endif() endforeach() if(DEFINED ${_IBPF_VARS_PREFIX}_BUILD_INCLUDEDIR OR DEFINED BUILD_${_IBPF_VARS_PREFIX}_INCLUDEDIR OR diff --git a/doc/dcTutorialCpp.md b/doc/dcTutorialCpp.md index ca1f9cb2edd..b0937b859de 100644 --- a/doc/dcTutorialCpp.md +++ b/doc/dcTutorialCpp.md @@ -9,7 +9,7 @@ avoid having to type the `iDynTree` namespace. #include #include -#include +#include using namespace iDynTree; ~~~ diff --git a/doc/dev/faqs.md b/doc/dev/faqs.md index b3f897eee2c..7d9871c8a6c 100644 --- a/doc/dev/faqs.md +++ b/doc/dev/faqs.md @@ -5,9 +5,9 @@ Some classes extremly used are the following: | Class name | Description | Doxygen docs | Headers (`.h`) | Source Code (`.cpp`) | Relevant Tests | |:----------:|:-----------:|:------------:|:--------------:|:-----------:|:-----:| -| `iDynTree::Transform` | Generic transform between two different frames. | [link](http://wiki.icub.org/codyco/dox/html/idyntree/html/classiDynTree_1_1Transform.html) | [ src/core/include/iDynTree/Core/Transform.h](../../src/core/include/iDynTree/Core/Transform.h) | [ src/core/src/Transform.cpp](../../src/core/src/Transform.cpp) | [src/core/tests](../../src/core/tests) | -| `iDynTree::Model` | Generic undirected (i.e. with no "base") multibody model. | [link](http://wiki.icub.org/codyco/dox/html/idyntree/html/classiDynTree_1_1Model.html) | [ src/model/include/iDynTree/Model/Model.h](../../src/model/include/iDynTree/Model/Model.h) | [ src/model/src/Model.cpp](../../src/model/src/Model.cpp) | [src/model/tests/ModelUnitTest.cpp](../../src/model/tests/ModelUnitTest.cpp) | -| `iDynTree::Traversal` | Class describing a order in which link are visited once a "base link" is choosen | [link](http://wiki.icub.org/codyco/dox/html/idyntree/html/classiDynTree_1_1Traversal.html) | [ src/model/include/iDynTree/Model/Traversal.h](../../src/model/include/iDynTree/Model/Traversal.h) | [ src/model/src/Traversal.cpp](../../src/model/src/Traversal.cpp) | [src/model/tests/ModelUnitTest.cpp](../../src/model/tests/ModelUnitTest.cpp) | +| `iDynTree::Transform` | Generic transform between two different frames. | [link](http://wiki.icub.org/codyco/dox/html/idyntree/html/classiDynTree_1_1Transform.html) | [ src/core/include/iDynTree/Transform.h](../../src/core/include/iDynTree/Transform.h) | [ src/core/src/Transform.cpp](../../src/core/src/Transform.cpp) | [src/core/tests](../../src/core/tests) | +| `iDynTree::Model` | Generic undirected (i.e. with no "base") multibody model. | [link](http://wiki.icub.org/codyco/dox/html/idyntree/html/classiDynTree_1_1Model.html) | [ src/model/include/iDynTree/Model.h](../../src/model/include/iDynTree/Model.h) | [ src/model/src/Model.cpp](../../src/model/src/Model.cpp) | [src/model/tests/ModelUnitTest.cpp](../../src/model/tests/ModelUnitTest.cpp) | +| `iDynTree::Traversal` | Class describing a order in which link are visited once a "base link" is choosen | [link](http://wiki.icub.org/codyco/dox/html/idyntree/html/classiDynTree_1_1Traversal.html) | [ src/model/include/iDynTree/Traversal.h](../../src/model/include/iDynTree/Traversal.h) | [ src/model/src/Traversal.cpp](../../src/model/src/Traversal.cpp) | [src/model/tests/ModelUnitTest.cpp](../../src/model/tests/ModelUnitTest.cpp) | If you need any additional doxygen comments, please [open an issue](https://github.com/robotology/idyntree/issues/new) detailing which point of the code you find difficul to understand. diff --git a/doc/generating-idyntree-matlab-bindings.md b/doc/generating-idyntree-matlab-bindings.md index 8d18027961d..ceecc6ace29 100644 --- a/doc/generating-idyntree-matlab-bindings.md +++ b/doc/generating-idyntree-matlab-bindings.md @@ -13,7 +13,7 @@ This can be found in the repository [https://github.com/robotology-dependencies/ This executable will be used to generate the iDynTree Matlab/Octave bindings for custom classes. ## Including the custom classes for bindings generations -In this section, as a toy example, let us consider a custom class `DiscreteKalmanFilterHelper` for which the bindings need to be generated. Let's assume the class `DiscreteKalmanFilterHelper` exists in the header file `iDynTree/Estimation/KalmanFilter.h`. +In this section, as a toy example, let us consider a custom class `DiscreteKalmanFilterHelper` for which the bindings need to be generated. Let's assume the class `DiscreteKalmanFilterHelper` exists in the header file `iDynTree/KalmanFilter.h`. - Take a glance at `bindings/iDynTree.i`. It follows a particular structure, adding the necessary header files twice for the desired classes, one using the prefix `#` and another using the prefix `%`. - It is also important to notice the order in which these headers are added, which ensures that the dependent header file is compiled after the dependee header file. @@ -22,11 +22,11 @@ In this section, as a toy example, let us consider a custom class `DiscreteKalma - Coming back to our toy example, we will add the header file in the section related to the `Estimation related classes`, in two different places ``` -#include "iDynTree/Estimation/KalmanFilter.h" +#include "iDynTree/KalmanFilter.h" ``` and ``` -%include "iDynTree/Estimation/KalmanFilter.h" +%include "iDynTree/KalmanFilter.h" ``` Now, all is left is to compile and generate the bindings. diff --git a/examples/cxx/InverseKinematics/iDynTreeExampleInverseKinematics.cpp b/examples/cxx/InverseKinematics/iDynTreeExampleInverseKinematics.cpp index 54a98766491..62efed37b3d 100644 --- a/examples/cxx/InverseKinematics/iDynTreeExampleInverseKinematics.cpp +++ b/examples/cxx/InverseKinematics/iDynTreeExampleInverseKinematics.cpp @@ -18,7 +18,7 @@ // iDynTree headers #include -#include +#include #include int main(int argc, char *argv[]) diff --git a/examples/cxx/KinDynComputationsWithEigen/main.cpp b/examples/cxx/KinDynComputationsWithEigen/main.cpp index 0dbaaa56e6f..e5a307306be 100644 --- a/examples/cxx/KinDynComputationsWithEigen/main.cpp +++ b/examples/cxx/KinDynComputationsWithEigen/main.cpp @@ -17,13 +17,13 @@ #include // iDynTree headers -#include +#include #include -#include +#include // Helpers function to convert between // iDynTree datastructures -#include +#include /** diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 557d90c7577..25e1186e451 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -10,7 +10,6 @@ endif() add_subdirectory(core) add_subdirectory(model) -add_subdirectory(sensors) add_subdirectory(model_io) add_subdirectory(estimation) add_subdirectory(solid-shapes) diff --git a/src/core/CMakeLists.txt b/src/core/CMakeLists.txt index b7353c456dd..99d9c2f3abd 100644 --- a/src/core/CMakeLists.txt +++ b/src/core/CMakeLists.txt @@ -1,45 +1,45 @@ # SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) # SPDX-License-Identifier: BSD-3-Clause -set(IDYNTREE_CORE_EXP_HEADERS include/iDynTree/Core/Axis.h - include/iDynTree/Core/ArticulatedBodyInertia.h - include/iDynTree/Core/ClassicalAcc.h - include/iDynTree/Core/Direction.h - include/iDynTree/Core/EigenSparseHelpers.h - include/iDynTree/Core/EigenMathHelpers.h - include/iDynTree/Core/EigenHelpers.h - include/iDynTree/Core/InertiaNonLinearParametrization.h - include/iDynTree/Core/MatrixDynSize.h - include/iDynTree/Core/MatrixFixSize.h - include/iDynTree/Core/Position.h - include/iDynTree/Core/PositionRaw.h - include/iDynTree/Core/Rotation.h - include/iDynTree/Core/RotationRaw.h - include/iDynTree/Core/RotationalInertiaRaw.h - include/iDynTree/Core/SpatialAcc.h - include/iDynTree/Core/SpatialForceVector.h - include/iDynTree/Core/SpatialInertiaRaw.h - include/iDynTree/Core/SpatialInertia.h - include/iDynTree/Core/SpatialMomentum.h - include/iDynTree/Core/SpatialMotionVector.h - include/iDynTree/Core/TestUtils.h - include/iDynTree/Core/Transform.h - include/iDynTree/Core/TransformDerivative.h - include/iDynTree/Core/Twist.h - include/iDynTree/Core/Utils.h - include/iDynTree/Core/VectorFixSize.h - include/iDynTree/Core/VectorDynSize.h - include/iDynTree/Core/Wrench.h - include/iDynTree/Core/PrivateUtils.h - include/iDynTree/Core/PrivatePreProcessorUtils.h - include/iDynTree/Core/GeomVector3.h - include/iDynTree/Core/SpatialVector.h - include/iDynTree/Core/SparseMatrix.h - include/iDynTree/Core/Triplets.h - include/iDynTree/Core/CubicSpline.h - include/iDynTree/Core/Span.h - include/iDynTree/Core/SO3Utils.h - include/iDynTree/Core/MatrixView.h) +set(IDYNTREE_CORE_EXP_HEADERS include/iDynTree/Axis.h + include/iDynTree/ArticulatedBodyInertia.h + include/iDynTree/ClassicalAcc.h + include/iDynTree/Direction.h + include/iDynTree/EigenSparseHelpers.h + include/iDynTree/EigenMathHelpers.h + include/iDynTree/EigenHelpers.h + include/iDynTree/InertiaNonLinearParametrization.h + include/iDynTree/MatrixDynSize.h + include/iDynTree/MatrixFixSize.h + include/iDynTree/Position.h + include/iDynTree/PositionRaw.h + include/iDynTree/Rotation.h + include/iDynTree/RotationRaw.h + include/iDynTree/RotationalInertiaRaw.h + include/iDynTree/SpatialAcc.h + include/iDynTree/SpatialForceVector.h + include/iDynTree/SpatialInertiaRaw.h + include/iDynTree/SpatialInertia.h + include/iDynTree/SpatialMomentum.h + include/iDynTree/SpatialMotionVector.h + include/iDynTree/TestUtils.h + include/iDynTree/Transform.h + include/iDynTree/TransformDerivative.h + include/iDynTree/Twist.h + include/iDynTree/Utils.h + include/iDynTree/VectorFixSize.h + include/iDynTree/VectorDynSize.h + include/iDynTree/Wrench.h + include/iDynTree/PrivateUtils.h + include/iDynTree/PrivatePreProcessorUtils.h + include/iDynTree/GeomVector3.h + include/iDynTree/SpatialVector.h + include/iDynTree/SparseMatrix.h + include/iDynTree/Triplets.h + include/iDynTree/CubicSpline.h + include/iDynTree/Span.h + include/iDynTree/SO3Utils.h + include/iDynTree/MatrixView.h) set(IDYNTREE_CORE_EXP_SOURCES src/Axis.cpp @@ -77,9 +77,9 @@ SOURCE_GROUP("Source Files" FILES ${IDYNTREE_CORE_EXP_SOURCES}) SOURCE_GROUP("Header Files" FILES ${IDYNTREE_CORE_EXP_HEADERS}) # Check if this does not break existing build -# reason: avoid including with inside .cpp but using directly +# reason: avoid including with inside .cpp but using directly # "**" which clearly states the difference between in-library files and external files -include_directories(include/iDynTree/Core) +include_directories(include/iDynTree) set(libraryname idyntree-core) @@ -130,11 +130,15 @@ install(TARGETS ${libraryname} RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT bin LIBRARY DESTINATION "${CMAKE_INSTALL_LIBDIR}" COMPONENT shlib ARCHIVE DESTINATION "${CMAKE_INSTALL_LIBDIR}" COMPONENT lib - PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/iDynTree/Core - PRIVATE_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/iDynTree/Core/impl) + PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/iDynTree + PRIVATE_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/iDynTree/impl) set_property(GLOBAL APPEND PROPERTY ${VARS_PREFIX}_TARGETS ${libraryname}) +# Install deprecated headers +install(DIRECTORY include/iDynTree + DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/iDynTree) + if(IDYNTREE_COMPILE_TESTS) add_subdirectory(tests) endif(IDYNTREE_COMPILE_TESTS) diff --git a/src/core/include/iDynTree/ArticulatedBodyInertia.h b/src/core/include/iDynTree/ArticulatedBodyInertia.h new file mode 100644 index 00000000000..a668c518519 --- /dev/null +++ b/src/core/include/iDynTree/ArticulatedBodyInertia.h @@ -0,0 +1,134 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_ARTICULATED_BODY_INERTIA_H +#define IDYNTREE_ARTICULATED_BODY_INERTIA_H + +#include +#include + +namespace iDynTree +{ + class SpatialInertia; + class SpatialForceVector; + class SpatialMotionVector; + class Wrench; + class SpatialAcc; + + /** + * Class representing an Articulated Body Inertia. + * + * For more information on Articulated Body Inertia, please + * check Featherstone 2008, Chapter 7 . + * + * Storage: + * The symmetric articulated body inertia is stored as 3 + * 3x3 matrices : + * * the linearLinear is the top left submatrix + * * the linearAngular is the top right submatrix + * * the angularAngular is the bottom right submatrix + * + * The bottom left submatrix can be obtained as the + * transpose of the linearAngular matrix. + * + * + * + * \warning This class is exposing for convenience the IMatrix interface. + * Notice that using this methods you can damage the underlyng articulated body inertia. + * In doubt, don't use them and rely on more high level functions. + * + * \ingroup iDynTreeCore + */ + class ArticulatedBodyInertia + { + private: + Matrix3x3 linearLinear; + Matrix3x3 linearAngular; + Matrix3x3 angularAngular; + + public: + /** + * The data is not reset to zeo for perfomance reason. + * Please initialize the data in the class before any use. + */ + ArticulatedBodyInertia(); + + /** + * Constructor from a buffer of 36 doubles, + * stored as a C-style array (i.e. row major). + * + */ + ArticulatedBodyInertia(const double* in_data, + const unsigned int in_rows, + const unsigned int in_cols); + + /** + * Constructor from a Rigid Body Inertia. + */ + ArticulatedBodyInertia(const iDynTree::SpatialInertia& rigidBodyInertia); + + /** + * Copy constructor: create a ArticulatedBodyInertia from another ArticulatedBodyInertia. + */ + ArticulatedBodyInertia(const ArticulatedBodyInertia & other); + + /** + * Low level data getters. + */ + Matrix3x3 & getLinearLinearSubmatrix(); + Matrix3x3 & getLinearAngularSubmatrix(); + Matrix3x3 & getAngularAngularSubmatrix(); + const Matrix3x3 & getLinearLinearSubmatrix() const; + const Matrix3x3 & getLinearAngularSubmatrix() const; + const Matrix3x3 & getAngularAngularSubmatrix() const; + + // Operations on SpatialInertia + static ArticulatedBodyInertia combine(const ArticulatedBodyInertia & op1, + const ArticulatedBodyInertia & op2); + + /** + * Return the inverse of this ArticulatedBodyInertia + * applied to the passed wrench. + */ + SpatialAcc applyInverse(const Wrench & wrench) const; + + /** + * Get the ArticulatedBodyInertia as a 6x6 matrix + */ + Matrix6x6 asMatrix() const; + + /** + * Get the inverse of the ArticulatedBodyInertia matrix. + */ + Matrix6x6 getInverse() const; + + // overloaded operators + ArticulatedBodyInertia operator+(const ArticulatedBodyInertia& other) const; + ArticulatedBodyInertia operator-(const ArticulatedBodyInertia& other) const; + ArticulatedBodyInertia& operator+=(const ArticulatedBodyInertia& other); + SpatialForceVector operator*(const SpatialMotionVector &other) const; + Wrench operator*(const SpatialAcc &other) const; + + + /** reset to zero (i.e. the inertia of body with zero mass) the ArticulatedBodyInertia */ + void zero(); + + // Static helpers + + /** + * Build the ArticulatedInertia \f$U d^{-1} U^\top\f$ . + * Used in the articulated body algorithm . + */ + static ArticulatedBodyInertia ABADyadHelper(const SpatialForceVector & U, const double d); + + /** + * Build the ArticulatedInertia dU inv_d U^\top + U d_inv_d U^\top + U inv_d dU^\top + * Used in the linearization of the articulated body algorithm. + */ + static ArticulatedBodyInertia ABADyadHelperLin(const SpatialForceVector & U, const double inv_d, + const SpatialForceVector & dU, const double d_inv_d); + + }; +} + +#endif diff --git a/src/core/include/iDynTree/Axis.h b/src/core/include/iDynTree/Axis.h new file mode 100644 index 00000000000..b23bc51d35a --- /dev/null +++ b/src/core/include/iDynTree/Axis.h @@ -0,0 +1,142 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_AXIS_H +#define IDYNTREE_AXIS_H + +#include + +#include +#include +#include + +namespace iDynTree +{ + class Transform; + class TransformDerivative; + class Twist; + class SpatialAcc; + /** + * Class representing an axis (a directed line) in space. + * + * The axis is represented as a origin plus a direction. + * + * \ingroup iDynTreeCore + * + * + */ + class Axis + { + private: + Direction direction; + Position origin; + + /** + * Set the object to the default axis: direction : 1, 0, 0 , point: 0, 0, 0 + */ + void setToDefault(); + + public: + /** + * Default constructor. + * The data is not reset to the default for perfomance reason. + * Please initialize the data in the class before any use. + */ + inline Axis() {} + + /** + * Constructor from a Direction and an origin, represented by a Position object. + */ + Axis(const Direction & _direction, const Position & _origin); + + /** + * Copy constructor: create a Axis from another Axis + */ + Axis(const Axis & other); + + /** + * Get the direction of the axis + */ + const Direction & getDirection() const; + + /** + * Get the origin of the axis + */ + const Position & getOrigin() const; + + /** + * Set the direction of the axis + */ + void setDirection(const Direction & _direction); + + /** + * Set the origin of the axis + */ + void setOrigin(const Position & _position); + + /** + * Get the transform induced by a rotation of an angle theta + * around this axis. The returned transform is the nonRotated_T_rotated, + * such that if we have a quantity expressed in the frame obtained + * by the rotation v_rotated, we can transform it back in the + * non-rotated frame using the returned transform: + * v_nonRotated = nonRotated_T_rotated*v_rotated + */ + Transform getRotationTransform(const double theta) const; + + /** + * Get the derivative of the getRotationTransform function with respect + * to the theta argument. + */ + TransformDerivative getRotationTransformDerivative(const double theta) const; + + Twist getRotationTwist(const double dtheta) const; + + SpatialAcc getRotationSpatialAcc(const double d2theta) const; + + /** + * Get the transform induced by a translation of a distance dist + * along this axis. The returned transform is the nonTranslated_T_translated, + * such that if we have a quantity expressed in the frame obtained + * by the translation v_translated, we can transform it back in the + * non-translated frame using the returned transform: + * v_nonTranslated = nonTranslated_T_translated*v_translated + */ + Transform getTranslationTransform(const double dist) const; + + /** + * Get the derivative of the getTranslationTransform function with respect + * to the dist argument. + */ + TransformDerivative getTranslationTransformDerivative(const double /*dist*/) const; + + Twist getTranslationTwist(const double ddist) const; + + SpatialAcc getTranslationSpatialAcc(const double d2dist) const; + + /** + * Check if two axes are parallel (i.e. their direction are parallel). + * + * @param otherAxis the axes to check for parallelism. + * @param tolerance tolerance to use in the parallelism check. + */ + bool isParallel(const Axis & otherAxis, const double tolerance) const; + + /** + * Return the axis with the same origin, but reversed direction. + */ + Axis reverse() const; + + /** + * @name Output helpers. + * Output helpers. + */ + ///@{ + std::string toString() const; + + std::string reservedToString() const; + ///@} + }; +} + +#endif diff --git a/src/core/include/iDynTree/ClassicalAcc.h b/src/core/include/iDynTree/ClassicalAcc.h new file mode 100644 index 00000000000..63a20251727 --- /dev/null +++ b/src/core/include/iDynTree/ClassicalAcc.h @@ -0,0 +1,69 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_CLASSICAL_ACC_H +#define IDYNTREE_CLASSICAL_ACC_H + +#include + +namespace iDynTree +{ + class RotationRaw; + class SpatialAcc; + class Twist; + + /** + * Class representing a classical 6D acceleration, i.e. the concatenation + * of the 3d vector of the acceleration of a point and of the 3d vector + * of the derivative of an angular velocity. + * + * \note Given that the classical acceleration is just a combination of two + * 3d vectors and not a spatial quantity, you + * cannot directly express a classical acceleration in + * a different reference frame. You have to convert it before + * in a spatial acceleration, and then transform the spatial acceleration + * in a new reference frame (and the corresponding twist) and then + * convert them back to a classical acceleation. + * However you can still change the orientation frame of the two 3d vector + * that compose the classical acceleration without any conversion. + * + * + * + * \ingroup iDynTreeCore + */ + class ClassicalAcc: public Vector6 + { + public: + /** + * Default constructor. + * The data is not reset to the default for perfomance reason. + * Please initialize the data in the class before any use. + */ + inline ClassicalAcc() {} + ClassicalAcc(const double* in_data, const unsigned int in_size); + ClassicalAcc(const ClassicalAcc& other); + + /* Geometric operations */ + const ClassicalAcc & changeCoordFrame(const RotationRaw & newCoordFrame); + + /** constructor helpers */ + static ClassicalAcc Zero(); + + Vector3 getLinearVec3() const; + Vector3 getAngularVec3() const; + + /** + * Build a classical acceleration from a spatial acceleation and spatial twist + * of a body. + */ + void fromSpatial(const SpatialAcc& spatialAcc, const Twist& vel); + + /** + * Convert a classical acceleation to a spatial one. + */ + void toSpatial(SpatialAcc & spatialAcc, const Twist & vel) const; + + }; +} + +#endif diff --git a/src/core/include/iDynTree/Core/ArticulatedBodyInertia.h b/src/core/include/iDynTree/Core/ArticulatedBodyInertia.h index 47d9102968f..567cd362a15 100644 --- a/src/core/include/iDynTree/Core/ArticulatedBodyInertia.h +++ b/src/core/include/iDynTree/Core/ArticulatedBodyInertia.h @@ -1,134 +1,13 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef IDYNTREE_ARTICULATED_BODY_INERTIA_H -#define IDYNTREE_ARTICULATED_BODY_INERTIA_H +#ifndef IDYNTREE_CORE_ARTICULATED_BODY_INERTIA_H +#define IDYNTREE_CORE_ARTICULATED_BODY_INERTIA_H -#include -#include - -namespace iDynTree -{ - class SpatialInertia; - class SpatialForceVector; - class SpatialMotionVector; - class Wrench; - class SpatialAcc; - - /** - * Class representing an Articulated Body Inertia. - * - * For more information on Articulated Body Inertia, please - * check Featherstone 2008, Chapter 7 . - * - * Storage: - * The symmetric articulated body inertia is stored as 3 - * 3x3 matrices : - * * the linearLinear is the top left submatrix - * * the linearAngular is the top right submatrix - * * the angularAngular is the bottom right submatrix - * - * The bottom left submatrix can be obtained as the - * transpose of the linearAngular matrix. - * - * - * - * \warning This class is exposing for convenience the IMatrix interface. - * Notice that using this methods you can damage the underlyng articulated body inertia. - * In doubt, don't use them and rely on more high level functions. - * - * \ingroup iDynTreeCore - */ - class ArticulatedBodyInertia - { - private: - Matrix3x3 linearLinear; - Matrix3x3 linearAngular; - Matrix3x3 angularAngular; - - public: - /** - * The data is not reset to zeo for perfomance reason. - * Please initialize the data in the class before any use. - */ - ArticulatedBodyInertia(); - - /** - * Constructor from a buffer of 36 doubles, - * stored as a C-style array (i.e. row major). - * - */ - ArticulatedBodyInertia(const double* in_data, - const unsigned int in_rows, - const unsigned int in_cols); - - /** - * Constructor from a Rigid Body Inertia. - */ - ArticulatedBodyInertia(const iDynTree::SpatialInertia& rigidBodyInertia); - - /** - * Copy constructor: create a ArticulatedBodyInertia from another ArticulatedBodyInertia. - */ - ArticulatedBodyInertia(const ArticulatedBodyInertia & other); - - /** - * Low level data getters. - */ - Matrix3x3 & getLinearLinearSubmatrix(); - Matrix3x3 & getLinearAngularSubmatrix(); - Matrix3x3 & getAngularAngularSubmatrix(); - const Matrix3x3 & getLinearLinearSubmatrix() const; - const Matrix3x3 & getLinearAngularSubmatrix() const; - const Matrix3x3 & getAngularAngularSubmatrix() const; - - // Operations on SpatialInertia - static ArticulatedBodyInertia combine(const ArticulatedBodyInertia & op1, - const ArticulatedBodyInertia & op2); - - /** - * Return the inverse of this ArticulatedBodyInertia - * applied to the passed wrench. - */ - SpatialAcc applyInverse(const Wrench & wrench) const; - - /** - * Get the ArticulatedBodyInertia as a 6x6 matrix - */ - Matrix6x6 asMatrix() const; - - /** - * Get the inverse of the ArticulatedBodyInertia matrix. - */ - Matrix6x6 getInverse() const; - - // overloaded operators - ArticulatedBodyInertia operator+(const ArticulatedBodyInertia& other) const; - ArticulatedBodyInertia operator-(const ArticulatedBodyInertia& other) const; - ArticulatedBodyInertia& operator+=(const ArticulatedBodyInertia& other); - SpatialForceVector operator*(const SpatialMotionVector &other) const; - Wrench operator*(const SpatialAcc &other) const; - - - /** reset to zero (i.e. the inertia of body with zero mass) the ArticulatedBodyInertia */ - void zero(); - - // Static helpers - - /** - * Build the ArticulatedInertia \f$U d^{-1} U^\top\f$ . - * Used in the articulated body algorithm . - */ - static ArticulatedBodyInertia ABADyadHelper(const SpatialForceVector & U, const double d); - - /** - * Build the ArticulatedInertia dU inv_d U^\top + U d_inv_d U^\top + U inv_d dU^\top - * Used in the linearization of the articulated body algorithm. - */ - static ArticulatedBodyInertia ABADyadHelperLin(const SpatialForceVector & U, const double inv_d, - const SpatialForceVector & dU, const double d_inv_d); +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif - }; -} +#include #endif diff --git a/src/core/include/iDynTree/Core/Axis.h b/src/core/include/iDynTree/Core/Axis.h index ff16fdcc4c2..95e60445d62 100644 --- a/src/core/include/iDynTree/Core/Axis.h +++ b/src/core/include/iDynTree/Core/Axis.h @@ -1,142 +1,13 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef IDYNTREE_AXIS_H -#define IDYNTREE_AXIS_H +#ifndef IDYNTREE_CORE_AXIS_H +#define IDYNTREE_CORE_AXIS_H -#include - -#include -#include -#include - -namespace iDynTree -{ - class Transform; - class TransformDerivative; - class Twist; - class SpatialAcc; - /** - * Class representing an axis (a directed line) in space. - * - * The axis is represented as a origin plus a direction. - * - * \ingroup iDynTreeCore - * - * - */ - class Axis - { - private: - Direction direction; - Position origin; - - /** - * Set the object to the default axis: direction : 1, 0, 0 , point: 0, 0, 0 - */ - void setToDefault(); - - public: - /** - * Default constructor. - * The data is not reset to the default for perfomance reason. - * Please initialize the data in the class before any use. - */ - inline Axis() {} - - /** - * Constructor from a Direction and an origin, represented by a Position object. - */ - Axis(const Direction & _direction, const Position & _origin); - - /** - * Copy constructor: create a Axis from another Axis - */ - Axis(const Axis & other); - - /** - * Get the direction of the axis - */ - const Direction & getDirection() const; - - /** - * Get the origin of the axis - */ - const Position & getOrigin() const; - - /** - * Set the direction of the axis - */ - void setDirection(const Direction & _direction); - - /** - * Set the origin of the axis - */ - void setOrigin(const Position & _position); - - /** - * Get the transform induced by a rotation of an angle theta - * around this axis. The returned transform is the nonRotated_T_rotated, - * such that if we have a quantity expressed in the frame obtained - * by the rotation v_rotated, we can transform it back in the - * non-rotated frame using the returned transform: - * v_nonRotated = nonRotated_T_rotated*v_rotated - */ - Transform getRotationTransform(const double theta) const; - - /** - * Get the derivative of the getRotationTransform function with respect - * to the theta argument. - */ - TransformDerivative getRotationTransformDerivative(const double theta) const; - - Twist getRotationTwist(const double dtheta) const; - - SpatialAcc getRotationSpatialAcc(const double d2theta) const; - - /** - * Get the transform induced by a translation of a distance dist - * along this axis. The returned transform is the nonTranslated_T_translated, - * such that if we have a quantity expressed in the frame obtained - * by the translation v_translated, we can transform it back in the - * non-translated frame using the returned transform: - * v_nonTranslated = nonTranslated_T_translated*v_translated - */ - Transform getTranslationTransform(const double dist) const; - - /** - * Get the derivative of the getTranslationTransform function with respect - * to the dist argument. - */ - TransformDerivative getTranslationTransformDerivative(const double /*dist*/) const; - - Twist getTranslationTwist(const double ddist) const; - - SpatialAcc getTranslationSpatialAcc(const double d2dist) const; - - /** - * Check if two axes are parallel (i.e. their direction are parallel). - * - * @param otherAxis the axes to check for parallelism. - * @param tolerance tolerance to use in the parallelism check. - */ - bool isParallel(const Axis & otherAxis, const double tolerance) const; - - /** - * Return the axis with the same origin, but reversed direction. - */ - Axis reverse() const; - - /** - * @name Output helpers. - * Output helpers. - */ - ///@{ - std::string toString() const; +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif - std::string reservedToString() const; - ///@} - }; -} +#include #endif diff --git a/src/core/include/iDynTree/Core/ClassicalAcc.h b/src/core/include/iDynTree/Core/ClassicalAcc.h index 8fb3afeedc8..50ebd96ca3e 100644 --- a/src/core/include/iDynTree/Core/ClassicalAcc.h +++ b/src/core/include/iDynTree/Core/ClassicalAcc.h @@ -1,69 +1,13 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef IDYNTREE_CLASSICAL_ACC_H -#define IDYNTREE_CLASSICAL_ACC_H +#ifndef IDYNTREE_CORE_CLASSICAL_ACC_H +#define IDYNTREE_CORE_CLASSICAL_ACC_H -#include - -namespace iDynTree -{ - class RotationRaw; - class SpatialAcc; - class Twist; - - /** - * Class representing a classical 6D acceleration, i.e. the concatenation - * of the 3d vector of the acceleration of a point and of the 3d vector - * of the derivative of an angular velocity. - * - * \note Given that the classical acceleration is just a combination of two - * 3d vectors and not a spatial quantity, you - * cannot directly express a classical acceleration in - * a different reference frame. You have to convert it before - * in a spatial acceleration, and then transform the spatial acceleration - * in a new reference frame (and the corresponding twist) and then - * convert them back to a classical acceleation. - * However you can still change the orientation frame of the two 3d vector - * that compose the classical acceleration without any conversion. - * - * - * - * \ingroup iDynTreeCore - */ - class ClassicalAcc: public Vector6 - { - public: - /** - * Default constructor. - * The data is not reset to the default for perfomance reason. - * Please initialize the data in the class before any use. - */ - inline ClassicalAcc() {} - ClassicalAcc(const double* in_data, const unsigned int in_size); - ClassicalAcc(const ClassicalAcc& other); - - /* Geometric operations */ - const ClassicalAcc & changeCoordFrame(const RotationRaw & newCoordFrame); - - /** constructor helpers */ - static ClassicalAcc Zero(); - - Vector3 getLinearVec3() const; - Vector3 getAngularVec3() const; - - /** - * Build a classical acceleration from a spatial acceleation and spatial twist - * of a body. - */ - void fromSpatial(const SpatialAcc& spatialAcc, const Twist& vel); - - /** - * Convert a classical acceleation to a spatial one. - */ - void toSpatial(SpatialAcc & spatialAcc, const Twist & vel) const; +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif - }; -} +#include #endif diff --git a/src/core/include/iDynTree/Core/CubicSpline.h b/src/core/include/iDynTree/Core/CubicSpline.h index 2f80f1afbc9..ee12a62d134 100644 --- a/src/core/include/iDynTree/Core/CubicSpline.h +++ b/src/core/include/iDynTree/Core/CubicSpline.h @@ -2,44 +2,13 @@ // SPDX-License-Identifier: BSD-3-Clause -#ifndef IDYNTREE_CUBIC_SPLINE_H -#define IDYNTREE_CUBIC_SPLINE_H +#ifndef IDYNTREE_CORE_CUBIC_SPLINE_H +#define IDYNTREE_CORE_CUBIC_SPLINE_H -#include -#include -#include - -namespace iDynTree -{ - class CubicSpline { - std::vector< iDynTree::Vector4 > m_coefficients; - iDynTree::VectorDynSize m_velocities; - iDynTree::VectorDynSize m_time; - iDynTree::VectorDynSize m_y; - iDynTree::VectorDynSize m_T; - - double m_v0; - double m_vf; - double m_a0; - double m_af; - - bool computePhasesDuration(); - bool computeIntermediateVelocities(); - bool computeCoefficients(); - - bool m_areCoefficientsUpdated; - - public: - CubicSpline(); - - CubicSpline(unsigned int buffersDimension); +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif - bool setData(const iDynTree::VectorDynSize& time, const iDynTree::VectorDynSize& yData); - void setInitialConditions(double initialVelocity, double initialAcceleration); - void setFinalConditions(double finalVelocity, double finalAcceleration); - double evaluatePoint(double t); - double evaluatePoint(double t, double& velocity, double& acceleration); - }; -} +#include #endif diff --git a/src/core/include/iDynTree/Core/Direction.h b/src/core/include/iDynTree/Core/Direction.h index 47d1367ecdc..7b93da535f1 100644 --- a/src/core/include/iDynTree/Core/Direction.h +++ b/src/core/include/iDynTree/Core/Direction.h @@ -1,99 +1,13 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef IDYNTREE_DIRECTION_H -#define IDYNTREE_DIRECTION_H +#ifndef IDYNTREE_CORE_DIRECTION_H +#define IDYNTREE_CORE_DIRECTION_H -#include - -#include -#include - - -namespace iDynTree -{ - /** - * Class representing the coordinates of a direction in the 3D space - * - * \ingroup iDynTreeCore - * - */ - class Direction: public Vector3 - { - private: - /** - * Set the object to the default direction : 1, 0, 0 . - */ - void setToDefault(); - - public: - /** - * Default constructor. - * The data is not reset to the default for perfomance reason. - * Please initialize the data in the class before any use. - */ - inline Direction() {} - - /** - * Constructor from 3 doubles: initialize the direction with the passed values. - * The vector passed is normalized to ensure that the direction is stored as a unit vector. - */ - Direction(double x, double y, double z); - - /** - * Copy constructor: create a Direction from another Direction - */ - Direction(const Direction & other); - - /** - * Copy constructor: create a Direction from a 3 double buffer - */ - Direction(const double* in_data, const unsigned int in_size); - - /** - * Normalize the representation of the direction, useful if - * the coordinates of the direction has been manually setted - * and you want to be sure that this direction is actually - * a unit vector. - * - * @param tol if the norm of the vector < tol, set the direction to 1,0,0 - */ - void Normalize(double tol=DEFAULT_TOL); - - /** - * Check if two directions are parallel. - * - * @param otherDirection the direction to check for parallelism. - * @param tolerance tolerance to use in the parallelism check. - */ - bool isParallel(const Direction & otherDirection, double tolerance) const; - - /** - * Check if two directions are perpendicular. - * - * @param otherDirection the direction to check for the perpendicular check. - * @param tolerance tolerance to use in the perpendicular check. - */ - bool isPerpendicular(const Direction & otherDirection, double tolerance) const; - - /** - * Return the direction, i.e. return its opposite. - * - */ - Direction reverse() const; - - /** - * @name Output helpers. - * Output helpers. - */ - ///@{ - std::string toString() const; - - std::string reservedToString() const; - ///@} +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif - static Direction Default(); - }; -} +#include #endif diff --git a/src/core/include/iDynTree/Core/EigenHelpers.h b/src/core/include/iDynTree/Core/EigenHelpers.h index 077bacbebf7..38c9f4ba8be 100644 --- a/src/core/include/iDynTree/Core/EigenHelpers.h +++ b/src/core/include/iDynTree/Core/EigenHelpers.h @@ -1,403 +1,13 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef IDYNTREE_EIGEN_HELPERS_H -#define IDYNTREE_EIGEN_HELPERS_H +#ifndef IDYNTREE_CORE_EIGEN_HELPERS_H +#define IDYNTREE_CORE_EIGEN_HELPERS_H -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#if __cplusplus > 199711L -#include -#endif - - -namespace iDynTree -{ - //Useful typedefs - //TODO: change methods below to use these typedefs - typedef Eigen::Map iDynTreeEigenVector; - typedef Eigen::Map iDynTreeEigenConstVector; - typedef Eigen::Matrix iDynTreeEigenMatrix; - typedef const Eigen::Matrix iDynTreeEigenConstMatrix; - typedef Eigen::Map iDynTreeEigenMatrixMap; - typedef Eigen::Map iDynTreeEigenConstMatrixMap; - -#if __cplusplus > 199711L - template - struct is_sparsematrix : std::false_type {}; - - template - struct is_sparsematrix> : std::true_type {}; -#endif - - - -// Dynamics size toEigen methods -inline Eigen::Map toEigen(VectorDynSize & vec) -{ - return Eigen::Map(vec.data(),vec.size()); -} - -inline Eigen::Map< Eigen::Matrix > toEigen(MatrixDynSize & mat) -{ - return Eigen::Map< Eigen::Matrix >(mat.data(),mat.rows(),mat.cols()); -} - -inline Eigen::Map toEigen(const VectorDynSize & vec) -{ - return Eigen::Map(vec.data(),vec.size()); -} - -#if !defined(SWIG_VERSION) || SWIG_VERSION >= 0x030000 -inline Eigen::Map toEigen(Span vec) -{ - return Eigen::Map(vec.data(),vec.size()); -} - -inline Eigen::Map toEigen(iDynTree::Span vec) -{ - return Eigen::Map(vec.data(),vec.size()); -} - -inline Eigen::Map, - 0, - Eigen::Stride> -toEigen(const MatrixView& mat) -{ - using MatrixRowMajor = Eigen::Matrix; - - // This is a trick required to see a ColumnMajor matrix as a RowMajor matrix. - // - // Given the following matrix - // _ _ _ - // / \ _____ | 1 2 3 4 5 | - // / _ \ |_____| | | - // / ___ \ |_____| | | - // /_/ \_\ |_ 6 7 8 9 10 _| - // - // If the matrix is stored as RowMajor matrix there will be a vector v_row in which - // the elements are saved - // v_row = [1 2 3 4 5 6 7 8 9 10] - // - // If the matrix is stored as ColumnMajor matrix there will be a vector v_col in which - // the elements are saved - // v_col = [1 6 2 7 3 8 4 9 5 10] - // - // Our goal here is to build a RowMajor Matrix (independently it is RowMajor/ColumnMajor) - // starting from the raw vactor (v_row, v_col) - // - // From the Eigen documentation https://eigen.tuxfamily.org/dox/classEigen_1_1Stride.html - // The inner stride is the pointer increment between two consecutive entries within a given row of a row-major matrix. - // The outer stride is the pointer increment between two consecutive rows of a row-major matrix. - // - // Starting from v_row we can build a RowMajor matrix by choosing the following pair of strides - // - inner_stride = 1 - // - outer_stride = 5 = number of columns of A - // - // Starting from v_col we can build a RowMajor matrix by choosing the following pair of strides - // - inner_stride = 2 = number of rows of A - // - outer_stride = 1 - - const std::ptrdiff_t innerStride = (mat.storageOrder() == MatrixStorageOrdering::ColumnMajor) ? mat.rows() : 1; - const std::ptrdiff_t outerStride = (mat.storageOrder() == MatrixStorageOrdering::ColumnMajor) ? 1 : mat.cols(); - - return Eigen::Map>( - mat.data(), - mat.rows(), - mat.cols(), - Eigen::Stride(outerStride, innerStride)); -} - -inline Eigen::Map, - 0, - Eigen::Stride> -toEigen(const MatrixView& mat) -{ - using MatrixRowMajor = Eigen::Matrix; - - // This is a trick required to see a ColumnMajor matrix as a RowMajor matrix. - // - // Given the following matrix - // _ _ _ - // / \ _____ | 1 2 3 4 5 | - // / _ \ |_____| | | - // / ___ \ |_____| | | - // /_/ \_\ |_ 6 7 8 9 10 _| - // - // If the matrix is stored as RowMajor matrix there will be a vector v_row in which - // the elements are saved as - // v_row = [1 2 3 4 5 6 7 8 9 10] - // - // If the matrix is stored as ColumnMajor matrix there will be a vector v_col in which - // the elements are saved as - // v_col = [1 6 2 7 3 8 4 9 5 10] - // - // Our goal here is to build a RowMajor Matrix (independently it is RowMajor/ColumnMajor) - // starting from the raw vactor (v_row, v_col) - // - // From the Eigen documentation https://eigen.tuxfamily.org/dox/classEigen_1_1Stride.html - // The inner stride is the pointer increment between two consecutive entries within a given row of a row-major matrix. - // The outer stride is the pointer increment between two consecutive rows of a row-major matrix. - // - // Starting from v_row we can build a RowMajor matrix by choosing the following pair of strides - // - inner_stride = 1 - // - outer_stride = 5 = number of columns of A - // - // Starting from v_col we can build a RowMajor matrix by choosing the following pair of strides - // - inner_stride = 2 = number of rows of A - // - outer_stride = 1 - - const std::ptrdiff_t innerStride = (mat.storageOrder() == MatrixStorageOrdering::ColumnMajor) ? mat.rows() : 1; - const std::ptrdiff_t outerStride = (mat.storageOrder() == MatrixStorageOrdering::ColumnMajor) ? 1 : mat.cols(); - - return Eigen::Map>( - mat.data(), - mat.rows(), - mat.cols(), - Eigen::Stride(outerStride, innerStride)); -} - -#endif - -inline Eigen::Map > toEigen(const MatrixDynSize & mat) -{ - return Eigen::Map >(mat.data(),mat.rows(),mat.cols()); -} - -// Fixed size toEigen methods -template -inline Eigen::Map > toEigen(VectorFixSize & vec) -{ - return Eigen::Map >(vec.data(),vec.size()); -} - -template -inline Eigen::Map > toEigen(const VectorFixSize & vec) -{ - return Eigen::Map >(vec.data()); -} - -template -inline Eigen::Map< Eigen::Matrix > toEigen(MatrixFixSize & mat) -{ - return Eigen::Map< Eigen::Matrix >(mat.data()); -} - -template -inline Eigen::Map< Eigen::Matrix > toEigen(MatrixFixSize & mat) -{ - return Eigen::Map< Eigen::Matrix >(mat.data()); -} - -template -inline Eigen::Map< Eigen::Matrix > toEigen(MatrixFixSize<1, nCols> & mat) -{ - return Eigen::Map< Eigen::Matrix >(mat.data()); -} - -template -inline Eigen::Map< const Eigen::Matrix > toEigen(const MatrixFixSize & mat) -{ - return Eigen::Map< const Eigen::Matrix >(mat.data()); -} - -// Spatial vectors -inline const Eigen::Matrix toEigen(const SpatialMotionVector & vec) -{ - Eigen::Matrix ret; - - ret.segment<3>(0) = toEigen(vec.getLinearVec3()); - ret.segment<3>(3) = toEigen(vec.getAngularVec3()); - - return ret; -} - -inline const Eigen::Matrix toEigen(const SpatialForceVector & vec) -{ - Eigen::Matrix ret; - - ret.segment<3>(0) = toEigen(vec.getLinearVec3()); - ret.segment<3>(3) = toEigen(vec.getAngularVec3()); - - return ret; -} - -// Spatial vectors -inline void fromEigen(SpatialMotionVector & vec, const Eigen::Matrix & eigVec) -{ - toEigen(vec.getLinearVec3()) = eigVec.segment<3>(0); - toEigen(vec.getAngularVec3()) = eigVec.segment<3>(3); -} - -inline void fromEigen(SpatialForceVector & vec, const Eigen::Matrix & eigVec) -{ - toEigen(vec.getLinearVec3()) = eigVec.segment<3>(0); - toEigen(vec.getAngularVec3()) = eigVec.segment<3>(3); -} - -inline void fromEigen(Transform & trans, const Eigen::Matrix4d & eigMat) -{ - Rotation rot; - Position pos; - - toEigen(rot) = eigMat.block<3,3>(0,0); - toEigen(pos) = eigMat.block<3,1>(0,3); - - trans.setRotation(rot); - trans.setPosition(pos); -} - -template -inline Eigen::Matrix skew(const Eigen::MatrixBase & vec) -{ - EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived, 3); - return (Eigen::Matrix() << 0.0, -vec[2], vec[1], - vec[2], 0.0, -vec[0], - -vec[1], vec[0], 0.0).finished(); -} - -template -inline Eigen::Matrix unskew(const Eigen::MatrixBase & mat) -{ - EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived, 3, 3); - return (Eigen::Matrix() << mat(2,1), mat(0,2), mat(1,0) ).finished(); -} - - -/** - * Submatrix helpers - */ -template -inline void setSubMatrix(iDynTreeMatrixType& mat, - const IndexRange rowRange, - const IndexRange colRange, - const MatrixFixSize& subMat) -{ -#if __cplusplus > 199711L - static_assert(!iDynTree::is_sparsematrix::value, "You cannot set a subMatrix on a SparseMatrix."); +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. #endif - toEigen(mat).block(rowRange.offset,colRange.offset,rowRange.size,colRange.size) = toEigen(subMat); - return; -} +#include -template -inline void setSubMatrix(iDynTreeMatrixType& mat, - const IndexRange rowRange, - const IndexRange colRange, - const EigMatType& subMat) -{ -#if __cplusplus > 199711L - static_assert(!iDynTree::is_sparsematrix::value, "You cannot set a subMatrix on a SparseMatrix."); #endif - toEigen(mat).block(rowRange.offset,colRange.offset,rowRange.size,colRange.size) = subMat; - return; -} - -template -inline void setSubMatrix(iDynTreeMatrixType& mat, - const IndexRange rowRange, - const IndexRange colRange, - const double subMat) -{ -#if __cplusplus > 199711L - static_assert(!iDynTree::is_sparsematrix::value, "You cannot set a subMatrix on a SparseMatrix."); -#endif - assert(rowRange.size == 1); - assert(colRange.size == 1); - mat(rowRange.offset,colRange.offset) = subMat; - return; -} - -template -inline void setSubMatrixToIdentity(iDynTreeMatrixType& mat, - const IndexRange rowRange, - const IndexRange colRange) -{ -#if __cplusplus > 199711L - static_assert(!iDynTree::is_sparsematrix::value, "You cannot set a setSubMatrixToIdentity on a SparseMatrix."); -#endif - assert(rowRange.size == colRange.size); - for(int i=0; i < rowRange.size; i++) - { - mat(rowRange.offset+i,colRange.offset+i) = 1.0; - } - return; -} - -template -inline void setSubMatrixToMinusIdentity(iDynTreeMatrixType& mat, - const IndexRange rowRange, - const IndexRange colRange) -{ -#if __cplusplus > 199711L - static_assert(!iDynTree::is_sparsematrix::value, "You cannot set a setSubMatrixToMinusIdentity on a SparseMatrix."); -#endif - assert(rowRange.size == colRange.size); - for(int i=0; i < rowRange.size; i++) - { - mat(rowRange.offset+i,colRange.offset+i) = -1.0; - } - return; -} - - -template -inline void setSubVector(VectorDynSize& vec, - const IndexRange range, - const VectorFixSize& subVec) -{ - toEigen(vec).segment(range.offset,range.size) = toEigen(subVec); - return; -} - -inline void setSubVector(VectorDynSize& vec, - const IndexRange range, - double subVec) -{ - assert(range.size==1); - vec(range.offset) = subVec; - return; -} - -inline void setSubVector(VectorDynSize& vec, - const IndexRange range, - const SpatialMotionVector& twist) -{ - assert(range.size==6); - toEigen(vec).segment(range.offset,range.size) = toEigen(twist); - return; -} - -inline void setSubVector(VectorDynSize& vec, - const IndexRange range, - const SpatialForceVector& wrench) -{ - assert(range.size==6); - toEigen(vec).segment(range.offset,range.size) = toEigen(wrench); - return; -} - -template -inline void setSubVector(VectorDynSize& vec, - const IndexRange range, - const T& subVec) -{ - toEigen(vec).segment(range.offset,range.size) = subVec; - return; -} - -} - -#endif /* IDYNTREE_EIGEN_HELPERS_H */ - diff --git a/src/core/include/iDynTree/Core/EigenMathHelpers.h b/src/core/include/iDynTree/Core/EigenMathHelpers.h index fb018712c0e..b6f391f4448 100644 --- a/src/core/include/iDynTree/Core/EigenMathHelpers.h +++ b/src/core/include/iDynTree/Core/EigenMathHelpers.h @@ -1,68 +1,13 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef IDYNTREE_EIGEN_MATH_HELPERS_H -#define IDYNTREE_EIGEN_MATH_HELPERS_H +#ifndef IDYNTREE_CORE_EIGEN_MATH_HELPERS_H +#define IDYNTREE_CORE_EIGEN_MATH_HELPERS_H -#include - -namespace iDynTree -{ - -// Methods imported from codyco commons, see the documentation -// for them in -// https://github.com/robotology-playground/codyco-commons/blob/master/include/codyco/MathUtils.h#L48 -template -void pseudoInverse_helper2(const MapType& A, - Eigen::JacobiSVD& svdDecomposition, - MapType& Apinv, - double tolerance, - unsigned int computationOptions = Eigen::ComputeThinU|Eigen::ComputeThinV) -{ - using namespace Eigen; - - if (computationOptions == 0) return; //if no computation options we cannot compute the pseudo inverse - svdDecomposition.compute(A, computationOptions); - - typename JacobiSVD::SingularValuesType singularValues = svdDecomposition.singularValues(); - int singularValuesSize = singularValues.size(); - int rank = 0; - for (int idx = 0; idx < singularValuesSize; idx++) { - if (tolerance > 0 && singularValues(idx) > tolerance) { - singularValues(idx) = 1.0 / singularValues(idx); - rank++; - } else { - singularValues(idx) = 0.0; - } - } - - //equivalent to this U/V matrix in case they are computed full - Apinv = svdDecomposition.matrixV().leftCols(singularValuesSize) * singularValues.asDiagonal() * svdDecomposition.matrixU().leftCols(singularValuesSize).adjoint(); -} - - -template -void pseudoInverse_helper1(const MapType& A, - Eigen::JacobiSVD& svdDecomposition, - MapType& Apinv, - double tolerance, - unsigned int computationOptions = Eigen::ComputeThinU|Eigen::ComputeThinV) -{ - using namespace Eigen; - pseudoInverse_helper2(A, svdDecomposition, Apinv, tolerance, computationOptions); -} - -template -void pseudoInverse(const MapType A, - MapType Apinv, - double tolerance, - unsigned int computationOptions = Eigen::ComputeThinU|Eigen::ComputeThinV) - -{ - Eigen::JacobiSVD svdDecomposition(A.rows(), A.cols()); - pseudoInverse_helper1(A, svdDecomposition, Apinv, tolerance, computationOptions); -} +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif -} +#include #endif diff --git a/src/core/include/iDynTree/Core/EigenSparseHelpers.h b/src/core/include/iDynTree/Core/EigenSparseHelpers.h index 0ef9c5db2e5..7fc2a9a278a 100644 --- a/src/core/include/iDynTree/Core/EigenSparseHelpers.h +++ b/src/core/include/iDynTree/Core/EigenSparseHelpers.h @@ -1,60 +1,13 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef IDYNTREE_EIGEN_SPARSE_HELPERS_H -#define IDYNTREE_EIGEN_SPARSE_HELPERS_H +#ifndef IDYNTREE_CORE_EIGEN_SPARSE_HELPERS_H +#define IDYNTREE_CORE_EIGEN_SPARSE_HELPERS_H -#include -#include +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif -namespace iDynTree -{ +#include -//SparseMatrix helpers -inline Eigen::Map< Eigen::SparseMatrix > toEigen(iDynTree::SparseMatrix & mat) -{ - return Eigen::Map >(mat.rows(), - mat.columns(), - mat.numberOfNonZeros(), - mat.outerIndicesBuffer(), - mat.innerIndicesBuffer(), - mat.valuesBuffer(), - 0); //compressed format -} - -inline Eigen::Map > toEigen(const iDynTree::SparseMatrix & mat) -{ - return Eigen::Map >(mat.rows(), - mat.columns(), - mat.numberOfNonZeros(), - mat.outerIndicesBuffer(), - mat.innerIndicesBuffer(), - mat.valuesBuffer(), - 0); //compressed format -} - -inline Eigen::Map< Eigen::SparseMatrix > toEigen(iDynTree::SparseMatrix & mat) -{ - return Eigen::Map >(mat.rows(), - mat.columns(), - mat.numberOfNonZeros(), - mat.outerIndicesBuffer(), - mat.innerIndicesBuffer(), - mat.valuesBuffer(), - 0); //compressed format -} - -inline Eigen::Map > toEigen(const iDynTree::SparseMatrix & mat) -{ - return Eigen::Map >(mat.rows(), - mat.columns(), - mat.numberOfNonZeros(), - mat.outerIndicesBuffer(), - mat.innerIndicesBuffer(), - mat.valuesBuffer(), - 0); //compressed format -} - -} - -#endif /* IDYNTREE_EIGEN_SPARSE_HELPERS_H */ +#endif diff --git a/src/core/include/iDynTree/Core/GeomVector3.h b/src/core/include/iDynTree/Core/GeomVector3.h index 2558a48cbbd..9cf156dd3f7 100644 --- a/src/core/include/iDynTree/Core/GeomVector3.h +++ b/src/core/include/iDynTree/Core/GeomVector3.h @@ -1,47 +1,13 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef IDYNTREE_GEOM_VECTOR_3_H -#define IDYNTREE_GEOM_VECTOR_3_H +#ifndef IDYNTREE_CORE_GEOM_VECTOR_3_H +#define IDYNTREE_CORE_GEOM_VECTOR_3_H -#include +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif -namespace iDynTree -{ - class Rotation; +#include - class GeomVector3 : public Vector3 { - public: - GeomVector3() = default; - GeomVector3(const double* in_data, const unsigned int in_size); - GeomVector3(const double x, const double y, const double z); - GeomVector3(const Vector3 other); - GeomVector3 changeCoordFrame(const Rotation& newCoordFrame) const; - GeomVector3 compose(const GeomVector3& op1, const GeomVector3& op2) const; - GeomVector3 inverse(const GeomVector3& op) const; - double dot(const GeomVector3& other) const; - GeomVector3 operator+(const GeomVector3& other) const; - GeomVector3 operator-(const GeomVector3& other) const; - GeomVector3 operator-() const; - Rotation exp() const; - GeomVector3 cross(const GeomVector3& other) const; - - }; - - typedef GeomVector3 LinearMotionVector3; - typedef LinearMotionVector3 LinVelocity; - typedef LinearMotionVector3 LinAcceleration; - typedef GeomVector3 AngularMotionVector3; - typedef AngularMotionVector3 AngVelocity; - typedef AngularMotionVector3 AngAcceleration; - typedef GeomVector3 LinearForceVector3; - typedef LinearForceVector3 LinMomentum; - typedef LinearForceVector3 Force; - typedef GeomVector3 AngularForceVector3; - typedef AngularForceVector3 AngMomentum; - typedef AngularForceVector3 Torque; - typedef GeomVector3 MotionVector3; - -} - -#endif /* IDYNTREE_GEOM_VECTOR_3_H */ +#endif \ No newline at end of file diff --git a/src/core/include/iDynTree/Core/InertiaNonLinearParametrization.h b/src/core/include/iDynTree/Core/InertiaNonLinearParametrization.h index 05110950ac1..7553c49262c 100644 --- a/src/core/include/iDynTree/Core/InertiaNonLinearParametrization.h +++ b/src/core/include/iDynTree/Core/InertiaNonLinearParametrization.h @@ -1,77 +1,13 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef IDYNTREE_INERTIA_NON_LINEAR_PARAMETRIZATION_H -#define IDYNTREE_INERTIA_NON_LINEAR_PARAMETRIZATION_H - -#include -#include -#include - -namespace iDynTree -{ - class RigidBodyInertiaNonLinearParametrization - { - public: - double mass; - Position com; - - /** - * Rotation matrix that takes a vector expressed in the centroidal - * frame (origin in the center of mass, orientation as the principal - * axis of the 3D inertia at the center of mass). - */ - Rotation link_R_centroidal; - - Vector3 centralSecondMomentOfMass; - - /** - * Get the transform that applied to a quantity expressed in the centroidal - * frame it express it in the link frame ( link_H_centroidal ). - */ - Transform getLinkCentroidalTransform() const; - - /** - * Build the parametrization from a RigidBodyInertia. - */ - void fromRigidBodyInertia(const SpatialInertia & inertia); - - /** - * Build the nonlinear parametrization from a vector of 10 inertial parameters. - */ - void fromInertialParameters(const Vector10 & inertialParams); - - /** - * Convert the parametrization to a RigidBodyInertia class. - */ - SpatialInertia toRigidBodyInertia() const; - - /** - * Check that the mass is positive - * and the central second moments of mass are nonnegative. - */ - bool isPhysicallyConsistent() const; - - /** - * Serialize the nonlinear parametrization as a - * vector of sixteen elements. - */ - Vector16 asVectorWithRotationAsVec() const; - - /** - * Build the nonlinear parametrization from a vector of sixteen elements. - */ - void fromVectorWithRotationAsVec(const Vector16 & vec); - - /** - * Given the mapping between this nonlinear representation - * and the classical inertia parameters. This function - * return the gradient between the inertial parameters - */ - Matrix10x16 getGradientWithRotationAsVec() const; - }; +#ifndef IDYNTREE_CORE_INERTIA_NON_LINEAR_PARAMETRIZATION_H +#define IDYNTREE_CORE_INERTIA_NON_LINEAR_PARAMETRIZATION_H +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif -} +#include #endif diff --git a/src/core/include/iDynTree/Core/MatrixDynSize.h b/src/core/include/iDynTree/Core/MatrixDynSize.h index 303a7edadce..c2c414b9a89 100644 --- a/src/core/include/iDynTree/Core/MatrixDynSize.h +++ b/src/core/include/iDynTree/Core/MatrixDynSize.h @@ -1,247 +1,13 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef IDYNTREE_MATRIX_DYN_SIZE_H -#define IDYNTREE_MATRIX_DYN_SIZE_H +#ifndef IDYNTREE_CORE_MATRIX_DYN_SIZE_H +#define IDYNTREE_CORE_MATRIX_DYN_SIZE_H - -#include - -#include - -namespace iDynTree -{ - /** - * Class providing a simple form of matrix with dynamic size. - * - * \ingroup iDynTreeCore - */ - class MatrixDynSize - { - private: - /** - * Return the raw index in the data vector of the - * element corresponding to row and col, using row - * major ordering. - */ - std::size_t rawIndexRowMajor(std::size_t row, std::size_t col) const; - - /** - * Return the raw index in the data vector of the - * element corresponding to row and col, using col - * major ordering. - * - * \warning The class stores data in row major order, - * this function is used just in the fillColMajorBuffer - * method. - */ - std::size_t rawIndexColMajor(std::size_t row, std::size_t col) const; - - /** - * Set the capacity of the vector, resizing the buffer pointed by m_data. - */ - void changeCapacityAndCopyData(const std::size_t _newCapacity); - - protected: - /** - * Storage for the MatrixDynSize - * - * Pointer to an area of capacity() doubles, managed by this class. - * - * \warning this class stores data using the row major order - */ - double * m_data; - std::size_t m_rows; - std::size_t m_cols; - - /** - * The buffer to which m_data is pointing is m_capacity*sizeof(double). - */ - std::size_t m_capacity; - - public: - /** - * Default constructor: create a 0x0 matrix. - */ - MatrixDynSize(); - - /** - * Constructor from the rows and columns, all the element assigned to 0 - * - * @param _rows the desired rows of the matrix. - * @param _cols the desired cols of the matrix. - * - * \warning performs dynamic memory allocation operations - */ - MatrixDynSize(std::size_t _rows, std::size_t _cols); - - /** - * Constructor from a C-style matrix. - * - * - * \warning this class stores data using the row major order - * \warning performs dynamic memory allocation operations - */ - MatrixDynSize(const double * in_data, const std::size_t in_rows, const std::size_t in_cols); - - /** - * Copy constructor - * - * @param other the object to copy - */ - MatrixDynSize(const MatrixDynSize& other); - - - /** - * Constructor from MatrixView object - * - * @param other MatrixView to copy - * \warning performs dynamic memory allocation operations - * - */ - MatrixDynSize(iDynTree::MatrixView other); - - /** - * Assignment operator - * - * @param other the object to copy into self - * - * @return *this - */ - MatrixDynSize& operator=(const MatrixDynSize& other); - - /** - * Assignment operator - * - * @param other the object to copy into self - * - * @return *this - */ - MatrixDynSize& operator=(iDynTree::MatrixView other); - - /** - * Denstructor - * - * \warning performs dynamic memory allocation operations - */ - virtual ~MatrixDynSize(); - - /** - * @name Matrix interface methods. - * Methods exposing a matrix-like interface to MatrixDynSize. - * - * \warning Notice that using this methods you can damage the underlyng rotation matrix. - * In doubt, don't use them and rely on more high level functions. - */ - ///@{ - double operator()(const std::size_t row, const std::size_t col) const; - double& operator()(const std::size_t row, const std::size_t col); - double getVal(const std::size_t row, const std::size_t col) const; - bool setVal(const std::size_t row, const std::size_t col, const double new_el); - std::size_t rows() const; - std::size_t cols() const; - ///@} - - /** - * Raw data accessor - * - * \warning this class stores matrix data using the row major order - * @return a const pointer to a vector of size() doubles - */ - const double * data() const; - - /** - * Raw data accessor - * - * \warning this class stores matrix data using the row major order - * @return a pointer to a vector of size() doubles - */ - double * data(); - - /** - * Assign all element of the matrix to 0. - */ - void zero(); - - /** - * Change the size of the matrix, without preserving old content. - * - * @param _newRows the new rows of the matrix - * @param _newCols the new cols of the matrix - * - * \warning performs dynamic memory allocation operations if newRows*newCols > capacity() - */ - void resize(const std::size_t _newRows, const std::size_t _newCols); - - /** - * Increase the capacity of the matrix, preserving old content. - * - * \warning performs dynamic memory allocation operations if _newCapacity > capacity() - */ - void reserve(const size_t _newCapacity); - - /** - * Get the dimension (in doubles) of the buffer to which m_data is pointing. - */ - size_t capacity() const; - - /** - * Change the capacity of the matrix such that capacity() == rows()*cols(), preserving old content. - * - * \warning performs dynamic memory allocation operations if newRows*newCols != capacity() - */ - void shrink_to_fit(); - - /** - * Assume that rowMajorBuf is pointing to - * a buffer of rows()*cols() doubles, and fill - * it with the content of this matrix, using - * row major order. - * - * @param rowMajorBuf pointer to the buffer to fill - * - * @todo provide this for all matrix types - * - * \warning use this function only if you are - * an expert C user - */ - void fillRowMajorBuffer(double * rowMajorBuf) const; - - /** - * Assume that colMajorBuf is pointing to - * a buffer of rows()*cols() doubles, and fill - * it with the content of this matrix, using - * column major order. - * - * @param colMajorBuf pointer to the buffer to fill - * - * @todo provide this for all matrix types - * - * \warning use this function only if you are - * an expert C user - */ - void fillColMajorBuffer(double * colMajorBuf) const; - - -#if !defined(SWIG_VERSION) || SWIG_VERSION >= 0x030000 - /** Typedefs to enable make_matrix_view. - */ - ///@{ - typedef double value_type; - ///@} +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. #endif - /** @name Output helpers. - * Output helpers. - */ - ///@{ - std::string toString() const; - - std::string reservedToString() const; - ///@} - - }; -} - -#endif /* IDYNTREE_MATRIX_DYN_SIZE_H */ +#include +#endif diff --git a/src/core/include/iDynTree/Core/MatrixFixSize.h b/src/core/include/iDynTree/Core/MatrixFixSize.h index f341897bd51..4aefbd45d7e 100644 --- a/src/core/include/iDynTree/Core/MatrixFixSize.h +++ b/src/core/include/iDynTree/Core/MatrixFixSize.h @@ -1,379 +1,14 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef IDYNTREE_MATRIX_FIX_SIZE_H -#define IDYNTREE_MATRIX_FIX_SIZE_H +#ifndef IDYNTREE_CORE_MATRIX_FIX_SIZE_H +#define IDYNTREE_CORE_MATRIX_FIX_SIZE_H - -#include -#include - -#include -#include -#include -#include -#include - -namespace iDynTree -{ - /** - * Class providing a simple form of matrix with dynamic size. - * - * \ingroup iDynTreeCore - */ - template - class MatrixFixSize - { - private: - /** - * Return the raw index in the data vector of the - * element corresponding to row and col, using row - * major ordering. - */ - std::size_t rawIndexRowMajor(std::size_t row, std::size_t col) const; - - /** - * Return the raw index in the data vector of the - * element corresponding to row and col, using col - * major ordering. - * - * \warning The class stores data in row major order, - * this function is used just in the fillColMajorBuffer - * method. - */ - std::size_t rawIndexColMajor(std::size_t row, std::size_t col) const; - - protected: - /** - * Storage for the MatrixDynSize - * - * Pointer to an area of size() doubles, managed by this class. - * - * \warning this class stores data using the row major order - */ - double m_data[nRows*nCols]; - - - public: - /** - * Default constructor. - * The data is not reset to 0 for perfomance reason. - * Please initialize the data in the vector before any use. - */ - MatrixFixSize(); - - - /** - * Constructor from a C-style matrix (row major). - * - * \warning this class stores data using the row major order - */ - MatrixFixSize(const double * in_data, const std::size_t in_rows, const std::size_t in_cols); - - /** - * Constructor from a MatrixView - * - * \warning this class stores data using the row major order - */ - MatrixFixSize(iDynTree::MatrixView other); - - /** - * @name Matrix interface methods. - * Methods exposing a matrix-like interface to MatrixFixSize. - * - */ - ///@{ - double operator()(const std::size_t row, const std::size_t col) const; - double& operator()(const std::size_t row, const std::size_t col); - double getVal(const std::size_t row, const std::size_t col) const; - bool setVal(const std::size_t row, const std::size_t col, const double new_el); - std::size_t rows() const; - std::size_t cols() const; - ///@} - - MatrixFixSize & operator=(iDynTree::MatrixView mat); - - /** - * Raw data accessor - * - * \warning this class stores matrix data using the row major order - * @return a const pointer to a vector of rows()*cols() doubles - */ - const double * data() const; - - /** - * Raw data accessor - * - * \warning this class stores matrix data using the row major order - * @return a pointer to a vector of rows()*cols() doubles - */ - double * data(); - - /** - * Assign all element of the matrix to 0. - */ - void zero(); - - /** - * Assume that rowMajorBuf is pointing to - * a buffer of rows()*cols() doubles, and fill - * it with the content of this matrix, using - * row major order. - * - * @param rowMajorBuf pointer to the buffer to fill - * - * @todo provide this for all matrix types - * - * \warning use this function only if you are - * an expert C user - */ - void fillRowMajorBuffer(double * rowMajorBuf) const; - - /** - * Assume that colMajorBuf is pointing to - * a buffer of rows()*cols() doubles, and fill - * it with the content of this matrix, using - * column major order. - * - * @param colMajorBuf pointer to the buffer to fill - * - * @todo provide this for all matrix types - * - * \warning use this function only if you are - * an expert C user - */ - void fillColMajorBuffer(double * colMajorBuf) const; - - #if !defined(SWIG_VERSION) || SWIG_VERSION >= 0x030000 - /** Typedefs to enable make_matrix_view. - */ - ///@{ - typedef double value_type; - ///@} +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. #endif +#include - /** @name Output helpers. - * Output helpers. - */ - ///@{ - std::string toString() const; - - std::string reservedToString() const; - ///@} - - }; - - - // Implementation - template - MatrixFixSize::MatrixFixSize() - { - } - - template - MatrixFixSize::MatrixFixSize(const double* in_data, - const std::size_t in_rows, - const std::size_t in_cols) - { - if( in_rows != nRows || - in_cols != nCols ) - { - reportError("MatrixFixSize","constructor","input matrix does not have the right size"); - this->zero(); - } - else - { - std::memcpy(this->m_data,in_data,nRows*nCols*sizeof(double)); - } - } - - template - MatrixFixSize::MatrixFixSize(iDynTree::MatrixView other) - { - if( other.rows() != nRows || - other.cols() != nCols ) - { - reportError("MatrixFixSize","constructor","input matrix does not have the right size"); - this->zero(); - } - else - { - for(std::size_t row=0; row < nRows; row++ ) - { - for(std::size_t col=0; col < nCols; col++ ) - { - this->m_data[rawIndexRowMajor(row,col)] = other(row, col); - } - } - } - } - - template - void MatrixFixSize::zero() - { - for(std::size_t row=0; row < this->rows(); row++ ) - { - for(std::size_t col=0; col < this->cols(); col++ ) - { - this->m_data[rawIndexRowMajor(row,col)] = 0.0; - } - } - } - - template - std::size_t MatrixFixSize::rows() const - { - return nRows; - } - - template - std::size_t MatrixFixSize::cols() const - { - return nCols; - } - - template - double* MatrixFixSize::data() - { - return this->m_data; - } - - template - const double* MatrixFixSize::data() const - { - return this->m_data; - } - - template - MatrixFixSize & MatrixFixSize::operator=(iDynTree::MatrixView mat) { - assert(nCols == mat.cols()); - assert(nRows == mat.rows()); - - for(std::size_t i = 0; i < nRows; i++) - { - for(std::size_t j = 0; j < nCols; j++) - { - this->m_data[this->rawIndexRowMajor(i,j)] = mat(i, j); - } - } - return *this; - } - - template - double& MatrixFixSize::operator()(const std::size_t row, const std::size_t col) - { - assert(row < nRows); - assert(col < nCols); - return this->m_data[rawIndexRowMajor(row,col)]; - } - - template - double MatrixFixSize::operator()(const std::size_t row, const std::size_t col) const - { - assert(row < nRows); - assert(col < nCols); - return this->m_data[rawIndexRowMajor(row,col)]; - } - - template - double MatrixFixSize::getVal(const std::size_t row, const std::size_t col) const - { - if( row >= this->rows() || - col >= this->cols() ) - { - reportError("MatrixDynSize","getVal","indices out of bounds"); - return 0.0; - } - - return this->m_data[rawIndexRowMajor(row,col)]; - } - - template - bool MatrixFixSize::setVal(const std::size_t row, const std::size_t col, const double new_el) - { - if( row >= this->rows() || - col >= this->cols() ) - { - reportError("MatrixDynSize","setVal","indices out of bounds"); - return false; - } - - this->m_data[rawIndexRowMajor(row,col)] = new_el; - return true; - } - - template - void MatrixFixSize::fillRowMajorBuffer(double* rowMajorBuf) const - { - // MatrixFixSize stores data in row major, a simply - // memcpy will be sufficient - memcpy(rowMajorBuf,this->m_data,this->rows()*this->cols()*sizeof(double)); - } - - template - void MatrixFixSize::fillColMajorBuffer(double* colMajorBuf) const - { - for(std::size_t row = 0; row < this->rows(); row++ ) - { - for(std::size_t col = 0; col < this->cols(); col++ ) - { - colMajorBuf[this->rawIndexColMajor(row,col)] = - this->m_data[this->rawIndexRowMajor(row,col)]; - } - } - } - - template - std::size_t MatrixFixSize::rawIndexRowMajor(std::size_t row, std::size_t col) const - { - return (nCols*row + col); - } - - template - std::size_t MatrixFixSize::rawIndexColMajor(std::size_t row, std::size_t col) const - { - return (row + nRows*col); - } - - template - std::string MatrixFixSize::toString() const - { - std::stringstream ss; - - for(std::size_t row=0; row < this->rows(); row++ ) - { - for(std::size_t col=0; col < this->cols(); col++ ) - { - ss << this->m_data[this->rawIndexRowMajor(row,col)] << " "; - } - ss << std::endl; - } - - return ss.str(); - } - - template - std::string MatrixFixSize::reservedToString() const - { - return this->toString(); - } - - // Explicit instantiations - // The explicit instantiations are the only ones that can be used in the API - // and the only ones that users are supposed to manipulate manipulate - // Add all the explicit instantiation that can be useful, but remember to add - // them also in the iDynTree.i SWIG file - typedef MatrixFixSize<1,6> Matrix1x6; - typedef MatrixFixSize<2,3> Matrix2x3; - typedef MatrixFixSize<3,3> Matrix3x3; - typedef MatrixFixSize<4,4> Matrix4x4; - typedef MatrixFixSize<6,1> Matrix6x1; - typedef MatrixFixSize<6,6> Matrix6x6 ; - typedef MatrixFixSize<6,10> Matrix6x10; - typedef MatrixFixSize<10,16> Matrix10x16; - -} - -#endif /* IDYNTREE_MATRIX_FIX_SIZE_H */ +#endif diff --git a/src/core/include/iDynTree/Core/MatrixView.h b/src/core/include/iDynTree/Core/MatrixView.h index f2af0814d36..b0ef3f57051 100644 --- a/src/core/include/iDynTree/Core/MatrixView.h +++ b/src/core/include/iDynTree/Core/MatrixView.h @@ -1,292 +1,13 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef IDYNTREE_MATRIX_VIEW_H -#define IDYNTREE_MATRIX_VIEW_H +#ifndef IDYNTREE_CORE_MATRIX_VIEW_H +#define IDYNTREE_CORE_MATRIX_VIEW_H -#include - -#include - -#include -#include - -// constexpr workaround for SWIG -#ifdef SWIG -#define IDYNTREE_CONSTEXPR -#else -#define IDYNTREE_CONSTEXPR constexpr +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. #endif +#include -namespace iDynTree -{ - - namespace MatrixViewInternal - { -#ifndef SWIG - /** - * has_IsRowMajor is used to build a type-dependent expression that check if an - * element has IsRowMajor argument. This specific implementation is used when - * the the object has not IsRowMajor. - */ - template - struct has_IsRowMajor : std::false_type {}; - - /** - * has_IsRowMajor is used to build a type-dependent expression that check if an - * element has IsRowMajor argument. This specific implementation is used when - * the the object has not IsRowMajor, indeed void_t<\endcode> is used to - * detect ill-formed types in SFINAE context. - */ - template - struct has_IsRowMajor> : std::true_type {}; #endif - - } // namespace MatrixViewIntenal - - /** - * MatrixView implements a view interface of Matrices. Both RowMajor and ColumnMajor matrices are - * supported. - * @note The user should define the storage ordering when the MatrixView is created (the default - order is RowMajor). However if the MatrixView is generated: - * - from an object having a public member called IsRowMajor, the correct storage - order is chosen. - * - from another MatrixView, the correct storage order is chosen. - */ - template class MatrixView - { - public: - using element_type = ElementType; - using value_type = std::remove_cv_t; - using index_type = std::ptrdiff_t; - using pointer = element_type*; - using reference = element_type&; - - private: - pointer m_storage; - index_type m_rows; - index_type m_cols; - - MatrixStorageOrdering m_storageOrder; - - index_type m_innerStride; - index_type m_outerStride; - - index_type rawIndex(index_type row, index_type col) const - { - if (m_storageOrder == MatrixStorageOrdering::RowMajor) - { - return (col + this->m_outerStride * row); - } else - { - return (this->m_outerStride * col + row); - } - } - - public: - - MatrixView() - : MatrixView(nullptr, 0, 0, MatrixStorageOrdering::RowMajor) - {} - MatrixView(const MatrixView& other) - : MatrixView(other.m_storage, other.m_rows, other.m_cols, other.m_storageOrder) - { - } - -#ifndef SWIG - template < - class OtherElementType, - class = std::enable_if_t< - details::is_allowed_element_type_conversion::value_type, - value_type>::value>> - IDYNTREE_CONSTEXPR MatrixView(const MatrixView& other) - : MatrixView(other.data(), other.rows(), other.cols(), other.storageOrder()) - { - } - - - template < - class Container, - std::enable_if_t::value - && std::is_convertible().data()), - pointer>::value - && MatrixViewInternal::has_IsRowMajor::value - && !std::is_same::value, - int> = 0> - MatrixView(const Container& matrix) - : MatrixView(matrix.data(), - matrix.rows(), - matrix.cols(), - Container::IsRowMajor ? MatrixStorageOrdering::RowMajor - : MatrixStorageOrdering::ColumnMajor) - { - } - - template < - class Container, - std::enable_if_t::value - && std::is_convertible().data()), - pointer>::value - && !MatrixViewInternal::has_IsRowMajor::value - && !std::is_same::value, - int> = 0> - MatrixView(const Container& matrix, const MatrixStorageOrdering& order = MatrixStorageOrdering::RowMajor) - : MatrixView(matrix.data(), matrix.rows(), matrix.cols(), order) - { - } - - template ().data()), pointer>::value - && MatrixViewInternal::has_IsRowMajor::value - && !std::is_same::value, - int> = 0> - MatrixView(Container& matrix) - : MatrixView(matrix.data(), - matrix.rows(), - matrix.cols(), - Container::IsRowMajor ? MatrixStorageOrdering::RowMajor - : MatrixStorageOrdering::ColumnMajor) - { - } - - template ().data()), pointer>::value - && !MatrixViewInternal::has_IsRowMajor::value - && !std::is_same::value - && !std::is_same().data())>>>::value, - int> = 0> - MatrixView(Container& matrix, const MatrixStorageOrdering& order = MatrixStorageOrdering::RowMajor) - : MatrixView(matrix.data(), matrix.rows(), matrix.cols(), order) - { - } - -#endif // SWIG - - MatrixView(pointer in_data, - index_type in_rows, - index_type in_cols, - const MatrixStorageOrdering& order = MatrixStorageOrdering::RowMajor) - : m_storage(in_data) - , m_rows(in_rows) - , m_cols(in_cols) - , m_storageOrder(order) - , m_innerStride(1) - , m_outerStride(order == MatrixStorageOrdering::RowMajor ? in_cols : in_rows) - { - } - - const MatrixStorageOrdering& storageOrder() const noexcept - { - return m_storageOrder; - } - - pointer data() const noexcept - { - return m_storage; - } - - /** - * @name Matrix interface methods. - * Methods exposing a matrix-like interface to MatrixView. - * - */ - ///@{ - reference operator()(index_type row, const index_type col) const - { - assert(row < this->rows()); - assert(col < this->cols()); - return this->m_storage[rawIndex(row, col)]; - } - - index_type rows() const noexcept - { - return this->m_rows; - } - - index_type cols() const noexcept - { - return this->m_cols; - } - ///@} - - MatrixView block(index_type startingRow, index_type startingColumn, - index_type rows, index_type cols) const - { - assert(rows <= this->rows()); - assert(cols <= this->cols()); - - const index_type offset = rawIndex(startingRow, startingColumn); - assert(offset < this->rows() * this->cols()); - - MatrixView block; - block = *this; - block.m_rows = rows; - block.m_cols = cols; - block.m_storage = this->m_storage + offset; - - return block; - } - - }; - - template - IDYNTREE_CONSTEXPR MatrixView - make_matrix_view(ElementType* ptr, - typename MatrixView::index_type rows, - typename MatrixView::index_type cols, - const MatrixStorageOrdering& order = MatrixStorageOrdering::RowMajor) - { - return MatrixView(ptr, rows, cols, order); - } - - template ::value - || std::is_same, Container>::value, - int> = 0> - IDYNTREE_CONSTEXPR MatrixView make_matrix_view(Container& cont) - { - return MatrixView(cont); - } - - template ::value - || std::is_same, Container>::value, - int> = 0> - IDYNTREE_CONSTEXPR MatrixView make_matrix_view(const Container& cont) - { - return MatrixView(cont); - } - - template ::value - && !std::is_same, Container>::value, - int> = 0> - IDYNTREE_CONSTEXPR MatrixView - make_matrix_view(Container& cont, - const MatrixStorageOrdering& order = MatrixStorageOrdering::RowMajor) - { - return MatrixView(cont, order); - } - - template ::value - && !std::is_same, Container>::value, - int> = 0> - IDYNTREE_CONSTEXPR MatrixView - make_matrix_view(const Container& cont, - const MatrixStorageOrdering& order = MatrixStorageOrdering::RowMajor) - { - return MatrixView(cont, order); - } - -} // namespace iDynTree - -#endif /* IDYNTREE_MATRIX_MATRIX_VIEW_H */ diff --git a/src/core/include/iDynTree/Core/Position.h b/src/core/include/iDynTree/Core/Position.h index 0d746874296..cfdeba32d56 100644 --- a/src/core/include/iDynTree/Core/Position.h +++ b/src/core/include/iDynTree/Core/Position.h @@ -1,106 +1,13 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef IDYNTREE_POSITION_H -#define IDYNTREE_POSITION_H +#ifndef IDYNTREE_CORE_POSITION_H +#define IDYNTREE_CORE_POSITION_H -#include -#include - -#include - -namespace iDynTree -{ - class Rotation; - class Twist; - class SpatialAcc; - class SpatialMomentum; - class Wrench; - class SpatialMotionVector; - class SpatialForceVector; - - /** - * Class representation the coordinates of the Position of - * a point with respect to another point. - * - * \ingroup iDynTreeCore - * - * \image html position.svg - * - * The Position object can briefly described as the position - * of a *point* with respect to a *refPoint*, expressed with - * respect to an orientation given by *orientFrame* . - * - */ - class Position: public PositionRaw - { - public: - /** - * Default constructor. - * The data is not initialized, please initialize the data in the created object before use. - */ - Position(); - - /** - * Constructor from 3 doubles: initialize the coordinates with the passed values. - */ - Position(double x, double y, double z); - - /** - * Copy constructor: create a Position from another Position - */ - Position(const Position & other); - - /** - * Copy constructor: create a Position from a PositionRaw - */ - Position(const PositionRaw & other); - - /** - * Create a Position from a span - */ - Position(iDynTree::Span other); - - /** - * Geometric operations - */ - const Position & changePoint(const Position & newPoint); - const Position & changeRefPoint(const Position & newRefPoint); - const Position & changeCoordinateFrame(const Rotation & newCoordinateFrame); - static Position compose(const Position & op1, const Position & op2); - static Position inverse(const Position & op); - SpatialMotionVector changePointOf(const SpatialMotionVector & other) const; - SpatialForceVector changePointOf(const SpatialForceVector & other) const; - Twist changePointOf(const Twist & other) const; - SpatialAcc changePointOf(const SpatialAcc & other) const; - SpatialMomentum changePointOf(const SpatialMomentum & other) const; - Wrench changePointOf(const Wrench & other) const; - - /** - * overloaded operators - */ - Position operator+(const Position &other) const; - Position operator-(const Position &other) const; - Position operator-() const; - Twist operator*(const Twist & other) const; - SpatialForceVector operator*(const SpatialForceVector & other) const; - SpatialAcc operator*(const SpatialAcc & other) const; - SpatialMomentum operator*(const SpatialMomentum & other) const; - Wrench operator*(const Wrench & other) const; - - /** @name Output helpers. - * Output helpers. - */ - ///@{ - std::string toString() const; - - std::string reservedToString() const; - ///@} - - friend Position Rotation::changeCoordFrameOf(const Position & op) const; +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif - static Position Zero(); - }; -} +#include #endif diff --git a/src/core/include/iDynTree/Core/PositionRaw.h b/src/core/include/iDynTree/Core/PositionRaw.h index 34c0b6e1e78..b7e60bf1b69 100644 --- a/src/core/include/iDynTree/Core/PositionRaw.h +++ b/src/core/include/iDynTree/Core/PositionRaw.h @@ -1,77 +1,13 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef IDYNTREE_POSITION_RAW_H -#define IDYNTREE_POSITION_RAW_H +#ifndef IDYNTREE_CORE_POSITION_RAW_H +#define IDYNTREE_CORE_POSITION_RAW_H +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif -#include -#include +#include -namespace iDynTree -{ - class RotationRaw; - class SpatialMotionVector; - class SpatialForceVector; - - /** - * Class providing the raw coordinates for iDynTree::Position class. - * - * \ingroup iDynTreeCore - */ - class PositionRaw: public Vector3 - { - public: - /** - * Default constructor. - * The data is not reset to 0 for perfomance reason. - * Please initialize the data in the vector before any use. - */ - PositionRaw(); - - /** - * Constructor from 3 doubles: initialize the coordinates with the passed values. - */ - PositionRaw(double x, double y, double z); - - /** - * Constructor from a raw buffer of 3 doubles. - */ - PositionRaw(const double* in_data, const unsigned int in_size); - - /** - * Copy constructor: create a PositionRaw from another PositionRaw - */ - PositionRaw(const PositionRaw & other); - - /** - * Construct from a span - * @warning if the Span size is different from 3 an assert is thrown at run-time. - */ - PositionRaw(iDynTree::Span other); - - /** - * Geometric operations - */ - const PositionRaw & changePoint(const PositionRaw & newPoint); - const PositionRaw & changeRefPoint(const PositionRaw & newRefPoint); - static PositionRaw compose(const PositionRaw & op1, const PositionRaw & op2); - static PositionRaw inverse(const PositionRaw & op); - SpatialMotionVector changePointOf(const SpatialMotionVector & other) const; - SpatialForceVector changePointOf(const SpatialForceVector & other) const; - - - /** @name Output helpers. - * Output helpers. - */ - ///@{ - std::string toString() const; - - std::string reservedToString() const; - ///@} - - }; - -} - -#endif /* IDYNTREE_POSITION_RAW_H */ +#endif diff --git a/src/core/include/iDynTree/Core/PrivatePreProcessorUtils.h b/src/core/include/iDynTree/Core/PrivatePreProcessorUtils.h index 31595f47925..0ff5137870e 100644 --- a/src/core/include/iDynTree/Core/PrivatePreProcessorUtils.h +++ b/src/core/include/iDynTree/Core/PrivatePreProcessorUtils.h @@ -1,13 +1,13 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef IDYNTREE_PRIVATE_PREPROCESSOR_UTILS_H -#define IDYNTREE_PRIVATE_PREPROCESSOR_UTILS_H +#ifndef IDYNTREE_CORE_PRIVATE_PREPROCESSOR_UTILS_H +#define IDYNTREE_CORE_PRIVATE_PREPROCESSOR_UTILS_H -#ifdef __PRETTY_FUNCTION__ -#define IDYNTREE_PRETTY_FUNCTION __PRETTY_FUNCTION__ -#else -#define IDYNTREE_PRETTY_FUNCTION __FUNCTION__ +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. #endif -#endif /* IDYNTREE_PRIVATE_PREPROCESSOR_UTILS_H */ +#include + +#endif \ No newline at end of file diff --git a/src/core/include/iDynTree/Core/PrivateUtils.h b/src/core/include/iDynTree/Core/PrivateUtils.h index 5816710efe2..e11a56830fd 100644 --- a/src/core/include/iDynTree/Core/PrivateUtils.h +++ b/src/core/include/iDynTree/Core/PrivateUtils.h @@ -1,101 +1,13 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef IDYNTREE_PRIVATE_UTILS_H -#define IDYNTREE_PRIVATE_UTILS_H +#ifndef IDYNTREE_CORE_PRIVATE_UTILS_H +#define IDYNTREE_CORE_PRIVATE_UTILS_H +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif -#include -#include +#include -namespace iDynTree -{ - - /** - * Maps a 3d vector to the square of the cross product matrix: - * v --> (v\times)^2 - * or, if you prefer another notation: - * v --> S^2(v) - */ - Eigen::Matrix3d squareCrossProductMatrix(const Eigen::Vector3d & v); - - /** - * Maps a 3d vector to the cross product matrix: - * v --> (v\times) - * or, if you prefer another notation: - * v --> S(v) - */ - Eigen::Matrix3d skew(const Eigen::Vector3d & vec); - - /** - * Efficient version of the copy from one 6D vector to another. - */ - template - void efficient6dCopy(vector6d* pthis, const vector6d& other) - { - toEigen(pthis->getLinearVec3()) = toEigen(other.getLinearVec3()); - toEigen(pthis->getAngularVec3()) = toEigen(other.getAngularVec3()); - return; - } - - /** - * Efficient version of the sum of two 6D vectors. - */ - template - vector6d efficient6dSum(const vector6d & op1, const vector6d & op2) - { - vector6d ret; - toEigen(ret.getLinearVec3()) = toEigen(op1.getLinearVec3()) + toEigen(op2.getLinearVec3()); - toEigen(ret.getAngularVec3()) = toEigen(op1.getAngularVec3()) + toEigen(op2.getAngularVec3()); - return ret; - } - - /** - * Efficient version of the different of two 6D vectors. - */ - template - vector6d efficient6ddifference(const vector6d & op1, const vector6d & op2) - { - vector6d ret; - toEigen(ret.getLinearVec3()) = toEigen(op1.getLinearVec3()) - toEigen(op2.getLinearVec3()); - toEigen(ret.getAngularVec3()) = toEigen(op1.getAngularVec3()) - toEigen(op2.getAngularVec3()); - return ret; - } - - /** - * Efficient version of the cross product between a twist - * and a spatial motion vector (another twist, acceleration, ..) - */ - template - resultType efficientTwistCrossTwist(const twistType & op1, const motionVectorType & op2) - { - // res.getLinearVec3() = this->angularVec3.cross(other.getLinearVec3()) + this->linearVec3.cross(other.getAngularVec3()); - // res.getAngularVec3() = this->angularVec3.cross(other.getAngularVec3()); - resultType ret; - - toEigen(ret.getLinearVec3()) = toEigen(op1.getAngularVec3()).cross(toEigen(op2.getLinearVec3())) - + toEigen(op1.getLinearVec3()).cross(toEigen(op2.getAngularVec3())); - toEigen(ret.getAngularVec3()) = toEigen(op1.getAngularVec3()).cross(toEigen(op2.getAngularVec3())); - - return ret; - } - - /** - * Efficient version of the cross product between a twist - * and a spatial force vector (momentum, wrench, ..) - */ - template - resultType efficientTwistCrossMomentum(const twistType & op1, const momentumVectorType & op2) - { - resultType ret; - - toEigen(ret.getLinearVec3()) = toEigen(op1.getAngularVec3()).cross(toEigen(op2.getLinearVec3())); - toEigen(ret.getAngularVec3()) = toEigen(op1.getLinearVec3()).cross(toEigen(op2.getLinearVec3())) - + toEigen(op1.getAngularVec3()).cross(toEigen(op2.getAngularVec3())); - - return ret; - } -} - - -#endif /* IDYNTREE_PRIVATE_UTILS_H */ +#endif diff --git a/src/core/include/iDynTree/Core/Rotation.h b/src/core/include/iDynTree/Core/Rotation.h index 1efd8c11b35..b882ef16218 100644 --- a/src/core/include/iDynTree/Core/Rotation.h +++ b/src/core/include/iDynTree/Core/Rotation.h @@ -1,473 +1,13 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef IDYNTREE_ROTATION_H -#define IDYNTREE_ROTATION_H +#ifndef IDYNTREE_CORE_ROTATION_H +#define IDYNTREE_CORE_ROTATION_H -#include -#include -#include -#include - -namespace iDynTree -{ - class Position; - class Twist; - class SpatialAcc; - class Wrench; - class Direction; - class Axis; - class SpatialAcc; - class SpatialMomentum; - class ClassicalAcc; - class RotationalInertiaRaw; - class SpatialMotionVector; - class SpatialForceVector; - class ArticulatedBodyInertia; - - /** - * Class representation the rotation of an orientation frame - * with respect to a reference orientation frame, expressed as a Rotation matrix. - * - * \ingroup iDynTreeCore - * - * The semantics for this class is based on the OrientationCoord in: - * - * De Laet T, Bellens S, Smits R, Aertbeliën E, Bruyninckx H, and De Schutter J - * (2013), Geometric Relations between Rigid Bodies: Semantics for Standardization, - * IEEE Robotics & Automation Magazine, Vol. 20, No. 1, pp. 84-93. - * URL : http://people.mech.kuleuven.be/~tdelaet/geometric_relations_semantics/geometric_relations_semantics_theory.pdf - * - * Given that this class uses the rotation matrix to represent orientation, some operation - * are disable because there is a semantic constraint induced by choice of representation, i.e. - * that the coordinate frame is always the reference orientation frame. Thus, some semantic operation - * are not enabled, namely: - * * the generic inverse, that does not change the coordinate frame. - * * changeCoordFrame, because CoordFrame is always the same of RefOrientFrame. - */ - class Rotation: public RotationRaw - { - public: - /** - * Default constructor. - * The data is not reset to the identity matrix for perfomance reason. - * Please initialize the data in the vector before any use. - */ - Rotation(); - - /** - * Constructor from 9 doubles: initialize elements of the rotation matrix. - */ - Rotation(double xx, double xy, double xz, - double yx, double yy, double yz, - double zx, double zy, double zz); - - /** - * Copy constructor: create a Rotation from another RotationRaw. - */ - Rotation(const RotationRaw & other); - - /** - * Copy constructor: create a Rotation from another Rotation. - */ - Rotation(const Rotation & other); - - /** - * Create a Rotation from a MatrixView. - */ - Rotation(iDynTree::MatrixView other); - - /** - * Geometric operations. - * For the inverse2() operation, both the forward and the inverse geometric relations have to - * be expressed in the reference orientation frame!! - * - */ - const Rotation & changeOrientFrame(const Rotation & newOrientFrame); - const Rotation & changeRefOrientFrame(const Rotation & newRefOrientFrame); - const Rotation & changeCoordinateFrame(const Rotation& newCoordinateFrame); - static Rotation compose(const Rotation & op1, const Rotation & op2); - static Rotation inverse2(const Rotation & orient); - Position changeCoordFrameOf(const Position & other) const; - SpatialMotionVector changeCoordFrameOf(const SpatialMotionVector & other) const; - SpatialForceVector changeCoordFrameOf(const SpatialForceVector & other) const; - Twist changeCoordFrameOf(const Twist & other) const; - SpatialAcc changeCoordFrameOf(const SpatialAcc & other) const; - SpatialMomentum changeCoordFrameOf(const SpatialMomentum & other) const; - Wrench changeCoordFrameOf(const Wrench & other) const; - Direction changeCoordFrameOf(const Direction & other) const; - Axis changeCoordFrameOf(const Axis & other) const; - ClassicalAcc changeCoordFrameOf(const ClassicalAcc & other) const; - RotationalInertiaRaw changeCoordFrameOf(const RotationalInertiaRaw & other) const; - - - /** - * overloaded operators - */ - Rotation operator*(const Rotation & other) const; - Rotation inverse() const; - Position operator*(const Position & other) const; - SpatialForceVector operator*(const SpatialForceVector & other) const; - Twist operator*(const Twist & other) const; - Wrench operator*(const Wrench & other) const; - Direction operator*(const Direction & other) const; - Axis operator*(const Axis & other) const; - SpatialAcc operator*(const SpatialAcc & other) const; - SpatialMomentum operator*(const SpatialMomentum & other) const; - ClassicalAcc operator*(const ClassicalAcc & other) const; - RotationalInertiaRaw operator*(const RotationalInertiaRaw & other) const; - - /** - * Log mapping between a generic element of SO(3) (iDynTree::Rotation) - * to the corresponding element of so(3) (iDynTree::AngularMotionVector). - */ - AngularMotionVector3 log() const; - - /** - * Set the rotation matrix as the passed rotation expressed in quaternion - * - * @note the quaternion is expressed as (real, imaginary) part with - * real \f$\in \mathbb{R}\f$ and imaginary \f$\in \mathbb{R}^3\f$ - * @note the quaternion is normalized - * @param quaternion the rotation expressed in quaternion - */ - void fromQuaternion(const iDynTree::Vector4& quaternion); - - - /** - * @name Conversion to others represention of matrices. - * - */ - ///@{ - - /** - * Get a roll, pitch and yaw corresponding to this rotation. - * - * Get \f$ (r,p,y) \in ( (-\pi, \pi] \times (-\frac{\pi}{2}, \frac{\pi}{2}) \times (-\pi, \pi] ) \cup ( \{0\} \times \{-\frac{\pi}{2}\} \times (-\pi,\pi] ) \cup ( \{0\} \times \{\frac{\pi}{2}\} \times [-\pi,\pi) )\f$ - * such that - * *this == RotZ(y)*RotY(p)*RotX(r) - * - * @param[out] r roll rotation angle - * @param[out] p pitch rotation angle - * @param[out] y yaw rotation angle - */ - void getRPY(double & r, double & p, double &y) const; - - /** - * Get a roll, pitch and yaw corresponding to this rotation, - * as for getRPY, but return a vector with the output - * parameters. This function is more suitable for bindings. - * - * @return the output vector with the r, p and y parameters. - */ - iDynTree::Vector3 asRPY() const; - - /** - * Get a unit quaternion corresponding to this rotation - * - * The quaternion is defined as [s, r] - * where \f$s \in \mathbb{R}\f$ is the real and - * \f$r \in \mathbb{R}^3\f$ is the imaginary part. - * - * The returned quaternion is such that *this is - * equal to RotationFromQuaternion(quaternion). - * - * \note For each rotation, there are two quaternion - * corresponding to it. In this method we return - * the one that has the first non-zero (with a tolerance of 1e-7) - * component positive. If the real part is non-zero, this - * mean that we return the quaternion with positive real part. - * - * @param[out] quaternion the output quaternion - */ - bool getQuaternion(iDynTree::Vector4& quaternion) const; - - /** - * Get a unit quaternion corresponding to this rotation - * - * The unit quaternion is defined as [s, r] - * where \f$s \in \mathbb{R}\f$ is the real and - * \f$r \in \mathbb{R}^3\f$ is the imaginary part. - * - * The returned quaternion is such that *this is - * equal to RotationFromQuaternion(quaternion). - * - * \note For each rotation, there are two quaternion - * corresponding to it. In this method we return - * the one that has the first non-zero (with a tolerance of 1e-7) - * component positive. If the real part is non-zero, this - * mean that we return the quaternion with positive real part. - * - * @param[out] s the real part - * @param[out] r1 the first component of the imaginary part (i.e. i base) - * @param[out] r2 the second component of the imaginary part (i.e. j base) - * @param[out] r3 the third component of the imaginary part (i.e. k base) - */ - bool getQuaternion(double &s, double &r1, double &r2, double &r3) const; - - /** - * Get a unit quaternion corresponding to this rotation - * - * The quaternion is defined as [s, r] - * where \f$s \in \mathbb{R}\f$ is the costituent and - * \f$r \in \mathbb{R}^3\f$ is the imaginary part. - * - * The returned quaternion is such that *this is - * equal to RotationFromQuaternion(quaternion). - * - * \note For each rotation, there are two quaternion - * corresponding to it. In this method we return - * the one that has the first non-zero (with a tolerance of 1e-7) - * component positive. If the real part is non-zero, this - * mean that we return the quaternion with positive real part. - * - * @return the output quaternion - */ - iDynTree::Vector4 asQuaternion() const; - - ///@} - - /** - * @name Initialization helpers. - * - */ - ///@{ - - /** - * Return a Rotation around axis X of given angle - * - * If \f$ \theta \f$ is the input angle, this function - * returns the \f$ R_x(\theta) \f$ rotation matrix such that : - * \f[ - * R_x(\theta) = - * \begin{bmatrix} - * 1 & 0 & 0 \\ - * 0 & \cos(\theta) & - \sin(\theta) \\ - * 0 & \sin(\theta) & \cos(\theta) \\ - * \end{bmatrix} - * \f] - * - * @param angle the angle (in Radians) of the rotation arount the X axis - */ - static Rotation RotX(const double angle); - - /** - * Return a Rotation around axis Y of given angle - * - * If \f$ \theta \f$ is the input angle, this function - * returns the \f$ R_y(\theta) \f$ rotation matrix such that : - * \f[ - * R_y(\theta) = - * \begin{bmatrix} - * \cos(\theta) & 0 & \sin(\theta) \\ - * 0 & 1 & 0 \\ - * -\sin(\theta) & 0 & \cos(\theta) \\ - * \end{bmatrix} - * \f] - * - * - * @param angle the angle (in Radians) of the rotation arount the Y axis - */ - static Rotation RotY(const double angle); - - /** - * Return a Rotation around axis Z of given angle - * - * If \f$ \theta \f$ is the input angle, this function - * returns the \f$ R_z(\theta) \f$ rotation matrix such that : - * \f[ - * R_z(\theta) = - * \begin{bmatrix} - * \cos(\theta) & -\sin(\theta) & 0 \\ - * \sin(\theta) & \cos(\theta) & 0 \\ - * 0 & 0 & 1 \\ - * \end{bmatrix} - * \f] - * - * - * @param angle the angle (in Radians) of the rotation arount the Z axis - */ - static Rotation RotZ(const double angle); - - /** - * Return a Rotation around axis given by direction of given angle - * - * If we indicate with \f$ d \in \mathbb{R}^3 \f$ the unit norm - * of the direction, and with \f$ \theta \f$ the input angle, the return rotation - * matrix \f$ R \f$ can be computed using the Rodrigues' rotation formula [1] : - * \f[ - * R = I_{3\times3} + d^{\wedge} \sin(\theta) + {d^{\wedge}}^2 (1-\cos(\theta)) - * \f] - * - * [1] : http://mathworld.wolfram.com/RodriguesRotationFormula.html - * @param direction the Direction around with to rotate - * @param angle the angle (in Radians) of the rotation arount the given axis - */ - static Rotation RotAxis(const Direction & direction, const double angle); - - /** - * Return the derivative of the RotAxis function with respect to the angle argument. - * - * If we indicate with \f$ d \in \mathbb{R}^3 \f$ the unit norm - * of the direction, and with \f$ \theta \f$ the input angle, the derivative of the rotation - * matrix \f$ \frac{\partial R}{\partial \theta} \f$ can be computed using the - * derivative of the Rodrigues' rotation formula [1] : - * \f[ - * \frac{\partial R}{\partial \theta} = d^{\vee} \cos(\theta) + {d^{\vee}}^2 \sin(\theta) - * \f] - * - * [1] : http://mathworld.wolfram.com/RodriguesRotationFormula.html - * - * @param direction the Direction around with to rotate - * @param angle the angle (in Radians) of the rotation arount the given axis - */ - static Matrix3x3 RotAxisDerivative(const Direction & direction, const double angle); - - /** - * Return a rotation object given Roll, Pitch and Yaw values. - * - * @note This is equivalent to RotZ(y)*RotY(p)*RotX(r) . - * @note This method is compatible with the KDL::Rotation::RPY method. - */ - static Rotation RPY(const double roll, const double pitch, const double yaw); - - /** - * Return the right-trivialized derivative of the RPY function. - * - * If we indicate with \f$ rpy \in \mathbb{R}^3 \f$ the roll pitch yaw vector, - * and with \f$ RPY(rpy) : \mathbb{R}^3 \mapsto SO(3) \f$ the function implemented - * in the Rotation::RPY method, this method returns the right-trivialized partial - * derivative of Rotation::RPY, i.e. : - * \f[ - * (RPY(rpy) \frac{\partial RPY(rpy)}{\partial rpy})^\vee - * \f] - */ - static Matrix3x3 RPYRightTrivializedDerivative(const double roll, const double pitch, const double yaw); - - /** - * Return the rate of change of the right-trivialized derivative of the RPY function. - * - * If we indicate with \f$ rpy \in \mathbb{R}^3 \f$ the roll pitch yaw vector, - * and with \f$ RPY(rpy) : \mathbb{R}^3 \mapsto SO(3) \f$ the function implemented - * in the Rotation::RPY method, this method returns the right-trivialized partial - * derivative of Rotation::RPY, i.e. : - * \f[ - * (RPY(rpy) \frac{d}{d t}\frac{\partial RPY(rpy)}{\partial rpy})^\vee - * \f] - */ - static Matrix3x3 RPYRightTrivializedDerivativeRateOfChange(const double roll, const double pitch, const double yaw, const double rollDot, const double pitchDot, const double yawDot); - - /** - * Return the inverse of the right-trivialized derivative of the RPY function. - * - * See RPYRightTrivializedDerivative for a detailed description of the method. - * - */ - static Matrix3x3 RPYRightTrivializedDerivativeInverse(const double roll, const double pitch, const double yaw); - - /** - * Return the rate of change of the inverse of the right-trivialized derivative of the RPY function. - * - * See RPYRightTrivializedDerivativeRateOfChange for a detailed description of the method. - * - */ - static Matrix3x3 RPYRightTrivializedDerivativeInverseRateOfChange(const double roll, const double pitch, const double yaw, const double rollDot, const double pitchDot, const double yawDot); - - /** - * Return the right-trivialized derivative of the Quaternion function. - * - * If we indicate with \f$ quat \in \mathbb{Q} \f$ the quaternion, - * and with \f$ QUAT(quat) : \mathbb{Q} \mapsto SO(3) \f$ the function implemented - * in the Rotation::RotationFromQuaternion method, this method returns the right-trivialized partial - * derivative of Rotation::RotationFromQuaternion, i.e. : - * \f[ - * (QUAT(quat) \frac{\partial QUAT(quat)}{\partial quat})^\vee - * \f] - */ - static MatrixFixSize<4, 3> QuaternionRightTrivializedDerivative(Vector4 quaternion); - - /** - * Return the inverse of the right-trivialized derivative of the Quaternion function. - * - * @see QuaternionRightTrivializedDerivative for a detailed description of the method. - * - */ - static MatrixFixSize<3, 4> QuaternionRightTrivializedDerivativeInverse(Vector4 quaternion); - - - /** - * Return an identity rotation. - * - * - */ - static Rotation Identity(); - - /** - * Construct a rotation matrix from the given unit quaternion representation - * - * The quaternion is expected to be ordered in the following way: - * - \f$s \in \mathbb{R}\f$ the real part of the quaterion - * - \f$r \in \mathbb{R}^3\f$ the imaginary part of the quaternion - * - * The returned rotation matrix is given by the following formula: - * \f[ - * R(s,r) = I_{3\times3} + 2s r^{\wedge} + 2{r^\wedge}^2, - * \f] - * where \f$ r^{\wedge} \f$ is the skew-symmetric matrix such that: - * \f[ - * r \times v = r^\wedge v - * \f] - * - * @note the quaternion is normalized - * @param quaternion a quaternion representing a rotation - * - * @return The rotation matrix - */ - static Rotation RotationFromQuaternion(const iDynTree::Vector4& quaternion); - - /** - * Get the left Jacobian of rotation matrix - * - * \f$ \omega \in \mathbb{R}^3 \f$ is the angular motion vector - * \f$ [\omega_\times]: \mathbb{R}^n \to \mathfrak{so}(3) \f$ where \f$ \mathfrak{so}(3) \f$ - * is the set of skew symmetric matrices or the Lie algebra of \f$ SO(3) \f$ - * \f[ J_{l_{SO(3)}} = \sum_{n = 0}^{\infty} \frac{1}{(n+1)!} [\omega_\times]^n = (I_3 + \frac{1 - \text{cos}(||\omega||)}{||\omega||^{2}} [\omega _{\times}] + \frac{||\omega|| - \text{sin}(||\omega||)}{||\omega||^{3}} [\omega _{\times}]^{2} \f] - * - * When simplified further, - * \f[ J_{l_{SO(3)}} = \frac{\text{sin}(||\omega||)}{||\omega||}I_3 + \frac{1 - \text{cos}(||\omega||)}{||\omega||} [\phi _{\times}] + \bigg(1 - \frac{\text{sin}(||\omega||)}{||\omega||}\bigg) \phi\phi^T \f] - * - * where \f$ \phi = \frac{\omega}{||\omega||} \f$ - * - * @param[in] omega angular motion vector - * @return \f$ 3 \times 3 \f$ left Jacobian matrix - */ - static Matrix3x3 leftJacobian(const iDynTree::AngularMotionVector3& omega); - - /** - * Get the left Jacobian inverse of rotation matrix - * - * \f$ \omega \in \mathbb{R}^3 \f$ is the angular motion vector - * \f$ [\omega_\times]: \mathbb{R}^n \to \mathfrak{so}(3) \f$ where \f$ \mathfrak{so}(3) \f$ - * is the set of skew symmetric matrices or the Lie algebra of \f$ SO(3) \f$ - * \f[ J^{-1} _{l _{SO(3)}} = \frac{||\omega||}{2} \text{cot} \bigg(\frac{||\omega||}{2}\bigg) I _3 + \bigg( 1 - \frac{||\omega||}{2} \text{cot} \bigg(\frac{||\omega||}{2}\bigg) \bigg) \phi \phi^T - \frac{||\omega||}{2} [\phi _{\times}] \f] - * - * where \f$ \phi = \frac{\omega}{||\omega||} \f$ - * - * @param[in] omega angular motion vector - * @return \f$ 3 \times 3 \f$ left Jacobian inverse matrix - */ - static Matrix3x3 leftJacobianInverse(const iDynTree::AngularMotionVector3& omega); - ///@} - - /** @name Output helpers. - * Output helpers. - */ - ///@{ - std::string toString() const; +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif - std::string reservedToString() const; - ///@} - }; -} +#include #endif diff --git a/src/core/include/iDynTree/Core/RotationRaw.h b/src/core/include/iDynTree/Core/RotationRaw.h index 95512c508ff..1394e898f52 100644 --- a/src/core/include/iDynTree/Core/RotationRaw.h +++ b/src/core/include/iDynTree/Core/RotationRaw.h @@ -1,140 +1,13 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef IDYNTREE_ROTATION_RAW_H -#define IDYNTREE_ROTATION_RAW_H +#ifndef IDYNTREE_CORE_ROTATION_RAW_H +#define IDYNTREE_CORE_ROTATION_RAW_H -#include -#include - -namespace iDynTree -{ - class PositionRaw; - class SpatialMotionVector; - class SpatialForceVector; - class ClassicalAcc; - class RotationalInertiaRaw; - template - class MatrixView; - - /** - * Class providing the raw coordinates for iDynTree::Rotation class. - * - * Storage for the Orientation: - * The rotation matrix representation of the orientation, stored in row major order, - * inside a Matrix3x3 parent object. - * - * \note This implementation is compatible with KDL::Rotation data. - * - * \warning This class uses for convenience the Matrix3x3 as a public parent. - * Notice that using this methods you can damage the underlyng rotation matrix. - * In doubt, don't use them and rely on more high level functions. - * - * \ingroup iDynTreeCore - */ - class RotationRaw: public Matrix3x3 - { - public: - /** - * Default constructor. - * The data is not reset to identity for perfomance reason. - * Please initialize the data in the vector before any use. - */ - RotationRaw(); - - /** - * Constructor from 9 doubles: initialize elements of the rotation matrix. - */ - RotationRaw(double xx, double xy, double xz, - double yx, double yy, double yz, - double zx, double zy, double zz); - - /** - * Constructor from a buffer of 9 doubles, - * stored as a C-style array (i.e. row major). - * - */ - RotationRaw(const double* in_data, - const unsigned int in_rows, - const unsigned int in_cols); - - RotationRaw(iDynTree::MatrixView other); - - /** - * Copy constructor: create a RotationRaw from another RotationRaw. - */ - RotationRaw(const RotationRaw & other); - - /** - * Geometric operations. - */ - const RotationRaw & changeOrientFrame(const RotationRaw & newOrientFrame); - const RotationRaw & changeRefOrientFrame(const RotationRaw & newRefOrientFrame); - static RotationRaw compose(const RotationRaw & op1, const RotationRaw & op2); - static RotationRaw inverse2(const RotationRaw & orient); - PositionRaw changeCoordFrameOf(const PositionRaw & other) const; - ClassicalAcc changeCoordFrameOf(const ClassicalAcc & other) const; - RotationalInertiaRaw changeCoordFrameOf(const RotationalInertiaRaw & other) const; - - - - /** - * overloaded operators - */ - - /** - * @name Initialization helpers. - * - */ - ///@{ - - /** - * Return a Rotation around axis X of given angle - * - * @param angle the angle (in Radians) of the rotation arount the X axis - */ - static RotationRaw RotX(const double angle); - - /** - * Return a Rotation around axis Y of given angle - * - * @param angle the angle (in Radians) of the rotation arount the Y axis - */ - static RotationRaw RotY(const double angle); - - /** - * Return a Rotation around axis Z of given angle - * - * @param angle the angle (in Radians) of the rotation arount the Z axis - */ - static RotationRaw RotZ(const double angle); - - /** - * Return a rotation object given Roll, Pitch and Yaw values. - * - * @note This method is compatible with the KDL::Rotation::RPY method. - */ - static RotationRaw RPY(const double roll, const double pitch, const double yaw); - - /** - * Return an identity rotation. - * - * - */ - static RotationRaw Identity(); - - ///@} - - - /** @name Output helpers. - * Output helpers. - */ - ///@{ - std::string toString() const; +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif - std::string reservedToString() const; - ///@} - }; -} +#include #endif diff --git a/src/core/include/iDynTree/Core/RotationalInertiaRaw.h b/src/core/include/iDynTree/Core/RotationalInertiaRaw.h index eb220d4c7ef..78a5c0b5176 100644 --- a/src/core/include/iDynTree/Core/RotationalInertiaRaw.h +++ b/src/core/include/iDynTree/Core/RotationalInertiaRaw.h @@ -1,47 +1,13 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef IDYNTREE_ROTATIONAL_INERTIA_RAW_H -#define IDYNTREE_ROTATIONAL_INERTIA_RAW_H +#ifndef IDYNTREE_CORE_ROTATIONAL_INERTIA_RAW_H +#define IDYNTREE_CORE_ROTATIONAL_INERTIA_RAW_H -#include +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif -namespace iDynTree -{ - class PositionRaw; +#include - /** - * Class providing the raw coordinates for a 3d inertia matrix. - * - * \ingroup iDynTreeCore - * - * \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. - * - * We use a parent Matrix3x3 for storage of the rotational inertia matrix: - * given that the inertia matrix is a 3x3 symmetric matrix, - * the ordering (row order or column order) is not influencing - * the storage of the matrix. - */ - class RotationalInertiaRaw: public Matrix3x3 - { - - public: - /** - * Default constructor. - * The data is not reset to zero for perfomance reason. - * Please initialize the data in the vector before any use. - */ - RotationalInertiaRaw(); - RotationalInertiaRaw(const double * in_data, const unsigned int in_rows, const unsigned int in_cols); - RotationalInertiaRaw(const RotationalInertiaRaw & other); - - /** - * Initializer helper: return a zero matrix. - */ - static RotationalInertiaRaw Zero(); - - }; -} - -#endif /* IDYNTREE_ROTATIONAL_INERTIA_RAW_H */ +#endif \ No newline at end of file diff --git a/src/core/include/iDynTree/Core/SO3Utils.h b/src/core/include/iDynTree/Core/SO3Utils.h index ab74e9f0a73..634d9c4c0dc 100644 --- a/src/core/include/iDynTree/Core/SO3Utils.h +++ b/src/core/include/iDynTree/Core/SO3Utils.h @@ -2,79 +2,13 @@ // SPDX-License-Identifier: BSD-3-Clause -#ifndef IDYNTREE_SO3UTILS_H -#define IDYNTREE_SO3UTILS_H +#ifndef IDYNTREE_CORE_SO3UTILS_H +#define IDYNTREE_CORE_SO3UTILS_H -#include -#include -#include +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif -namespace iDynTree -{ - /** - * @brief Struct containing the options for geodesicL2MeanRotation and geodesicL2WeightedMeanRotation. - */ - struct GeodesicL2MeanOptions - { - double tolerance{1e-5}; /** Tolerance for terminating the inner refinement loop of the mean rotation. **/ - double timeoutInSeconds{-1}; /** Timeout for the refinement loop. **/ - int maxIterations{-1}; /** Max number of iterations for the refinement loop. **/ - bool verbose{false}; /** Add a message when the solution is found. **/ - double stepSize{1.0}; /** Step-size for the refinement loop. **/ - }; +#include - /** - * @brief Check if the rotation matrix is valid. - * - * It checks that the determinant is 1, that the Frobenius norm is finite and that it is orthogonal. - * @param r The input rotation - * @return True if it is a rotation matrix. - */ - bool isValidRotationMatrix(const iDynTree::Rotation& r); - - /** - * @brief Computes the geodesic distance between two rotation matrices. - * - * It implements the angular distance presented in Sec. 4 of "Rotation Averaging" (available at http://users.cecs.anu.edu.au/~hongdong/rotationaveraging.pdf), - * in particular \f$d = ||log(R_1^\top R_2)||^2 \f$. - * @param rotation1 The first rotation. - * @param rotation2 The other rotation. - * @return the geodesic L2 distance between the two rotation matrices. - */ - double geodesicL2Distance(const iDynTree::Rotation& rotation1, const iDynTree::Rotation& rotation2); - - /** - * @brief Computes the geodesic mean amongst the provided rotations. - * - * It implements Algorithm 1 in Sec. 5.3 of "Rotation Averaging" (available at http://users.cecs.anu.edu.au/~hongdong/rotationaveraging.pdf). - * - * Inside it calls geodesicL2WeightedMeanRotation. - * - * @param inputRotations The rotations to average. - * @param meanRotation The mean rotation. - * @param options The options for the inner optimization (refinement) loop. - * @return false in case of failure, true otherwise. - */ - bool geodesicL2MeanRotation(const std::vector& inputRotations, - iDynTree::Rotation& meanRotation, - const GeodesicL2MeanOptions& options = GeodesicL2MeanOptions()); - - /** - * @brief Computes the weighted geodesic mean amongst the provided rotations - * - * It implements Algorithm 1 in Sec. 5.3 of "Rotation Averaging" (available at http://users.cecs.anu.edu.au/~hongdong/rotationaveraging.pdf), - * with a small modification to take into accounts weights different from 1. - * - * @param inputRotations The rotations to average. - * @param weights The weights for each rotation. If this vector is null assumes that each weight is 1.0 (equivalent to geodesicL2MeanRotation) - * @param meanRotation The weighted mean rotation. - * @param options The options for the inner optimization (refinement) loop. - * @return false in case of failure, true otherwise. - */ - bool geodesicL2WeightedMeanRotation(const std::vector& inputRotations, - const std::vector& weights, - iDynTree::Rotation& meanRotation, - const GeodesicL2MeanOptions& options = GeodesicL2MeanOptions()); -} - -#endif // IDYNTREE_SO3UTILS_H +#endif diff --git a/src/core/include/iDynTree/Core/Span.h b/src/core/include/iDynTree/Core/Span.h index fe8a76261c9..56ee402c0d5 100644 --- a/src/core/include/iDynTree/Core/Span.h +++ b/src/core/include/iDynTree/Core/Span.h @@ -1,777 +1,10 @@ -#ifndef IDYNTREE_SPAN_H -#define IDYNTREE_SPAN_H -/////////////////////////////////////////////////////////////////////////////// -// -// Copyright (c) 2015 Microsoft Corporation. All rights reserved. -// -// This code is licensed under the MIT License (MIT). -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. -// -/////////////////////////////////////////////////////////////////////////////// +#ifndef IDYNTREE_CORE_SPAN_H +#define IDYNTREE_CORE_SPAN_H -//Most of this file has been taken from https://github.com/Microsoft/GSL/blob/master/include/gsl/span - -// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) -// SPDX-License-Identifier: BSD-3-Clause - - -#include - -#include // for lexicographical_compare -#include // for array -#include // for ptrdiff_t, size_t, nullptr_t -#include // for reverse_iterator, distance, random_access_... -#include -#include -#include // for enable_if_t, declval, is_convertible, inte... -#include - -#ifdef _MSC_VER -#pragma warning(push) - -// turn off some warnings that are noisy about our Expects statements -#pragma warning(disable : 4127) // conditional expression is constant -#pragma warning(disable : 4702) // unreachable code - -// blanket turn off warnings from CppCoreCheck for now -// so people aren't annoyed by them when running the tool. -#pragma warning(disable : 26481 26482 26483 26485 26490 26491 26492 26493 26495) - -#if _MSC_VER < 1910 -#pragma push_macro("constexpr") -#define constexpr /*constexpr*/ -#define IDYNTREE_USE_STATIC_CONSTEXPR_WORKAROUND - -#endif // _MSC_VER < 1910 -#else // _MSC_VER - -// See if we have enough C++17 power to use a static constexpr data member -// without needing an out-of-line definition -#if !(defined(__cplusplus) && (__cplusplus >= 201703L)) -#define IDYNTREE_USE_STATIC_CONSTEXPR_WORKAROUND -#endif // !(defined(__cplusplus) && (__cplusplus >= 201703L)) - -#endif // _MSC_VER - -// constexpr workaround for SWIG -#ifdef SWIG -#define IDYNTREE_CONSTEXPR -#else -#define IDYNTREE_CONSTEXPR constexpr +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. #endif -namespace iDynTree -{ - -namespace SpanUtils { -#ifndef SWIG -// this is required to be compatible with c++17 -template struct make_void { typedef void type; }; -template using void_t = typename make_void::type; - -//Small utility to detect if type T has value_type defined -template struct is_value_defined : std::false_type -{ -}; - -template -struct is_value_defined> : std::true_type -{ -}; - -//Small utility to detect if type T has element_type defined -template struct is_element_defined : std::false_type -{ -}; - -template -struct is_element_defined> : std::true_type -{ -}; - -//Small utility to detect if class T has the data() method defined -template struct has_data_method : std::false_type -{ -}; - -template -struct has_data_method().data())>> : std::true_type -{ -}; - -//Small utility to detect if class T has the size() method defined -template struct has_size_method : std::false_type -{ -}; - -template -struct has_size_method().size())>> : std::true_type -{ -}; +#include #endif -} - -// [views.constants], constants -IDYNTREE_CONSTEXPR const std::ptrdiff_t dynamic_extent = -1; - -template -class Span; - -// implementation details -namespace details -{ - template - struct is_span_oracle : std::false_type - { - }; - - template - struct is_span_oracle> : std::true_type - { - }; - - template - struct is_span : public is_span_oracle> - { - }; - - template - struct is_std_array_oracle : std::false_type - { - }; - - template - struct is_std_array_oracle> : std::true_type - { - }; - - template - struct is_std_array : public is_std_array_oracle> - { - }; - - template - struct is_allowed_extent_conversion - : public std::integral_constant - { - }; - - template - struct is_allowed_element_type_conversion - : public std::integral_constant::value> - { - }; - - template - class span_iterator - { - using element_type_ = typename Span::element_type; - - public: - - using iterator_category = std::random_access_iterator_tag; - using value_type = std::remove_cv_t; - using difference_type = typename Span::index_type; - - using reference = std::conditional_t&; - using pointer = std::add_pointer_t; - - span_iterator() = default; - - IDYNTREE_CONSTEXPR span_iterator(const Span* span, typename Span::index_type idx) noexcept - : span_(span), index_(idx) - {} - - friend span_iterator; - template* = nullptr> - IDYNTREE_CONSTEXPR span_iterator(const span_iterator& other) noexcept - : span_iterator(other.span_, other.index_) - { - } - - IDYNTREE_CONSTEXPR reference operator*() const - { - assert(index_ != span_->size()); - return *(span_->data() + index_); - } - - IDYNTREE_CONSTEXPR pointer operator->() const - { - assert(index_ != span_->size()); - return span_->data() + index_; - } - - IDYNTREE_CONSTEXPR span_iterator& operator++() - { - assert(0 <= index_ && index_ != span_->size()); - ++index_; - return *this; - } - - IDYNTREE_CONSTEXPR span_iterator operator++(int) - { - auto ret = *this; - ++(*this); - return ret; - } - - IDYNTREE_CONSTEXPR span_iterator& operator--() - { - assert(index_ != 0 && index_ <= span_->size()); - --index_; - return *this; - } - - IDYNTREE_CONSTEXPR span_iterator operator--(int) - { - auto ret = *this; - --(*this); - return ret; - } - - IDYNTREE_CONSTEXPR span_iterator operator+(difference_type n) const - { - auto ret = *this; - return ret += n; - } - - IDYNTREE_CONSTEXPR span_iterator& operator+=(difference_type n) - { - assert((index_ + n) >= 0 && (index_ + n) <= span_->size()); - index_ += n; - return *this; - } - - IDYNTREE_CONSTEXPR span_iterator operator-(difference_type n) const - { - auto ret = *this; - return ret -= n; - } - - IDYNTREE_CONSTEXPR span_iterator& operator-=(difference_type n) { return *this += -n; } - - IDYNTREE_CONSTEXPR difference_type operator-(span_iterator rhs) const - { - assert(span_ == rhs.span_); - return index_ - rhs.index_; - } - - IDYNTREE_CONSTEXPR reference operator[](difference_type n) const - { - return *(*this + n); - } - - IDYNTREE_CONSTEXPR friend bool operator==(span_iterator lhs, - span_iterator rhs) noexcept - { - return lhs.span_ == rhs.span_ && lhs.index_ == rhs.index_; - } - - IDYNTREE_CONSTEXPR friend bool operator!=(span_iterator lhs, - span_iterator rhs) noexcept - { - return !(lhs == rhs); - } - - IDYNTREE_CONSTEXPR friend bool operator<(span_iterator lhs, - span_iterator rhs) noexcept - { - return lhs.index_ < rhs.index_; - } - - IDYNTREE_CONSTEXPR friend bool operator<=(span_iterator lhs, - span_iterator rhs) noexcept - { - return !(rhs < lhs); - } - - IDYNTREE_CONSTEXPR friend bool operator>(span_iterator lhs, - span_iterator rhs) noexcept - { - return rhs < lhs; - } - - IDYNTREE_CONSTEXPR friend bool operator>=(span_iterator lhs, - span_iterator rhs) noexcept - { - return !(rhs > lhs); - } - - protected: - const Span* span_ = nullptr; - std::ptrdiff_t index_ = 0; - }; - - template - IDYNTREE_CONSTEXPR span_iterator - operator+(typename span_iterator::difference_type n, - span_iterator rhs) - { - return rhs + n; - } - - template - IDYNTREE_CONSTEXPR span_iterator - operator-(typename span_iterator::difference_type n, - span_iterator rhs) - { - return rhs - n; - } - - template - class extent_type - { - public: - using index_type = std::ptrdiff_t; - - static_assert(Ext >= 0, "A fixed-size span must be >= 0 in size."); - - IDYNTREE_CONSTEXPR extent_type() noexcept {} - - template - IDYNTREE_CONSTEXPR extent_type(extent_type ext) - { - static_assert(Other == Ext || Other == dynamic_extent, - "Mismatch between fixed-size extent and size of initializing data."); - assert(ext.size() == Ext); - } - - IDYNTREE_CONSTEXPR extent_type(index_type size) { assert(size == Ext); } - - IDYNTREE_CONSTEXPR index_type size() const noexcept { return Ext; } - }; - - template <> - class extent_type - { - public: - using index_type = std::ptrdiff_t; - - template - explicit IDYNTREE_CONSTEXPR extent_type(extent_type ext) : size_(ext.size()) - { - } - - explicit IDYNTREE_CONSTEXPR extent_type(index_type size) : size_(size) { assert(size >= 0); } - - IDYNTREE_CONSTEXPR index_type size() const noexcept { return size_; } - - private: - index_type size_; - }; - - template - struct calculate_subspan_type - { - using type = Span; - }; -} // namespace details - -// [span], class template span -template -class Span -{ -public: - // constants and types - using element_type = ElementType; - using value_type = std::remove_cv_t; - using index_type = std::ptrdiff_t; - using pointer = element_type*; - using reference = element_type&; - using const_reference = const element_type&; - - using iterator = details::span_iterator, false>; - using const_iterator = details::span_iterator, true>; - using reverse_iterator = std::reverse_iterator; - using const_reverse_iterator = std::reverse_iterator; - - using size_type = index_type; - -#if defined(IDYNTREE_USE_STATIC_CONSTEXPR_WORKAROUND) - static constexpr const index_type extent { Extent }; -#else - static constexpr index_type extent { Extent }; -#endif - -#ifndef SWIG - // [span.cons], span constructors, copy, assignment, and destructor - template " SFINAE, - // since "std::enable_if_t" is ill-formed when Extent is greater than 0. - class = std::enable_if_t<(Dependent || Extent <= 0)>> - IDYNTREE_CONSTEXPR Span() noexcept : storage_(nullptr, details::extent_type<0>()) - { - } -#endif - - IDYNTREE_CONSTEXPR Span(pointer ptr, index_type count) : storage_(ptr, count) {} - - IDYNTREE_CONSTEXPR Span(pointer firstElem, pointer lastElem) - : storage_(firstElem, std::distance(firstElem, lastElem)) - { - } - - template - IDYNTREE_CONSTEXPR Span(element_type (&arr)[N]) noexcept - : storage_(KnownNotNull{&arr[0]}, details::extent_type()) - { - } - - template > - IDYNTREE_CONSTEXPR Span(std::array& arr) noexcept - : storage_(&arr[0], details::extent_type()) - { - } - - template - IDYNTREE_CONSTEXPR Span(const std::array, N>& arr) noexcept - : storage_(&arr[0], details::extent_type()) - { - } - - // NB: the SFINAE here uses .data() as a incomplete/imperfect proxy for the requirement - // on Container to be a contiguous sequence container. -#ifndef SWIG - template ::value && SpanUtils::has_size_method::value>, - class = std::enable_if_t< - !details::is_span::value && !details::is_std_array::value && - std::is_convertible().data()), pointer>::value>> - IDYNTREE_CONSTEXPR Span(Container& cont) : Span(cont.data(), static_cast(cont.size())) - { - } - - template ::value && SpanUtils::has_size_method::value>, - class = std::enable_if_t< - std::is_const::value && !details::is_span::value && - std::is_convertible().data()), pointer>::value>> - IDYNTREE_CONSTEXPR Span(const Container& cont) : Span(cont.data(), static_cast(cont.size())) - { - } -#endif - - IDYNTREE_CONSTEXPR Span(const Span& other) noexcept = default; - -#ifndef SWIG - template < - class OtherElementType, std::ptrdiff_t OtherExtent, - class = std::enable_if_t< - details::is_allowed_extent_conversion::value && - details::is_allowed_element_type_conversion::value>> - IDYNTREE_CONSTEXPR Span(const Span& other) - : storage_(other.data(), details::extent_type(other.size())) - { - } -#endif - - ~Span() noexcept = default; - IDYNTREE_CONSTEXPR Span& operator=(const Span& other) noexcept = default; - - // [span.sub], span subviews - template - IDYNTREE_CONSTEXPR Span first() const - { - assert(Count >= 0 && Count <= size()); - return {data(), Count}; - } - - template - IDYNTREE_CONSTEXPR Span last() const - { - assert(Count >= 0 && size() - Count >= 0); - return {data() + (size() - Count), Count}; - } - -#ifndef SWIG - template - IDYNTREE_CONSTEXPR auto subspan() const -> typename details::calculate_subspan_type::type - { - assert((Offset >= 0 && size() - Offset >= 0) && - (Count == dynamic_extent || (Count >= 0 && Offset + Count <= size()))); - - return {data() + Offset, Count == dynamic_extent ? size() - Offset : Count}; - } -#endif - - IDYNTREE_CONSTEXPR Span first(index_type count) const - { - assert(count >= 0 && count <= size()); - return {data(), count}; - } - - IDYNTREE_CONSTEXPR Span last(index_type count) const - { - return make_subspan(size() - count, dynamic_extent, subspan_selector{}); - } - - IDYNTREE_CONSTEXPR Span subspan(index_type offset, - index_type count = dynamic_extent) const - { - return make_subspan(offset, count, subspan_selector{}); - } - - - // [span.obs], span observers - IDYNTREE_CONSTEXPR index_type size() const noexcept { return storage_.size(); } - IDYNTREE_CONSTEXPR index_type size_bytes() const noexcept - { - return size() * static_cast(sizeof(element_type)); - } - IDYNTREE_CONSTEXPR bool empty() const noexcept { return size() == 0; } - - // [span.elem], span element access - IDYNTREE_CONSTEXPR reference operator[](index_type idx) const - { - assert(idx >= 0 && idx < storage_.size()); - return data()[idx]; - } - - IDYNTREE_CONSTEXPR const_reference getVal(index_type idx) const { return this->operator[](idx);} - IDYNTREE_CONSTEXPR bool setVal(index_type idx, const_reference val) - { - assert(idx >= 0 && idx < storage_.size()); - data()[idx] = val; - return true; - } - - IDYNTREE_CONSTEXPR reference at(index_type idx) const { return this->operator[](idx); } - IDYNTREE_CONSTEXPR reference operator()(index_type idx) const { return this->operator[](idx); } - IDYNTREE_CONSTEXPR pointer data() const noexcept { return storage_.data(); } - - // [span.iter], span iterator support - IDYNTREE_CONSTEXPR iterator begin() const noexcept { return {this, 0}; } - IDYNTREE_CONSTEXPR iterator end() const noexcept { return {this, size()}; } - - IDYNTREE_CONSTEXPR const_iterator cbegin() const noexcept { return {this, 0}; } - IDYNTREE_CONSTEXPR const_iterator cend() const noexcept { return {this, size()}; } - - IDYNTREE_CONSTEXPR reverse_iterator rbegin() const noexcept { return reverse_iterator{end()}; } - IDYNTREE_CONSTEXPR reverse_iterator rend() const noexcept { return reverse_iterator{begin()}; } - - IDYNTREE_CONSTEXPR const_reverse_iterator crbegin() const noexcept { return const_reverse_iterator{cend()}; } - IDYNTREE_CONSTEXPR const_reverse_iterator crend() const noexcept { return const_reverse_iterator{cbegin()}; } - -private: - - // Needed to remove unnecessary null check in subspans - struct KnownNotNull - { - pointer p; - }; - - // this implementation detail class lets us take advantage of the - // empty base class optimization to pay for only storage of a single - // pointer in the case of fixed-size spans - template - class storage_type : public ExtentType - { - public: - // KnownNotNull parameter is needed to remove unnecessary null check - // in subspans and constructors from arrays - template - IDYNTREE_CONSTEXPR storage_type(KnownNotNull data, OtherExtentType ext) : ExtentType(ext), data_(data.p) - { - assert(ExtentType::size() >= 0); - } - - - template - IDYNTREE_CONSTEXPR storage_type(pointer data, OtherExtentType ext) : ExtentType(ext), data_(data) - { - assert(ExtentType::size() >= 0); - assert(data || ExtentType::size() == 0); - } - - IDYNTREE_CONSTEXPR pointer data() const noexcept { return data_; } - - private: - pointer data_; - }; - - storage_type> storage_; - - // The rest is needed to remove unnecessary null check - // in subspans and constructors from arrays - IDYNTREE_CONSTEXPR Span(KnownNotNull ptr, index_type count) : storage_(ptr, count) {} - - template - class subspan_selector {}; - - template - Span make_subspan(index_type offset, - index_type count, - subspan_selector) const - { - Span tmp(*this); - return tmp.subspan(offset, count); - } - - Span make_subspan(index_type offset, - index_type count, - subspan_selector) const - { - assert(offset >= 0 && size() - offset >= 0); - if (count == dynamic_extent) - { - return { KnownNotNull{ data() + offset }, size() - offset }; - } - - assert(count >= 0 && size() - offset >= count); - return { KnownNotNull{ data() + offset }, count }; - } -}; - -#if defined(IDYNTREE_USE_STATIC_CONSTEXPR_WORKAROUND) -template -IDYNTREE_CONSTEXPR const typename Span::index_type Span::extent; -#endif - - -// [span.comparison], span comparison operators -template -IDYNTREE_CONSTEXPR bool operator==(Span l, - Span r) -{ - return std::equal(l.begin(), l.end(), r.begin(), r.end()); -} - -template -IDYNTREE_CONSTEXPR bool operator!=(Span l, - Span r) -{ - return !(l == r); -} - -template -IDYNTREE_CONSTEXPR bool operator<(Span l, - Span r) -{ - return std::lexicographical_compare(l.begin(), l.end(), r.begin(), r.end()); -} - -template -IDYNTREE_CONSTEXPR bool operator<=(Span l, - Span r) -{ - return !(l > r); -} - -template -IDYNTREE_CONSTEXPR bool operator>(Span l, - Span r) -{ - return r < l; -} - -template -IDYNTREE_CONSTEXPR bool operator>=(Span l, - Span r) -{ - return !(l < r); -} - -namespace details -{ - // if we only supported compilers with good constexpr support then - // this pair of classes could collapse down to a constexpr function - - // we should use a narrow_cast<> to go to std::size_t, but older compilers may not see it as - // constexpr - // and so will fail compilation of the template -#ifndef SWIG - template - struct calculate_byte_size - : std::integral_constant(sizeof(ElementType) * - static_cast(Extent))> - { - }; - - template - struct calculate_byte_size - : std::integral_constant - { - }; -#endif -} - - -#ifndef SWIG -// -// make_span() - Utility functions for creating spans -// -template -IDYNTREE_CONSTEXPR Span make_span(ElementType* ptr, typename Span::index_type count) -{ - return Span(ptr, count); -} - -template -IDYNTREE_CONSTEXPR Span make_span(ElementType* firstElem, ElementType* lastElem) -{ - return Span(firstElem, lastElem); -} - -template -IDYNTREE_CONSTEXPR Span make_span(ElementType (&arr)[N]) noexcept -{ - return Span(arr); -} - -template -IDYNTREE_CONSTEXPR Span make_span(Container& cont) -{ - return Span(cont); -} - -template ::value>::type> -IDYNTREE_CONSTEXPR Span make_span(const Container& cont) -{ - return Span(cont); -} - -template -IDYNTREE_CONSTEXPR Span make_span(Ptr& cont, std::ptrdiff_t count) -{ - return Span(cont, count); -} - -template ::value && SpanUtils::is_element_defined::value>::type> -IDYNTREE_CONSTEXPR Span make_span(Ptr& cont) -{ - return Span(cont); -} - -template ::value && - !SpanUtils::is_element_defined::value && - SpanUtils::has_data_method::value>::type> -IDYNTREE_CONSTEXPR Span().data())>::type> make_span(Container& cont) -{ - return Span().data())>::type>(cont); -} - -#endif - -} // namespace iDynTree - -#ifdef _MSC_VER -#if _MSC_VER < 1910 -#undef constexpr -#pragma pop_macro("constexpr") - -#endif // _MSC_VER < 1910 - -#pragma warning(pop) -#endif // _MSC_VER - -#endif // IDYNTREE_SPAN_H diff --git a/src/core/include/iDynTree/Core/SparseMatrix.h b/src/core/include/iDynTree/Core/SparseMatrix.h index 9f519d599fa..04f9c96cb6b 100644 --- a/src/core/include/iDynTree/Core/SparseMatrix.h +++ b/src/core/include/iDynTree/Core/SparseMatrix.h @@ -1,399 +1,13 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef IDYNTREE_SPARSE_MATRIX_H -#define IDYNTREE_SPARSE_MATRIX_H +#ifndef IDYNTREE_CORE_SPARSE_MATRIX_H +#define IDYNTREE_CORE_SPARSE_MATRIX_H -#include -#include -#include - -#include - -namespace iDynTree { - - template - class SparseMatrix; - - class Triplet; - class Triplets; -} - -// MARK: - SparseMatrix class - -/** - * \brief Sparse Matrix class - * - * This class uses the Compressed Column (Row) Storage scheme - * (see https://en.wikipedia.org/wiki/Sparse_matrix#Compressed_sparse_row_.28CSR.2C_CRS_or_Yale_format.29) - * which is compatible with the format used in the Eigen library (by using Map). - */ -template -class iDynTree::SparseMatrix -{ -private: - - iDynTree::VectorDynSize m_values; /**< Contains all the non zero (NZ) elements */ - - std::vector m_innerIndices; /**< column index of the NZ elements */ - std::vector m_outerStarts; /**< for each row contains the index of the first NZ in the - * previous two arrays - */ - - - std::size_t m_allocatedSize; /**< size of the memory allocated for m_values and m_innerIndices */ - - std::size_t m_rows; - std::size_t m_columns; - - void initializeMatrix(std::size_t outerSize, const double* vector, std::size_t vectorSize); - - /** - * Insert a new element at the specified position - * - * It returns the index in m_values of the inserted element - * @param row row of the new element - * @param col column of the new element - * @param value value to be inserted - * @return index in m_values of the inserted element - */ - std::size_t insert(std::size_t row, std::size_t col, double value); - - /** - * Check if the element at row-col is present in the matrix. - * - * If it is present the function returns true and the index in the - * out parameter index, otherwise it returns false and index will - * contain the index of the next element in the value array - * - * @param[in] row row index - * @param[in] col column index - * @param[out] index if found contains the index, if not the index of the next element - * @return true if the value has been found - */ - bool valueIndexForOuterAndInnerIndices(std::size_t outerIndex, std::size_t innerIndex, std::size_t& valueIndex) const; - -public: - - /** - * Creates an empty sparse matrix. - */ - SparseMatrix(); - - /** - * Creates a zero sparse matrix with the specified dimensions - * - */ - SparseMatrix(std::size_t rows, std::size_t cols); - - - SparseMatrix(std::size_t rows, std::size_t cols, - const iDynTree::VectorDynSize& memoryReserveDescription); - - template - SparseMatrix(const SparseMatrix&); - - template - SparseMatrix& operator=(const SparseMatrix&); - - - /** - * Default destructor - */ - ~SparseMatrix(); - - /** - * Returns the number of nonzero elements in this sparse matrix - * - * @return the number of non zero elements - */ - std::size_t numberOfNonZeros() const; - - /** - * Resize the matrix to the specified new dimensions - * - * \note the content of the matrix is not preserved in any case - * \warning this function may perform memory allocation - * @param rows the new number of rows of this matrix - * @param columns the new number of columns of this matrix - */ - void resize(std::size_t rows, std::size_t columns); - - /** - * Resize the matrix to the specified new dimensions - * - * \note the content of the matrix is not preserved in any case - * \warning this function may perform memory allocation - * @param rows the new number of rows of this matrix - * @param columns the new number of columns of this matrix - * @param innerIndicesInformation information on the NNZ for each column (row), used to reserve memory in advance. It depends on the storage ordering - */ - void resize(std::size_t rows, std::size_t columns, const iDynTree::VectorDynSize &innerIndicesInformation); - - void reserve(std::size_t nonZeroElements); - - - /** - * Set the sparse matrix to be zero - * - * @note In C++11 it is not guaranteed that this function performs no memory allocation, - * depending on the standard library implementation. - */ - void zero(); - - - /** - * Sets the content of this sparse matrix to the content of triplets - * - * This function does not set the dimensions of the matrix - * which must be set beforehand. If the dimensions are wrong the - * behaviour is undefined. - * - * \note duplicate elements in triplets will be summed up - * \note the content of this matrix will be totally overwritten - * - * \warning this function performs a copy of the triplets parameter so - * that it does not modify the input parameter. - * Use setFromTriplets(iDynTree::Triplets& triplets) if you accept to have - * triplets modified. - * - * @param triplets triplets containing the non zero elements - */ - void setFromConstTriplets(const iDynTree::Triplets& triplets); - - /** - * Sets the content of this sparse matrix to the content of triplets - * - * This function does not set the dimensions of the matrix - * which must be set beforehand. If the dimensions are wrong the - * behaviour is undefined. - * - * \note duplicate elements in triplets will be summed up - * \note the content of this matrix will be totally overwritten - * - * \warning this function modifies the input parameter triplets. - * Use setFromConstTriplets(const iDynTree::Triplets& triplets) if - * you want to preserve the content of triplets. - * - * @param triplets triplets containing the non zero elements - */ - void setFromTriplets(iDynTree::Triplets& triplets); - - static SparseMatrix sparseMatrixFromTriplets(std::size_t rows, - std::size_t cols, - const iDynTree::Triplets& nonZeroElements); - - - /** - * Access operation to the element of the matrix identified by row-col - * - * \note this method is slow as it has to look for the proper index (it performs a linear search) - * @param row row index - * @param col column index - * @return the value at the specified row and column - */ - double operator()(std::size_t row, std::size_t col) const; - - /** - * Access operation to the element of the matrix identified by row-col - * - * \note this method is slow as it has to look for the proper index (it performs a linear search) - * \warning if no elements are found, this method insert a zero value at the specified position - * @param row row index - * @param col column index - * @return reference to the value at the specified row and column - */ - double& operator()(std::size_t row, std::size_t col); - - inline double getValue(std::size_t row, std::size_t col) const - { - return this->operator()(row, col); - } - - inline void setValue(std::size_t row, std::size_t col, double newValue) - { - double &value = this->operator()(row, col); - value = newValue; - } - - - /** - * Returns the number of rows of the matrix - * @return the number of rows - */ - std::size_t rows() const; - - /** - * Returns the number of columns of the matrix - * @return the number of columns - */ - std::size_t columns() const; - - //Raw buffers access - double * valuesBuffer(); - - double const * valuesBuffer() const; - - int * innerIndicesBuffer(); - - int const * innerIndicesBuffer() const; - - int * outerIndicesBuffer(); - - int const * outerIndicesBuffer() const; - - - /** - * Returns a textual description of the matrix. - * - * If true is passed, the whole matrix (with also zero elements) is printed - * Default to false - * @param fullMatrix true to return the full matrix, false for only the non zero elements. Default to false - * @return a textual representation of the matrix - */ - std::string description(bool fullMatrix = false) const; - -#ifndef NDEBUG - std::string internalDescription() const; +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. #endif - class Iterator; - class ConstIterator; - - typedef Iterator iterator; - typedef ConstIterator const_iterator; - - //Why the compiler is not able to choose the const version - //if we have both? @traversaro - iterator begin(); - const_iterator begin() const; - - iterator end(); - const_iterator end() const; - -}; - -// MARK: - Iterator classes - -#ifndef SWIG - -template -class iDynTree::SparseMatrix::Iterator -{ -public: - class TripletRef { - private: - int m_row; - int m_column; - double *m_value; - - TripletRef(std::size_t row, std::size_t column, double *value); - friend class iDynTree::SparseMatrix::Iterator; - - public: - - inline int row() const { return m_row; } - inline int column() const { return m_column; } - inline double& value() { return *m_value; } - inline double value() const { return *m_value; } - - }; - -private: - /** - * Construct a new iterator - * - * @param matrix the sparse matrix to iterate - * @param valid false if the created iterator is invalid. True by default - */ - Iterator(iDynTree::SparseMatrix &matrix, bool valid = true); - friend class iDynTree::SparseMatrix; - - iDynTree::SparseMatrix &m_matrix; - - int m_index; - TripletRef m_currentTriplet; - int m_nonZerosInOuterDirection; - - void updateTriplet(); - -public: - - typedef std::ptrdiff_t difference_type; - typedef typename iDynTree::SparseMatrix::Iterator::TripletRef value_type; - typedef value_type& reference; - typedef value_type* pointer; - typedef std::output_iterator_tag iterator_category; - - // Required by the iterator type - Iterator& operator++(); - Iterator operator++(int); - - // Required by the input iterator - bool operator==(const Iterator&) const; - bool operator==(const ConstIterator&) const; - inline bool operator!=(const Iterator& s) const { return !this->operator==(s); } - inline bool operator!=(const ConstIterator& s) const { return !this->operator==(s); } - - // Required by the input iterator - // Also Output iterator if the reference modifies the container - reference operator*(); - pointer operator->(); - - bool isValid() const; -}; - -template -class iDynTree::SparseMatrix::ConstIterator -{ - -private: - /** - * Construct a new const iterator - * - * @param matrix the sparse matrix to iterate - * @param valid false if the created iterator is invalid. True by default - */ - ConstIterator(const iDynTree::SparseMatrix &matrix, bool valid = true); - friend class iDynTree::SparseMatrix; - - const iDynTree::SparseMatrix &m_matrix; - - int m_index; - iDynTree::Triplet m_currentTriplet; - int m_nonZerosInOuterDirection; - - void updateTriplet(); - -public: - - typedef std::ptrdiff_t difference_type; - typedef iDynTree::Triplet value_type; - typedef const value_type& reference; - typedef const value_type* pointer; - typedef std::output_iterator_tag iterator_category; - - //Copy from non const version - ConstIterator(const Iterator& iterator); - - // Required by the iterator type - ConstIterator& operator++(); - ConstIterator operator++(int); - - // Required by the input iterator - bool operator==(const ConstIterator&) const; - bool operator==(const Iterator&) const; - inline bool operator!=(const ConstIterator& s) const { return !this->operator==(s); } - inline bool operator!=(const Iterator& s) const { return !this->operator==(s); } - - // Required by the input iterator - // Also Output iterator if the reference modifies the container - reference operator*(); - pointer operator->(); - - bool isValid() const; -}; +#include #endif - -#endif /* end of include guard: IDYNTREE_SPARSE_MATRIX_H */ diff --git a/src/core/include/iDynTree/Core/SpatialAcc.h b/src/core/include/iDynTree/Core/SpatialAcc.h index e7ad1792e11..1c6bb73e8ea 100644 --- a/src/core/include/iDynTree/Core/SpatialAcc.h +++ b/src/core/include/iDynTree/Core/SpatialAcc.h @@ -1,42 +1,13 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef IDYNTREE_SPATIAL_ACC_H -#define IDYNTREE_SPATIAL_ACC_H +#ifndef IDYNTREE_CORE_SPATIAL_ACC_H +#define IDYNTREE_CORE_SPATIAL_ACC_H -#include - -namespace iDynTree -{ - /** - * Class representing a spatial acceleration, i.e. the representation - * of the time derivative of the twist. - * - * \note The linear part of this spatial vector **is not** the acceleration - * of a point. - * - * - * \ingroup iDynTreeCore - */ - class SpatialAcc: public SpatialMotionVector - { - public: - /** - * Default constructor. - * The data is not reset to zero for perfomance reason. - * Please initialize the data in the class before any use. - */ - inline SpatialAcc() {} - SpatialAcc(const LinAcceleration & _linearVec3, const AngAcceleration & _angularVec3); - SpatialAcc(const SpatialMotionVector& other); - SpatialAcc(const SpatialAcc& other); - - // overloaded operator - SpatialAcc operator+(const SpatialAcc &other) const; - SpatialAcc operator-(const SpatialAcc &other) const; - SpatialAcc operator-() const; +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif - }; -} +#include #endif diff --git a/src/core/include/iDynTree/Core/SpatialForceVector.h b/src/core/include/iDynTree/Core/SpatialForceVector.h index a981bbe86d0..7e2cac941d3 100644 --- a/src/core/include/iDynTree/Core/SpatialForceVector.h +++ b/src/core/include/iDynTree/Core/SpatialForceVector.h @@ -1,56 +1,13 @@ // 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_CORE_SPATIAL_FORCE_RAW_H +#define IDYNTREE_CORE_SPATIAL_FORCE_RAW_H -#include -#include -#include +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif -namespace iDynTree -{ - /** - * Class providing the raw coordinates for any spatial force vector, - * (i.e. vector form of an element of se*(3)). - * - * \ingroup iDynTreeCore - * - * A force spatial vector can be used to to described spatial momentum, wrench, - * or their derivatives. - * - * 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. - * - * \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. - */ - class SpatialForceVector: public SpatialVector - { - public: - /** - * We use traits here to have the associations SpatialVector <=> Linear/Angular 3D vectors types - * defined in a single place. - */ - typedef SpatialMotionForceVectorT_traits::LinearVector3Type LinearVector3T; - typedef SpatialMotionForceVectorT_traits::AngularVector3Type AngularVector3T; +#include - /** - * Default constructor. - * The data is not reset to zero for perfomance reason. - * Please initialize the data in the class before any use. - */ - inline SpatialForceVector() {} - SpatialForceVector(const LinearVector3T & _linearVec3, const AngularVector3T & _angularVec3); - SpatialForceVector(const SpatialForceVector & other); - SpatialForceVector(const SpatialVector & other); - virtual ~SpatialForceVector(); - - - SpatialForceVector operator*(const double scalar) const; - }; -} - -#endif /* IDYNTREE_SPATIAL_FORCE_RAW_H */ +#endif diff --git a/src/core/include/iDynTree/Core/SpatialInertia.h b/src/core/include/iDynTree/Core/SpatialInertia.h index 92d9b4f52c6..8641bb0688f 100644 --- a/src/core/include/iDynTree/Core/SpatialInertia.h +++ b/src/core/include/iDynTree/Core/SpatialInertia.h @@ -1,238 +1,13 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef IDYNTREE_SPATIAL_INERTIA_H -#define IDYNTREE_SPATIAL_INERTIA_H +#ifndef IDYNTREE_CORE_SPATIAL_INERTIA_H +#define IDYNTREE_CORE_SPATIAL_INERTIA_H -#include -#include -#include -#include -#include -#include - -namespace iDynTree -{ - /** - * @brief Class representing a six dimensional inertia. - * - * - * \ingroup iDynTreeCore - */ - class SpatialInertia: public SpatialInertiaRaw - { - public: - /** - * Default constructor. - * The data is not reset to zero for perfomance reason. - * Please initialize the data in the vector before any use. - */ - inline SpatialInertia() {} - SpatialInertia(const double mass, - const PositionRaw & com, - const RotationalInertiaRaw & rotInertia); - SpatialInertia(const SpatialInertiaRaw& other); - SpatialInertia(const SpatialInertia& other); - - // Operations on SpatialInertia - static SpatialInertia combine(const SpatialInertia & op1, - const SpatialInertia & op2); - - /** - * @brief Get the SpatialInertia as a 6x6 matrix - * - * If \f$ m \in \mathbb{R} \f$ is the mass, - * \f$ c \in \mathbb{R}^3 \f$ is the center of mass, - * \f$ I \in \mathbb{R}^{3 \times 3} \f$ is the 3d inertia, and - * \f$ 1_3 \in \mathbb{R}^{3 \times 3} \f$ is the 3d identity matrix this - * method returns the \f$ \mathbb{M} \in \mathbb{R}^{6 \times 6} \f$ matrix such that: - * \f[ - * \mathbb{M} = - * \begin{bmatrix} - * m 1_3 & -m c \times \\ - * m c \times & I - * \end{bmatrix} - * \f]. - * - * @note As all quantities in iDynTree, this inertia assumes the linear-angular serialization. - */ - Matrix6x6 asMatrix() const; - - Twist applyInverse(const SpatialMomentum& mom) const; - Matrix6x6 getInverse() const; - - // overloaded operators - SpatialInertia operator+(const SpatialInertia& other) const; - SpatialForceVector operator*(const SpatialMotionVector &other) const; - SpatialMomentum operator*(const Twist &other) const; - Wrench operator*(const SpatialAcc &other) const; - - // Efficient operations - - /** - * Return the bias wrench v.cross(M*v). - * - * Defining \f$ \mathbb{M} \f$ as this inertia, return - * the bias wrench v.cross(M*v), defined in math as: - * \f[ - * \mathrm{v} \bar\times^* \mathbb{M} \mathrm{v} = \\ - * \begin{bmatrix} \omega \times & 0 \\ - * v \times & \omega \times - * \end{bmatrix} - * \begin{bmatrix} m 1_3 & -mc\times \\ - * mc\times & I - * \end{bmatrix} - * \begin{bmatrix} v \\ \omega \end{bmatrix} = \\ - * \begin{bmatrix} - * m \omega \times v - \omega \times ( m c \times \omega) \\ - * m c \times ( \omega \times v ) + \omega \times I \omega - * \end{bmatrix} - * \f] - */ - Wrench biasWrench(const Twist & V) const; - - /** - * @brief Return the derivative of the bias wrench with respect to the link 6D velocity. - * - * Defining \f$ \mathbb{M} \in \mathbb{R}^{6 \times 6} \f$ as this inertia, return the derivative - * with respect to \f$ \mathrm{v} = \begin{bmatrix} v \\ \omega \end{bmatrix} \in \mathbb{R}^6 \f$ - * of the bias wrench \f$ \mathrm{v} \bar\times^* \mathbb{M} \mathrm{v} \f$ (i.e. v.cross(M*v)). - * - * The bias wrench is: - * \f[ - * \mathrm{v} \bar\times^* \mathbb{M} \mathrm{v} = \\ - * \begin{bmatrix} - * m \omega \times v - \omega \times ( m c \times \omega) \\ - * m c \times ( \omega \times v ) + \omega \times I \omega - * \end{bmatrix} - * \f] - * - * So the derivative with respect to the twist V is : - * \f[ - * \partial_\mathrm{v} ( \mathrm{v} \bar\times^* \mathbb{M} \mathrm{v} ) = \\ - * \begin{bmatrix} - * m \omega \times & - m v \times + ( m c \times \omega) \times - (\omega \times) (mc \times) \\ - * (m c \times) ( \omega \times) & - (m c \times )(v \times) + \omega \times I - (I \omega) \times - * \end{bmatrix} - * \f] - */ - Matrix6x6 biasWrenchDerivative(const Twist & V) const; - - static SpatialInertia Zero(); - - - /** - * \brief Get the Rigid Body Inertia as a vector of 10 inertial parameters. - * - * Return the rigid body inertia inertial parameters, defined as: - * - * | Elements | Symbol | Description | - * |:--------:|:-------:|:--------:| - * | 0 | \f$ m \f$ | The mass of the rigid body | - * | 1-3 | \f$ m c \f$ | The first moment of mass of the rigid body | - * | 4-9 | \f$ \mathop{vech}(I) \f$ | The 6 independent elements of the 3d inertia matrix, i.e. \f$ \begin{bmatrix} I_{xx} \\ I_{xy} \\ I_{xz} \\ I_{yy} \\ I_{yz} \\ I_{zz} \end{bmatrix} \f$ . | - * - * The first moment of mass is the center of mass (\f$ c \in \mathbb{R}^3 \f$ ) w.r.t. to the frame where this - * rigid body inertia is expressed multiplied by the rigid body mass \f$ m \f$. - * - * The 3d rigid body inertia \f$ I \in \mathbb{R}^{3 \times 3} \f$ is expressed with the orientation of the frame - * in which this rigid body inertia is expressed, and with respect to the frame origin. - * - */ - Vector10 asVector() const; - - /** - * \brief Set the Rigid Body Inertia from the inertial parameters in the vector. - * - * The serialization assumed in the inertialParams is the same used in the asVector method. - */ - void fromVector(const Vector10 & inertialParams); - - /** - * \brief Check if the Rigid Body Inertia is physically consistent. - * - * This method will check: - * * if the mass is positive, - * * if the 3d inertia at the COM is positive semidefinite, - * (semidefinite to cover also the case of the inertia of a point mass), - * * if the moment of inertia along the principal axes at the COM respect the triangle inequality. - * - * It will return true if all this check will pass, or false otherwise. - * - */ - bool isPhysicallyConsistent() const; - - /** - * \brief Get the momentum inertial parameters regressor. - * - * Get the matrix - * \f[ - * Y(\mathrm{v}) \in \mathbb{R}^{6 \times 10} - * \f] - * such that: - * \f[ - * \mathbb{M} \mathrm{v} = Y(\mathrm{v}) \alpha - * \f] - * - * If \f$ \alpha \in \mathbb{R}^{10} \f$ is the inertial parameters representation of \f$ \mathbb{M} \f$, - * as returned by the asVector method. - */ - static Matrix6x10 momentumRegressor(const iDynTree::Twist & v); - - /** - * \brief Get the momentum derivative inertial parameters regressor. - * - * Get the matrix - * \f[ - * Y(\mathrm{v},a) \in \mathbb{R}^{6 \times 10} - * \f] - * such that: - * \f[ - * \mathbb{M} a + \mathrm{v} \overline{\times}^{*} \mathbb{M} \mathrm{v} = Y(\mathrm{v}, a)\alpha - * \f] - * - * If \f$ \alpha \in \mathbb{R}^{10} \f$ is the inertial parameters representation of \f$ \mathbb{M} \f$, - * as returned by the asVector method. - * - * This is also the regressor of the net wrench acting on a rigid body. - * As such, it is the building block of all other algorithms to compute dynamics - * regressors. - */ - static Matrix6x10 momentumDerivativeRegressor(const iDynTree::Twist & v, - const iDynTree::SpatialAcc & a); +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif - /** - * \brief Get the momentum derivative inertial parameters regressor. - * - * Get the matrix - * \f[ - * Y(\mathrm{v},\mathrm{v}_r,a_r) \in \mathbb{R}^{6\times10} - * \f] - * such that: - * \f[ - * \mathbb{M} a_r + (\mathrm{v} \overline{\times}^{*} \mathbb{M} - \mathbb{M} \mathrm{v} \times) \mathrm{v}_r = Y(\mathrm{v},\mathrm{v}_r,a_r)\alpha - * \f] - * - * If \f$ \alpha \in \mathbb{R}^{10} \f$ is the inertial parameters representation of \f$ \mathbb{M} \f$, as returned by the - * asVector method. - * - * Notice that if \f$ \mathrm{v} = \mathrm{v}_r \f$, this regressor reduces to the one computed by momentumDerivativeRegressor. - * The main difference is that (assuming constant \f$ \mathbb{M} \f$) this regressor respect the passivity condition and - * thus is the basic building block for building Slotine Li style regressors. - * - * For more on this, please check: - * - * Garofalo, G.; Ott, C.; Albu-Schaffer, A., - * "On the closed form computation of the dynamic matrices and their differentiations," in - * Intelligent Robots and Systems (IROS), 2013 IEEE/RSJ International Conference on - * doi: 10.1109/IROS.2013.6696688 - * URL: http://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=6696688&isnumber=6696319 - * - */ - static Matrix6x10 momentumDerivativeSlotineLiRegressor(const iDynTree::Twist & v, - const iDynTree::Twist & vRef, - const iDynTree::SpatialAcc & aRef); - }; -} +#include #endif diff --git a/src/core/include/iDynTree/Core/SpatialInertiaRaw.h b/src/core/include/iDynTree/Core/SpatialInertiaRaw.h index 1f70e5d588b..a1a1643edcf 100644 --- a/src/core/include/iDynTree/Core/SpatialInertiaRaw.h +++ b/src/core/include/iDynTree/Core/SpatialInertiaRaw.h @@ -1,99 +1,14 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef IDYNTREE_SPATIAL_INERTIA_RAW_H -#define IDYNTREE_SPATIAL_INERTIA_RAW_H +#ifndef IDYNTREE_CORE_SPATIAL_INERTIA_RAW_H +#define IDYNTREE_CORE_SPATIAL_INERTIA_RAW_H -#include - -namespace iDynTree -{ - class PositionRaw; - class SpatialForceVector; - class SpatialMotionVector; - - /** - * Class providing the raw coordinates for a spatial inertia, i.e. - * a spatial dyadic mapping the motion space to the force space. - * - * \ingroup iDynTreeCore - * - * \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. - */ - class SpatialInertiaRaw - { - protected: - double m_mass; ///< Mass. - double m_mcom[3]; ///< First moment of mass (i.e. mass * center of mass). - RotationalInertiaRaw m_rotInertia; ///< Three dimensional rotational inertia. - - public: - /** - * Default constructor. - * The data is not reset to zero for perfomance reason. - * Please initialize the data in the vector before any use. - */ - inline SpatialInertiaRaw() {} - - /** - * @param mass mass of the rigid body - * @param com center of mass of the rigid body, expressed in the frame - * in which the spatial inertia is expressed - * @param rotInertia rotational inertia expressed with respect to the origin of the frame. - * - * \warning the KDL::RigidBodyInertia class has a similar constructor, but in that one - * the rotational inerta in input is expressed in the center of mass of the body. - */ - SpatialInertiaRaw(const double mass, const PositionRaw & com, const RotationalInertiaRaw & rotInertia); - SpatialInertiaRaw(const SpatialInertiaRaw & other); - - /** - * Helper constructor-like function that takes mass, center of mass - * and the rotational inertia expressed in the center of mass. - * - */ - void fromRotationalInertiaWrtCenterOfMass(const double mass, const PositionRaw & com, const RotationalInertiaRaw & rotInertia); - - - /** multiplication operator - * - * overloading happens on proper classes - * - */ - - - /** - * Getter functions - * - * \note for preserving consistency, no setters are implemented.. - * if you want to modify a spatial inertia create a new one, - * and assign it to the spatial inertia that you want modify. - * Given that no memory allocation happens it should be still - * efficient. - */ - double getMass() const; - PositionRaw getCenterOfMass() const; - const RotationalInertiaRaw& getRotationalInertiaWrtFrameOrigin() const; - RotationalInertiaRaw getRotationalInertiaWrtCenterOfMass() const; - - - /** - * Function to combine the rigid body inertia of two different rigid bodies, - * giving the rigid body inertia of of the rigid body obtanined by welding the two bodies. - */ - static SpatialInertiaRaw combine(const SpatialInertiaRaw& op1, const SpatialInertiaRaw& op2); - - /** - * Multiplication function - * - */ - SpatialForceVector multiply(const SpatialMotionVector & op) const; +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif - /** reset to zero (i.e. the inertia of body with zero mass) the SpatialInertia */ - void zero(); - }; -} +#include #endif diff --git a/src/core/include/iDynTree/Core/SpatialMomentum.h b/src/core/include/iDynTree/Core/SpatialMomentum.h index a6e92a0f4ed..fb510d7dfb5 100644 --- a/src/core/include/iDynTree/Core/SpatialMomentum.h +++ b/src/core/include/iDynTree/Core/SpatialMomentum.h @@ -1,39 +1,13 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef IDYNTREE_SPATIALMOMENTUM_H -#define IDYNTREE_SPATIALMOMENTUM_H +#ifndef IDYNTREE_CORE_SPATIALMOMENTUM_H +#define IDYNTREE_CORE_SPATIALMOMENTUM_H +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif -#include - -namespace iDynTree -{ - /** - * Class representing a spatial momentum, - * i.e. a 6D combination of linear and angular momentum. - * - * \ingroup iDynTreeCore - * - */ - class SpatialMomentum: public SpatialForceVector - { - public: - /** - * Default constructor. - * The data is not reset to the zero for perfomance reason. - * Please initialize the data in the class before any use. - */ - inline SpatialMomentum() {} - SpatialMomentum(const LinMomentum & _linearVec3, const AngMomentum & _angularVec3); - SpatialMomentum(const SpatialForceVector & other); - SpatialMomentum(const SpatialMomentum & other); - - // overloaded operators - SpatialMomentum operator+(const SpatialMomentum &other) const; - SpatialMomentum operator-(const SpatialMomentum &other) const; - SpatialMomentum operator-() const; - }; -} +#include #endif diff --git a/src/core/include/iDynTree/Core/SpatialMotionVector.h b/src/core/include/iDynTree/Core/SpatialMotionVector.h index 91784835f80..eea2bc3af96 100644 --- a/src/core/include/iDynTree/Core/SpatialMotionVector.h +++ b/src/core/include/iDynTree/Core/SpatialMotionVector.h @@ -1,138 +1,13 @@ // 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_CORE_SPATIAL_MOTION_RAW_H +#define IDYNTREE_CORE_SPATIAL_MOTION_RAW_H -#include -#include -#include - -namespace iDynTree -{ - class SpatialForceVector; - class Transform; - class Dummy {}; - - /** - * Class providing the raw coordinates for any motion spatial vector - * (i.e. vector form of an element of se(3)). - * - * \ingroup iDynTreeCore - * - * A motion spatial vector can be used to to describe a twist, twist acceleration - * or their derivatives. - * - * A generic motion spatial vector can also be used to store the logarithm of - * an iDynTree::Transform (i.e. an element of SE(3)). - * - * 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. - * - * \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. - */ - - class SpatialMotionVector: public SpatialVector - { - public: - /** - * constructors - */ - inline SpatialMotionVector() {} - SpatialMotionVector(const LinearMotionVector3 & _linearVec3, const AngularMotionVector3 & _angularVec3); - SpatialMotionVector(const SpatialMotionVector & other); - SpatialMotionVector(const SpatialVector & other); - - /** - * Multiplication for a scalar. - * Mainly used if SpatialMotionVector is used to represent a motion subspace. - */ - SpatialMotionVector operator*(const double scalar) const; - -/** - * Cross product between two 6D motion vectors - * \f$ V_1 = \begin{bmatrix} v_1 \\ \omega_1 \end{bmatrix} \f$ - * and - * \f$ V_2 = \begin{bmatrix} v_2 \\ \omega_2 \end{bmatrix} \f$ - * - * Returns: - * \f[ - * V_1 \times V_2 = - * \begin{bmatrix} - * v_1 \times \omega_2 + \omega_1 \times v_2 \\ - * \omega_1 \times \omega_2 - * \end{bmatrix} - * \f] - */ - SpatialMotionVector cross(const SpatialMotionVector& other) const; - - /** - * Cross product between a 6D motion vector \f$ V = \begin{bmatrix} v \\ \omega \end{bmatrix} \f$ and - * a 6D force vector \f$ F = \begin{bmatrix} f \\ \mu \end{bmatrix} \f$. - * - * Returns: - * \f[ - * V \bar{\times}^* F = - * \begin{bmatrix} - * \omega \times f \\ - * v \times f + \omega \times \mu - * \end{bmatrix} - * \f] - */ - SpatialForceVector cross(const SpatialForceVector& other) const; - - /** - * Cross product matrices - */ - - /** - * If this object is \f$ V = \begin{bmatrix} v \\ \omega \end{bmatrix} \f$, - * return the 6x6 matrix \f$ V\times \f$ - * such that, if U is a SpatialMotionVector : - * \f[ - * (V \times) U = V\texttt{.cross}(U) - * \f] - * - * The returned matrix is then the following one: - * \f[ - * V \times = - * \begin{bmatrix} - * \omega \times & v \times \\ - * 0_{3\times3} & \omega \times - * \end{bmatrix} - * \f] - */ - Matrix6x6 asCrossProductMatrix() const; - - /** - * If this object is \f$ V = \begin{bmatrix} v \\ \omega \end{bmatrix} \f$, return the 6x6 matrix \f$ V\times \f$ - * such that, if F is a SpatialForceVector : - * \f[ - * (V \bar{\times}^*) F = V\texttt{.cross}(F) - * \f] - * - The returned matrix is then the following one: - * \f[ - * V \bar{\times}^* = - * \begin{bmatrix} - * \omega \times & 0_{3\times3} \\ - * v \times & \omega \times - * \end{bmatrix} - * \f] - */ - Matrix6x6 asCrossProductMatrixWrench() const; - - - /** - * Exp mapping between a generic element of se(3) (iDynTree::SpatialMotionVector) - * to the corresponding element of SE(3) (iDynTree::Transform). - */ - Transform exp() const; - }; +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif -} +#include #endif diff --git a/src/core/include/iDynTree/Core/SpatialVector.h b/src/core/include/iDynTree/Core/SpatialVector.h index 34c51bcc931..8455ae0a83b 100644 --- a/src/core/include/iDynTree/Core/SpatialVector.h +++ b/src/core/include/iDynTree/Core/SpatialVector.h @@ -1,401 +1,13 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef IDYNTREE_SPATIAL_VECTOR_H -#define IDYNTREE_SPATIAL_VECTOR_H +#ifndef IDYNTREE_CORE_SPATIAL_VECTOR_H +#define IDYNTREE_CORE_SPATIAL_VECTOR_H -#include "Position.h" -#include "Rotation.h" -#include "Utils.h" -#include +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif +#include -#include -#include - -namespace iDynTree -{ - class SpatialMotionVector; - class SpatialForceVector; - class Position; - class Rotation; - - /** - * Traits class for SpatialMotionVector and SpatialForceVector classes - */ - template - class SpatialMotionForceVectorT_traits {}; - - template <> - class SpatialMotionForceVectorT_traits - { - public: - typedef LinearMotionVector3 LinearVector3Type; - typedef AngularMotionVector3 AngularVector3Type; - }; - - template <> - class SpatialMotionForceVectorT_traits - { - public: - typedef LinearForceVector3 LinearVector3Type; - typedef AngularForceVector3 AngularVector3Type; - }; - - /** - * Helper structure for dual space definition - */ - template struct DualSpace {}; - - template <> struct DualSpace {typedef SpatialForceVector Type;}; - - template <> struct DualSpace {typedef SpatialMotionVector Type;}; - - /** - * Class providing an interface to any spatial motion or force vector, which provides - * raw coordinates. - * - * \ingroup iDynTreeCore - * - * A motion spatial vector can be used to describe twist, spatial acceleration, - * and derivatives. - * - * A force spatial vector can be used to describe spatial momentum, wrench, - * and derivatives. - * - * \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. - */ -#define SPATIALVECTOR_TEMPLATE_HDR \ -template - -#define SPATIALVECTOR_INSTANCE_HDR \ -SpatialVector - - SPATIALVECTOR_TEMPLATE_HDR - class SpatialVector - { - public: - typedef typename SpatialMotionForceVectorT_traits::LinearVector3Type LinearVector3T; - typedef typename SpatialMotionForceVectorT_traits::AngularVector3Type AngularVector3T; - typedef typename DualSpace::Type DualVectorT; - - protected: - LinearVector3T linearVec3; - AngularVector3T angularVec3; - static const unsigned int linearOffset = 0; - static const unsigned int angularOffset = 3; - static const unsigned int totalSize = 6; - - public: - /** - * constructors - */ - SpatialVector(); - SpatialVector(const LinearVector3T & _linearVec3, const AngularVector3T & _angularVec3); - SpatialVector(const SpatialVector & other); - SpatialVector(iDynTree::Span other); - - /** - * Vector accessors, getters, setters - */ - LinearVector3T & getLinearVec3(); - AngularVector3T & getAngularVec3(); - const LinearVector3T & getLinearVec3() const; - const AngularVector3T & getAngularVec3() const; - void setLinearVec3(const LinearVector3T & _linearVec3); - void setAngularVec3(const AngularVector3T & _angularVec3); - - /** - * Vector element accessors, getters, setters - */ - double operator()(const unsigned int index) const; // No input checking. - double& operator()(const unsigned int index); // No input checking. - double getVal(const unsigned int index) const; // Perform boundary checking - bool setVal(const unsigned int index, const double new_el); // Perform boundary checking - unsigned int size() const; - void zero(); - - /** - * Geometric operations - */ - const DerivedSpatialVecT changePoint(const Position & newPoint); - const DerivedSpatialVecT changeCoordFrame(const Rotation & newCoordFrame); - static DerivedSpatialVecT compose(const DerivedSpatialVecT & op1, const DerivedSpatialVecT & op2); - static DerivedSpatialVecT inverse(const DerivedSpatialVecT & op); - - /** - * dot product - */ - double dot(const DualVectorT & other) const; - - /** - * overloaded operators - */ - DerivedSpatialVecT operator+(const DerivedSpatialVecT &other) const; - DerivedSpatialVecT operator-(const DerivedSpatialVecT &other) const; - DerivedSpatialVecT operator-() const; - - /** - * constructor helpers - */ - static DerivedSpatialVecT Zero(); - - /** - * Conversion to basic vector. - */ - Vector6 asVector() const; - - /** @name Output helpers. - * Output helpers. - */ - ///@{ - std::string toString() const; - - std::string reservedToString() const; - ///@} - - - }; - - - - /** - *==================================================================================== - * SpatialVector Method definitions - */ - - // constructors - SPATIALVECTOR_TEMPLATE_HDR - SPATIALVECTOR_INSTANCE_HDR::SpatialVector(): - linearVec3(), - angularVec3() - { - } - - SPATIALVECTOR_TEMPLATE_HDR - SPATIALVECTOR_INSTANCE_HDR::SpatialVector(const LinearVector3T & _linearVec3, - const AngularVector3T & _angularVec3): - linearVec3(_linearVec3), - angularVec3(_angularVec3) - { - } - - SPATIALVECTOR_TEMPLATE_HDR - SPATIALVECTOR_INSTANCE_HDR::SpatialVector(const SpatialVector & other): - linearVec3(other.getLinearVec3()), - angularVec3(other.getAngularVec3()) - { - } - - SPATIALVECTOR_TEMPLATE_HDR - SPATIALVECTOR_INSTANCE_HDR::SpatialVector(Span other) - { - if( other.size() != totalSize) - { - reportError("SpatialVector","constructor","input vector does not have the right size"); - this->zero(); - } - else - { - linearVec3 = LinearVector3T(other.subspan(0, 3)); - angularVec3 = AngularVector3T(other.subspan(3, 3)); - } - } - - // Vector accessors, Getters, setters - SPATIALVECTOR_TEMPLATE_HDR - typename SPATIALVECTOR_INSTANCE_HDR::LinearVector3T & SPATIALVECTOR_INSTANCE_HDR::getLinearVec3() - { - return this->linearVec3; - } - - SPATIALVECTOR_TEMPLATE_HDR - typename SPATIALVECTOR_INSTANCE_HDR::AngularVector3T & SPATIALVECTOR_INSTANCE_HDR::getAngularVec3() - { - return this->angularVec3; - } - - SPATIALVECTOR_TEMPLATE_HDR - const typename SPATIALVECTOR_INSTANCE_HDR::LinearVector3T & SPATIALVECTOR_INSTANCE_HDR::getLinearVec3() const - { - return this->linearVec3; - } - - SPATIALVECTOR_TEMPLATE_HDR - const typename SPATIALVECTOR_INSTANCE_HDR::AngularVector3T & SPATIALVECTOR_INSTANCE_HDR::getAngularVec3() const - { - return this->angularVec3; - } - - SPATIALVECTOR_TEMPLATE_HDR - void SPATIALVECTOR_INSTANCE_HDR::setLinearVec3(const LinearVector3T & _linearVec3) - { - // set linear component - this->linearVec3 = _linearVec3; - } - - SPATIALVECTOR_TEMPLATE_HDR - void SPATIALVECTOR_INSTANCE_HDR::setAngularVec3(const AngularVector3T & _angularVec3) - { - // set angular component - this->angularVec3 = _angularVec3; - } - - // Vector element accessors, getters, setters - SPATIALVECTOR_TEMPLATE_HDR - double SPATIALVECTOR_INSTANCE_HDR::operator()(const unsigned int index) const - { - assert(index < SpatialVector::totalSize); - return (indexlinearVec3(index) : this->angularVec3(index-3)); - } - - SPATIALVECTOR_TEMPLATE_HDR - double& SPATIALVECTOR_INSTANCE_HDR::operator()(const unsigned int index) - { - assert(index < SpatialVector::totalSize); - return (indexlinearVec3(index) : this->angularVec3(index-3)); - } - - SPATIALVECTOR_TEMPLATE_HDR - double SPATIALVECTOR_INSTANCE_HDR::getVal(const unsigned int index) const - { - if( index >= SpatialVector::totalSize ) - { - reportError("VectorFixSize","getVal","index out of bounds"); - return 0.0; - } - return (*this)(index); - } - - SPATIALVECTOR_TEMPLATE_HDR - bool SPATIALVECTOR_INSTANCE_HDR::setVal(const unsigned int index, const double new_el) - { - if( index >= SpatialVector::totalSize ) - { - reportError("VectorFixSize","getVal","index out of bounds"); - return false; - } - (*this)(index) = new_el; - - return true; - } - - SPATIALVECTOR_TEMPLATE_HDR - unsigned int SPATIALVECTOR_INSTANCE_HDR::size() const - { - return SpatialVector::totalSize; - } - - SPATIALVECTOR_TEMPLATE_HDR - void SPATIALVECTOR_INSTANCE_HDR::zero() - { - for(unsigned int i=0; i < SpatialVector::totalSize; i++ ) - { - (*this)(i) = 0.0; - } - } - - - - // Geometric operations - SPATIALVECTOR_TEMPLATE_HDR - const DerivedSpatialVecT SPATIALVECTOR_INSTANCE_HDR::changePoint(const Position & newPoint) - { - return newPoint.changePointOf(*this); - } - - SPATIALVECTOR_TEMPLATE_HDR - const DerivedSpatialVecT SPATIALVECTOR_INSTANCE_HDR::changeCoordFrame(const Rotation & newCoordFrame) - { - return newCoordFrame.changeCoordFrameOf(*this); - } - - SPATIALVECTOR_TEMPLATE_HDR - DerivedSpatialVecT SPATIALVECTOR_INSTANCE_HDR::compose(const DerivedSpatialVecT & op1, const DerivedSpatialVecT & op2) - { - return DerivedSpatialVecT(op1.getLinearVec3()+op2.getLinearVec3(), - op1.getAngularVec3()+op2.getAngularVec3()); - } - - SPATIALVECTOR_TEMPLATE_HDR - DerivedSpatialVecT SPATIALVECTOR_INSTANCE_HDR::inverse(const DerivedSpatialVecT & op) - { - return DerivedSpatialVecT(-op.getLinearVec3(), - -op.getAngularVec3()); - } - - // dot product - SPATIALVECTOR_TEMPLATE_HDR - double SPATIALVECTOR_INSTANCE_HDR::dot(const DualVectorT & other) const - { - return (this->getLinearVec3().dot(other.getLinearVec3()) - + this->getAngularVec3().dot(other.getAngularVec3())); - } - - // overloaded operators - SPATIALVECTOR_TEMPLATE_HDR - DerivedSpatialVecT SPATIALVECTOR_INSTANCE_HDR::operator+(const DerivedSpatialVecT &other) const - { - return compose(*this, other); - } - - SPATIALVECTOR_TEMPLATE_HDR - DerivedSpatialVecT SPATIALVECTOR_INSTANCE_HDR::operator-(const DerivedSpatialVecT &other) const - { - return compose(*this, inverse(other)); - } - - SPATIALVECTOR_TEMPLATE_HDR - DerivedSpatialVecT SPATIALVECTOR_INSTANCE_HDR::operator-() const - { - return inverse(*this); - } - - // constructor helpers - SPATIALVECTOR_TEMPLATE_HDR - DerivedSpatialVecT SPATIALVECTOR_INSTANCE_HDR::Zero() - { - DerivedSpatialVecT ret; - ret.linearVec3.zero(); - ret.angularVec3.zero(); - return ret; - } - - SPATIALVECTOR_TEMPLATE_HDR - Vector6 SPATIALVECTOR_INSTANCE_HDR::asVector() const - { - Vector6 ret; - ret.data()[0] = linearVec3.data()[0]; - ret.data()[1] = linearVec3.data()[1]; - ret.data()[2] = linearVec3.data()[2]; - ret.data()[3] = angularVec3.data()[0]; - ret.data()[4] = angularVec3.data()[1]; - ret.data()[5] = angularVec3.data()[2]; - return ret; - } - - - SPATIALVECTOR_TEMPLATE_HDR - std::string SPATIALVECTOR_INSTANCE_HDR::toString() const - { - std::stringstream ss; - - ss << linearVec3.toString() << " " - << angularVec3.toString(); - ss << std::endl; - - return ss.str(); - } - - SPATIALVECTOR_TEMPLATE_HDR - std::string SPATIALVECTOR_INSTANCE_HDR::reservedToString() const - { - return this->toString(); - } - -#undef SPATIALVECTOR_TEMPLATE_HDR -#undef SPATIALVECTOR_INSTANCE_HDR -} - -#endif /* IDYNTREE_SPATIAL_VECTOR_H */ +#endif diff --git a/src/core/include/iDynTree/Core/TestUtils.h b/src/core/include/iDynTree/Core/TestUtils.h index 09d26848008..965b5256990 100644 --- a/src/core/include/iDynTree/Core/TestUtils.h +++ b/src/core/include/iDynTree/Core/TestUtils.h @@ -1,403 +1,13 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef IDYNTREE_TEST_UTILS_H -#define IDYNTREE_TEST_UTILS_H +#ifndef IDYNTREE_CORE_TEST_UTILS_H +#define IDYNTREE_CORE_TEST_UTILS_H -#include - -#include - -#include -#include -#include -#include -#include - -#include -#include - - -namespace iDynTree -{ - class Transform; - class SpatialMotionVector; - class SpatialForceVector; - class Axis; - class SpatialForceVector; - class SpatialMotionVector; - class SpatialInertia; - class Position; - class Rotation; - -#define ASSERT_IS_TRUE(prop) iDynTree::assertTrue(prop,__FILE__,__LINE__) -#define ASSERT_IS_FALSE(prop) iDynTree::assertTrue(!(prop),__FILE__,__LINE__) -#define ASSERT_EQUAL_STRING(val1,val2) iDynTree::assertStringAreEqual(val1,val2,iDynTree::DEFAULT_TOL,__FILE__,__LINE__) -#define ASSERT_EQUAL_DOUBLE(val1,val2) iDynTree::assertDoubleAreEqual(val1,val2,iDynTree::DEFAULT_TOL,__FILE__,__LINE__) -#define ASSERT_EQUAL_DOUBLE_TOL(val1,val2,tol) iDynTree::assertDoubleAreEqual(val1,val2,tol,__FILE__,__LINE__) -#define ASSERT_EQUAL_VECTOR(val1,val2) assertVectorAreEqual(val1,val2,iDynTree::DEFAULT_TOL,__FILE__,__LINE__) -#define ASSERT_EQUAL_VECTOR_TOL(val1,val2,tol) assertVectorAreEqual(val1,val2,tol,__FILE__,__LINE__) -#define ASSERT_EQUAL_VECTOR_REL_TOL(val1,val2,relTol,minAbsTol) assertVectorAreEqualWithRelativeTol(val1,val2,relTol,minAbsTol,__FILE__,__LINE__) -#define ASSERT_EQUAL_SPATIAL_MOTION(val1,val2) assertSpatialMotionAreEqual(val1,val2,iDynTree::DEFAULT_TOL,__FILE__,__LINE__) -#define ASSERT_EQUAL_SPATIAL_FORCE(val1,val2) assertSpatialForceAreEqual(val1,val2,iDynTree::DEFAULT_TOL,__FILE__,__LINE__) -#define ASSERT_EQUAL_SPATIAL_FORCE_TOL(val1,val2,tol) assertSpatialForceAreEqual(val1,val2,tol,__FILE__,__LINE__) -#define ASSERT_EQUAL_MATRIX(val1,val2) assertMatrixAreEqual(val1,val2,iDynTree::DEFAULT_TOL,__FILE__,__LINE__) -#define ASSERT_EQUAL_MATRIX_TOL(val1,val2,tol) assertMatrixAreEqual(val1,val2,tol,__FILE__,__LINE__) -#define ASSERT_EQUAL_TRANSFORM(val1,val2) assertTransformsAreEqual(val1,val2,iDynTree::DEFAULT_TOL,__FILE__,__LINE__) -#define ASSERT_EQUAL_TRANSFORM_TOL(val1,val2,tol) assertTransformsAreEqual(val1,val2,tol,__FILE__,__LINE__) - - - struct TestMatrixMismatch { - bool match; - double expected; - double realElement; - - TestMatrixMismatch(bool _match, double _expected, double _realElement) - : match(_match), expected(_expected), realElement(_realElement) {} - }; - - void assertStringAreEqual(const std::string & val1, const std::string & val2, double tol = DEFAULT_TOL, std::string file="", int line=-1); - - - void assertDoubleAreEqual(const double & val1, const double & val2, double tol = DEFAULT_TOL, std::string file="", int line=-1); - - /** - * Assert that two transforms are equal, and - * exit with EXIT_FAILURE if they are not. - * - */ - void assertTransformsAreEqual(const Transform & trans1, const Transform & trans2, double tol = DEFAULT_TOL, std::string file="", int line=-1); - - /** - * Assert that two spatial motion vectors are equal, - * and exit with EXIT_FAILURE if they are not. - * - */ - void assertSpatialMotionAreEqual(const SpatialMotionVector & t1, const SpatialMotionVector & t2, double tol = DEFAULT_TOL, std::string file="", int line=-1); - - /** - * Assert that two spatial force vectors are equal, - * and exit with EXIT_FAILURE if they are not. - * - */ - void assertSpatialForceAreEqual(const SpatialForceVector & f1, const SpatialForceVector & f2, double tol = DEFAULT_TOL, std::string file="", int line=-1); - - void assertTrue(bool prop,std::string file="", int line=-1); - - /** - * Get random bool - */ - bool getRandomBool(); - - /** - * Get a random double between min and max . - */ - double getRandomDouble(double min=0.0, double max=1.0); - - /** - * Get a random integer between min and max (included). - * For example a dice could be simulated with getRandomInteger(1,6); - */ - int getRandomInteger(int min, int max); - - /** - * Fill a vector with random double. - */ - template - void getRandomVector(VectorType & vec, double min=0.0, double max=1.0) - { - for(unsigned int i=0; i - void getRandomMatrix(MatrixType & mat) - { - for(unsigned int i=0; i - void printVector(std::string /*name*/, const VectorType& vec) - { - for(unsigned int i=0; i < vec.size(); i++ ) - { - std::cerr << vec(i) << "\n"; - } - std::cerr << "\n"; - } - - /** - * Helper for printing difference of two vectors - */ - template - void printVectorDifference(std::string name, const VectorType1& vec1, const VectorType2& vec2) - { - std::cerr << name << " : \n"; - size_t minSize = vec1.size(); - - if( vec2.size() < minSize ) - { - minSize = vec2.size(); - } - - for(unsigned int i=0; i < minSize; i++ ) - { - std::cerr << vec1(i) - vec2(i) << " ( " << (vec1(i) - vec2(i))/std::max(vec1(i),vec2(i)) << " ) " << "\n"; - } - } - - /** - * Helper for printing the patter of wrong elements - * in between two vectors - */ - inline void printVectorWrongElements(std::string name, std::vector & correctElems) - { - std::cerr << name << " ( . match, X mismatch): \n"; - - for(unsigned int i=0; i < correctElems.size(); i++ ) - { - if( correctElems[i] ) - { - std::cerr << "." << "\n"; - } - else - { - std::cerr << "X" << "\n"; - } - } - } - - /** - * Helper for printing the patter of wrong elements - * in between two matrix - */ - inline void printMatrixWrongElements(std::string name, std::vector< std::vector > & correctElems) - { -#ifndef _WIN32 -#define IDYNTREE_MATCH_CHARACTER "\u2714" -#else -#define IDYNTREE_MATCH_CHARACTER "o" +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. #endif - std::cerr << name << "( " IDYNTREE_MATCH_CHARACTER " match, (expected,got:error) mismatch): \n"; - - size_t rows = correctElems.size(); - size_t cols = correctElems[0].size(); - for(unsigned int row=0; row < rows; row++ ) - { - for( unsigned int col = 0; col < cols; col++ ) - { - if( correctElems[row][col].match ) - { - std::cerr << IDYNTREE_MATCH_CHARACTER; - } - else - { - std::cerr << "(" << correctElems[row][col].expected << "," << correctElems[row][col].realElement - << ":" << correctElems[row][col].realElement - correctElems[row][col].expected << ")"; - } - - std::cerr << " "; - } - - std::cerr << "\n"; - } - } - - /** - * Helper for printing the patter of wrong elements - * in between two matrix - */ - template - void printMatrixPercentageError(const MatrixType1& mat1, const MatrixType2& mat2) - { - size_t rows = mat1.rows(); - size_t cols = mat2.cols(); - for(unsigned int row=0; row < rows; row++ ) - { - for( unsigned int col = 0; col < cols; col++ ) - { - double mat1el = mat1(row,col); - double mat2el = mat2(row,col); - double percentageError = std::abs(mat1el-mat2el)/std::max(mat1el,mat2el); - std::cerr << std::fixed << std::setprecision(3) << percentageError << " "; - } - - std::cerr << "\n"; - } - } - - - /** - * Assert that two vectors are equal, and - * exit with EXIT_FAILURE if they are not. - */ - template - void assertVectorAreEqual(const VectorType1& vec1, const VectorType2& vec2, double tol, std::string file, int line) - { - if( vec1.size() != vec2.size() ) - { - std::cerr << file << ":" << line << " : assertVectorAreEqual failure: vec1 has size " << vec1.size() - << " while vec2 has size " << vec2.size() << std::endl; - exit(EXIT_FAILURE); - } - - std::vector correctElements(vec1.size(),true); - bool checkCorrect = true; - - for( unsigned int i = 0; i < vec1.size(); i++ ) - { - if( !(fabs(vec1(i)-vec2(i)) < tol) ) - { - checkCorrect = false; - correctElements[i] = false; - } - } - - if( !checkCorrect ) - { - std::cerr << file << ":" << line << " : assertVectorAreEqual failure: " << std::endl; - printVector("vec1",vec1); - printVector("vec2",vec2); - printVectorDifference("vec1-vec2",vec1,vec2); - printVectorWrongElements("wrong el:",correctElements); - exit(EXIT_FAILURE); - } - } - - /** - * Assert that two vectors are equal, and exit with EXIT_FAILURE if they are not. - * - * The tolerance passed in this function is a relative tolerance on the max element of the comparison, i.e. - * absoluteTol = max(relativeTol*max(val1,val2), minAbsoluteTol) - */ - template - void assertVectorAreEqualWithRelativeTol(const VectorType1& vec1, const VectorType2& vec2, double relativeTol, double minAbsoluteTol, std::string file, int line) - { - if( vec1.size() != vec2.size() ) - { - std::cerr << file << ":" << line << " : assertVectorAreEqualWithRelativeTol failure: vec1 has size " << vec1.size() - << " while vec2 has size " << vec2.size() << std::endl; - exit(EXIT_FAILURE); - } - - std::vector correctElements(vec1.size(),true); - bool checkCorrect = true; - - for( unsigned int i = 0; i < vec1.size(); i++ ) - { - double absoluteTol = std::max(relativeTol*std::max(std::abs(vec1(i)), std::abs(vec2(i))), minAbsoluteTol); - if( fabs(vec1(i)-vec2(i)) >= absoluteTol ) - { - checkCorrect = false; - correctElements[i] = false; - } - } - - if( !checkCorrect ) - { - std::cerr << file << ":" << line << " : assertVectorAreEqualWithRelativeTol failure: " << std::endl; - printVector("vec1",vec1); - printVector("vec2",vec2); - printVectorDifference("vec1-vec2",vec1,vec2); - printVectorWrongElements("wrong el:",correctElements); - exit(EXIT_FAILURE); - } - } - - /** - * Assert that two matrices are equal, and - * exit with EXIT_FAILURE if they are not. - * - */ - template - void assertMatrixAreEqual(const MatrixType1& mat1, const MatrixType2& mat2, double tol, std::string file, int line) - { - if( mat1.rows() != mat2.rows() || - mat2.cols() != mat1.cols() ) - { - std::cerr << file << ":" << line << " : assertMatrixAreEqual failure: mat1 has size " << mat1.rows() << " " << mat1.cols() - << " while mat2 has size " << mat2.rows() << " " << mat2.cols() << std::endl; - exit(EXIT_FAILURE); - } - - std::vector< std::vector > correctElements(mat2.rows(), std::vector(mat1.cols(), TestMatrixMismatch(true, 0, 0)) ); - bool checkCorrect = true; - - for( unsigned int row = 0; row < mat2.rows(); row++ ) - { - for( unsigned int col = 0; col < mat2.cols(); col++ ) - { - if( fabs(mat1(row,col)-mat2(row,col)) >= tol ) - { - checkCorrect = false; - correctElements[row][col].match = false; - correctElements[row][col].expected = mat1(row,col); - correctElements[row][col].realElement = mat2(row,col); - } - } - } - - if( !checkCorrect ) - { - std::cerr << file << ":" << line << " : assertMatrixAreEqual failure with tol " << tol << " : " << std::endl; - printMatrixWrongElements("wrong el:",correctElements); - //std::cerr << "percentage error : " << std::endl; - //printMatrixPercentageError(mat1,mat2); - exit(EXIT_FAILURE); - } - - } - -} +#include #endif diff --git a/src/core/include/iDynTree/Core/Transform.h b/src/core/include/iDynTree/Core/Transform.h index 91da2a6124d..1a1600401fc 100644 --- a/src/core/include/iDynTree/Core/Transform.h +++ b/src/core/include/iDynTree/Core/Transform.h @@ -1,538 +1,13 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef IDYNTREE_TRANSFORM_H -#define IDYNTREE_TRANSFORM_H +#ifndef IDYNTREE_CORE_TRANSFORM_H +#define IDYNTREE_CORE_TRANSFORM_H -#include -#include -#include - -#include - -namespace iDynTree -{ - class Position; - class Rotation; - class Wrench; - class Twist; - class SpatialMomentum; - class SpatialAcc; - class SpatialMotionVector; - class SpatialForceVector; - class SpatialInertia; - class ArticulatedBodyInertia; - - /** - * Class representation the relative displacement between two different frames. - * - * \ingroup iDynTreeCore - * - * \image html transform.svg - * - * - * This class is designed to be an easy to use proxy to perform change of frame of - * expression for iDynTree::Position, iDynTree::Twist, iDynTree::Wrench and other data - * structure in \ref iDynTreeCore. For this reason the class is called "Transform", because it will be mainly - * used to transform quantities between frames. - * - * Given that this class it may used to represent homogeneous transform matrix as well as adjoint - * matrix, no raw access to the underline storage ( data() method ) is provided, because it does not - * have a canonical representation. Instead, access is given to the two elements of a transform: - * The position vector \f${}^{\texttt{refOrient}} p_{\texttt{refPoint},\texttt{point}}\f$ and - * the rotation matrix \f${}^{\texttt{refOrient}} R_{\texttt{orient}}\f$. - * - * We will indicate this tranform as \f$( {}^{\texttt{refOrient}} p_{\texttt{refPoint},\texttt{point}} , {}^{\texttt{refOrient}} R_{\texttt{orient}} )\f$ - * - * The frame whose origin is the reference point and whose orientation the reference orientation is often - * indicated as \f$ \texttt{refFrame} = (\texttt{refPoint},\texttt{refOrient}) \f$. - * - * Similarly the frame whose origin is the point and whose orientation is the orientation is indicated - * as \f$ \texttt{frame} = (\texttt{point},\texttt{orient}) \f$. - * - * This transform object can be obtained as the \f${}^{\texttt{refFrame}} H_{\texttt{frame}}\f$ 4x4 homogeneous matrix - * using the asHomogeneousTransform method, or as the \f${}^{\texttt{refFrame}} X_{\texttt{frame}}\f$ 6x6 adjoint matrix using the - * asAdjointTransform . - * - * - * - */ - class Transform - { - protected: - /** - * \brief The position part of the transform. - * - * The 3d vector \f${}^{\texttt{refOrient}} p_{\texttt{refPoint},\texttt{point}}\f$, - * that is the vector between the point and the - * reference point, pointing toward the point and expressed - * in the reference orientation frame. - */ - Position pos; - - /** - * \brief The rotation part of the transform - * - * Set the rotation matrix \f${}^{\texttt{refOrient}} R_{\texttt{orient}} \in \mathbb{R}^{3 \times 3}\f$, - * that is the rotation matrix that takes - * a 3d vector expressed in the orientationFrame and transform it - * in a 3d vector expressed in the reference orientation frame. - */ - Rotation rot; - - public: - /** - * Default constructor. - * The data is not reset to the default for perfomance reason. - * Please initialize the data in the class before any use. - */ - Transform(); - - /** - * Constructor from a rotation and a point - */ - Transform(const Rotation & _rot, const Position & origin); - - /** - * Constructor from a Matrix4x4 object. It is equivalent of calling fromHomogeneousTransform() - * @param transform The input homogeneous matrix - */ - Transform(const Matrix4x4 & transform); - - /** - * Copy constructor: create a Transform from another Transform. - */ - Transform(const Transform & other); - - /** - * Set rotation and translation from a iDynTree::Matrix4x4 object - * @param transform The input homogeneous matrix - */ - void fromHomogeneousTransform(const Matrix4x4& transform); - - /** - * Assigment operator - * - * @param other the rhs - * - * @return *this equal to the other object - */ - Transform& operator=(const Transform& other); - - /** - * Get the rotation part of the transform - * - * Get the rotation matrix \f${}^{\texttt{refOrient}} R_{\texttt{orient}} \in \mathbb{R}^{3 \times 3}\f$, - * that is the rotation matrix that takes - * a 3d vector expressed in the orientationFrame and transform it - * in a 3d vector expressed in the reference orientation frame. - */ - const Rotation & getRotation() const; - - /** - * \brief Get the translation part of the transform - * - * Get 3d vector \f${}^{\texttt{refOrient}} p_{\texttt{refPoint},\texttt{point}}\f$, - * that is the vector between the point and the - * reference point, pointing toward the point and expressed - * in the reference orientation frame. - */ - const Position & getPosition() const; - - /** - * Set the rotation part of the transform - * - * Set the rotation matrix \f${}^{\texttt{refOrient}} R_{\texttt{orient}} \in \mathbb{R}^{3 \times 3}\f$, - * that is the rotation matrix that takes - * a 3d vector expressed in the orientationFrame and transform it - * in a 3d vector expressed in the reference orientation frame. - */ - void setRotation(const Rotation & rotation); - - /** - * \brief Set the translation part of the transform - * - * Set 3d vector \f${}^{\texttt{refOrient}} p_{\texttt{refPoint},\texttt{point}}\f$, - * that is the vector between the point and the - * reference point, pointing toward the point and expressed - * in the reference orientation frame. - */ - void setPosition(const Position & position); - - // geometric operations on 3x1 vectors (positions and rotations and homogemeous tranform) - static Transform compose(const Transform & op1, const Transform & op2); - static Transform inverse2(const Transform & trans); - - /** - * \name Overloaded operators. - * - * This operators are used to change the frame in which a quantity is - * expressed from the \f$\texttt{frame}\f$ to the \f$\texttt{refFrame}\f$ of - * the transform. - */ - ///@{ - - /** - * \brief Combine two transforms. - * - * If this object is - * \f[ - * (p,R) = ( {}^{\texttt{refOrient}} p_{\texttt{refPoint},\texttt{point}} , {}^{\texttt{refOrient}} R_{\texttt{orient}} ) - * \f] - * and the argument is - * \f[ - * (p',R') = ( {}^{\texttt{orient}} p_{\texttt{point},\texttt{newPoint}} , {}^{\texttt{orient}} R_{\texttt{newOrient}} ) - * \f]. - * - * The resulting transform will be: - * \f[ - * (p+Rp', RR') = ( {}^{\texttt{refOrient}} p_{\texttt{refPoint},\texttt{newPoint}} , {}^{\texttt{refOrient}} R_{\texttt{newOrient}} ) - * \f]. - * - * Notice that this is equivalent to multiply the associated homogemeous matrices: - * \f[ - * {}^{\texttt{refFrame}} H_{\texttt{newFrame}} = {}^{\texttt{refFrame}} H_{\texttt{frame}} {}^{\texttt{frame}} H_{\texttt{newFrame}} - * \f] - * or the associated adjoint matrices : - * \f[ - * {}^{\texttt{refFrame}} X_{\texttt{newFrame}} = {}^{\texttt{refFrame}} X_{\texttt{frame}} {}^{\texttt{frame}} X_{\texttt{newFrame}} - * \f] - */ - Transform operator*(const Transform & other) const; - - /** - * Return the inverse of this Transform. - * - * If this object is - * \f[ - * (p,R) = ( {}^{\texttt{refOrient}} p_{\texttt{refPoint},\texttt{point}} , {}^{\texttt{refOrient}} R_{\texttt{orient}} ) - * \f] - * this function will return: - * \f[ - * (-R^\top p , R^\top) = ( {}^{\texttt{orient}} p_{\texttt{point},\texttt{refPoint}} , {}^{\texttt{orient}} R_{\texttt{refOrient}} ) - * \f] - * - */ - Transform inverse() const; - - /** - * Change reference frame of a point. - * - * If this object is - * \f[ - * (p,R) = ( {}^{\texttt{refOrient}} p_{\texttt{refPoint},\texttt{point}} , {}^{\texttt{refOrient}} R_{\texttt{orient}} ) - * \f] - * - * And the Position argument represent a point: - * \f[ - * p' = {}^{\texttt{orient}} p_{\texttt{point},\texttt{newPoint}} - * \f] - * - * The result of the operation is: - * \f[ - * Rp' + p = {}^{\texttt{refOrient}} p_{\texttt{refPoint},\texttt{newPoint}} - * \f] - * - */ - Position operator*(const Position & other) const; - - /** - * Change frame in which a Wrench is expressed. - * - * If this object is - * \f[ - * (p,R) = ( {}^{\texttt{refOrient}} p_{\texttt{refPoint},\texttt{point}} , {}^{\texttt{refOrient}} R_{\texttt{orient}} ) - * \f] - * - * And the argument is a wrench: - * \f[ - * {}_{\texttt{frame}} F = - * \begin{bmatrix} - * f \\ \tau - * \end{bmatrix} - * \f] - * - * The result of this operation is : - * \f[ - * {}_{\texttt{refFrame}} F - * = - * {}_{\texttt{refFrame}}X^{\texttt{frame}} - * {}_{\texttt{frame}} F - * = - * \begin{bmatrix} - * R & - * 0_{3\times3} \\ - * p \times R & - * R - * \end{bmatrix} - * \begin{bmatrix} - * f \\ \tau - * \end{bmatrix} - * = - \begin{bmatrix} - * Rf \\ p \times R f + R\tau - * \end{bmatrix} - * \f] - */ - Wrench operator*(const Wrench & other) const; - - /** - * Change the frame in which a Twist is expressed. - * - * If this object is - * \f[ - * (p,R) = ( {}^{\texttt{refOrient}} p_{\texttt{refPoint},\texttt{point}} , {}^{\texttt{refOrient}} R_{\texttt{orient}} ) - * \f] - * - * And the argument is a twist: - * \f[ - * {}^{\texttt{frame}} V = - * \begin{bmatrix} - * v \\ \omega - * \end{bmatrix} - * \f] - * - * The result of this operation is : - * \f[ - * {}^{\texttt{refFrame}} V - * = - * {}^{\texttt{refFrame}}X_{\texttt{frame}} - * {}^{\texttt{frame}} V - * = - * \begin{bmatrix} - * R & - * p \times R\\ - * 0_{3\times3} & - * R - * \end{bmatrix} - * \begin{bmatrix} - * v \\ \omega - * \end{bmatrix} - * = - \begin{bmatrix} - * R v + p \times R \omega \\ R\omega - * \end{bmatrix} - * \f] - */ - Twist operator*(const Twist & other) const; - - /** - * Change the frame in which a SpatialMomentum is expressed. - * - * Check the operator*(const Wrench & other) documentation - * for the mathematical details. - */ - SpatialMomentum operator*(const SpatialMomentum & other) const; - - /** - * Change the frame in which a SpatialAcc is expressed. - * - * Check the operator*(const Twist & other) documentation - * for the mathematical details. - */ - SpatialAcc operator*(const SpatialAcc & other) const; - - /** - * Change the frame in which a SpatialMotionVector is expressed. - * - * Check the operator*(const Twist & other) documentation - * for the mathematical details. - */ - SpatialMotionVector operator*(const SpatialMotionVector & other) const; - - /** - * Change the frame in which a SpatialForceVector is expressed. - * - * Check the operator*(const Wrench & other) documentation - * for the mathematical details. - */ - SpatialForceVector operator*(const SpatialForceVector & other) const; - - /** - * Change the frame in which a SpatialInertia is expressed. - * - */ - SpatialInertia operator*(const SpatialInertia & other) const; - - /** - * Change the frame in which a ArticulatedBodyInertia is expressed. - * - */ - ArticulatedBodyInertia operator*(const ArticulatedBodyInertia & other) const; - - - /** - * Change the frame in which a Direction is expressed. - * - * If this object is - * \f[ - * (p,R) = ( {}^{\texttt{refOrient}} p_{\texttt{refPoint},\texttt{point}} , {}^{\texttt{refOrient}} R_{\texttt{orient}} ) - * \f] - * - * And the argument is a direction represented by a unit norm 3d vector : - * \f[ - * {}^{\texttt{orient}} d - * \f] - * - * The result of this operation is: - * \f[ - * {}^{\texttt{refOrient}} d = R {}^{\texttt{orient}} d - * \f] - * - */ - Direction operator*(const Direction & other) const; - - /** - * Change the frame in which a Axis is expressed. - * - * If this object is - * \f[ - * (p,R) = ( {}^{\texttt{refOrient}} p_{\texttt{refPoint},\texttt{point}} , {}^{\texttt{refOrient}} R_{\texttt{orient}} ) - * \f] - * - * And the argument is an axis, specified by the axis origin and the axis direction: - * \f[ - * {}^{\texttt{frame}} A = ({}^{\texttt{orient}} p_{\texttt{point},\texttt{axisOrigin}} , {}^{\texttt{orient}} d) = (p',d) - * \f] - * - * The result of this operation is: - * \f[ - * {}^{\texttt{refFrame}} A = ({}^{\texttt{refOrient}} p_{\texttt{refPoint},\texttt{axisOrigin}} , {}^{\texttt{refOrient}} d) = ( Rp' + p , Rd) - * \f] - * - */ - Axis operator*(const Axis & other) const; - ///@} - - /** - * Return an identity transfom - * - * Set the rotation to the identity and the translation to 0 : - * \f[ - * (0_{3 \times 1}, I_{3 \times 3}) - * \f] - * - * @return an identity transform. - */ - static Transform Identity(); - - /** - * @name Raw data accessors. - * - * For several applications it may be useful to access the fransform - * contents in the raw form of homogeneous matrix or an adjoint matrix. - */ - ///@{ - - /** - * Return the 4x4 homogeneous matrix representing the transform. - * - * The returned matrix is the one with this structure: - * - * \f[ - * {}^{\texttt{refFrame}} H_{\texttt{frame}} = - * \begin{bmatrix} - * {}^{\texttt{refOrient}} R_{\texttt{orient}} & {}^{\texttt{refOrient}} p_{\texttt{refPoint},\texttt{point}} \\ - * 0_{1\times3} & 1 - * \end{bmatrix} - * \f] - * Where \f${}^{\texttt{refOrient}} R_{\texttt{orient}} \in \mathbb{R}^{3 \times 3}\f$ is the rotation matrix that takes - * a 3d vector expressed in the orientationFrame and transform it - * in a 3d vector expressed in the reference orientation frame. - * - * \f${}^{\texttt{refOrient}} p_{\texttt{refPoint},\texttt{point}} \in \mathbb{R}^3\f$ - * is the vector between the point and the - * reference point, pointing toward the point and expressed - * in the reference orientation frame. - * - */ - Matrix4x4 asHomogeneousTransform() const; - - /** - * Return the 6x6 adjoint matrix representing this transform. - * - * The returned matrix is the one with this structure: - * - * \f[ - * {}^{\texttt{refFrame}} X_{\texttt{frame}} = - * \begin{bmatrix} - * R & - * p \times R \\ - * 0_{3\times3} & - * R - * \end{bmatrix} - * \f] - * - * Where \f$R = {}^{\texttt{refOrient}} R_{\texttt{orient}} \in \mathbb{R}^{3\times3}\f$ - * is the rotation matrix that takes - * a 3d vector expressed in the orientationFrame and transform it - * in a 3d vector expressed in the reference orientation frame. - * - * \f$p = p_{\texttt{refPoint},\texttt{point}} \in \mathbb{R}^3\f$ - * is the vector between the point and the - * reference point, pointing toward the point and expressed - * in the reference orientation frame \f$p \times\f$ is the skew simmetric - * matrix such that \f$(p \times) v = p \times v\f$ . - * - * \warning Note that in iDynTree the matrix are stored - * in row major order, and the 6d quantites are - * serialized in linear/angular order. - * - */ - Matrix6x6 asAdjointTransform() const; - - /** - * Return the 6x6 adjoint matrix (for wrench) representing this transform. - * - * The returned matrix is the one with this structure: - * - * \f[ - * {}_{\texttt{refFrame}} X^{\texttt{frame}} = - * \begin{bmatrix} - * R & - * 0_{3\times3} \\ - * p \times R & - * R - * \end{bmatrix} - * \f] - * - * Where \f$R = {}^{\texttt{refOrient}} R_{\texttt{orient}} \in \mathbb{R}^{3\times3}\f$ - * is the rotation matrix that takes - * a 3d vector expressed in the orientationFrame and transform it - * in a 3d vector expressed in the reference orientation frame. - * - * \f$p = p_{\texttt{refPoint},\texttt{point}} \in \mathbb{R}^3\f$ - * is the vector between the point and the - * reference point, pointing toward the point and expressed - * in the reference orientation frame \f$p \times\f$ is the skew simmetric - * matrix such that \f$(p \times) v = p \times v\f$ . - * - * \warning Note that in iDynTree the matrix are stored - * in row major order, and the 6d quantites are - * serialized in linear/angular order. - * - */ - Matrix6x6 asAdjointTransformWrench() const; - - /* - * Exp mapping between a generic element of se(3) (iDynTree::SpatialMotionVector) - * to the corresponding element of SE(3) (iDynTree::Transform). - */ - SpatialMotionVector log() const; - - ///@} - - /** - * @name Output helpers. - * Function to print out the Transform. - */ - ///@{ - std::string toString() const; - - std::string reservedToString() const; - ///@} +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif - }; -} +#include #endif diff --git a/src/core/include/iDynTree/Core/TransformDerivative.h b/src/core/include/iDynTree/Core/TransformDerivative.h index 5ba4f0a1014..523a2bbae60 100644 --- a/src/core/include/iDynTree/Core/TransformDerivative.h +++ b/src/core/include/iDynTree/Core/TransformDerivative.h @@ -1,258 +1,13 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef IDYNTREE_TRANSFORM_DERIVATIVE_H -#define IDYNTREE_TRANSFORM_DERIVATIVE_H +#ifndef IDYNTREE_CORE_TRANSFORM_DERIVATIVE_H +#define IDYNTREE_CORE_TRANSFORM_DERIVATIVE_H -#include -#include - -namespace iDynTree -{ - class Transform; - class ArticulatedBodyInertia; - class SpatialForceVector; - - /** - * Class representing the derivative of Transform object - * - * \ingroup iDynTreeCore - * - * \note Even if most methods documentation are written - * assuming that this class represents the derivative - * of a Transform with respect to time, it can be used - * to represent a derivative with respect to an arbitrary - * variable. - * - */ - class TransformDerivative - { - protected: - /** - * \brief The derivative of the translation part of a Transform - */ - Vector3 posDerivative; - - /** - * \brief The derivative of the rotation part of a Transform - */ - Matrix3x3 rotDerivative; - - public: - /** - * Default constructor. - * The data is not reset to the default for perfomance reasons. - * Please initialize the data in the class before any use. - */ - TransformDerivative() {} - - /** - * Constructor from a Matrix3x3 and a Vector3 . - * The matrix represent the derivative of a rotation matrix, - * while the vector the derivative of the position part of the tranform. - */ - TransformDerivative(const Matrix3x3 & _rotDeriv, const Vector3 & posDeriv); - - /** - * Copy constructor: create a TransformDerivative from another TransformDerivative. - */ - TransformDerivative(const TransformDerivative & other); - - /** - * Default destructor. - */ - ~TransformDerivative() {} - - /** - * Get the derivative of the rotation part of the transform - * - */ - const Matrix3x3 & getRotationDerivative() const; - - /** - * Get the derivative of the translation part of the transform - * - */ - const Vector3 & getPositionDerivative() const; - - /** - * Set the derivative of the rotation part of the transform - * - */ - void setRotationDerivative(const Matrix3x3 & rotationDerivative); - - /** - * \brief Set the derivative of translation part of the transform - * - */ - void setPositionDerivative(const Vector3 & positionDerivative); - - /** - * Return a zero transfom derivative - * - * @return an zero transform derivative. - */ - static TransformDerivative Zero(); - - /** - * @name Raw data accessors. - * - * For several applications it may be useful to access the fransform - * contents in the raw form of homogeneous matrix or an adjoint matrix. - */ - ///@{ - - /** - * Return the derivative of the 4x4 homogeneous matrix representing the transform. - * - * The returned matrix is the one with this structure: - * - * \f[ - * {}^{\texttt{refFrame}} \dot{H}_{\texttt{frame}} = - * \begin{bmatrix} - * {}^{\texttt{refOrient}} \dot{R}_{\texttt{orient}} & {}^{\texttt{refOrient}} \dot{p}_{\texttt{refPoint},\texttt{point}} \\ - * 0_{1\times3} & 1 - * \end{bmatrix} - * \f] - * - * For details on this notation, check the Transform::asHomogeneousTransform() method . - * - */ - Matrix4x4 asHomogeneousTransformDerivative() const; - - /** - * Return the derivative of the 6x6 adjoint matrix representing this transform. - * - * The returned matrix is the one with this structure: - * - * \f[ - * {}^{\texttt{refFrame}} X_{\texttt{frame}} = - * \begin{bmatrix} - * \dot{R} & - * \dot{p} \times R + p \times \dot{R} \\ - * 0_{3\times3} & - * \dot{R} - * \end{bmatrix} - * \f] - * - * Where \f$R = {}^{\texttt{refOrient}} R_{\texttt{orient}} \in \mathbb{R}^{3\times3}\f$ - * is the rotation matrix that takes - * a 3d vector expressed in the orientationFrame and transform it - * in a 3d vector expressed in the reference orientation frame. - * - * \f$p = p_{\texttt{refPoint},\texttt{point}} \in \mathbb{R}^3\f$ - * is the vector between the point and the - * reference point, pointing toward the point and expressed - * in the reference orientation frame \f$p \times\f$ is the skew simmetric - * matrix such that \f$(p \times) v = p \times v\f$ . - * - * Given that the TransformDerivative object contains only \f$ \dot{R} \f$ and - * \f$ \dot{p} \f$, you need to provide \f$ R \f$ and \f$ p \f$ to this method - * by passing the relative Transform object. - * - * \warning Note that in iDynTree the matrix are stored - * in row major order, and the 6d quantites are - * serialized in linear/angular order. - * - */ - Matrix6x6 asAdjointTransformDerivative(const Transform & transform) const; - - /** - * Return the 6x6 adjoint matrix (for wrench) representing this transform. - * - * The returned matrix is the one with this structure: - * - * \f[ - * {}_{\texttt{refFrame}} X^{\texttt{frame}} = - * \begin{bmatrix} - * \dot{R} & - * 0_{3\times3} \\ - * \dot{p} \times R + p \times \dot{R} & - * \dot{R} - * \end{bmatrix} - * \f] - * - * Where \f$R = {}^{\texttt{refOrient}} R_{\texttt{orient}} \in \mathbb{R}^{3\times3}\f$ - * is the rotation matrix that takes - * a 3d vector expressed in the orientationFrame and transform it - * in a 3d vector expressed in the reference orientation frame. - * - * \f$p = p_{\texttt{refPoint},\texttt{point}} \in \mathbb{R}^3\f$ - * is the vector between the point and the - * reference point, pointing toward the point and expressed - * in the reference orientation frame \f$p \times\f$ is the skew simmetric - * matrix such that \f$(p \times) v = p \times v\f$ . - * - * Given that the TransformDerivative object contains only \f$ \dot{R} \f$ and - * \f$ \dot{p} \f$, you need to provide \f$ R \f$ and \f$ p \f$ to this method - * by passing the relative Transform object. - * - * \warning Note that in iDynTree the matrix are stored - * in row major order, and the 6d quantites are - * serialized in linear/angular order. - * - */ - Matrix6x6 asAdjointTransformWrenchDerivative(const Transform & transform) const; - - /** - * Multiply a transform derivative for a transform. - * - * This operation is useful to compose the derivative of a transform - * for a constant transform. - * - * If this transform derivative represent the derivative of a transform - * with respect to time \f$ ~^a \dot{H}_b \f$, and we have a constant - * transform \f$ ~^b H_c \f$, then this operation returns the derivative - * of the transform \f$ ~^a H_c \f$ : - * \f[ - * ~^a \dot{H}_c = ~^a \dot{H}_b ~^b H_c - * \f] - * - * \warning the otherTransform must be a quantity that does not depend - * on the derivation variable. - */ - TransformDerivative operator*(const Transform & otherTransform) const; - - /** - * Get the derivative of the inverse of the transform. - * - * If this TransformDerivative is \f$ ~^a \dot{H}_b \f$ and the - * transform argument is \f$ ~^a H_b \f$, returns the derivative of - * the inverse transform \f$ ~^b {H}_a \f$ computed as: - * \f[ - * ~^b \dot{H}_a = - ~^b {H}_a ~^a \dot{H}_b ~^b H_a - * \f] - * - */ - TransformDerivative derivativeOfInverse(const Transform & transform) const; - - /** - * Given a articulated inertia \f$I\f$ (other argument), this TransformDerivative \f$ ~^a\dot{X}_b \f$ - * and a transform \f$ ~^a{X}_b \f$ return the articulated body inertia: - * \f[ - * ~_a \dot{X}^b Ia ~^a X_b + ~_a {X}^b Ia ~^a \dot{X}_b - * \f] - */ - ArticulatedBodyInertia transform(const Transform & transform, ArticulatedBodyInertia& other); - - /** - * Given a SpatialForceVector \f$F\f$ (other argument), this TransformDerivative \f$ ~^a\dot{X}_b \f$ - * and a transform \f$ ~^a{X}_b \f$ return the spatial force vector: - * \f[ - * ~_a \dot{X}^b F - * \f] - */ - SpatialForceVector transform(const Transform & transform, SpatialForceVector& other); +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif - /** - * Given a SpatialMotionVector \f$V\f$ (other argument), this TransformDerivative \f$ ~^a\dot{X}_b \f$ - * and a transform \f$ ~^a{X}_b \f$ return the spatial force vector: - * \f[ - * ~^a \dot{X}_b F - * \f] - */ - SpatialMotionVector transform(const Transform & transform, SpatialMotionVector& other); - }; -} +#include #endif diff --git a/src/core/include/iDynTree/Core/Triplets.h b/src/core/include/iDynTree/Core/Triplets.h index bad32ea23b6..d6fc46e6c7b 100644 --- a/src/core/include/iDynTree/Core/Triplets.h +++ b/src/core/include/iDynTree/Core/Triplets.h @@ -1,174 +1,13 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef IDYNTREE_TRIPLETS_H -#define IDYNTREE_TRIPLETS_H +#ifndef IDYNTREE_CORE_TRIPLETS_H +#define IDYNTREE_CORE_TRIPLETS_H -#include -#include +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif -#include -#include -#include +#include - -namespace iDynTree { - class Triplet; - class Triplets; - - class MatrixDynSize; - - template - class SparseMatrix; - - struct IndexRange; -} - -class iDynTree::Triplet -{ -public: - - static bool rowMajorCompare(const iDynTree::Triplet& a, const iDynTree::Triplet &b); - static bool columnMajorCompare(const iDynTree::Triplet& a, const iDynTree::Triplet &b); - - Triplet(std::size_t row, std::size_t column, double value); - - bool operator<(const iDynTree::Triplet&) const; - bool operator==(const iDynTree::Triplet&) const; - - std::size_t row; - std::size_t column; - double value; -}; - -class iDynTree::Triplets -{ - //Understand if vector is the best solution - std::vector m_triplets; - -public: - - void reserve(std::size_t size); - void clear(); - - template - inline void addSubMatrix(std::size_t startingRow, - std::size_t startingColumn, - const MatrixFixSize &matrix) - { - //if enough memory has been reserved this should be a noop - m_triplets.reserve(m_triplets.size() + (rows * cols)); - for (std::size_t row = 0; row < rows; ++row) { - for (std::size_t col = 0; col < cols; ++col) { - m_triplets.push_back(Triplet(startingRow + row, startingColumn + col, matrix(row, col))); - } - } - } - - void addSubMatrix(std::size_t startingRow, - std::size_t startingColumn, - const MatrixDynSize&); - - template - void addSubMatrix(std::size_t startingRow, - std::size_t startingColumn, - const SparseMatrix& matrix) - { - //if enough memory has been reserved this should be a noop - m_triplets.reserve(m_triplets.size() + matrix.numberOfNonZeros()); - - for (typename SparseMatrix::const_iterator it(matrix.begin()); - it != matrix.end(); ++it) { - m_triplets.push_back(Triplet(startingRow + it->row, - startingColumn + it->column, - it->value)); - } - } - - inline void addDiagonalMatrix(IndexRange startingRow, - IndexRange startingColumn, - double value) - { - addDiagonalMatrix(startingRow.offset, - startingColumn.offset, - value, - startingRow.size); - } - - void addDiagonalMatrix(std::size_t startingRow, - std::size_t startingColumn, - double value, - std::size_t diagonalMatrixSize); - - void pushTriplet(const Triplet& triplet); - - template - inline void setSubMatrix(std::size_t startingRow, - std::size_t startingColumn, - const MatrixFixSize &matrix) - { - //if enough memory has been reserved this should be a noop - m_triplets.reserve(m_triplets.size() + (rows * cols)); - for (std::size_t row = 0; row < rows; ++row) { - for (std::size_t col = 0; col < cols; ++col) { - setTriplet(Triplet(startingRow + row, startingColumn + col, matrix(row, col))); - } - } - } - - void setSubMatrix(std::size_t startingRow, - std::size_t startingColumn, - const MatrixDynSize&); - - template - void setSubMatrix(std::size_t startingRow, - std::size_t startingColumn, - const SparseMatrix& matrix) - { - //if enough memory has been reserved this should be a noop - m_triplets.reserve(m_triplets.size() + matrix.numberOfNonZeros()); - - for (typename SparseMatrix::const_iterator it(matrix.begin()); - it != matrix.end(); ++it) { - setTriplet(Triplet(startingRow + it->row, - startingColumn + it->column, - it->value)); - } - } - - inline void setDiagonalMatrix(IndexRange startingRow, - IndexRange startingColumn, - double value) - { - setDiagonalMatrix(startingRow.offset, - startingColumn.offset, - value, - startingRow.size); - } - - void setDiagonalMatrix(std::size_t startingRow, - std::size_t startingColumn, - double value, - std::size_t diagonalMatrixSize); - - - void setTriplet(const Triplet& triplet); - - bool isEmpty() const; - std::size_t size() const; - - std::string description() const; - - typedef std::vector::iterator iterator; - typedef std::vector::const_iterator const_iterator; - - //for now simply return the vector iterator and do not implement our own iterator - const_iterator begin() const; - iterator begin(); - const_iterator end() const; - iterator end(); - -}; - - -#endif /* end of include guard: IDYNTREE_TRIPLETS_H */ +#endif diff --git a/src/core/include/iDynTree/Core/Twist.h b/src/core/include/iDynTree/Core/Twist.h index d4c8b4a457a..eb67b80ac8b 100644 --- a/src/core/include/iDynTree/Core/Twist.h +++ b/src/core/include/iDynTree/Core/Twist.h @@ -1,42 +1,13 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef IDYNTREE_TWIST_H -#define IDYNTREE_TWIST_H +#ifndef IDYNTREE_CORE_TWIST_H +#define IDYNTREE_CORE_TWIST_H +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif -#include - -namespace iDynTree -{ - class SpatialAcc; - class SpatialMomentum; - class Wrench; - - /** - * Class representing a twist, i.e. a 6D combination of linear an angular velocity. - * - * - * \ingroup iDynTreeCore - */ - class Twist: public SpatialMotionVector - { - public: - Twist(); - Twist(const LinVelocity & _linearVec3, const AngVelocity & _angularVec3); - Twist(const SpatialMotionVector& other); - Twist(const Twist& other); - - /** overloaded operators **/ - Twist operator+(const Twist &other) const; - Twist operator-(const Twist &other) const; - Twist operator-() const; - - /** overloaded cross products */ - SpatialAcc operator*(const Twist &other) const; - Wrench operator*(const SpatialMomentum &other) const; - - }; -} +#include #endif diff --git a/src/core/include/iDynTree/Core/Utils.h b/src/core/include/iDynTree/Core/Utils.h index 4aa7923a680..4c311c7a1e8 100644 --- a/src/core/include/iDynTree/Core/Utils.h +++ b/src/core/include/iDynTree/Core/Utils.h @@ -1,129 +1,13 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef IDYNTREE_UTILS_H -#define IDYNTREE_UTILS_H +#ifndef IDYNTREE_CORE_UTILS_H +#define IDYNTREE_CORE_UTILS_H -#include - -/** - * \brief Macro to suppress unused variable warnings - * - * see http://stackoverflow.com/a/4851173 - */ -#define IDYNTREE_UNUSED(var) ((void)var) - - -// Note: we set IDYNTREE_DEPRECATED to nothing when compiling with SWIG has some -// SWIG has some problems with this attributes, so until we use a recent SWIG version -// we disabled them for SWIG -/** - * \brief Macro to deprecate functions and methods - * - * see https://blog.samat.io/2017/02/27/Deprecating-functions-and-methods-in-Cplusplus/ - */ -#if defined(SWIG) -#define IDYNTREE_DEPRECATED -#define IDYNTREE_DEPRECATED_WITH_MSG(msg) -// Workaround for SWIG problems with GenerateExportHeader-generated code -#define IDYNTREE_CORE_EXPORT -#else -#define IDYNTREE_DEPRECATED [[deprecated]] -#define IDYNTREE_DEPRECATED_WITH_MSG(msg) [[deprecated(msg)]] -#include "CoreExport.h" +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. #endif - -namespace iDynTree -{ - IDYNTREE_CORE_EXPORT extern int UNKNOWN; - - /// Default tolerance for methods with a tolerance, setted to 1e-10 - IDYNTREE_CORE_EXPORT extern double DEFAULT_TOL; - - /** - * Function embedding the semantic checks - * - * This function can throw an exception if the semantic check detects an error (returns False). - */ - void assertWoAbort(const char * semCheck, const char * file, const char* func, int line); - - /** - * Helper class for semantic checking. - * - * Return true if two values are equal, or if one of the two is unknown - * All negative values are used for represent an unknown value. - */ - bool checkEqualOrUnknown(const int op1, const int op2); - - /** - * Helper function for reporting error if the semantic check fails. - * - */ - void reportError(const char * className, const char* methodName, const char * errorMessage); - - /** - * Call report error if condition is true - */ - bool reportErrorIf(bool condition, const char * className_methodName, const char * errorMessage); - - /** - * Helper function for reporting warnings in iDynTree - * - */ - void reportWarning(const char * className, const char* methodName, const char * errorMessage); - - /** - * Helper function for reporting information messages in iDynTree - * - */ - void reportInfo(const char* className, const char* methodName, const char* message); - - /** - * Helper function for reporting debug messages in iDynTree - * - */ - void reportDebug(const char* className, const char* methodName, const char* message); - - /** - * Convert a double from degrees to radians. - */ - double deg2rad(const double valueInDeg); - - /** - * Convert a double from radians to degree. - */ - double rad2deg(const double valueInRad); - - /** - * Simple structure describing a range of rows or columns in a vector or a matrix. - * The offset attributes indicates the index of the first element of the range, while - * the size indicates the size of the range. - * - * Example: offset = 2, size = 3 measn the elements 2 3 4 . - */ - struct IndexRange - { - std::ptrdiff_t offset; - std::ptrdiff_t size; - - bool isValid() const; - static IndexRange InvalidRange(); - }; - - /** - * Enum describing the possible matrix storage ordering - */ - enum MatrixStorageOrdering { - RowMajor, /*!< Row Major ordering, i.e. matrix is serialized row by row */ - ColumnMajor /*!< Column Major ordering, i.e. matrix is serialized row by column */ - }; - - /** - * Check whether two doubles are equal given a tolerance tol. - */ - bool checkDoublesAreEqual(const double & val1, const double & val2, double tol = DEFAULT_TOL); - -} +#include #endif diff --git a/src/core/include/iDynTree/Core/VectorDynSize.h b/src/core/include/iDynTree/Core/VectorDynSize.h index c4ec1e36a22..9004604dfa7 100644 --- a/src/core/include/iDynTree/Core/VectorDynSize.h +++ b/src/core/include/iDynTree/Core/VectorDynSize.h @@ -1,269 +1,13 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef IDYNTREE_DYNAMIC_SIZE_VECTOR_H -#define IDYNTREE_DYNAMIC_SIZE_VECTOR_H +#ifndef IDYNTREE_CORE_DYNAMIC_SIZE_VECTOR_H +#define IDYNTREE_CORE_DYNAMIC_SIZE_VECTOR_H -#if !defined(SWIG_VERSION) || SWIG_VERSION >= 0x030000 -#include +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. #endif -#include +#include -namespace iDynTree -{ - /** - * Class providing a simple form of vector with dynamic size. - * It is designed to provide seamless integration with SWIG. - * - * \ingroup iDynTreeCore - */ - class VectorDynSize - { - protected: - /** - * Storage for the VectorDynSize - * - * Pointer to an area of capacity() doubles, managed by this class. - */ - double * m_data; - - /** - * Size of the vector. - */ - std::size_t m_size; - - /** - * The buffer to which m_data is pointing is m_capacity*sizeof(double). - */ - std::size_t m_capacity; - - /** - * Set the capacity of the vector, resizing the buffer pointed by m_data. - */ - void changeCapacityAndCopyData(const std::size_t _newCapacity); - - public: - /** - * Default constructor: initialize the size of the array to zero. - */ - VectorDynSize(); - - /** - * Constructor from the size, all the element assigned to 0 - * - * @param _size the desired size of the array. - * - * \warning performs dynamic memory allocation operations - */ - VectorDynSize(std::size_t _size); - - /** - * Constructor from a C-style array. - * - * Build - * - * \warning performs dynamic memory allocation operations - */ - VectorDynSize(const double * in_data, const std::size_t in_size); - - /** - * Copy constructor - * \warning performs dynamic memory allocation operations - */ - VectorDynSize(const VectorDynSize& vec); - -#if !defined(SWIG_VERSION) || SWIG_VERSION >= 0x030000 - - /** - * Constructor from an iDynTree::Span - * - * @param vec span representing a vector - * - * \warning performs dynamic memory allocation operations - */ - VectorDynSize(iDynTree::Span vec); -#endif - - /** - * Denstructor - * - * \warning performs dynamic memory allocation operations - */ - virtual ~VectorDynSize(); - - /** - * Copy assignment operator. - * - * The vector will be resize to match the - * size of the argument, and the data will - * then be copied. - * - * \warning performs dynamic memory allocation operations - */ - VectorDynSize & operator=(const VectorDynSize& vec); - -#if !defined(SWIG_VERSION) || SWIG_VERSION >= 0x030000 - /** - * Copy assignment operator. - * - * The vector will be resize to match the - * size of the argument, and the data will - * then be copied. - * - * \warning performs dynamic memory allocation operations - */ - VectorDynSize & operator=(iDynTree::Span vec); #endif - /** - * @name Vector interface methods. - * Methods exposing a vector-like interface to PositionRaw. - */ - ///@{ - double operator()(const std::size_t index) const; - - double& operator()(const std::size_t index); - - double operator[](const std::size_t index) const; - - double& operator[](const std::size_t index); - - double getVal(const std::size_t index) const; - - bool setVal(const std::size_t index, const double new_el); - - /** - * Returns a const iterator to the beginning of the vector - * @note At the moment iterator is implemented as a pointer, it may change in the future. - * For this reason it should not be used as a pointer to the data, use data() instead. - */ - const double* begin() const noexcept; - - /** - * Returns a const iterator to the beginning of the vector - * @note At the moment iterator is implemented as a pointer, it may change in the future. - * For this reason it should not be used as a pointer to the data, use data() instead. - */ - const double* end() const noexcept; - - /** - * Returns a const iterator to the beginning of the vector - * @note At the moment iterator is implemented as a pointer, it may change in the future. - * For this reason it should not be used as a pointer to the data, use data() instead. - */ - const double* cbegin() const noexcept; - - /** - * Returns a const iterator to the beginning of the vector - * @note At the moment iterator is implemented as a pointer, it may change in the future. - * For this reason it should not be used as a pointer to the data, use data() instead. - */ - const double* cend() const noexcept; - - /** - * Returns a iterator to the beginning of the vector - * @note At the moment iterator is implemented as a pointer, it may change in the future. - * For this reason it should not be used as a pointer to the data, use data() instead. - */ - double* begin() noexcept; - - /** - * Returns a iterator to the beginning of the vector - * @note At the moment iterator is implemented as a pointer, it may change in the future. - * For this reason it should not be used as a pointer to the data, use data() instead. - */ - double* end() noexcept; - - std::size_t size() const; - - ///@} - - /** - * Raw data accessor - * - * @return a const pointer to a vector of size() doubles - */ - const double * data() const; - - /** - * Raw data accessor - * - * @return a pointer to a vector of size() doubles - */ - double * data(); - - /** - * Assign all element of the vector to 0. - */ - void zero(); - - /** - * Increase the capacity of the vector preserving content. - * - * @param newCapacity the new capacity of the vector - * \warning performs dynamic memory allocation operations if newCapacity > capacity() - * \note if newCapacity <= capacity(), this method does nothing and the capacity will remain unchanged. - */ - void reserve(const std::size_t newCapacity); - - /** - * Change the size of the vector preserving old content. - * - * @param newSize the new size of the vector - * \warning performs dynamic memory allocation operations if newSize > capacity() - */ - void resize(const std::size_t newSize); - - /** - * Change the capacity of the vector to match the size. - * - * \warning performs dynamic memory allocation operations if size() != capacity() - */ - void shrink_to_fit(); - - /** - * The buffer pointed by data() has size capacity()*sizeof(double) - */ - size_t capacity() const; - - /** - * Assume that buf is pointing to - * a buffer of size() doubles, and fill - * it with the content of this vector. - * - * @param buf pointer to the buffer to fill - * - * @todo provide this for all matrix types - * - * \warning use this function only if you are - * an expert C user - */ - void fillBuffer(double * buf) const; - -#if !defined(SWIG_VERSION) || SWIG_VERSION >= 0x030000 - /** Typedefs to enable make_span. - */ - ///@{ - typedef double value_type; - - typedef std::allocator allocator_type; - - typedef typename std::allocator_traits>::pointer pointer; - - typedef typename std::allocator_traits>::const_pointer const_pointer; - ///@} -#endif - - /** @name Output helpers. - * Output helpers. - */ - ///@{ - std::string toString() const; - - std::string reservedToString() const; - ///@} - - }; -} - -#endif /* IDYNTREE_VECTOR_DYN_SIZE_H */ diff --git a/src/core/include/iDynTree/Core/VectorFixSize.h b/src/core/include/iDynTree/Core/VectorFixSize.h index ec12f225885..5115a92d492 100644 --- a/src/core/include/iDynTree/Core/VectorFixSize.h +++ b/src/core/include/iDynTree/Core/VectorFixSize.h @@ -1,400 +1,13 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef IDYNTREE_VECTOR_FIX_SIZE_H -#define IDYNTREE_VECTOR_FIX_SIZE_H +#ifndef IDYNTREE_CORE_VECTOR_FIX_SIZE_H +#define IDYNTREE_CORE_VECTOR_FIX_SIZE_H -#include -#if !defined(SWIG_VERSION) || SWIG_VERSION >= 0x030000 -#include +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. #endif -#include -#include -#include -#include -#include -namespace iDynTree -{ - /** - * Class providing a simple vector of N elements. - * The size of the vector is known at compile time, - * and it enables to avoid using dynamic memory allocation. - * - * \ingroup iDynTreeCore - */ - template class VectorFixSize - { - protected: - /** - * Storage for the VectorDynSize - * - * Array of VecSize doubles. - */ - double m_data[VecSize]; +#include - public: - /** - * Default constructor. - * The data is not reset to 0 for perfomance reason. - * Please initialize the data in the vector before any use. - */ - VectorFixSize(); - - /** - * Constructor from a C-style array. - * - * Print an error an build a vector full of zeros if in_size is not size(). - */ - VectorFixSize(const double * in_data, const std::size_t in_size); - -#if !defined(SWIG_VERSION) || SWIG_VERSION >= 0x030000 - /** - * Constructor from an iDynTree::Span - * - * Print an error an build a vector full of zeros if in_size is not size(). - */ - VectorFixSize(iDynTree::Span vec); -#endif - - /** - * @name Vector interface methods. - * Methods exposing a vector-like interface to VectorFixSize. - */ - ///@{ - double operator()(const std::size_t index) const; - - double& operator()(const std::size_t index); - - double operator[](const std::size_t index) const; - - double& operator[](const std::size_t index); - - double getVal(const std::size_t index) const; - - bool setVal(const std::size_t index, const double new_el); - - /** - * Returns a const iterator to the beginning of the vector - * @note At the moment iterator is implemented as a pointer, it may change in the future. - * For this reason it should not be used as a pointer to the data, use data() instead. - */ - const double* begin() const noexcept; - - /** - * Returns a const iterator to the beginning of the vector - * @note At the moment iterator is implemented as a pointer, it may change in the future. - * For this reason it should not be used as a pointer to the data, use data() instead. - */ - const double* end() const noexcept; - - /** - * Returns a const iterator to the beginning of the vector - * @note At the moment iterator is implemented as a pointer, it may change in the future. - * For this reason it should not be used as a pointer to the data, use data() instead. - */ - const double* cbegin() const noexcept; - - /** - * Returns a const iterator to the beginning of the vector - * @note At the moment iterator is implemented as a pointer, it may change in the future. - * For this reason it should not be used as a pointer to the data, use data() instead. - */ - const double* cend() const noexcept; - - /** - * Returns a iterator to the beginning of the vector - * @note At the moment iterator is implemented as a pointer, it may change in the future. - * For this reason it should not be used as a pointer to the data, use data() instead. - */ - double* begin() noexcept; - - /** - * Returns a iterator to the beginning of the vector - * @note At the moment iterator is implemented as a pointer, it may change in the future. - * For this reason it should not be used as a pointer to the data, use data() instead. - */ - double* end() noexcept; - - std::size_t size() const; - - ///@} - -#if !defined(SWIG_VERSION) || SWIG_VERSION >= 0x030000 - /** - * Copy assignment operator for spans. - * - * Checks that dimensions are matching through an assert. - * - */ - VectorFixSize & operator=(iDynTree::Span vec); -#endif - - /** - * Raw data accessor - * - * @return a const pointer to a vector of size() doubles - */ - const double * data() const; - - /** - * Raw data accessor - * - * @return a pointer to a vector of size() doubles - */ - double * data(); - - /** - * Assign all element of the vector to 0. - */ - void zero(); - - /** - * Assume that buf is pointing to - * a buffer of size() doubles, and fill - * it with the content of this vector. - * - * @param buf pointer to the buffer to fill - * - * @todo provide this for all matrix types - * - * \warning use this function only if you are - * an expert C user - */ - void fillBuffer(double * buf) const; - - /** - * @name Output helpers. - * Output helpers. - */ - ///@{ - std::string toString() const; - - std::string reservedToString() const; - ///@} - -#if !defined(SWIG_VERSION) || SWIG_VERSION >= 0x030000 - /** Typedefs to enable make_span. - */ - ///@{ - typedef double value_type; - - typedef std::allocator allocator_type; - - typedef typename std::allocator_traits>::pointer pointer; - - typedef typename std::allocator_traits>::const_pointer const_pointer; - ///@} -#endif - - }; - - //Implementation - template - VectorFixSize::VectorFixSize() - { - - } - - - template - VectorFixSize::VectorFixSize(const double* in_data, - const std::size_t in_size) - { - if( in_size != VecSize ) - { - reportError("VectorFixSize","constructor","input vector does not have the right number of elements"); - this->zero(); - } - else - { - memcpy(this->m_data,in_data,sizeof(double)*VecSize); - } - } - -#if !defined(SWIG_VERSION) || SWIG_VERSION >= 0x030000 - - template - VectorFixSize::VectorFixSize(iDynTree::Span vec) - : VectorFixSize::VectorFixSize(vec.data(), vec.size()) - {} - -#endif - - template - void VectorFixSize::zero() - { - for(std::size_t i=0; i < VecSize; i++ ) - { - this->m_data[i] = 0.0; - } - } - - - template - double* VectorFixSize::data() - { - return this->m_data; - } - - template - const double* VectorFixSize::data() const - { - return this->m_data; - } - - template - const double* VectorFixSize::begin() const noexcept - { - return this->m_data; - } - - template - const double* VectorFixSize::end() const noexcept - { - return this->m_data + VecSize; - } - - template - const double* VectorFixSize::cbegin() const noexcept - { - return this->m_data; - } - - template - const double* VectorFixSize::cend() const noexcept - { - return this->m_data + VecSize; - } - - template - double *VectorFixSize::begin() noexcept - { - return this->m_data; - } - - template - double* VectorFixSize::end() noexcept - { - return this->m_data + VecSize; - } - - template - std::size_t VectorFixSize::size() const - { - return VecSize; - } - -#if !defined(SWIG_VERSION) || SWIG_VERSION >= 0x030000 - template - VectorFixSize & VectorFixSize::operator=(iDynTree::Span vec) { - assert(VecSize == vec.size()); - std::memcpy(this->m_data, vec.data(), VecSize*sizeof(double)); - return *this; - } #endif - - template - double VectorFixSize::operator()(const std::size_t index) const - { - assert(index < VecSize); - return this->m_data[index]; - } - - template - double & VectorFixSize::operator()(const std::size_t index) - { - assert(index < VecSize); - return this->m_data[index]; - } - - template - double VectorFixSize::operator[](const std::size_t index) const - { - assert(index < VecSize); - return this->m_data[index]; - } - - template - double & VectorFixSize::operator[](const std::size_t index) - { - assert(index < VecSize); - return this->m_data[index]; - } - - template - double VectorFixSize::getVal(const std::size_t index) const - { - if( index >= this->size() ) - { - reportError("VectorFixSize","getVal","index out of bounds"); - return 0.0; - } - - return this->m_data[index]; - } - - template - bool VectorFixSize::setVal(const std::size_t index, const double new_el) - { - if( index >= this->size() ) - { - reportError("VectorFixSize","getVal","index out of bounds"); - return false; - } - - this->m_data[index] = new_el; - - return true; - } - - template - void VectorFixSize::fillBuffer(double* buf) const - { - for(std::size_t i=0; i < this->size(); i++ ) - { - buf[i] = this->m_data[i]; - } - } - - template - std::string VectorFixSize::toString() const - { - std::stringstream ss; - - for(std::size_t i=0; i < this->size(); i++ ) - { - ss << this->m_data[i] << " "; - } - - return ss.str(); - } - - template - std::string VectorFixSize::reservedToString() const - { - std::stringstream ss; - - for(std::size_t i=0; i < this->size(); i++ ) - { - ss << this->m_data[i] << " "; - } - - return ss.str(); - } - - // Explicit instantiations - // The explicit instantiations are the only ones that can be used in the API - // and the only ones that users are supposed to manipulate manipulate - // Add all the explicit instantiation that can be useful, but remember to add - // them also in the iDynTree.i SWIG file - typedef VectorFixSize<2> Vector2; - typedef VectorFixSize<3> Vector3; - typedef VectorFixSize<4> Vector4; - typedef VectorFixSize<6> Vector6; - typedef VectorFixSize<10> Vector10; - typedef VectorFixSize<16> Vector16; - -} - -#endif /* IDYNTREE_VECTOR_FIX_SIZE_H */ diff --git a/src/core/include/iDynTree/Core/Wrench.h b/src/core/include/iDynTree/Core/Wrench.h index b6f5e50352c..6f7b5208415 100644 --- a/src/core/include/iDynTree/Core/Wrench.h +++ b/src/core/include/iDynTree/Core/Wrench.h @@ -1,32 +1,13 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef IDYNTREE_WRENCH_H -#define IDYNTREE_WRENCH_H +#ifndef IDYNTREE_CORE_WRENCH_H +#define IDYNTREE_CORE_WRENCH_H -#include - -namespace iDynTree -{ - /** - * Class representing a wrench, i.e. a 6D combination of linear force an angular torque. - * - * \ingroup iDynTreeCore - * - */ - class Wrench: public SpatialForceVector - { - public: - inline Wrench() {} - Wrench(const Force & _linearVec3, const Torque & _angularVec3); - Wrench(const SpatialForceVector & other); - Wrench(const Wrench & other); +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif - // overloaded operators - Wrench operator+(const Wrench &other) const; - Wrench operator-(const Wrench &other) const; - Wrench operator-() const; - }; -} +#include #endif diff --git a/src/core/include/iDynTree/CubicSpline.h b/src/core/include/iDynTree/CubicSpline.h new file mode 100644 index 00000000000..a5eb1d2080b --- /dev/null +++ b/src/core/include/iDynTree/CubicSpline.h @@ -0,0 +1,45 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + + +#ifndef IDYNTREE_CUBIC_SPLINE_H +#define IDYNTREE_CUBIC_SPLINE_H + +#include +#include +#include + +namespace iDynTree +{ + class CubicSpline { + std::vector< iDynTree::Vector4 > m_coefficients; + iDynTree::VectorDynSize m_velocities; + iDynTree::VectorDynSize m_time; + iDynTree::VectorDynSize m_y; + iDynTree::VectorDynSize m_T; + + double m_v0; + double m_vf; + double m_a0; + double m_af; + + bool computePhasesDuration(); + bool computeIntermediateVelocities(); + bool computeCoefficients(); + + bool m_areCoefficientsUpdated; + + public: + CubicSpline(); + + CubicSpline(unsigned int buffersDimension); + + bool setData(const iDynTree::VectorDynSize& time, const iDynTree::VectorDynSize& yData); + void setInitialConditions(double initialVelocity, double initialAcceleration); + void setFinalConditions(double finalVelocity, double finalAcceleration); + double evaluatePoint(double t); + double evaluatePoint(double t, double& velocity, double& acceleration); + }; +} + +#endif diff --git a/src/core/include/iDynTree/Direction.h b/src/core/include/iDynTree/Direction.h new file mode 100644 index 00000000000..fb50593b576 --- /dev/null +++ b/src/core/include/iDynTree/Direction.h @@ -0,0 +1,99 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_DIRECTION_H +#define IDYNTREE_DIRECTION_H + +#include + +#include +#include + + +namespace iDynTree +{ + /** + * Class representing the coordinates of a direction in the 3D space + * + * \ingroup iDynTreeCore + * + */ + class Direction: public Vector3 + { + private: + /** + * Set the object to the default direction : 1, 0, 0 . + */ + void setToDefault(); + + public: + /** + * Default constructor. + * The data is not reset to the default for perfomance reason. + * Please initialize the data in the class before any use. + */ + inline Direction() {} + + /** + * Constructor from 3 doubles: initialize the direction with the passed values. + * The vector passed is normalized to ensure that the direction is stored as a unit vector. + */ + Direction(double x, double y, double z); + + /** + * Copy constructor: create a Direction from another Direction + */ + Direction(const Direction & other); + + /** + * Copy constructor: create a Direction from a 3 double buffer + */ + Direction(const double* in_data, const unsigned int in_size); + + /** + * Normalize the representation of the direction, useful if + * the coordinates of the direction has been manually setted + * and you want to be sure that this direction is actually + * a unit vector. + * + * @param tol if the norm of the vector < tol, set the direction to 1,0,0 + */ + void Normalize(double tol=DEFAULT_TOL); + + /** + * Check if two directions are parallel. + * + * @param otherDirection the direction to check for parallelism. + * @param tolerance tolerance to use in the parallelism check. + */ + bool isParallel(const Direction & otherDirection, double tolerance) const; + + /** + * Check if two directions are perpendicular. + * + * @param otherDirection the direction to check for the perpendicular check. + * @param tolerance tolerance to use in the perpendicular check. + */ + bool isPerpendicular(const Direction & otherDirection, double tolerance) const; + + /** + * Return the direction, i.e. return its opposite. + * + */ + Direction reverse() const; + + /** + * @name Output helpers. + * Output helpers. + */ + ///@{ + std::string toString() const; + + std::string reservedToString() const; + ///@} + + static Direction Default(); + }; +} + +#endif diff --git a/src/core/include/iDynTree/EigenHelpers.h b/src/core/include/iDynTree/EigenHelpers.h new file mode 100644 index 00000000000..fad3af5786a --- /dev/null +++ b/src/core/include/iDynTree/EigenHelpers.h @@ -0,0 +1,403 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_EIGEN_HELPERS_H +#define IDYNTREE_EIGEN_HELPERS_H + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#if __cplusplus > 199711L +#include +#endif + + +namespace iDynTree +{ + //Useful typedefs + //TODO: change methods below to use these typedefs + typedef Eigen::Map iDynTreeEigenVector; + typedef Eigen::Map iDynTreeEigenConstVector; + typedef Eigen::Matrix iDynTreeEigenMatrix; + typedef const Eigen::Matrix iDynTreeEigenConstMatrix; + typedef Eigen::Map iDynTreeEigenMatrixMap; + typedef Eigen::Map iDynTreeEigenConstMatrixMap; + +#if __cplusplus > 199711L + template + struct is_sparsematrix : std::false_type {}; + + template + struct is_sparsematrix> : std::true_type {}; +#endif + + + +// Dynamics size toEigen methods +inline Eigen::Map toEigen(VectorDynSize & vec) +{ + return Eigen::Map(vec.data(),vec.size()); +} + +inline Eigen::Map< Eigen::Matrix > toEigen(MatrixDynSize & mat) +{ + return Eigen::Map< Eigen::Matrix >(mat.data(),mat.rows(),mat.cols()); +} + +inline Eigen::Map toEigen(const VectorDynSize & vec) +{ + return Eigen::Map(vec.data(),vec.size()); +} + +#if !defined(SWIG_VERSION) || SWIG_VERSION >= 0x030000 +inline Eigen::Map toEigen(Span vec) +{ + return Eigen::Map(vec.data(),vec.size()); +} + +inline Eigen::Map toEigen(iDynTree::Span vec) +{ + return Eigen::Map(vec.data(),vec.size()); +} + +inline Eigen::Map, + 0, + Eigen::Stride> +toEigen(const MatrixView& mat) +{ + using MatrixRowMajor = Eigen::Matrix; + + // This is a trick required to see a ColumnMajor matrix as a RowMajor matrix. + // + // Given the following matrix + // _ _ _ + // / \ _____ | 1 2 3 4 5 | + // / _ \ |_____| | | + // / ___ \ |_____| | | + // /_/ \_\ |_ 6 7 8 9 10 _| + // + // If the matrix is stored as RowMajor matrix there will be a vector v_row in which + // the elements are saved + // v_row = [1 2 3 4 5 6 7 8 9 10] + // + // If the matrix is stored as ColumnMajor matrix there will be a vector v_col in which + // the elements are saved + // v_col = [1 6 2 7 3 8 4 9 5 10] + // + // Our goal here is to build a RowMajor Matrix (independently it is RowMajor/ColumnMajor) + // starting from the raw vactor (v_row, v_col) + // + // From the Eigen documentation https://eigen.tuxfamily.org/dox/classEigen_1_1Stride.html + // The inner stride is the pointer increment between two consecutive entries within a given row of a row-major matrix. + // The outer stride is the pointer increment between two consecutive rows of a row-major matrix. + // + // Starting from v_row we can build a RowMajor matrix by choosing the following pair of strides + // - inner_stride = 1 + // - outer_stride = 5 = number of columns of A + // + // Starting from v_col we can build a RowMajor matrix by choosing the following pair of strides + // - inner_stride = 2 = number of rows of A + // - outer_stride = 1 + + const std::ptrdiff_t innerStride = (mat.storageOrder() == MatrixStorageOrdering::ColumnMajor) ? mat.rows() : 1; + const std::ptrdiff_t outerStride = (mat.storageOrder() == MatrixStorageOrdering::ColumnMajor) ? 1 : mat.cols(); + + return Eigen::Map>( + mat.data(), + mat.rows(), + mat.cols(), + Eigen::Stride(outerStride, innerStride)); +} + +inline Eigen::Map, + 0, + Eigen::Stride> +toEigen(const MatrixView& mat) +{ + using MatrixRowMajor = Eigen::Matrix; + + // This is a trick required to see a ColumnMajor matrix as a RowMajor matrix. + // + // Given the following matrix + // _ _ _ + // / \ _____ | 1 2 3 4 5 | + // / _ \ |_____| | | + // / ___ \ |_____| | | + // /_/ \_\ |_ 6 7 8 9 10 _| + // + // If the matrix is stored as RowMajor matrix there will be a vector v_row in which + // the elements are saved as + // v_row = [1 2 3 4 5 6 7 8 9 10] + // + // If the matrix is stored as ColumnMajor matrix there will be a vector v_col in which + // the elements are saved as + // v_col = [1 6 2 7 3 8 4 9 5 10] + // + // Our goal here is to build a RowMajor Matrix (independently it is RowMajor/ColumnMajor) + // starting from the raw vactor (v_row, v_col) + // + // From the Eigen documentation https://eigen.tuxfamily.org/dox/classEigen_1_1Stride.html + // The inner stride is the pointer increment between two consecutive entries within a given row of a row-major matrix. + // The outer stride is the pointer increment between two consecutive rows of a row-major matrix. + // + // Starting from v_row we can build a RowMajor matrix by choosing the following pair of strides + // - inner_stride = 1 + // - outer_stride = 5 = number of columns of A + // + // Starting from v_col we can build a RowMajor matrix by choosing the following pair of strides + // - inner_stride = 2 = number of rows of A + // - outer_stride = 1 + + const std::ptrdiff_t innerStride = (mat.storageOrder() == MatrixStorageOrdering::ColumnMajor) ? mat.rows() : 1; + const std::ptrdiff_t outerStride = (mat.storageOrder() == MatrixStorageOrdering::ColumnMajor) ? 1 : mat.cols(); + + return Eigen::Map>( + mat.data(), + mat.rows(), + mat.cols(), + Eigen::Stride(outerStride, innerStride)); +} + +#endif + +inline Eigen::Map > toEigen(const MatrixDynSize & mat) +{ + return Eigen::Map >(mat.data(),mat.rows(),mat.cols()); +} + +// Fixed size toEigen methods +template +inline Eigen::Map > toEigen(VectorFixSize & vec) +{ + return Eigen::Map >(vec.data(),vec.size()); +} + +template +inline Eigen::Map > toEigen(const VectorFixSize & vec) +{ + return Eigen::Map >(vec.data()); +} + +template +inline Eigen::Map< Eigen::Matrix > toEigen(MatrixFixSize & mat) +{ + return Eigen::Map< Eigen::Matrix >(mat.data()); +} + +template +inline Eigen::Map< Eigen::Matrix > toEigen(MatrixFixSize & mat) +{ + return Eigen::Map< Eigen::Matrix >(mat.data()); +} + +template +inline Eigen::Map< Eigen::Matrix > toEigen(MatrixFixSize<1, nCols> & mat) +{ + return Eigen::Map< Eigen::Matrix >(mat.data()); +} + +template +inline Eigen::Map< const Eigen::Matrix > toEigen(const MatrixFixSize & mat) +{ + return Eigen::Map< const Eigen::Matrix >(mat.data()); +} + +// Spatial vectors +inline const Eigen::Matrix toEigen(const SpatialMotionVector & vec) +{ + Eigen::Matrix ret; + + ret.segment<3>(0) = toEigen(vec.getLinearVec3()); + ret.segment<3>(3) = toEigen(vec.getAngularVec3()); + + return ret; +} + +inline const Eigen::Matrix toEigen(const SpatialForceVector & vec) +{ + Eigen::Matrix ret; + + ret.segment<3>(0) = toEigen(vec.getLinearVec3()); + ret.segment<3>(3) = toEigen(vec.getAngularVec3()); + + return ret; +} + +// Spatial vectors +inline void fromEigen(SpatialMotionVector & vec, const Eigen::Matrix & eigVec) +{ + toEigen(vec.getLinearVec3()) = eigVec.segment<3>(0); + toEigen(vec.getAngularVec3()) = eigVec.segment<3>(3); +} + +inline void fromEigen(SpatialForceVector & vec, const Eigen::Matrix & eigVec) +{ + toEigen(vec.getLinearVec3()) = eigVec.segment<3>(0); + toEigen(vec.getAngularVec3()) = eigVec.segment<3>(3); +} + +inline void fromEigen(Transform & trans, const Eigen::Matrix4d & eigMat) +{ + Rotation rot; + Position pos; + + toEigen(rot) = eigMat.block<3,3>(0,0); + toEigen(pos) = eigMat.block<3,1>(0,3); + + trans.setRotation(rot); + trans.setPosition(pos); +} + +template +inline Eigen::Matrix skew(const Eigen::MatrixBase & vec) +{ + EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived, 3); + return (Eigen::Matrix() << 0.0, -vec[2], vec[1], + vec[2], 0.0, -vec[0], + -vec[1], vec[0], 0.0).finished(); +} + +template +inline Eigen::Matrix unskew(const Eigen::MatrixBase & mat) +{ + EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived, 3, 3); + return (Eigen::Matrix() << mat(2,1), mat(0,2), mat(1,0) ).finished(); +} + + +/** + * Submatrix helpers + */ +template +inline void setSubMatrix(iDynTreeMatrixType& mat, + const IndexRange rowRange, + const IndexRange colRange, + const MatrixFixSize& subMat) +{ +#if __cplusplus > 199711L + static_assert(!iDynTree::is_sparsematrix::value, "You cannot set a subMatrix on a SparseMatrix."); +#endif + + toEigen(mat).block(rowRange.offset,colRange.offset,rowRange.size,colRange.size) = toEigen(subMat); + return; +} + +template +inline void setSubMatrix(iDynTreeMatrixType& mat, + const IndexRange rowRange, + const IndexRange colRange, + const EigMatType& subMat) +{ +#if __cplusplus > 199711L + static_assert(!iDynTree::is_sparsematrix::value, "You cannot set a subMatrix on a SparseMatrix."); +#endif + toEigen(mat).block(rowRange.offset,colRange.offset,rowRange.size,colRange.size) = subMat; + return; +} + +template +inline void setSubMatrix(iDynTreeMatrixType& mat, + const IndexRange rowRange, + const IndexRange colRange, + const double subMat) +{ +#if __cplusplus > 199711L + static_assert(!iDynTree::is_sparsematrix::value, "You cannot set a subMatrix on a SparseMatrix."); +#endif + assert(rowRange.size == 1); + assert(colRange.size == 1); + mat(rowRange.offset,colRange.offset) = subMat; + return; +} + +template +inline void setSubMatrixToIdentity(iDynTreeMatrixType& mat, + const IndexRange rowRange, + const IndexRange colRange) +{ +#if __cplusplus > 199711L + static_assert(!iDynTree::is_sparsematrix::value, "You cannot set a setSubMatrixToIdentity on a SparseMatrix."); +#endif + assert(rowRange.size == colRange.size); + for(int i=0; i < rowRange.size; i++) + { + mat(rowRange.offset+i,colRange.offset+i) = 1.0; + } + return; +} + +template +inline void setSubMatrixToMinusIdentity(iDynTreeMatrixType& mat, + const IndexRange rowRange, + const IndexRange colRange) +{ +#if __cplusplus > 199711L + static_assert(!iDynTree::is_sparsematrix::value, "You cannot set a setSubMatrixToMinusIdentity on a SparseMatrix."); +#endif + assert(rowRange.size == colRange.size); + for(int i=0; i < rowRange.size; i++) + { + mat(rowRange.offset+i,colRange.offset+i) = -1.0; + } + return; +} + + +template +inline void setSubVector(VectorDynSize& vec, + const IndexRange range, + const VectorFixSize& subVec) +{ + toEigen(vec).segment(range.offset,range.size) = toEigen(subVec); + return; +} + +inline void setSubVector(VectorDynSize& vec, + const IndexRange range, + double subVec) +{ + assert(range.size==1); + vec(range.offset) = subVec; + return; +} + +inline void setSubVector(VectorDynSize& vec, + const IndexRange range, + const SpatialMotionVector& twist) +{ + assert(range.size==6); + toEigen(vec).segment(range.offset,range.size) = toEigen(twist); + return; +} + +inline void setSubVector(VectorDynSize& vec, + const IndexRange range, + const SpatialForceVector& wrench) +{ + assert(range.size==6); + toEigen(vec).segment(range.offset,range.size) = toEigen(wrench); + return; +} + +template +inline void setSubVector(VectorDynSize& vec, + const IndexRange range, + const T& subVec) +{ + toEigen(vec).segment(range.offset,range.size) = subVec; + return; +} + +} + +#endif /* IDYNTREE_EIGEN_HELPERS_H */ + diff --git a/src/core/include/iDynTree/EigenMathHelpers.h b/src/core/include/iDynTree/EigenMathHelpers.h new file mode 100644 index 00000000000..fb018712c0e --- /dev/null +++ b/src/core/include/iDynTree/EigenMathHelpers.h @@ -0,0 +1,68 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_EIGEN_MATH_HELPERS_H +#define IDYNTREE_EIGEN_MATH_HELPERS_H + +#include + +namespace iDynTree +{ + +// Methods imported from codyco commons, see the documentation +// for them in +// https://github.com/robotology-playground/codyco-commons/blob/master/include/codyco/MathUtils.h#L48 +template +void pseudoInverse_helper2(const MapType& A, + Eigen::JacobiSVD& svdDecomposition, + MapType& Apinv, + double tolerance, + unsigned int computationOptions = Eigen::ComputeThinU|Eigen::ComputeThinV) +{ + using namespace Eigen; + + if (computationOptions == 0) return; //if no computation options we cannot compute the pseudo inverse + svdDecomposition.compute(A, computationOptions); + + typename JacobiSVD::SingularValuesType singularValues = svdDecomposition.singularValues(); + int singularValuesSize = singularValues.size(); + int rank = 0; + for (int idx = 0; idx < singularValuesSize; idx++) { + if (tolerance > 0 && singularValues(idx) > tolerance) { + singularValues(idx) = 1.0 / singularValues(idx); + rank++; + } else { + singularValues(idx) = 0.0; + } + } + + //equivalent to this U/V matrix in case they are computed full + Apinv = svdDecomposition.matrixV().leftCols(singularValuesSize) * singularValues.asDiagonal() * svdDecomposition.matrixU().leftCols(singularValuesSize).adjoint(); +} + + +template +void pseudoInverse_helper1(const MapType& A, + Eigen::JacobiSVD& svdDecomposition, + MapType& Apinv, + double tolerance, + unsigned int computationOptions = Eigen::ComputeThinU|Eigen::ComputeThinV) +{ + using namespace Eigen; + pseudoInverse_helper2(A, svdDecomposition, Apinv, tolerance, computationOptions); +} + +template +void pseudoInverse(const MapType A, + MapType Apinv, + double tolerance, + unsigned int computationOptions = Eigen::ComputeThinU|Eigen::ComputeThinV) + +{ + Eigen::JacobiSVD svdDecomposition(A.rows(), A.cols()); + pseudoInverse_helper1(A, svdDecomposition, Apinv, tolerance, computationOptions); +} + +} + +#endif diff --git a/src/core/include/iDynTree/EigenSparseHelpers.h b/src/core/include/iDynTree/EigenSparseHelpers.h new file mode 100644 index 00000000000..a2c48fd243d --- /dev/null +++ b/src/core/include/iDynTree/EigenSparseHelpers.h @@ -0,0 +1,60 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_EIGEN_SPARSE_HELPERS_H +#define IDYNTREE_EIGEN_SPARSE_HELPERS_H + +#include +#include + +namespace iDynTree +{ + +//SparseMatrix helpers +inline Eigen::Map< Eigen::SparseMatrix > toEigen(iDynTree::SparseMatrix & mat) +{ + return Eigen::Map >(mat.rows(), + mat.columns(), + mat.numberOfNonZeros(), + mat.outerIndicesBuffer(), + mat.innerIndicesBuffer(), + mat.valuesBuffer(), + 0); //compressed format +} + +inline Eigen::Map > toEigen(const iDynTree::SparseMatrix & mat) +{ + return Eigen::Map >(mat.rows(), + mat.columns(), + mat.numberOfNonZeros(), + mat.outerIndicesBuffer(), + mat.innerIndicesBuffer(), + mat.valuesBuffer(), + 0); //compressed format +} + +inline Eigen::Map< Eigen::SparseMatrix > toEigen(iDynTree::SparseMatrix & mat) +{ + return Eigen::Map >(mat.rows(), + mat.columns(), + mat.numberOfNonZeros(), + mat.outerIndicesBuffer(), + mat.innerIndicesBuffer(), + mat.valuesBuffer(), + 0); //compressed format +} + +inline Eigen::Map > toEigen(const iDynTree::SparseMatrix & mat) +{ + return Eigen::Map >(mat.rows(), + mat.columns(), + mat.numberOfNonZeros(), + mat.outerIndicesBuffer(), + mat.innerIndicesBuffer(), + mat.valuesBuffer(), + 0); //compressed format +} + +} + +#endif /* IDYNTREE_EIGEN_SPARSE_HELPERS_H */ diff --git a/src/core/include/iDynTree/GeomVector3.h b/src/core/include/iDynTree/GeomVector3.h new file mode 100644 index 00000000000..51d3a1f683b --- /dev/null +++ b/src/core/include/iDynTree/GeomVector3.h @@ -0,0 +1,47 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_GEOM_VECTOR_3_H +#define IDYNTREE_GEOM_VECTOR_3_H + +#include + +namespace iDynTree +{ + class Rotation; + + class GeomVector3 : public Vector3 { + public: + GeomVector3() = default; + GeomVector3(const double* in_data, const unsigned int in_size); + GeomVector3(const double x, const double y, const double z); + GeomVector3(const Vector3 other); + GeomVector3 changeCoordFrame(const Rotation& newCoordFrame) const; + GeomVector3 compose(const GeomVector3& op1, const GeomVector3& op2) const; + GeomVector3 inverse(const GeomVector3& op) const; + double dot(const GeomVector3& other) const; + GeomVector3 operator+(const GeomVector3& other) const; + GeomVector3 operator-(const GeomVector3& other) const; + GeomVector3 operator-() const; + Rotation exp() const; + GeomVector3 cross(const GeomVector3& other) const; + + }; + + typedef GeomVector3 LinearMotionVector3; + typedef LinearMotionVector3 LinVelocity; + typedef LinearMotionVector3 LinAcceleration; + typedef GeomVector3 AngularMotionVector3; + typedef AngularMotionVector3 AngVelocity; + typedef AngularMotionVector3 AngAcceleration; + typedef GeomVector3 LinearForceVector3; + typedef LinearForceVector3 LinMomentum; + typedef LinearForceVector3 Force; + typedef GeomVector3 AngularForceVector3; + typedef AngularForceVector3 AngMomentum; + typedef AngularForceVector3 Torque; + typedef GeomVector3 MotionVector3; + +} + +#endif /* IDYNTREE_GEOM_VECTOR_3_H */ diff --git a/src/core/include/iDynTree/InertiaNonLinearParametrization.h b/src/core/include/iDynTree/InertiaNonLinearParametrization.h new file mode 100644 index 00000000000..8550e75c78c --- /dev/null +++ b/src/core/include/iDynTree/InertiaNonLinearParametrization.h @@ -0,0 +1,77 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_INERTIA_NON_LINEAR_PARAMETRIZATION_H +#define IDYNTREE_INERTIA_NON_LINEAR_PARAMETRIZATION_H + +#include +#include +#include + +namespace iDynTree +{ + class RigidBodyInertiaNonLinearParametrization + { + public: + double mass; + Position com; + + /** + * Rotation matrix that takes a vector expressed in the centroidal + * frame (origin in the center of mass, orientation as the principal + * axis of the 3D inertia at the center of mass). + */ + Rotation link_R_centroidal; + + Vector3 centralSecondMomentOfMass; + + /** + * Get the transform that applied to a quantity expressed in the centroidal + * frame it express it in the link frame ( link_H_centroidal ). + */ + Transform getLinkCentroidalTransform() const; + + /** + * Build the parametrization from a RigidBodyInertia. + */ + void fromRigidBodyInertia(const SpatialInertia & inertia); + + /** + * Build the nonlinear parametrization from a vector of 10 inertial parameters. + */ + void fromInertialParameters(const Vector10 & inertialParams); + + /** + * Convert the parametrization to a RigidBodyInertia class. + */ + SpatialInertia toRigidBodyInertia() const; + + /** + * Check that the mass is positive + * and the central second moments of mass are nonnegative. + */ + bool isPhysicallyConsistent() const; + + /** + * Serialize the nonlinear parametrization as a + * vector of sixteen elements. + */ + Vector16 asVectorWithRotationAsVec() const; + + /** + * Build the nonlinear parametrization from a vector of sixteen elements. + */ + void fromVectorWithRotationAsVec(const Vector16 & vec); + + /** + * Given the mapping between this nonlinear representation + * and the classical inertia parameters. This function + * return the gradient between the inertial parameters + */ + Matrix10x16 getGradientWithRotationAsVec() const; + }; + + +} + +#endif diff --git a/src/core/include/iDynTree/MatrixDynSize.h b/src/core/include/iDynTree/MatrixDynSize.h new file mode 100644 index 00000000000..65cf912dcb4 --- /dev/null +++ b/src/core/include/iDynTree/MatrixDynSize.h @@ -0,0 +1,247 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_MATRIX_DYN_SIZE_H +#define IDYNTREE_MATRIX_DYN_SIZE_H + + +#include + +#include + +namespace iDynTree +{ + /** + * Class providing a simple form of matrix with dynamic size. + * + * \ingroup iDynTreeCore + */ + class MatrixDynSize + { + private: + /** + * Return the raw index in the data vector of the + * element corresponding to row and col, using row + * major ordering. + */ + std::size_t rawIndexRowMajor(std::size_t row, std::size_t col) const; + + /** + * Return the raw index in the data vector of the + * element corresponding to row and col, using col + * major ordering. + * + * \warning The class stores data in row major order, + * this function is used just in the fillColMajorBuffer + * method. + */ + std::size_t rawIndexColMajor(std::size_t row, std::size_t col) const; + + /** + * Set the capacity of the vector, resizing the buffer pointed by m_data. + */ + void changeCapacityAndCopyData(const std::size_t _newCapacity); + + protected: + /** + * Storage for the MatrixDynSize + * + * Pointer to an area of capacity() doubles, managed by this class. + * + * \warning this class stores data using the row major order + */ + double * m_data; + std::size_t m_rows; + std::size_t m_cols; + + /** + * The buffer to which m_data is pointing is m_capacity*sizeof(double). + */ + std::size_t m_capacity; + + public: + /** + * Default constructor: create a 0x0 matrix. + */ + MatrixDynSize(); + + /** + * Constructor from the rows and columns, all the element assigned to 0 + * + * @param _rows the desired rows of the matrix. + * @param _cols the desired cols of the matrix. + * + * \warning performs dynamic memory allocation operations + */ + MatrixDynSize(std::size_t _rows, std::size_t _cols); + + /** + * Constructor from a C-style matrix. + * + * + * \warning this class stores data using the row major order + * \warning performs dynamic memory allocation operations + */ + MatrixDynSize(const double * in_data, const std::size_t in_rows, const std::size_t in_cols); + + /** + * Copy constructor + * + * @param other the object to copy + */ + MatrixDynSize(const MatrixDynSize& other); + + + /** + * Constructor from MatrixView object + * + * @param other MatrixView to copy + * \warning performs dynamic memory allocation operations + * + */ + MatrixDynSize(iDynTree::MatrixView other); + + /** + * Assignment operator + * + * @param other the object to copy into self + * + * @return *this + */ + MatrixDynSize& operator=(const MatrixDynSize& other); + + /** + * Assignment operator + * + * @param other the object to copy into self + * + * @return *this + */ + MatrixDynSize& operator=(iDynTree::MatrixView other); + + /** + * Denstructor + * + * \warning performs dynamic memory allocation operations + */ + virtual ~MatrixDynSize(); + + /** + * @name Matrix interface methods. + * Methods exposing a matrix-like interface to MatrixDynSize. + * + * \warning Notice that using this methods you can damage the underlyng rotation matrix. + * In doubt, don't use them and rely on more high level functions. + */ + ///@{ + double operator()(const std::size_t row, const std::size_t col) const; + double& operator()(const std::size_t row, const std::size_t col); + double getVal(const std::size_t row, const std::size_t col) const; + bool setVal(const std::size_t row, const std::size_t col, const double new_el); + std::size_t rows() const; + std::size_t cols() const; + ///@} + + /** + * Raw data accessor + * + * \warning this class stores matrix data using the row major order + * @return a const pointer to a vector of size() doubles + */ + const double * data() const; + + /** + * Raw data accessor + * + * \warning this class stores matrix data using the row major order + * @return a pointer to a vector of size() doubles + */ + double * data(); + + /** + * Assign all element of the matrix to 0. + */ + void zero(); + + /** + * Change the size of the matrix, without preserving old content. + * + * @param _newRows the new rows of the matrix + * @param _newCols the new cols of the matrix + * + * \warning performs dynamic memory allocation operations if newRows*newCols > capacity() + */ + void resize(const std::size_t _newRows, const std::size_t _newCols); + + /** + * Increase the capacity of the matrix, preserving old content. + * + * \warning performs dynamic memory allocation operations if _newCapacity > capacity() + */ + void reserve(const size_t _newCapacity); + + /** + * Get the dimension (in doubles) of the buffer to which m_data is pointing. + */ + size_t capacity() const; + + /** + * Change the capacity of the matrix such that capacity() == rows()*cols(), preserving old content. + * + * \warning performs dynamic memory allocation operations if newRows*newCols != capacity() + */ + void shrink_to_fit(); + + /** + * Assume that rowMajorBuf is pointing to + * a buffer of rows()*cols() doubles, and fill + * it with the content of this matrix, using + * row major order. + * + * @param rowMajorBuf pointer to the buffer to fill + * + * @todo provide this for all matrix types + * + * \warning use this function only if you are + * an expert C user + */ + void fillRowMajorBuffer(double * rowMajorBuf) const; + + /** + * Assume that colMajorBuf is pointing to + * a buffer of rows()*cols() doubles, and fill + * it with the content of this matrix, using + * column major order. + * + * @param colMajorBuf pointer to the buffer to fill + * + * @todo provide this for all matrix types + * + * \warning use this function only if you are + * an expert C user + */ + void fillColMajorBuffer(double * colMajorBuf) const; + + +#if !defined(SWIG_VERSION) || SWIG_VERSION >= 0x030000 + /** Typedefs to enable make_matrix_view. + */ + ///@{ + typedef double value_type; + ///@} +#endif + + /** @name Output helpers. + * Output helpers. + */ + ///@{ + std::string toString() const; + + std::string reservedToString() const; + ///@} + + }; +} + +#endif /* IDYNTREE_MATRIX_DYN_SIZE_H */ + diff --git a/src/core/include/iDynTree/MatrixFixSize.h b/src/core/include/iDynTree/MatrixFixSize.h new file mode 100644 index 00000000000..be198ea7972 --- /dev/null +++ b/src/core/include/iDynTree/MatrixFixSize.h @@ -0,0 +1,379 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_MATRIX_FIX_SIZE_H +#define IDYNTREE_MATRIX_FIX_SIZE_H + + +#include +#include + +#include +#include +#include +#include +#include + +namespace iDynTree +{ + /** + * Class providing a simple form of matrix with dynamic size. + * + * \ingroup iDynTreeCore + */ + template + class MatrixFixSize + { + private: + /** + * Return the raw index in the data vector of the + * element corresponding to row and col, using row + * major ordering. + */ + std::size_t rawIndexRowMajor(std::size_t row, std::size_t col) const; + + /** + * Return the raw index in the data vector of the + * element corresponding to row and col, using col + * major ordering. + * + * \warning The class stores data in row major order, + * this function is used just in the fillColMajorBuffer + * method. + */ + std::size_t rawIndexColMajor(std::size_t row, std::size_t col) const; + + protected: + /** + * Storage for the MatrixDynSize + * + * Pointer to an area of size() doubles, managed by this class. + * + * \warning this class stores data using the row major order + */ + double m_data[nRows*nCols]; + + + public: + /** + * Default constructor. + * The data is not reset to 0 for perfomance reason. + * Please initialize the data in the vector before any use. + */ + MatrixFixSize(); + + + /** + * Constructor from a C-style matrix (row major). + * + * \warning this class stores data using the row major order + */ + MatrixFixSize(const double * in_data, const std::size_t in_rows, const std::size_t in_cols); + + /** + * Constructor from a MatrixView + * + * \warning this class stores data using the row major order + */ + MatrixFixSize(iDynTree::MatrixView other); + + /** + * @name Matrix interface methods. + * Methods exposing a matrix-like interface to MatrixFixSize. + * + */ + ///@{ + double operator()(const std::size_t row, const std::size_t col) const; + double& operator()(const std::size_t row, const std::size_t col); + double getVal(const std::size_t row, const std::size_t col) const; + bool setVal(const std::size_t row, const std::size_t col, const double new_el); + std::size_t rows() const; + std::size_t cols() const; + ///@} + + MatrixFixSize & operator=(iDynTree::MatrixView mat); + + /** + * Raw data accessor + * + * \warning this class stores matrix data using the row major order + * @return a const pointer to a vector of rows()*cols() doubles + */ + const double * data() const; + + /** + * Raw data accessor + * + * \warning this class stores matrix data using the row major order + * @return a pointer to a vector of rows()*cols() doubles + */ + double * data(); + + /** + * Assign all element of the matrix to 0. + */ + void zero(); + + /** + * Assume that rowMajorBuf is pointing to + * a buffer of rows()*cols() doubles, and fill + * it with the content of this matrix, using + * row major order. + * + * @param rowMajorBuf pointer to the buffer to fill + * + * @todo provide this for all matrix types + * + * \warning use this function only if you are + * an expert C user + */ + void fillRowMajorBuffer(double * rowMajorBuf) const; + + /** + * Assume that colMajorBuf is pointing to + * a buffer of rows()*cols() doubles, and fill + * it with the content of this matrix, using + * column major order. + * + * @param colMajorBuf pointer to the buffer to fill + * + * @todo provide this for all matrix types + * + * \warning use this function only if you are + * an expert C user + */ + void fillColMajorBuffer(double * colMajorBuf) const; + + #if !defined(SWIG_VERSION) || SWIG_VERSION >= 0x030000 + /** Typedefs to enable make_matrix_view. + */ + ///@{ + typedef double value_type; + ///@} +#endif + + + /** @name Output helpers. + * Output helpers. + */ + ///@{ + std::string toString() const; + + std::string reservedToString() const; + ///@} + + }; + + + // Implementation + template + MatrixFixSize::MatrixFixSize() + { + } + + template + MatrixFixSize::MatrixFixSize(const double* in_data, + const std::size_t in_rows, + const std::size_t in_cols) + { + if( in_rows != nRows || + in_cols != nCols ) + { + reportError("MatrixFixSize","constructor","input matrix does not have the right size"); + this->zero(); + } + else + { + std::memcpy(this->m_data,in_data,nRows*nCols*sizeof(double)); + } + } + + template + MatrixFixSize::MatrixFixSize(iDynTree::MatrixView other) + { + if( other.rows() != nRows || + other.cols() != nCols ) + { + reportError("MatrixFixSize","constructor","input matrix does not have the right size"); + this->zero(); + } + else + { + for(std::size_t row=0; row < nRows; row++ ) + { + for(std::size_t col=0; col < nCols; col++ ) + { + this->m_data[rawIndexRowMajor(row,col)] = other(row, col); + } + } + } + } + + template + void MatrixFixSize::zero() + { + for(std::size_t row=0; row < this->rows(); row++ ) + { + for(std::size_t col=0; col < this->cols(); col++ ) + { + this->m_data[rawIndexRowMajor(row,col)] = 0.0; + } + } + } + + template + std::size_t MatrixFixSize::rows() const + { + return nRows; + } + + template + std::size_t MatrixFixSize::cols() const + { + return nCols; + } + + template + double* MatrixFixSize::data() + { + return this->m_data; + } + + template + const double* MatrixFixSize::data() const + { + return this->m_data; + } + + template + MatrixFixSize & MatrixFixSize::operator=(iDynTree::MatrixView mat) { + assert(nCols == mat.cols()); + assert(nRows == mat.rows()); + + for(std::size_t i = 0; i < nRows; i++) + { + for(std::size_t j = 0; j < nCols; j++) + { + this->m_data[this->rawIndexRowMajor(i,j)] = mat(i, j); + } + } + return *this; + } + + template + double& MatrixFixSize::operator()(const std::size_t row, const std::size_t col) + { + assert(row < nRows); + assert(col < nCols); + return this->m_data[rawIndexRowMajor(row,col)]; + } + + template + double MatrixFixSize::operator()(const std::size_t row, const std::size_t col) const + { + assert(row < nRows); + assert(col < nCols); + return this->m_data[rawIndexRowMajor(row,col)]; + } + + template + double MatrixFixSize::getVal(const std::size_t row, const std::size_t col) const + { + if( row >= this->rows() || + col >= this->cols() ) + { + reportError("MatrixDynSize","getVal","indices out of bounds"); + return 0.0; + } + + return this->m_data[rawIndexRowMajor(row,col)]; + } + + template + bool MatrixFixSize::setVal(const std::size_t row, const std::size_t col, const double new_el) + { + if( row >= this->rows() || + col >= this->cols() ) + { + reportError("MatrixDynSize","setVal","indices out of bounds"); + return false; + } + + this->m_data[rawIndexRowMajor(row,col)] = new_el; + return true; + } + + template + void MatrixFixSize::fillRowMajorBuffer(double* rowMajorBuf) const + { + // MatrixFixSize stores data in row major, a simply + // memcpy will be sufficient + memcpy(rowMajorBuf,this->m_data,this->rows()*this->cols()*sizeof(double)); + } + + template + void MatrixFixSize::fillColMajorBuffer(double* colMajorBuf) const + { + for(std::size_t row = 0; row < this->rows(); row++ ) + { + for(std::size_t col = 0; col < this->cols(); col++ ) + { + colMajorBuf[this->rawIndexColMajor(row,col)] = + this->m_data[this->rawIndexRowMajor(row,col)]; + } + } + } + + template + std::size_t MatrixFixSize::rawIndexRowMajor(std::size_t row, std::size_t col) const + { + return (nCols*row + col); + } + + template + std::size_t MatrixFixSize::rawIndexColMajor(std::size_t row, std::size_t col) const + { + return (row + nRows*col); + } + + template + std::string MatrixFixSize::toString() const + { + std::stringstream ss; + + for(std::size_t row=0; row < this->rows(); row++ ) + { + for(std::size_t col=0; col < this->cols(); col++ ) + { + ss << this->m_data[this->rawIndexRowMajor(row,col)] << " "; + } + ss << std::endl; + } + + return ss.str(); + } + + template + std::string MatrixFixSize::reservedToString() const + { + return this->toString(); + } + + // Explicit instantiations + // The explicit instantiations are the only ones that can be used in the API + // and the only ones that users are supposed to manipulate manipulate + // Add all the explicit instantiation that can be useful, but remember to add + // them also in the iDynTree.i SWIG file + typedef MatrixFixSize<1,6> Matrix1x6; + typedef MatrixFixSize<2,3> Matrix2x3; + typedef MatrixFixSize<3,3> Matrix3x3; + typedef MatrixFixSize<4,4> Matrix4x4; + typedef MatrixFixSize<6,1> Matrix6x1; + typedef MatrixFixSize<6,6> Matrix6x6 ; + typedef MatrixFixSize<6,10> Matrix6x10; + typedef MatrixFixSize<10,16> Matrix10x16; + +} + +#endif /* IDYNTREE_MATRIX_FIX_SIZE_H */ + diff --git a/src/core/include/iDynTree/MatrixView.h b/src/core/include/iDynTree/MatrixView.h new file mode 100644 index 00000000000..300a5026e47 --- /dev/null +++ b/src/core/include/iDynTree/MatrixView.h @@ -0,0 +1,292 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_MATRIX_VIEW_H +#define IDYNTREE_MATRIX_VIEW_H + +#include + +#include + +#include +#include + +// constexpr workaround for SWIG +#ifdef SWIG +#define IDYNTREE_CONSTEXPR +#else +#define IDYNTREE_CONSTEXPR constexpr +#endif + + +namespace iDynTree +{ + + namespace MatrixViewInternal + { +#ifndef SWIG + /** + * has_IsRowMajor is used to build a type-dependent expression that check if an + * element has IsRowMajor argument. This specific implementation is used when + * the the object has not IsRowMajor. + */ + template + struct has_IsRowMajor : std::false_type {}; + + /** + * has_IsRowMajor is used to build a type-dependent expression that check if an + * element has IsRowMajor argument. This specific implementation is used when + * the the object has not IsRowMajor, indeed void_t<\endcode> is used to + * detect ill-formed types in SFINAE context. + */ + template + struct has_IsRowMajor> : std::true_type {}; +#endif + + } // namespace MatrixViewIntenal + + /** + * MatrixView implements a view interface of Matrices. Both RowMajor and ColumnMajor matrices are + * supported. + * @note The user should define the storage ordering when the MatrixView is created (the default + order is RowMajor). However if the MatrixView is generated: + * - from an object having a public member called IsRowMajor, the correct storage + order is chosen. + * - from another MatrixView, the correct storage order is chosen. + */ + template class MatrixView + { + public: + using element_type = ElementType; + using value_type = std::remove_cv_t; + using index_type = std::ptrdiff_t; + using pointer = element_type*; + using reference = element_type&; + + private: + pointer m_storage; + index_type m_rows; + index_type m_cols; + + MatrixStorageOrdering m_storageOrder; + + index_type m_innerStride; + index_type m_outerStride; + + index_type rawIndex(index_type row, index_type col) const + { + if (m_storageOrder == MatrixStorageOrdering::RowMajor) + { + return (col + this->m_outerStride * row); + } else + { + return (this->m_outerStride * col + row); + } + } + + public: + + MatrixView() + : MatrixView(nullptr, 0, 0, MatrixStorageOrdering::RowMajor) + {} + MatrixView(const MatrixView& other) + : MatrixView(other.m_storage, other.m_rows, other.m_cols, other.m_storageOrder) + { + } + +#ifndef SWIG + template < + class OtherElementType, + class = std::enable_if_t< + details::is_allowed_element_type_conversion::value_type, + value_type>::value>> + IDYNTREE_CONSTEXPR MatrixView(const MatrixView& other) + : MatrixView(other.data(), other.rows(), other.cols(), other.storageOrder()) + { + } + + + template < + class Container, + std::enable_if_t::value + && std::is_convertible().data()), + pointer>::value + && MatrixViewInternal::has_IsRowMajor::value + && !std::is_same::value, + int> = 0> + MatrixView(const Container& matrix) + : MatrixView(matrix.data(), + matrix.rows(), + matrix.cols(), + Container::IsRowMajor ? MatrixStorageOrdering::RowMajor + : MatrixStorageOrdering::ColumnMajor) + { + } + + template < + class Container, + std::enable_if_t::value + && std::is_convertible().data()), + pointer>::value + && !MatrixViewInternal::has_IsRowMajor::value + && !std::is_same::value, + int> = 0> + MatrixView(const Container& matrix, const MatrixStorageOrdering& order = MatrixStorageOrdering::RowMajor) + : MatrixView(matrix.data(), matrix.rows(), matrix.cols(), order) + { + } + + template ().data()), pointer>::value + && MatrixViewInternal::has_IsRowMajor::value + && !std::is_same::value, + int> = 0> + MatrixView(Container& matrix) + : MatrixView(matrix.data(), + matrix.rows(), + matrix.cols(), + Container::IsRowMajor ? MatrixStorageOrdering::RowMajor + : MatrixStorageOrdering::ColumnMajor) + { + } + + template ().data()), pointer>::value + && !MatrixViewInternal::has_IsRowMajor::value + && !std::is_same::value + && !std::is_same().data())>>>::value, + int> = 0> + MatrixView(Container& matrix, const MatrixStorageOrdering& order = MatrixStorageOrdering::RowMajor) + : MatrixView(matrix.data(), matrix.rows(), matrix.cols(), order) + { + } + +#endif // SWIG + + MatrixView(pointer in_data, + index_type in_rows, + index_type in_cols, + const MatrixStorageOrdering& order = MatrixStorageOrdering::RowMajor) + : m_storage(in_data) + , m_rows(in_rows) + , m_cols(in_cols) + , m_storageOrder(order) + , m_innerStride(1) + , m_outerStride(order == MatrixStorageOrdering::RowMajor ? in_cols : in_rows) + { + } + + const MatrixStorageOrdering& storageOrder() const noexcept + { + return m_storageOrder; + } + + pointer data() const noexcept + { + return m_storage; + } + + /** + * @name Matrix interface methods. + * Methods exposing a matrix-like interface to MatrixView. + * + */ + ///@{ + reference operator()(index_type row, const index_type col) const + { + assert(row < this->rows()); + assert(col < this->cols()); + return this->m_storage[rawIndex(row, col)]; + } + + index_type rows() const noexcept + { + return this->m_rows; + } + + index_type cols() const noexcept + { + return this->m_cols; + } + ///@} + + MatrixView block(index_type startingRow, index_type startingColumn, + index_type rows, index_type cols) const + { + assert(rows <= this->rows()); + assert(cols <= this->cols()); + + const index_type offset = rawIndex(startingRow, startingColumn); + assert(offset < this->rows() * this->cols()); + + MatrixView block; + block = *this; + block.m_rows = rows; + block.m_cols = cols; + block.m_storage = this->m_storage + offset; + + return block; + } + + }; + + template + IDYNTREE_CONSTEXPR MatrixView + make_matrix_view(ElementType* ptr, + typename MatrixView::index_type rows, + typename MatrixView::index_type cols, + const MatrixStorageOrdering& order = MatrixStorageOrdering::RowMajor) + { + return MatrixView(ptr, rows, cols, order); + } + + template ::value + || std::is_same, Container>::value, + int> = 0> + IDYNTREE_CONSTEXPR MatrixView make_matrix_view(Container& cont) + { + return MatrixView(cont); + } + + template ::value + || std::is_same, Container>::value, + int> = 0> + IDYNTREE_CONSTEXPR MatrixView make_matrix_view(const Container& cont) + { + return MatrixView(cont); + } + + template ::value + && !std::is_same, Container>::value, + int> = 0> + IDYNTREE_CONSTEXPR MatrixView + make_matrix_view(Container& cont, + const MatrixStorageOrdering& order = MatrixStorageOrdering::RowMajor) + { + return MatrixView(cont, order); + } + + template ::value + && !std::is_same, Container>::value, + int> = 0> + IDYNTREE_CONSTEXPR MatrixView + make_matrix_view(const Container& cont, + const MatrixStorageOrdering& order = MatrixStorageOrdering::RowMajor) + { + return MatrixView(cont, order); + } + +} // namespace iDynTree + +#endif /* IDYNTREE_MATRIX_MATRIX_VIEW_H */ diff --git a/src/core/include/iDynTree/Position.h b/src/core/include/iDynTree/Position.h new file mode 100644 index 00000000000..0aa78c02ded --- /dev/null +++ b/src/core/include/iDynTree/Position.h @@ -0,0 +1,106 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_POSITION_H +#define IDYNTREE_POSITION_H + +#include +#include + +#include + +namespace iDynTree +{ + class Rotation; + class Twist; + class SpatialAcc; + class SpatialMomentum; + class Wrench; + class SpatialMotionVector; + class SpatialForceVector; + + /** + * Class representation the coordinates of the Position of + * a point with respect to another point. + * + * \ingroup iDynTreeCore + * + * \image html position.svg + * + * The Position object can briefly described as the position + * of a *point* with respect to a *refPoint*, expressed with + * respect to an orientation given by *orientFrame* . + * + */ + class Position: public PositionRaw + { + public: + /** + * Default constructor. + * The data is not initialized, please initialize the data in the created object before use. + */ + Position(); + + /** + * Constructor from 3 doubles: initialize the coordinates with the passed values. + */ + Position(double x, double y, double z); + + /** + * Copy constructor: create a Position from another Position + */ + Position(const Position & other); + + /** + * Copy constructor: create a Position from a PositionRaw + */ + Position(const PositionRaw & other); + + /** + * Create a Position from a span + */ + Position(iDynTree::Span other); + + /** + * Geometric operations + */ + const Position & changePoint(const Position & newPoint); + const Position & changeRefPoint(const Position & newRefPoint); + const Position & changeCoordinateFrame(const Rotation & newCoordinateFrame); + static Position compose(const Position & op1, const Position & op2); + static Position inverse(const Position & op); + SpatialMotionVector changePointOf(const SpatialMotionVector & other) const; + SpatialForceVector changePointOf(const SpatialForceVector & other) const; + Twist changePointOf(const Twist & other) const; + SpatialAcc changePointOf(const SpatialAcc & other) const; + SpatialMomentum changePointOf(const SpatialMomentum & other) const; + Wrench changePointOf(const Wrench & other) const; + + /** + * overloaded operators + */ + Position operator+(const Position &other) const; + Position operator-(const Position &other) const; + Position operator-() const; + Twist operator*(const Twist & other) const; + SpatialForceVector operator*(const SpatialForceVector & other) const; + SpatialAcc operator*(const SpatialAcc & other) const; + SpatialMomentum operator*(const SpatialMomentum & other) const; + Wrench operator*(const Wrench & other) const; + + /** @name Output helpers. + * Output helpers. + */ + ///@{ + std::string toString() const; + + std::string reservedToString() const; + ///@} + + friend Position Rotation::changeCoordFrameOf(const Position & op) const; + + static Position Zero(); + }; +} + +#endif diff --git a/src/core/include/iDynTree/PositionRaw.h b/src/core/include/iDynTree/PositionRaw.h new file mode 100644 index 00000000000..4190e53a537 --- /dev/null +++ b/src/core/include/iDynTree/PositionRaw.h @@ -0,0 +1,77 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_POSITION_RAW_H +#define IDYNTREE_POSITION_RAW_H + + +#include +#include + +namespace iDynTree +{ + class RotationRaw; + class SpatialMotionVector; + class SpatialForceVector; + + /** + * Class providing the raw coordinates for iDynTree::Position class. + * + * \ingroup iDynTreeCore + */ + class PositionRaw: public Vector3 + { + public: + /** + * Default constructor. + * The data is not reset to 0 for perfomance reason. + * Please initialize the data in the vector before any use. + */ + PositionRaw(); + + /** + * Constructor from 3 doubles: initialize the coordinates with the passed values. + */ + PositionRaw(double x, double y, double z); + + /** + * Constructor from a raw buffer of 3 doubles. + */ + PositionRaw(const double* in_data, const unsigned int in_size); + + /** + * Copy constructor: create a PositionRaw from another PositionRaw + */ + PositionRaw(const PositionRaw & other); + + /** + * Construct from a span + * @warning if the Span size is different from 3 an assert is thrown at run-time. + */ + PositionRaw(iDynTree::Span other); + + /** + * Geometric operations + */ + const PositionRaw & changePoint(const PositionRaw & newPoint); + const PositionRaw & changeRefPoint(const PositionRaw & newRefPoint); + static PositionRaw compose(const PositionRaw & op1, const PositionRaw & op2); + static PositionRaw inverse(const PositionRaw & op); + SpatialMotionVector changePointOf(const SpatialMotionVector & other) const; + SpatialForceVector changePointOf(const SpatialForceVector & other) const; + + + /** @name Output helpers. + * Output helpers. + */ + ///@{ + std::string toString() const; + + std::string reservedToString() const; + ///@} + + }; + +} + +#endif /* IDYNTREE_POSITION_RAW_H */ diff --git a/src/core/include/iDynTree/PrivatePreProcessorUtils.h b/src/core/include/iDynTree/PrivatePreProcessorUtils.h new file mode 100644 index 00000000000..31595f47925 --- /dev/null +++ b/src/core/include/iDynTree/PrivatePreProcessorUtils.h @@ -0,0 +1,13 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_PRIVATE_PREPROCESSOR_UTILS_H +#define IDYNTREE_PRIVATE_PREPROCESSOR_UTILS_H + +#ifdef __PRETTY_FUNCTION__ +#define IDYNTREE_PRETTY_FUNCTION __PRETTY_FUNCTION__ +#else +#define IDYNTREE_PRETTY_FUNCTION __FUNCTION__ +#endif + +#endif /* IDYNTREE_PRIVATE_PREPROCESSOR_UTILS_H */ diff --git a/src/core/include/iDynTree/PrivateUtils.h b/src/core/include/iDynTree/PrivateUtils.h new file mode 100644 index 00000000000..f2828b88ba1 --- /dev/null +++ b/src/core/include/iDynTree/PrivateUtils.h @@ -0,0 +1,101 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_PRIVATE_UTILS_H +#define IDYNTREE_PRIVATE_UTILS_H + + +#include +#include + +namespace iDynTree +{ + + /** + * Maps a 3d vector to the square of the cross product matrix: + * v --> (v\times)^2 + * or, if you prefer another notation: + * v --> S^2(v) + */ + Eigen::Matrix3d squareCrossProductMatrix(const Eigen::Vector3d & v); + + /** + * Maps a 3d vector to the cross product matrix: + * v --> (v\times) + * or, if you prefer another notation: + * v --> S(v) + */ + Eigen::Matrix3d skew(const Eigen::Vector3d & vec); + + /** + * Efficient version of the copy from one 6D vector to another. + */ + template + void efficient6dCopy(vector6d* pthis, const vector6d& other) + { + toEigen(pthis->getLinearVec3()) = toEigen(other.getLinearVec3()); + toEigen(pthis->getAngularVec3()) = toEigen(other.getAngularVec3()); + return; + } + + /** + * Efficient version of the sum of two 6D vectors. + */ + template + vector6d efficient6dSum(const vector6d & op1, const vector6d & op2) + { + vector6d ret; + toEigen(ret.getLinearVec3()) = toEigen(op1.getLinearVec3()) + toEigen(op2.getLinearVec3()); + toEigen(ret.getAngularVec3()) = toEigen(op1.getAngularVec3()) + toEigen(op2.getAngularVec3()); + return ret; + } + + /** + * Efficient version of the different of two 6D vectors. + */ + template + vector6d efficient6ddifference(const vector6d & op1, const vector6d & op2) + { + vector6d ret; + toEigen(ret.getLinearVec3()) = toEigen(op1.getLinearVec3()) - toEigen(op2.getLinearVec3()); + toEigen(ret.getAngularVec3()) = toEigen(op1.getAngularVec3()) - toEigen(op2.getAngularVec3()); + return ret; + } + + /** + * Efficient version of the cross product between a twist + * and a spatial motion vector (another twist, acceleration, ..) + */ + template + resultType efficientTwistCrossTwist(const twistType & op1, const motionVectorType & op2) + { + // res.getLinearVec3() = this->angularVec3.cross(other.getLinearVec3()) + this->linearVec3.cross(other.getAngularVec3()); + // res.getAngularVec3() = this->angularVec3.cross(other.getAngularVec3()); + resultType ret; + + toEigen(ret.getLinearVec3()) = toEigen(op1.getAngularVec3()).cross(toEigen(op2.getLinearVec3())) + + toEigen(op1.getLinearVec3()).cross(toEigen(op2.getAngularVec3())); + toEigen(ret.getAngularVec3()) = toEigen(op1.getAngularVec3()).cross(toEigen(op2.getAngularVec3())); + + return ret; + } + + /** + * Efficient version of the cross product between a twist + * and a spatial force vector (momentum, wrench, ..) + */ + template + resultType efficientTwistCrossMomentum(const twistType & op1, const momentumVectorType & op2) + { + resultType ret; + + toEigen(ret.getLinearVec3()) = toEigen(op1.getAngularVec3()).cross(toEigen(op2.getLinearVec3())); + toEigen(ret.getAngularVec3()) = toEigen(op1.getLinearVec3()).cross(toEigen(op2.getLinearVec3())) + + toEigen(op1.getAngularVec3()).cross(toEigen(op2.getAngularVec3())); + + return ret; + } +} + + +#endif /* IDYNTREE_PRIVATE_UTILS_H */ diff --git a/src/core/include/iDynTree/Rotation.h b/src/core/include/iDynTree/Rotation.h new file mode 100644 index 00000000000..0ac0fef1dfa --- /dev/null +++ b/src/core/include/iDynTree/Rotation.h @@ -0,0 +1,473 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_ROTATION_H +#define IDYNTREE_ROTATION_H + +#include +#include +#include +#include + +namespace iDynTree +{ + class Position; + class Twist; + class SpatialAcc; + class Wrench; + class Direction; + class Axis; + class SpatialAcc; + class SpatialMomentum; + class ClassicalAcc; + class RotationalInertiaRaw; + class SpatialMotionVector; + class SpatialForceVector; + class ArticulatedBodyInertia; + + /** + * Class representation the rotation of an orientation frame + * with respect to a reference orientation frame, expressed as a Rotation matrix. + * + * \ingroup iDynTreeCore + * + * The semantics for this class is based on the OrientationCoord in: + * + * De Laet T, Bellens S, Smits R, Aertbeliën E, Bruyninckx H, and De Schutter J + * (2013), Geometric Relations between Rigid Bodies: Semantics for Standardization, + * IEEE Robotics & Automation Magazine, Vol. 20, No. 1, pp. 84-93. + * URL : http://people.mech.kuleuven.be/~tdelaet/geometric_relations_semantics/geometric_relations_semantics_theory.pdf + * + * Given that this class uses the rotation matrix to represent orientation, some operation + * are disable because there is a semantic constraint induced by choice of representation, i.e. + * that the coordinate frame is always the reference orientation frame. Thus, some semantic operation + * are not enabled, namely: + * * the generic inverse, that does not change the coordinate frame. + * * changeCoordFrame, because CoordFrame is always the same of RefOrientFrame. + */ + class Rotation: public RotationRaw + { + public: + /** + * Default constructor. + * The data is not reset to the identity matrix for perfomance reason. + * Please initialize the data in the vector before any use. + */ + Rotation(); + + /** + * Constructor from 9 doubles: initialize elements of the rotation matrix. + */ + Rotation(double xx, double xy, double xz, + double yx, double yy, double yz, + double zx, double zy, double zz); + + /** + * Copy constructor: create a Rotation from another RotationRaw. + */ + Rotation(const RotationRaw & other); + + /** + * Copy constructor: create a Rotation from another Rotation. + */ + Rotation(const Rotation & other); + + /** + * Create a Rotation from a MatrixView. + */ + Rotation(iDynTree::MatrixView other); + + /** + * Geometric operations. + * For the inverse2() operation, both the forward and the inverse geometric relations have to + * be expressed in the reference orientation frame!! + * + */ + const Rotation & changeOrientFrame(const Rotation & newOrientFrame); + const Rotation & changeRefOrientFrame(const Rotation & newRefOrientFrame); + const Rotation & changeCoordinateFrame(const Rotation& newCoordinateFrame); + static Rotation compose(const Rotation & op1, const Rotation & op2); + static Rotation inverse2(const Rotation & orient); + Position changeCoordFrameOf(const Position & other) const; + SpatialMotionVector changeCoordFrameOf(const SpatialMotionVector & other) const; + SpatialForceVector changeCoordFrameOf(const SpatialForceVector & other) const; + Twist changeCoordFrameOf(const Twist & other) const; + SpatialAcc changeCoordFrameOf(const SpatialAcc & other) const; + SpatialMomentum changeCoordFrameOf(const SpatialMomentum & other) const; + Wrench changeCoordFrameOf(const Wrench & other) const; + Direction changeCoordFrameOf(const Direction & other) const; + Axis changeCoordFrameOf(const Axis & other) const; + ClassicalAcc changeCoordFrameOf(const ClassicalAcc & other) const; + RotationalInertiaRaw changeCoordFrameOf(const RotationalInertiaRaw & other) const; + + + /** + * overloaded operators + */ + Rotation operator*(const Rotation & other) const; + Rotation inverse() const; + Position operator*(const Position & other) const; + SpatialForceVector operator*(const SpatialForceVector & other) const; + Twist operator*(const Twist & other) const; + Wrench operator*(const Wrench & other) const; + Direction operator*(const Direction & other) const; + Axis operator*(const Axis & other) const; + SpatialAcc operator*(const SpatialAcc & other) const; + SpatialMomentum operator*(const SpatialMomentum & other) const; + ClassicalAcc operator*(const ClassicalAcc & other) const; + RotationalInertiaRaw operator*(const RotationalInertiaRaw & other) const; + + /** + * Log mapping between a generic element of SO(3) (iDynTree::Rotation) + * to the corresponding element of so(3) (iDynTree::AngularMotionVector). + */ + AngularMotionVector3 log() const; + + /** + * Set the rotation matrix as the passed rotation expressed in quaternion + * + * @note the quaternion is expressed as (real, imaginary) part with + * real \f$\in \mathbb{R}\f$ and imaginary \f$\in \mathbb{R}^3\f$ + * @note the quaternion is normalized + * @param quaternion the rotation expressed in quaternion + */ + void fromQuaternion(const iDynTree::Vector4& quaternion); + + + /** + * @name Conversion to others represention of matrices. + * + */ + ///@{ + + /** + * Get a roll, pitch and yaw corresponding to this rotation. + * + * Get \f$ (r,p,y) \in ( (-\pi, \pi] \times (-\frac{\pi}{2}, \frac{\pi}{2}) \times (-\pi, \pi] ) \cup ( \{0\} \times \{-\frac{\pi}{2}\} \times (-\pi,\pi] ) \cup ( \{0\} \times \{\frac{\pi}{2}\} \times [-\pi,\pi) )\f$ + * such that + * *this == RotZ(y)*RotY(p)*RotX(r) + * + * @param[out] r roll rotation angle + * @param[out] p pitch rotation angle + * @param[out] y yaw rotation angle + */ + void getRPY(double & r, double & p, double &y) const; + + /** + * Get a roll, pitch and yaw corresponding to this rotation, + * as for getRPY, but return a vector with the output + * parameters. This function is more suitable for bindings. + * + * @return the output vector with the r, p and y parameters. + */ + iDynTree::Vector3 asRPY() const; + + /** + * Get a unit quaternion corresponding to this rotation + * + * The quaternion is defined as [s, r] + * where \f$s \in \mathbb{R}\f$ is the real and + * \f$r \in \mathbb{R}^3\f$ is the imaginary part. + * + * The returned quaternion is such that *this is + * equal to RotationFromQuaternion(quaternion). + * + * \note For each rotation, there are two quaternion + * corresponding to it. In this method we return + * the one that has the first non-zero (with a tolerance of 1e-7) + * component positive. If the real part is non-zero, this + * mean that we return the quaternion with positive real part. + * + * @param[out] quaternion the output quaternion + */ + bool getQuaternion(iDynTree::Vector4& quaternion) const; + + /** + * Get a unit quaternion corresponding to this rotation + * + * The unit quaternion is defined as [s, r] + * where \f$s \in \mathbb{R}\f$ is the real and + * \f$r \in \mathbb{R}^3\f$ is the imaginary part. + * + * The returned quaternion is such that *this is + * equal to RotationFromQuaternion(quaternion). + * + * \note For each rotation, there are two quaternion + * corresponding to it. In this method we return + * the one that has the first non-zero (with a tolerance of 1e-7) + * component positive. If the real part is non-zero, this + * mean that we return the quaternion with positive real part. + * + * @param[out] s the real part + * @param[out] r1 the first component of the imaginary part (i.e. i base) + * @param[out] r2 the second component of the imaginary part (i.e. j base) + * @param[out] r3 the third component of the imaginary part (i.e. k base) + */ + bool getQuaternion(double &s, double &r1, double &r2, double &r3) const; + + /** + * Get a unit quaternion corresponding to this rotation + * + * The quaternion is defined as [s, r] + * where \f$s \in \mathbb{R}\f$ is the costituent and + * \f$r \in \mathbb{R}^3\f$ is the imaginary part. + * + * The returned quaternion is such that *this is + * equal to RotationFromQuaternion(quaternion). + * + * \note For each rotation, there are two quaternion + * corresponding to it. In this method we return + * the one that has the first non-zero (with a tolerance of 1e-7) + * component positive. If the real part is non-zero, this + * mean that we return the quaternion with positive real part. + * + * @return the output quaternion + */ + iDynTree::Vector4 asQuaternion() const; + + ///@} + + /** + * @name Initialization helpers. + * + */ + ///@{ + + /** + * Return a Rotation around axis X of given angle + * + * If \f$ \theta \f$ is the input angle, this function + * returns the \f$ R_x(\theta) \f$ rotation matrix such that : + * \f[ + * R_x(\theta) = + * \begin{bmatrix} + * 1 & 0 & 0 \\ + * 0 & \cos(\theta) & - \sin(\theta) \\ + * 0 & \sin(\theta) & \cos(\theta) \\ + * \end{bmatrix} + * \f] + * + * @param angle the angle (in Radians) of the rotation arount the X axis + */ + static Rotation RotX(const double angle); + + /** + * Return a Rotation around axis Y of given angle + * + * If \f$ \theta \f$ is the input angle, this function + * returns the \f$ R_y(\theta) \f$ rotation matrix such that : + * \f[ + * R_y(\theta) = + * \begin{bmatrix} + * \cos(\theta) & 0 & \sin(\theta) \\ + * 0 & 1 & 0 \\ + * -\sin(\theta) & 0 & \cos(\theta) \\ + * \end{bmatrix} + * \f] + * + * + * @param angle the angle (in Radians) of the rotation arount the Y axis + */ + static Rotation RotY(const double angle); + + /** + * Return a Rotation around axis Z of given angle + * + * If \f$ \theta \f$ is the input angle, this function + * returns the \f$ R_z(\theta) \f$ rotation matrix such that : + * \f[ + * R_z(\theta) = + * \begin{bmatrix} + * \cos(\theta) & -\sin(\theta) & 0 \\ + * \sin(\theta) & \cos(\theta) & 0 \\ + * 0 & 0 & 1 \\ + * \end{bmatrix} + * \f] + * + * + * @param angle the angle (in Radians) of the rotation arount the Z axis + */ + static Rotation RotZ(const double angle); + + /** + * Return a Rotation around axis given by direction of given angle + * + * If we indicate with \f$ d \in \mathbb{R}^3 \f$ the unit norm + * of the direction, and with \f$ \theta \f$ the input angle, the return rotation + * matrix \f$ R \f$ can be computed using the Rodrigues' rotation formula [1] : + * \f[ + * R = I_{3\times3} + d^{\wedge} \sin(\theta) + {d^{\wedge}}^2 (1-\cos(\theta)) + * \f] + * + * [1] : http://mathworld.wolfram.com/RodriguesRotationFormula.html + * @param direction the Direction around with to rotate + * @param angle the angle (in Radians) of the rotation arount the given axis + */ + static Rotation RotAxis(const Direction & direction, const double angle); + + /** + * Return the derivative of the RotAxis function with respect to the angle argument. + * + * If we indicate with \f$ d \in \mathbb{R}^3 \f$ the unit norm + * of the direction, and with \f$ \theta \f$ the input angle, the derivative of the rotation + * matrix \f$ \frac{\partial R}{\partial \theta} \f$ can be computed using the + * derivative of the Rodrigues' rotation formula [1] : + * \f[ + * \frac{\partial R}{\partial \theta} = d^{\vee} \cos(\theta) + {d^{\vee}}^2 \sin(\theta) + * \f] + * + * [1] : http://mathworld.wolfram.com/RodriguesRotationFormula.html + * + * @param direction the Direction around with to rotate + * @param angle the angle (in Radians) of the rotation arount the given axis + */ + static Matrix3x3 RotAxisDerivative(const Direction & direction, const double angle); + + /** + * Return a rotation object given Roll, Pitch and Yaw values. + * + * @note This is equivalent to RotZ(y)*RotY(p)*RotX(r) . + * @note This method is compatible with the KDL::Rotation::RPY method. + */ + static Rotation RPY(const double roll, const double pitch, const double yaw); + + /** + * Return the right-trivialized derivative of the RPY function. + * + * If we indicate with \f$ rpy \in \mathbb{R}^3 \f$ the roll pitch yaw vector, + * and with \f$ RPY(rpy) : \mathbb{R}^3 \mapsto SO(3) \f$ the function implemented + * in the Rotation::RPY method, this method returns the right-trivialized partial + * derivative of Rotation::RPY, i.e. : + * \f[ + * (RPY(rpy) \frac{\partial RPY(rpy)}{\partial rpy})^\vee + * \f] + */ + static Matrix3x3 RPYRightTrivializedDerivative(const double roll, const double pitch, const double yaw); + + /** + * Return the rate of change of the right-trivialized derivative of the RPY function. + * + * If we indicate with \f$ rpy \in \mathbb{R}^3 \f$ the roll pitch yaw vector, + * and with \f$ RPY(rpy) : \mathbb{R}^3 \mapsto SO(3) \f$ the function implemented + * in the Rotation::RPY method, this method returns the right-trivialized partial + * derivative of Rotation::RPY, i.e. : + * \f[ + * (RPY(rpy) \frac{d}{d t}\frac{\partial RPY(rpy)}{\partial rpy})^\vee + * \f] + */ + static Matrix3x3 RPYRightTrivializedDerivativeRateOfChange(const double roll, const double pitch, const double yaw, const double rollDot, const double pitchDot, const double yawDot); + + /** + * Return the inverse of the right-trivialized derivative of the RPY function. + * + * See RPYRightTrivializedDerivative for a detailed description of the method. + * + */ + static Matrix3x3 RPYRightTrivializedDerivativeInverse(const double roll, const double pitch, const double yaw); + + /** + * Return the rate of change of the inverse of the right-trivialized derivative of the RPY function. + * + * See RPYRightTrivializedDerivativeRateOfChange for a detailed description of the method. + * + */ + static Matrix3x3 RPYRightTrivializedDerivativeInverseRateOfChange(const double roll, const double pitch, const double yaw, const double rollDot, const double pitchDot, const double yawDot); + + /** + * Return the right-trivialized derivative of the Quaternion function. + * + * If we indicate with \f$ quat \in \mathbb{Q} \f$ the quaternion, + * and with \f$ QUAT(quat) : \mathbb{Q} \mapsto SO(3) \f$ the function implemented + * in the Rotation::RotationFromQuaternion method, this method returns the right-trivialized partial + * derivative of Rotation::RotationFromQuaternion, i.e. : + * \f[ + * (QUAT(quat) \frac{\partial QUAT(quat)}{\partial quat})^\vee + * \f] + */ + static MatrixFixSize<4, 3> QuaternionRightTrivializedDerivative(Vector4 quaternion); + + /** + * Return the inverse of the right-trivialized derivative of the Quaternion function. + * + * @see QuaternionRightTrivializedDerivative for a detailed description of the method. + * + */ + static MatrixFixSize<3, 4> QuaternionRightTrivializedDerivativeInverse(Vector4 quaternion); + + + /** + * Return an identity rotation. + * + * + */ + static Rotation Identity(); + + /** + * Construct a rotation matrix from the given unit quaternion representation + * + * The quaternion is expected to be ordered in the following way: + * - \f$s \in \mathbb{R}\f$ the real part of the quaterion + * - \f$r \in \mathbb{R}^3\f$ the imaginary part of the quaternion + * + * The returned rotation matrix is given by the following formula: + * \f[ + * R(s,r) = I_{3\times3} + 2s r^{\wedge} + 2{r^\wedge}^2, + * \f] + * where \f$ r^{\wedge} \f$ is the skew-symmetric matrix such that: + * \f[ + * r \times v = r^\wedge v + * \f] + * + * @note the quaternion is normalized + * @param quaternion a quaternion representing a rotation + * + * @return The rotation matrix + */ + static Rotation RotationFromQuaternion(const iDynTree::Vector4& quaternion); + + /** + * Get the left Jacobian of rotation matrix + * + * \f$ \omega \in \mathbb{R}^3 \f$ is the angular motion vector + * \f$ [\omega_\times]: \mathbb{R}^n \to \mathfrak{so}(3) \f$ where \f$ \mathfrak{so}(3) \f$ + * is the set of skew symmetric matrices or the Lie algebra of \f$ SO(3) \f$ + * \f[ J_{l_{SO(3)}} = \sum_{n = 0}^{\infty} \frac{1}{(n+1)!} [\omega_\times]^n = (I_3 + \frac{1 - \text{cos}(||\omega||)}{||\omega||^{2}} [\omega _{\times}] + \frac{||\omega|| - \text{sin}(||\omega||)}{||\omega||^{3}} [\omega _{\times}]^{2} \f] + * + * When simplified further, + * \f[ J_{l_{SO(3)}} = \frac{\text{sin}(||\omega||)}{||\omega||}I_3 + \frac{1 - \text{cos}(||\omega||)}{||\omega||} [\phi _{\times}] + \bigg(1 - \frac{\text{sin}(||\omega||)}{||\omega||}\bigg) \phi\phi^T \f] + * + * where \f$ \phi = \frac{\omega}{||\omega||} \f$ + * + * @param[in] omega angular motion vector + * @return \f$ 3 \times 3 \f$ left Jacobian matrix + */ + static Matrix3x3 leftJacobian(const iDynTree::AngularMotionVector3& omega); + + /** + * Get the left Jacobian inverse of rotation matrix + * + * \f$ \omega \in \mathbb{R}^3 \f$ is the angular motion vector + * \f$ [\omega_\times]: \mathbb{R}^n \to \mathfrak{so}(3) \f$ where \f$ \mathfrak{so}(3) \f$ + * is the set of skew symmetric matrices or the Lie algebra of \f$ SO(3) \f$ + * \f[ J^{-1} _{l _{SO(3)}} = \frac{||\omega||}{2} \text{cot} \bigg(\frac{||\omega||}{2}\bigg) I _3 + \bigg( 1 - \frac{||\omega||}{2} \text{cot} \bigg(\frac{||\omega||}{2}\bigg) \bigg) \phi \phi^T - \frac{||\omega||}{2} [\phi _{\times}] \f] + * + * where \f$ \phi = \frac{\omega}{||\omega||} \f$ + * + * @param[in] omega angular motion vector + * @return \f$ 3 \times 3 \f$ left Jacobian inverse matrix + */ + static Matrix3x3 leftJacobianInverse(const iDynTree::AngularMotionVector3& omega); + ///@} + + /** @name Output helpers. + * Output helpers. + */ + ///@{ + std::string toString() const; + + std::string reservedToString() const; + ///@} + }; +} + +#endif diff --git a/src/core/include/iDynTree/RotationRaw.h b/src/core/include/iDynTree/RotationRaw.h new file mode 100644 index 00000000000..e7fb9ae3824 --- /dev/null +++ b/src/core/include/iDynTree/RotationRaw.h @@ -0,0 +1,140 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_ROTATION_RAW_H +#define IDYNTREE_ROTATION_RAW_H + +#include +#include + +namespace iDynTree +{ + class PositionRaw; + class SpatialMotionVector; + class SpatialForceVector; + class ClassicalAcc; + class RotationalInertiaRaw; + template + class MatrixView; + + /** + * Class providing the raw coordinates for iDynTree::Rotation class. + * + * Storage for the Orientation: + * The rotation matrix representation of the orientation, stored in row major order, + * inside a Matrix3x3 parent object. + * + * \note This implementation is compatible with KDL::Rotation data. + * + * \warning This class uses for convenience the Matrix3x3 as a public parent. + * Notice that using this methods you can damage the underlyng rotation matrix. + * In doubt, don't use them and rely on more high level functions. + * + * \ingroup iDynTreeCore + */ + class RotationRaw: public Matrix3x3 + { + public: + /** + * Default constructor. + * The data is not reset to identity for perfomance reason. + * Please initialize the data in the vector before any use. + */ + RotationRaw(); + + /** + * Constructor from 9 doubles: initialize elements of the rotation matrix. + */ + RotationRaw(double xx, double xy, double xz, + double yx, double yy, double yz, + double zx, double zy, double zz); + + /** + * Constructor from a buffer of 9 doubles, + * stored as a C-style array (i.e. row major). + * + */ + RotationRaw(const double* in_data, + const unsigned int in_rows, + const unsigned int in_cols); + + RotationRaw(iDynTree::MatrixView other); + + /** + * Copy constructor: create a RotationRaw from another RotationRaw. + */ + RotationRaw(const RotationRaw & other); + + /** + * Geometric operations. + */ + const RotationRaw & changeOrientFrame(const RotationRaw & newOrientFrame); + const RotationRaw & changeRefOrientFrame(const RotationRaw & newRefOrientFrame); + static RotationRaw compose(const RotationRaw & op1, const RotationRaw & op2); + static RotationRaw inverse2(const RotationRaw & orient); + PositionRaw changeCoordFrameOf(const PositionRaw & other) const; + ClassicalAcc changeCoordFrameOf(const ClassicalAcc & other) const; + RotationalInertiaRaw changeCoordFrameOf(const RotationalInertiaRaw & other) const; + + + + /** + * overloaded operators + */ + + /** + * @name Initialization helpers. + * + */ + ///@{ + + /** + * Return a Rotation around axis X of given angle + * + * @param angle the angle (in Radians) of the rotation arount the X axis + */ + static RotationRaw RotX(const double angle); + + /** + * Return a Rotation around axis Y of given angle + * + * @param angle the angle (in Radians) of the rotation arount the Y axis + */ + static RotationRaw RotY(const double angle); + + /** + * Return a Rotation around axis Z of given angle + * + * @param angle the angle (in Radians) of the rotation arount the Z axis + */ + static RotationRaw RotZ(const double angle); + + /** + * Return a rotation object given Roll, Pitch and Yaw values. + * + * @note This method is compatible with the KDL::Rotation::RPY method. + */ + static RotationRaw RPY(const double roll, const double pitch, const double yaw); + + /** + * Return an identity rotation. + * + * + */ + static RotationRaw Identity(); + + ///@} + + + /** @name Output helpers. + * Output helpers. + */ + ///@{ + std::string toString() const; + + std::string reservedToString() const; + ///@} + }; +} + +#endif diff --git a/src/core/include/iDynTree/RotationalInertiaRaw.h b/src/core/include/iDynTree/RotationalInertiaRaw.h new file mode 100644 index 00000000000..7f96b28f1a5 --- /dev/null +++ b/src/core/include/iDynTree/RotationalInertiaRaw.h @@ -0,0 +1,47 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_ROTATIONAL_INERTIA_RAW_H +#define IDYNTREE_ROTATIONAL_INERTIA_RAW_H + +#include + +namespace iDynTree +{ + class PositionRaw; + + /** + * Class providing the raw coordinates for a 3d inertia matrix. + * + * \ingroup iDynTreeCore + * + * \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. + * + * We use a parent Matrix3x3 for storage of the rotational inertia matrix: + * given that the inertia matrix is a 3x3 symmetric matrix, + * the ordering (row order or column order) is not influencing + * the storage of the matrix. + */ + class RotationalInertiaRaw: public Matrix3x3 + { + + public: + /** + * Default constructor. + * The data is not reset to zero for perfomance reason. + * Please initialize the data in the vector before any use. + */ + RotationalInertiaRaw(); + RotationalInertiaRaw(const double * in_data, const unsigned int in_rows, const unsigned int in_cols); + RotationalInertiaRaw(const RotationalInertiaRaw & other); + + /** + * Initializer helper: return a zero matrix. + */ + static RotationalInertiaRaw Zero(); + + }; +} + +#endif /* IDYNTREE_ROTATIONAL_INERTIA_RAW_H */ diff --git a/src/core/include/iDynTree/SO3Utils.h b/src/core/include/iDynTree/SO3Utils.h new file mode 100644 index 00000000000..01af7e48df3 --- /dev/null +++ b/src/core/include/iDynTree/SO3Utils.h @@ -0,0 +1,80 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + + +#ifndef IDYNTREE_SO3UTILS_H +#define IDYNTREE_SO3UTILS_H + +#include +#include +#include + +namespace iDynTree +{ + /** + * @brief Struct containing the options for geodesicL2MeanRotation and geodesicL2WeightedMeanRotation. + */ + struct GeodesicL2MeanOptions + { + double tolerance{1e-5}; /** Tolerance for terminating the inner refinement loop of the mean rotation. **/ + double timeoutInSeconds{-1}; /** Timeout for the refinement loop. **/ + int maxIterations{-1}; /** Max number of iterations for the refinement loop. **/ + bool verbose{false}; /** Add a message when the solution is found. **/ + double stepSize{1.0}; /** Step-size for the refinement loop. **/ + }; + + /** + * @brief Check if the rotation matrix is valid. + * + * It checks that the determinant is 1, that the Frobenius norm is finite and that it is orthogonal. + * @param r The input rotation + * @return True if it is a rotation matrix. + */ + bool isValidRotationMatrix(const iDynTree::Rotation& r); + + /** + * @brief Computes the geodesic distance between two rotation matrices. + * + * It implements the angular distance presented in Sec. 4 of "Rotation Averaging" (available at http://users.cecs.anu.edu.au/~hongdong/rotationaveraging.pdf), + * in particular \f$d = ||log(R_1^\top R_2)||^2 \f$. + * @param rotation1 The first rotation. + * @param rotation2 The other rotation. + * @return the geodesic L2 distance between the two rotation matrices. + */ + double geodesicL2Distance(const iDynTree::Rotation& rotation1, const iDynTree::Rotation& rotation2); + + /** + * @brief Computes the geodesic mean amongst the provided rotations. + * + * It implements Algorithm 1 in Sec. 5.3 of "Rotation Averaging" (available at http://users.cecs.anu.edu.au/~hongdong/rotationaveraging.pdf). + * + * Inside it calls geodesicL2WeightedMeanRotation. + * + * @param inputRotations The rotations to average. + * @param meanRotation The mean rotation. + * @param options The options for the inner optimization (refinement) loop. + * @return false in case of failure, true otherwise. + */ + bool geodesicL2MeanRotation(const std::vector& inputRotations, + iDynTree::Rotation& meanRotation, + const GeodesicL2MeanOptions& options = GeodesicL2MeanOptions()); + + /** + * @brief Computes the weighted geodesic mean amongst the provided rotations + * + * It implements Algorithm 1 in Sec. 5.3 of "Rotation Averaging" (available at http://users.cecs.anu.edu.au/~hongdong/rotationaveraging.pdf), + * with a small modification to take into accounts weights different from 1. + * + * @param inputRotations The rotations to average. + * @param weights The weights for each rotation. If this vector is null assumes that each weight is 1.0 (equivalent to geodesicL2MeanRotation) + * @param meanRotation The weighted mean rotation. + * @param options The options for the inner optimization (refinement) loop. + * @return false in case of failure, true otherwise. + */ + bool geodesicL2WeightedMeanRotation(const std::vector& inputRotations, + const std::vector& weights, + iDynTree::Rotation& meanRotation, + const GeodesicL2MeanOptions& options = GeodesicL2MeanOptions()); +} + +#endif // IDYNTREE_SO3UTILS_H diff --git a/src/core/include/iDynTree/Span.h b/src/core/include/iDynTree/Span.h new file mode 100644 index 00000000000..fe8a76261c9 --- /dev/null +++ b/src/core/include/iDynTree/Span.h @@ -0,0 +1,777 @@ +#ifndef IDYNTREE_SPAN_H +#define IDYNTREE_SPAN_H +/////////////////////////////////////////////////////////////////////////////// +// +// Copyright (c) 2015 Microsoft Corporation. All rights reserved. +// +// This code is licensed under the MIT License (MIT). +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. +// +/////////////////////////////////////////////////////////////////////////////// + +//Most of this file has been taken from https://github.com/Microsoft/GSL/blob/master/include/gsl/span + +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + + +#include + +#include // for lexicographical_compare +#include // for array +#include // for ptrdiff_t, size_t, nullptr_t +#include // for reverse_iterator, distance, random_access_... +#include +#include +#include // for enable_if_t, declval, is_convertible, inte... +#include + +#ifdef _MSC_VER +#pragma warning(push) + +// turn off some warnings that are noisy about our Expects statements +#pragma warning(disable : 4127) // conditional expression is constant +#pragma warning(disable : 4702) // unreachable code + +// blanket turn off warnings from CppCoreCheck for now +// so people aren't annoyed by them when running the tool. +#pragma warning(disable : 26481 26482 26483 26485 26490 26491 26492 26493 26495) + +#if _MSC_VER < 1910 +#pragma push_macro("constexpr") +#define constexpr /*constexpr*/ +#define IDYNTREE_USE_STATIC_CONSTEXPR_WORKAROUND + +#endif // _MSC_VER < 1910 +#else // _MSC_VER + +// See if we have enough C++17 power to use a static constexpr data member +// without needing an out-of-line definition +#if !(defined(__cplusplus) && (__cplusplus >= 201703L)) +#define IDYNTREE_USE_STATIC_CONSTEXPR_WORKAROUND +#endif // !(defined(__cplusplus) && (__cplusplus >= 201703L)) + +#endif // _MSC_VER + +// constexpr workaround for SWIG +#ifdef SWIG +#define IDYNTREE_CONSTEXPR +#else +#define IDYNTREE_CONSTEXPR constexpr +#endif + +namespace iDynTree +{ + +namespace SpanUtils { +#ifndef SWIG +// this is required to be compatible with c++17 +template struct make_void { typedef void type; }; +template using void_t = typename make_void::type; + +//Small utility to detect if type T has value_type defined +template struct is_value_defined : std::false_type +{ +}; + +template +struct is_value_defined> : std::true_type +{ +}; + +//Small utility to detect if type T has element_type defined +template struct is_element_defined : std::false_type +{ +}; + +template +struct is_element_defined> : std::true_type +{ +}; + +//Small utility to detect if class T has the data() method defined +template struct has_data_method : std::false_type +{ +}; + +template +struct has_data_method().data())>> : std::true_type +{ +}; + +//Small utility to detect if class T has the size() method defined +template struct has_size_method : std::false_type +{ +}; + +template +struct has_size_method().size())>> : std::true_type +{ +}; + +#endif +} + +// [views.constants], constants +IDYNTREE_CONSTEXPR const std::ptrdiff_t dynamic_extent = -1; + +template +class Span; + +// implementation details +namespace details +{ + template + struct is_span_oracle : std::false_type + { + }; + + template + struct is_span_oracle> : std::true_type + { + }; + + template + struct is_span : public is_span_oracle> + { + }; + + template + struct is_std_array_oracle : std::false_type + { + }; + + template + struct is_std_array_oracle> : std::true_type + { + }; + + template + struct is_std_array : public is_std_array_oracle> + { + }; + + template + struct is_allowed_extent_conversion + : public std::integral_constant + { + }; + + template + struct is_allowed_element_type_conversion + : public std::integral_constant::value> + { + }; + + template + class span_iterator + { + using element_type_ = typename Span::element_type; + + public: + + using iterator_category = std::random_access_iterator_tag; + using value_type = std::remove_cv_t; + using difference_type = typename Span::index_type; + + using reference = std::conditional_t&; + using pointer = std::add_pointer_t; + + span_iterator() = default; + + IDYNTREE_CONSTEXPR span_iterator(const Span* span, typename Span::index_type idx) noexcept + : span_(span), index_(idx) + {} + + friend span_iterator; + template* = nullptr> + IDYNTREE_CONSTEXPR span_iterator(const span_iterator& other) noexcept + : span_iterator(other.span_, other.index_) + { + } + + IDYNTREE_CONSTEXPR reference operator*() const + { + assert(index_ != span_->size()); + return *(span_->data() + index_); + } + + IDYNTREE_CONSTEXPR pointer operator->() const + { + assert(index_ != span_->size()); + return span_->data() + index_; + } + + IDYNTREE_CONSTEXPR span_iterator& operator++() + { + assert(0 <= index_ && index_ != span_->size()); + ++index_; + return *this; + } + + IDYNTREE_CONSTEXPR span_iterator operator++(int) + { + auto ret = *this; + ++(*this); + return ret; + } + + IDYNTREE_CONSTEXPR span_iterator& operator--() + { + assert(index_ != 0 && index_ <= span_->size()); + --index_; + return *this; + } + + IDYNTREE_CONSTEXPR span_iterator operator--(int) + { + auto ret = *this; + --(*this); + return ret; + } + + IDYNTREE_CONSTEXPR span_iterator operator+(difference_type n) const + { + auto ret = *this; + return ret += n; + } + + IDYNTREE_CONSTEXPR span_iterator& operator+=(difference_type n) + { + assert((index_ + n) >= 0 && (index_ + n) <= span_->size()); + index_ += n; + return *this; + } + + IDYNTREE_CONSTEXPR span_iterator operator-(difference_type n) const + { + auto ret = *this; + return ret -= n; + } + + IDYNTREE_CONSTEXPR span_iterator& operator-=(difference_type n) { return *this += -n; } + + IDYNTREE_CONSTEXPR difference_type operator-(span_iterator rhs) const + { + assert(span_ == rhs.span_); + return index_ - rhs.index_; + } + + IDYNTREE_CONSTEXPR reference operator[](difference_type n) const + { + return *(*this + n); + } + + IDYNTREE_CONSTEXPR friend bool operator==(span_iterator lhs, + span_iterator rhs) noexcept + { + return lhs.span_ == rhs.span_ && lhs.index_ == rhs.index_; + } + + IDYNTREE_CONSTEXPR friend bool operator!=(span_iterator lhs, + span_iterator rhs) noexcept + { + return !(lhs == rhs); + } + + IDYNTREE_CONSTEXPR friend bool operator<(span_iterator lhs, + span_iterator rhs) noexcept + { + return lhs.index_ < rhs.index_; + } + + IDYNTREE_CONSTEXPR friend bool operator<=(span_iterator lhs, + span_iterator rhs) noexcept + { + return !(rhs < lhs); + } + + IDYNTREE_CONSTEXPR friend bool operator>(span_iterator lhs, + span_iterator rhs) noexcept + { + return rhs < lhs; + } + + IDYNTREE_CONSTEXPR friend bool operator>=(span_iterator lhs, + span_iterator rhs) noexcept + { + return !(rhs > lhs); + } + + protected: + const Span* span_ = nullptr; + std::ptrdiff_t index_ = 0; + }; + + template + IDYNTREE_CONSTEXPR span_iterator + operator+(typename span_iterator::difference_type n, + span_iterator rhs) + { + return rhs + n; + } + + template + IDYNTREE_CONSTEXPR span_iterator + operator-(typename span_iterator::difference_type n, + span_iterator rhs) + { + return rhs - n; + } + + template + class extent_type + { + public: + using index_type = std::ptrdiff_t; + + static_assert(Ext >= 0, "A fixed-size span must be >= 0 in size."); + + IDYNTREE_CONSTEXPR extent_type() noexcept {} + + template + IDYNTREE_CONSTEXPR extent_type(extent_type ext) + { + static_assert(Other == Ext || Other == dynamic_extent, + "Mismatch between fixed-size extent and size of initializing data."); + assert(ext.size() == Ext); + } + + IDYNTREE_CONSTEXPR extent_type(index_type size) { assert(size == Ext); } + + IDYNTREE_CONSTEXPR index_type size() const noexcept { return Ext; } + }; + + template <> + class extent_type + { + public: + using index_type = std::ptrdiff_t; + + template + explicit IDYNTREE_CONSTEXPR extent_type(extent_type ext) : size_(ext.size()) + { + } + + explicit IDYNTREE_CONSTEXPR extent_type(index_type size) : size_(size) { assert(size >= 0); } + + IDYNTREE_CONSTEXPR index_type size() const noexcept { return size_; } + + private: + index_type size_; + }; + + template + struct calculate_subspan_type + { + using type = Span; + }; +} // namespace details + +// [span], class template span +template +class Span +{ +public: + // constants and types + using element_type = ElementType; + using value_type = std::remove_cv_t; + using index_type = std::ptrdiff_t; + using pointer = element_type*; + using reference = element_type&; + using const_reference = const element_type&; + + using iterator = details::span_iterator, false>; + using const_iterator = details::span_iterator, true>; + using reverse_iterator = std::reverse_iterator; + using const_reverse_iterator = std::reverse_iterator; + + using size_type = index_type; + +#if defined(IDYNTREE_USE_STATIC_CONSTEXPR_WORKAROUND) + static constexpr const index_type extent { Extent }; +#else + static constexpr index_type extent { Extent }; +#endif + +#ifndef SWIG + // [span.cons], span constructors, copy, assignment, and destructor + template " SFINAE, + // since "std::enable_if_t" is ill-formed when Extent is greater than 0. + class = std::enable_if_t<(Dependent || Extent <= 0)>> + IDYNTREE_CONSTEXPR Span() noexcept : storage_(nullptr, details::extent_type<0>()) + { + } +#endif + + IDYNTREE_CONSTEXPR Span(pointer ptr, index_type count) : storage_(ptr, count) {} + + IDYNTREE_CONSTEXPR Span(pointer firstElem, pointer lastElem) + : storage_(firstElem, std::distance(firstElem, lastElem)) + { + } + + template + IDYNTREE_CONSTEXPR Span(element_type (&arr)[N]) noexcept + : storage_(KnownNotNull{&arr[0]}, details::extent_type()) + { + } + + template > + IDYNTREE_CONSTEXPR Span(std::array& arr) noexcept + : storage_(&arr[0], details::extent_type()) + { + } + + template + IDYNTREE_CONSTEXPR Span(const std::array, N>& arr) noexcept + : storage_(&arr[0], details::extent_type()) + { + } + + // NB: the SFINAE here uses .data() as a incomplete/imperfect proxy for the requirement + // on Container to be a contiguous sequence container. +#ifndef SWIG + template ::value && SpanUtils::has_size_method::value>, + class = std::enable_if_t< + !details::is_span::value && !details::is_std_array::value && + std::is_convertible().data()), pointer>::value>> + IDYNTREE_CONSTEXPR Span(Container& cont) : Span(cont.data(), static_cast(cont.size())) + { + } + + template ::value && SpanUtils::has_size_method::value>, + class = std::enable_if_t< + std::is_const::value && !details::is_span::value && + std::is_convertible().data()), pointer>::value>> + IDYNTREE_CONSTEXPR Span(const Container& cont) : Span(cont.data(), static_cast(cont.size())) + { + } +#endif + + IDYNTREE_CONSTEXPR Span(const Span& other) noexcept = default; + +#ifndef SWIG + template < + class OtherElementType, std::ptrdiff_t OtherExtent, + class = std::enable_if_t< + details::is_allowed_extent_conversion::value && + details::is_allowed_element_type_conversion::value>> + IDYNTREE_CONSTEXPR Span(const Span& other) + : storage_(other.data(), details::extent_type(other.size())) + { + } +#endif + + ~Span() noexcept = default; + IDYNTREE_CONSTEXPR Span& operator=(const Span& other) noexcept = default; + + // [span.sub], span subviews + template + IDYNTREE_CONSTEXPR Span first() const + { + assert(Count >= 0 && Count <= size()); + return {data(), Count}; + } + + template + IDYNTREE_CONSTEXPR Span last() const + { + assert(Count >= 0 && size() - Count >= 0); + return {data() + (size() - Count), Count}; + } + +#ifndef SWIG + template + IDYNTREE_CONSTEXPR auto subspan() const -> typename details::calculate_subspan_type::type + { + assert((Offset >= 0 && size() - Offset >= 0) && + (Count == dynamic_extent || (Count >= 0 && Offset + Count <= size()))); + + return {data() + Offset, Count == dynamic_extent ? size() - Offset : Count}; + } +#endif + + IDYNTREE_CONSTEXPR Span first(index_type count) const + { + assert(count >= 0 && count <= size()); + return {data(), count}; + } + + IDYNTREE_CONSTEXPR Span last(index_type count) const + { + return make_subspan(size() - count, dynamic_extent, subspan_selector{}); + } + + IDYNTREE_CONSTEXPR Span subspan(index_type offset, + index_type count = dynamic_extent) const + { + return make_subspan(offset, count, subspan_selector{}); + } + + + // [span.obs], span observers + IDYNTREE_CONSTEXPR index_type size() const noexcept { return storage_.size(); } + IDYNTREE_CONSTEXPR index_type size_bytes() const noexcept + { + return size() * static_cast(sizeof(element_type)); + } + IDYNTREE_CONSTEXPR bool empty() const noexcept { return size() == 0; } + + // [span.elem], span element access + IDYNTREE_CONSTEXPR reference operator[](index_type idx) const + { + assert(idx >= 0 && idx < storage_.size()); + return data()[idx]; + } + + IDYNTREE_CONSTEXPR const_reference getVal(index_type idx) const { return this->operator[](idx);} + IDYNTREE_CONSTEXPR bool setVal(index_type idx, const_reference val) + { + assert(idx >= 0 && idx < storage_.size()); + data()[idx] = val; + return true; + } + + IDYNTREE_CONSTEXPR reference at(index_type idx) const { return this->operator[](idx); } + IDYNTREE_CONSTEXPR reference operator()(index_type idx) const { return this->operator[](idx); } + IDYNTREE_CONSTEXPR pointer data() const noexcept { return storage_.data(); } + + // [span.iter], span iterator support + IDYNTREE_CONSTEXPR iterator begin() const noexcept { return {this, 0}; } + IDYNTREE_CONSTEXPR iterator end() const noexcept { return {this, size()}; } + + IDYNTREE_CONSTEXPR const_iterator cbegin() const noexcept { return {this, 0}; } + IDYNTREE_CONSTEXPR const_iterator cend() const noexcept { return {this, size()}; } + + IDYNTREE_CONSTEXPR reverse_iterator rbegin() const noexcept { return reverse_iterator{end()}; } + IDYNTREE_CONSTEXPR reverse_iterator rend() const noexcept { return reverse_iterator{begin()}; } + + IDYNTREE_CONSTEXPR const_reverse_iterator crbegin() const noexcept { return const_reverse_iterator{cend()}; } + IDYNTREE_CONSTEXPR const_reverse_iterator crend() const noexcept { return const_reverse_iterator{cbegin()}; } + +private: + + // Needed to remove unnecessary null check in subspans + struct KnownNotNull + { + pointer p; + }; + + // this implementation detail class lets us take advantage of the + // empty base class optimization to pay for only storage of a single + // pointer in the case of fixed-size spans + template + class storage_type : public ExtentType + { + public: + // KnownNotNull parameter is needed to remove unnecessary null check + // in subspans and constructors from arrays + template + IDYNTREE_CONSTEXPR storage_type(KnownNotNull data, OtherExtentType ext) : ExtentType(ext), data_(data.p) + { + assert(ExtentType::size() >= 0); + } + + + template + IDYNTREE_CONSTEXPR storage_type(pointer data, OtherExtentType ext) : ExtentType(ext), data_(data) + { + assert(ExtentType::size() >= 0); + assert(data || ExtentType::size() == 0); + } + + IDYNTREE_CONSTEXPR pointer data() const noexcept { return data_; } + + private: + pointer data_; + }; + + storage_type> storage_; + + // The rest is needed to remove unnecessary null check + // in subspans and constructors from arrays + IDYNTREE_CONSTEXPR Span(KnownNotNull ptr, index_type count) : storage_(ptr, count) {} + + template + class subspan_selector {}; + + template + Span make_subspan(index_type offset, + index_type count, + subspan_selector) const + { + Span tmp(*this); + return tmp.subspan(offset, count); + } + + Span make_subspan(index_type offset, + index_type count, + subspan_selector) const + { + assert(offset >= 0 && size() - offset >= 0); + if (count == dynamic_extent) + { + return { KnownNotNull{ data() + offset }, size() - offset }; + } + + assert(count >= 0 && size() - offset >= count); + return { KnownNotNull{ data() + offset }, count }; + } +}; + +#if defined(IDYNTREE_USE_STATIC_CONSTEXPR_WORKAROUND) +template +IDYNTREE_CONSTEXPR const typename Span::index_type Span::extent; +#endif + + +// [span.comparison], span comparison operators +template +IDYNTREE_CONSTEXPR bool operator==(Span l, + Span r) +{ + return std::equal(l.begin(), l.end(), r.begin(), r.end()); +} + +template +IDYNTREE_CONSTEXPR bool operator!=(Span l, + Span r) +{ + return !(l == r); +} + +template +IDYNTREE_CONSTEXPR bool operator<(Span l, + Span r) +{ + return std::lexicographical_compare(l.begin(), l.end(), r.begin(), r.end()); +} + +template +IDYNTREE_CONSTEXPR bool operator<=(Span l, + Span r) +{ + return !(l > r); +} + +template +IDYNTREE_CONSTEXPR bool operator>(Span l, + Span r) +{ + return r < l; +} + +template +IDYNTREE_CONSTEXPR bool operator>=(Span l, + Span r) +{ + return !(l < r); +} + +namespace details +{ + // if we only supported compilers with good constexpr support then + // this pair of classes could collapse down to a constexpr function + + // we should use a narrow_cast<> to go to std::size_t, but older compilers may not see it as + // constexpr + // and so will fail compilation of the template +#ifndef SWIG + template + struct calculate_byte_size + : std::integral_constant(sizeof(ElementType) * + static_cast(Extent))> + { + }; + + template + struct calculate_byte_size + : std::integral_constant + { + }; +#endif +} + + +#ifndef SWIG +// +// make_span() - Utility functions for creating spans +// +template +IDYNTREE_CONSTEXPR Span make_span(ElementType* ptr, typename Span::index_type count) +{ + return Span(ptr, count); +} + +template +IDYNTREE_CONSTEXPR Span make_span(ElementType* firstElem, ElementType* lastElem) +{ + return Span(firstElem, lastElem); +} + +template +IDYNTREE_CONSTEXPR Span make_span(ElementType (&arr)[N]) noexcept +{ + return Span(arr); +} + +template +IDYNTREE_CONSTEXPR Span make_span(Container& cont) +{ + return Span(cont); +} + +template ::value>::type> +IDYNTREE_CONSTEXPR Span make_span(const Container& cont) +{ + return Span(cont); +} + +template +IDYNTREE_CONSTEXPR Span make_span(Ptr& cont, std::ptrdiff_t count) +{ + return Span(cont, count); +} + +template ::value && SpanUtils::is_element_defined::value>::type> +IDYNTREE_CONSTEXPR Span make_span(Ptr& cont) +{ + return Span(cont); +} + +template ::value && + !SpanUtils::is_element_defined::value && + SpanUtils::has_data_method::value>::type> +IDYNTREE_CONSTEXPR Span().data())>::type> make_span(Container& cont) +{ + return Span().data())>::type>(cont); +} + +#endif + +} // namespace iDynTree + +#ifdef _MSC_VER +#if _MSC_VER < 1910 +#undef constexpr +#pragma pop_macro("constexpr") + +#endif // _MSC_VER < 1910 + +#pragma warning(pop) +#endif // _MSC_VER + +#endif // IDYNTREE_SPAN_H diff --git a/src/core/include/iDynTree/SparseMatrix.h b/src/core/include/iDynTree/SparseMatrix.h new file mode 100644 index 00000000000..e106c452eec --- /dev/null +++ b/src/core/include/iDynTree/SparseMatrix.h @@ -0,0 +1,399 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_SPARSE_MATRIX_H +#define IDYNTREE_SPARSE_MATRIX_H + +#include +#include +#include + +#include + +namespace iDynTree { + + template + class SparseMatrix; + + class Triplet; + class Triplets; +} + +// MARK: - SparseMatrix class + +/** + * \brief Sparse Matrix class + * + * This class uses the Compressed Column (Row) Storage scheme + * (see https://en.wikipedia.org/wiki/Sparse_matrix#Compressed_sparse_row_.28CSR.2C_CRS_or_Yale_format.29) + * which is compatible with the format used in the Eigen library (by using Map). + */ +template +class iDynTree::SparseMatrix +{ +private: + + iDynTree::VectorDynSize m_values; /**< Contains all the non zero (NZ) elements */ + + std::vector m_innerIndices; /**< column index of the NZ elements */ + std::vector m_outerStarts; /**< for each row contains the index of the first NZ in the + * previous two arrays + */ + + + std::size_t m_allocatedSize; /**< size of the memory allocated for m_values and m_innerIndices */ + + std::size_t m_rows; + std::size_t m_columns; + + void initializeMatrix(std::size_t outerSize, const double* vector, std::size_t vectorSize); + + /** + * Insert a new element at the specified position + * + * It returns the index in m_values of the inserted element + * @param row row of the new element + * @param col column of the new element + * @param value value to be inserted + * @return index in m_values of the inserted element + */ + std::size_t insert(std::size_t row, std::size_t col, double value); + + /** + * Check if the element at row-col is present in the matrix. + * + * If it is present the function returns true and the index in the + * out parameter index, otherwise it returns false and index will + * contain the index of the next element in the value array + * + * @param[in] row row index + * @param[in] col column index + * @param[out] index if found contains the index, if not the index of the next element + * @return true if the value has been found + */ + bool valueIndexForOuterAndInnerIndices(std::size_t outerIndex, std::size_t innerIndex, std::size_t& valueIndex) const; + +public: + + /** + * Creates an empty sparse matrix. + */ + SparseMatrix(); + + /** + * Creates a zero sparse matrix with the specified dimensions + * + */ + SparseMatrix(std::size_t rows, std::size_t cols); + + + SparseMatrix(std::size_t rows, std::size_t cols, + const iDynTree::VectorDynSize& memoryReserveDescription); + + template + SparseMatrix(const SparseMatrix&); + + template + SparseMatrix& operator=(const SparseMatrix&); + + + /** + * Default destructor + */ + ~SparseMatrix(); + + /** + * Returns the number of nonzero elements in this sparse matrix + * + * @return the number of non zero elements + */ + std::size_t numberOfNonZeros() const; + + /** + * Resize the matrix to the specified new dimensions + * + * \note the content of the matrix is not preserved in any case + * \warning this function may perform memory allocation + * @param rows the new number of rows of this matrix + * @param columns the new number of columns of this matrix + */ + void resize(std::size_t rows, std::size_t columns); + + /** + * Resize the matrix to the specified new dimensions + * + * \note the content of the matrix is not preserved in any case + * \warning this function may perform memory allocation + * @param rows the new number of rows of this matrix + * @param columns the new number of columns of this matrix + * @param innerIndicesInformation information on the NNZ for each column (row), used to reserve memory in advance. It depends on the storage ordering + */ + void resize(std::size_t rows, std::size_t columns, const iDynTree::VectorDynSize &innerIndicesInformation); + + void reserve(std::size_t nonZeroElements); + + + /** + * Set the sparse matrix to be zero + * + * @note In C++11 it is not guaranteed that this function performs no memory allocation, + * depending on the standard library implementation. + */ + void zero(); + + + /** + * Sets the content of this sparse matrix to the content of triplets + * + * This function does not set the dimensions of the matrix + * which must be set beforehand. If the dimensions are wrong the + * behaviour is undefined. + * + * \note duplicate elements in triplets will be summed up + * \note the content of this matrix will be totally overwritten + * + * \warning this function performs a copy of the triplets parameter so + * that it does not modify the input parameter. + * Use setFromTriplets(iDynTree::Triplets& triplets) if you accept to have + * triplets modified. + * + * @param triplets triplets containing the non zero elements + */ + void setFromConstTriplets(const iDynTree::Triplets& triplets); + + /** + * Sets the content of this sparse matrix to the content of triplets + * + * This function does not set the dimensions of the matrix + * which must be set beforehand. If the dimensions are wrong the + * behaviour is undefined. + * + * \note duplicate elements in triplets will be summed up + * \note the content of this matrix will be totally overwritten + * + * \warning this function modifies the input parameter triplets. + * Use setFromConstTriplets(const iDynTree::Triplets& triplets) if + * you want to preserve the content of triplets. + * + * @param triplets triplets containing the non zero elements + */ + void setFromTriplets(iDynTree::Triplets& triplets); + + static SparseMatrix sparseMatrixFromTriplets(std::size_t rows, + std::size_t cols, + const iDynTree::Triplets& nonZeroElements); + + + /** + * Access operation to the element of the matrix identified by row-col + * + * \note this method is slow as it has to look for the proper index (it performs a linear search) + * @param row row index + * @param col column index + * @return the value at the specified row and column + */ + double operator()(std::size_t row, std::size_t col) const; + + /** + * Access operation to the element of the matrix identified by row-col + * + * \note this method is slow as it has to look for the proper index (it performs a linear search) + * \warning if no elements are found, this method insert a zero value at the specified position + * @param row row index + * @param col column index + * @return reference to the value at the specified row and column + */ + double& operator()(std::size_t row, std::size_t col); + + inline double getValue(std::size_t row, std::size_t col) const + { + return this->operator()(row, col); + } + + inline void setValue(std::size_t row, std::size_t col, double newValue) + { + double &value = this->operator()(row, col); + value = newValue; + } + + + /** + * Returns the number of rows of the matrix + * @return the number of rows + */ + std::size_t rows() const; + + /** + * Returns the number of columns of the matrix + * @return the number of columns + */ + std::size_t columns() const; + + //Raw buffers access + double * valuesBuffer(); + + double const * valuesBuffer() const; + + int * innerIndicesBuffer(); + + int const * innerIndicesBuffer() const; + + int * outerIndicesBuffer(); + + int const * outerIndicesBuffer() const; + + + /** + * Returns a textual description of the matrix. + * + * If true is passed, the whole matrix (with also zero elements) is printed + * Default to false + * @param fullMatrix true to return the full matrix, false for only the non zero elements. Default to false + * @return a textual representation of the matrix + */ + std::string description(bool fullMatrix = false) const; + +#ifndef NDEBUG + std::string internalDescription() const; +#endif + + class Iterator; + class ConstIterator; + + typedef Iterator iterator; + typedef ConstIterator const_iterator; + + //Why the compiler is not able to choose the const version + //if we have both? @traversaro + iterator begin(); + const_iterator begin() const; + + iterator end(); + const_iterator end() const; + +}; + +// MARK: - Iterator classes + +#ifndef SWIG + +template +class iDynTree::SparseMatrix::Iterator +{ +public: + class TripletRef { + private: + int m_row; + int m_column; + double *m_value; + + TripletRef(std::size_t row, std::size_t column, double *value); + friend class iDynTree::SparseMatrix::Iterator; + + public: + + inline int row() const { return m_row; } + inline int column() const { return m_column; } + inline double& value() { return *m_value; } + inline double value() const { return *m_value; } + + }; + +private: + /** + * Construct a new iterator + * + * @param matrix the sparse matrix to iterate + * @param valid false if the created iterator is invalid. True by default + */ + Iterator(iDynTree::SparseMatrix &matrix, bool valid = true); + friend class iDynTree::SparseMatrix; + + iDynTree::SparseMatrix &m_matrix; + + int m_index; + TripletRef m_currentTriplet; + int m_nonZerosInOuterDirection; + + void updateTriplet(); + +public: + + typedef std::ptrdiff_t difference_type; + typedef typename iDynTree::SparseMatrix::Iterator::TripletRef value_type; + typedef value_type& reference; + typedef value_type* pointer; + typedef std::output_iterator_tag iterator_category; + + // Required by the iterator type + Iterator& operator++(); + Iterator operator++(int); + + // Required by the input iterator + bool operator==(const Iterator&) const; + bool operator==(const ConstIterator&) const; + inline bool operator!=(const Iterator& s) const { return !this->operator==(s); } + inline bool operator!=(const ConstIterator& s) const { return !this->operator==(s); } + + // Required by the input iterator + // Also Output iterator if the reference modifies the container + reference operator*(); + pointer operator->(); + + bool isValid() const; +}; + +template +class iDynTree::SparseMatrix::ConstIterator +{ + +private: + /** + * Construct a new const iterator + * + * @param matrix the sparse matrix to iterate + * @param valid false if the created iterator is invalid. True by default + */ + ConstIterator(const iDynTree::SparseMatrix &matrix, bool valid = true); + friend class iDynTree::SparseMatrix; + + const iDynTree::SparseMatrix &m_matrix; + + int m_index; + iDynTree::Triplet m_currentTriplet; + int m_nonZerosInOuterDirection; + + void updateTriplet(); + +public: + + typedef std::ptrdiff_t difference_type; + typedef iDynTree::Triplet value_type; + typedef const value_type& reference; + typedef const value_type* pointer; + typedef std::output_iterator_tag iterator_category; + + //Copy from non const version + ConstIterator(const Iterator& iterator); + + // Required by the iterator type + ConstIterator& operator++(); + ConstIterator operator++(int); + + // Required by the input iterator + bool operator==(const ConstIterator&) const; + bool operator==(const Iterator&) const; + inline bool operator!=(const ConstIterator& s) const { return !this->operator==(s); } + inline bool operator!=(const Iterator& s) const { return !this->operator==(s); } + + // Required by the input iterator + // Also Output iterator if the reference modifies the container + reference operator*(); + pointer operator->(); + + bool isValid() const; +}; + +#endif + +#endif /* end of include guard: IDYNTREE_SPARSE_MATRIX_H */ diff --git a/src/core/include/iDynTree/SpatialAcc.h b/src/core/include/iDynTree/SpatialAcc.h new file mode 100644 index 00000000000..aa385b34ed8 --- /dev/null +++ b/src/core/include/iDynTree/SpatialAcc.h @@ -0,0 +1,42 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_SPATIAL_ACC_H +#define IDYNTREE_SPATIAL_ACC_H + +#include + +namespace iDynTree +{ + /** + * Class representing a spatial acceleration, i.e. the representation + * of the time derivative of the twist. + * + * \note The linear part of this spatial vector **is not** the acceleration + * of a point. + * + * + * \ingroup iDynTreeCore + */ + class SpatialAcc: public SpatialMotionVector + { + public: + /** + * Default constructor. + * The data is not reset to zero for perfomance reason. + * Please initialize the data in the class before any use. + */ + inline SpatialAcc() {} + SpatialAcc(const LinAcceleration & _linearVec3, const AngAcceleration & _angularVec3); + SpatialAcc(const SpatialMotionVector& other); + SpatialAcc(const SpatialAcc& other); + + // overloaded operator + SpatialAcc operator+(const SpatialAcc &other) const; + SpatialAcc operator-(const SpatialAcc &other) const; + SpatialAcc operator-() const; + + }; +} + +#endif diff --git a/src/core/include/iDynTree/SpatialForceVector.h b/src/core/include/iDynTree/SpatialForceVector.h new file mode 100644 index 00000000000..318c0858061 --- /dev/null +++ b/src/core/include/iDynTree/SpatialForceVector.h @@ -0,0 +1,56 @@ +// 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 + +#include +#include +#include + +namespace iDynTree +{ + /** + * Class providing the raw coordinates for any spatial force vector, + * (i.e. vector form of an element of se*(3)). + * + * \ingroup iDynTreeCore + * + * A force spatial vector can be used to to described spatial momentum, wrench, + * or their derivatives. + * + * 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. + * + * \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. + */ + class SpatialForceVector: public SpatialVector + { + public: + /** + * We use traits here to have the associations SpatialVector <=> Linear/Angular 3D vectors types + * defined in a single place. + */ + typedef SpatialMotionForceVectorT_traits::LinearVector3Type LinearVector3T; + typedef SpatialMotionForceVectorT_traits::AngularVector3Type AngularVector3T; + + /** + * Default constructor. + * The data is not reset to zero for perfomance reason. + * Please initialize the data in the class before any use. + */ + inline SpatialForceVector() {} + SpatialForceVector(const LinearVector3T & _linearVec3, const AngularVector3T & _angularVec3); + SpatialForceVector(const SpatialForceVector & other); + SpatialForceVector(const SpatialVector & other); + virtual ~SpatialForceVector(); + + + SpatialForceVector operator*(const double scalar) const; + }; +} + +#endif /* IDYNTREE_SPATIAL_FORCE_RAW_H */ diff --git a/src/core/include/iDynTree/SpatialInertia.h b/src/core/include/iDynTree/SpatialInertia.h new file mode 100644 index 00000000000..b1abdf606e3 --- /dev/null +++ b/src/core/include/iDynTree/SpatialInertia.h @@ -0,0 +1,238 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_SPATIAL_INERTIA_H +#define IDYNTREE_SPATIAL_INERTIA_H + +#include +#include +#include +#include +#include +#include + +namespace iDynTree +{ + /** + * @brief Class representing a six dimensional inertia. + * + * + * \ingroup iDynTreeCore + */ + class SpatialInertia: public SpatialInertiaRaw + { + public: + /** + * Default constructor. + * The data is not reset to zero for perfomance reason. + * Please initialize the data in the vector before any use. + */ + inline SpatialInertia() {} + SpatialInertia(const double mass, + const PositionRaw & com, + const RotationalInertiaRaw & rotInertia); + SpatialInertia(const SpatialInertiaRaw& other); + SpatialInertia(const SpatialInertia& other); + + // Operations on SpatialInertia + static SpatialInertia combine(const SpatialInertia & op1, + const SpatialInertia & op2); + + /** + * @brief Get the SpatialInertia as a 6x6 matrix + * + * If \f$ m \in \mathbb{R} \f$ is the mass, + * \f$ c \in \mathbb{R}^3 \f$ is the center of mass, + * \f$ I \in \mathbb{R}^{3 \times 3} \f$ is the 3d inertia, and + * \f$ 1_3 \in \mathbb{R}^{3 \times 3} \f$ is the 3d identity matrix this + * method returns the \f$ \mathbb{M} \in \mathbb{R}^{6 \times 6} \f$ matrix such that: + * \f[ + * \mathbb{M} = + * \begin{bmatrix} + * m 1_3 & -m c \times \\ + * m c \times & I + * \end{bmatrix} + * \f]. + * + * @note As all quantities in iDynTree, this inertia assumes the linear-angular serialization. + */ + Matrix6x6 asMatrix() const; + + Twist applyInverse(const SpatialMomentum& mom) const; + Matrix6x6 getInverse() const; + + // overloaded operators + SpatialInertia operator+(const SpatialInertia& other) const; + SpatialForceVector operator*(const SpatialMotionVector &other) const; + SpatialMomentum operator*(const Twist &other) const; + Wrench operator*(const SpatialAcc &other) const; + + // Efficient operations + + /** + * Return the bias wrench v.cross(M*v). + * + * Defining \f$ \mathbb{M} \f$ as this inertia, return + * the bias wrench v.cross(M*v), defined in math as: + * \f[ + * \mathrm{v} \bar\times^* \mathbb{M} \mathrm{v} = \\ + * \begin{bmatrix} \omega \times & 0 \\ + * v \times & \omega \times + * \end{bmatrix} + * \begin{bmatrix} m 1_3 & -mc\times \\ + * mc\times & I + * \end{bmatrix} + * \begin{bmatrix} v \\ \omega \end{bmatrix} = \\ + * \begin{bmatrix} + * m \omega \times v - \omega \times ( m c \times \omega) \\ + * m c \times ( \omega \times v ) + \omega \times I \omega + * \end{bmatrix} + * \f] + */ + Wrench biasWrench(const Twist & V) const; + + /** + * @brief Return the derivative of the bias wrench with respect to the link 6D velocity. + * + * Defining \f$ \mathbb{M} \in \mathbb{R}^{6 \times 6} \f$ as this inertia, return the derivative + * with respect to \f$ \mathrm{v} = \begin{bmatrix} v \\ \omega \end{bmatrix} \in \mathbb{R}^6 \f$ + * of the bias wrench \f$ \mathrm{v} \bar\times^* \mathbb{M} \mathrm{v} \f$ (i.e. v.cross(M*v)). + * + * The bias wrench is: + * \f[ + * \mathrm{v} \bar\times^* \mathbb{M} \mathrm{v} = \\ + * \begin{bmatrix} + * m \omega \times v - \omega \times ( m c \times \omega) \\ + * m c \times ( \omega \times v ) + \omega \times I \omega + * \end{bmatrix} + * \f] + * + * So the derivative with respect to the twist V is : + * \f[ + * \partial_\mathrm{v} ( \mathrm{v} \bar\times^* \mathbb{M} \mathrm{v} ) = \\ + * \begin{bmatrix} + * m \omega \times & - m v \times + ( m c \times \omega) \times - (\omega \times) (mc \times) \\ + * (m c \times) ( \omega \times) & - (m c \times )(v \times) + \omega \times I - (I \omega) \times + * \end{bmatrix} + * \f] + */ + Matrix6x6 biasWrenchDerivative(const Twist & V) const; + + static SpatialInertia Zero(); + + + /** + * \brief Get the Rigid Body Inertia as a vector of 10 inertial parameters. + * + * Return the rigid body inertia inertial parameters, defined as: + * + * | Elements | Symbol | Description | + * |:--------:|:-------:|:--------:| + * | 0 | \f$ m \f$ | The mass of the rigid body | + * | 1-3 | \f$ m c \f$ | The first moment of mass of the rigid body | + * | 4-9 | \f$ \mathop{vech}(I) \f$ | The 6 independent elements of the 3d inertia matrix, i.e. \f$ \begin{bmatrix} I_{xx} \\ I_{xy} \\ I_{xz} \\ I_{yy} \\ I_{yz} \\ I_{zz} \end{bmatrix} \f$ . | + * + * The first moment of mass is the center of mass (\f$ c \in \mathbb{R}^3 \f$ ) w.r.t. to the frame where this + * rigid body inertia is expressed multiplied by the rigid body mass \f$ m \f$. + * + * The 3d rigid body inertia \f$ I \in \mathbb{R}^{3 \times 3} \f$ is expressed with the orientation of the frame + * in which this rigid body inertia is expressed, and with respect to the frame origin. + * + */ + Vector10 asVector() const; + + /** + * \brief Set the Rigid Body Inertia from the inertial parameters in the vector. + * + * The serialization assumed in the inertialParams is the same used in the asVector method. + */ + void fromVector(const Vector10 & inertialParams); + + /** + * \brief Check if the Rigid Body Inertia is physically consistent. + * + * This method will check: + * * if the mass is positive, + * * if the 3d inertia at the COM is positive semidefinite, + * (semidefinite to cover also the case of the inertia of a point mass), + * * if the moment of inertia along the principal axes at the COM respect the triangle inequality. + * + * It will return true if all this check will pass, or false otherwise. + * + */ + bool isPhysicallyConsistent() const; + + /** + * \brief Get the momentum inertial parameters regressor. + * + * Get the matrix + * \f[ + * Y(\mathrm{v}) \in \mathbb{R}^{6 \times 10} + * \f] + * such that: + * \f[ + * \mathbb{M} \mathrm{v} = Y(\mathrm{v}) \alpha + * \f] + * + * If \f$ \alpha \in \mathbb{R}^{10} \f$ is the inertial parameters representation of \f$ \mathbb{M} \f$, + * as returned by the asVector method. + */ + static Matrix6x10 momentumRegressor(const iDynTree::Twist & v); + + /** + * \brief Get the momentum derivative inertial parameters regressor. + * + * Get the matrix + * \f[ + * Y(\mathrm{v},a) \in \mathbb{R}^{6 \times 10} + * \f] + * such that: + * \f[ + * \mathbb{M} a + \mathrm{v} \overline{\times}^{*} \mathbb{M} \mathrm{v} = Y(\mathrm{v}, a)\alpha + * \f] + * + * If \f$ \alpha \in \mathbb{R}^{10} \f$ is the inertial parameters representation of \f$ \mathbb{M} \f$, + * as returned by the asVector method. + * + * This is also the regressor of the net wrench acting on a rigid body. + * As such, it is the building block of all other algorithms to compute dynamics + * regressors. + */ + static Matrix6x10 momentumDerivativeRegressor(const iDynTree::Twist & v, + const iDynTree::SpatialAcc & a); + + /** + * \brief Get the momentum derivative inertial parameters regressor. + * + * Get the matrix + * \f[ + * Y(\mathrm{v},\mathrm{v}_r,a_r) \in \mathbb{R}^{6\times10} + * \f] + * such that: + * \f[ + * \mathbb{M} a_r + (\mathrm{v} \overline{\times}^{*} \mathbb{M} - \mathbb{M} \mathrm{v} \times) \mathrm{v}_r = Y(\mathrm{v},\mathrm{v}_r,a_r)\alpha + * \f] + * + * If \f$ \alpha \in \mathbb{R}^{10} \f$ is the inertial parameters representation of \f$ \mathbb{M} \f$, as returned by the + * asVector method. + * + * Notice that if \f$ \mathrm{v} = \mathrm{v}_r \f$, this regressor reduces to the one computed by momentumDerivativeRegressor. + * The main difference is that (assuming constant \f$ \mathbb{M} \f$) this regressor respect the passivity condition and + * thus is the basic building block for building Slotine Li style regressors. + * + * For more on this, please check: + * + * Garofalo, G.; Ott, C.; Albu-Schaffer, A., + * "On the closed form computation of the dynamic matrices and their differentiations," in + * Intelligent Robots and Systems (IROS), 2013 IEEE/RSJ International Conference on + * doi: 10.1109/IROS.2013.6696688 + * URL: http://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=6696688&isnumber=6696319 + * + */ + static Matrix6x10 momentumDerivativeSlotineLiRegressor(const iDynTree::Twist & v, + const iDynTree::Twist & vRef, + const iDynTree::SpatialAcc & aRef); + }; +} + +#endif diff --git a/src/core/include/iDynTree/SpatialInertiaRaw.h b/src/core/include/iDynTree/SpatialInertiaRaw.h new file mode 100644 index 00000000000..cc21947dc53 --- /dev/null +++ b/src/core/include/iDynTree/SpatialInertiaRaw.h @@ -0,0 +1,99 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_SPATIAL_INERTIA_RAW_H +#define IDYNTREE_SPATIAL_INERTIA_RAW_H + +#include + +namespace iDynTree +{ + class PositionRaw; + class SpatialForceVector; + class SpatialMotionVector; + + /** + * Class providing the raw coordinates for a spatial inertia, i.e. + * a spatial dyadic mapping the motion space to the force space. + * + * \ingroup iDynTreeCore + * + * \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. + */ + class SpatialInertiaRaw + { + protected: + double m_mass; ///< Mass. + double m_mcom[3]; ///< First moment of mass (i.e. mass * center of mass). + RotationalInertiaRaw m_rotInertia; ///< Three dimensional rotational inertia. + + public: + /** + * Default constructor. + * The data is not reset to zero for perfomance reason. + * Please initialize the data in the vector before any use. + */ + inline SpatialInertiaRaw() {} + + /** + * @param mass mass of the rigid body + * @param com center of mass of the rigid body, expressed in the frame + * in which the spatial inertia is expressed + * @param rotInertia rotational inertia expressed with respect to the origin of the frame. + * + * \warning the KDL::RigidBodyInertia class has a similar constructor, but in that one + * the rotational inerta in input is expressed in the center of mass of the body. + */ + SpatialInertiaRaw(const double mass, const PositionRaw & com, const RotationalInertiaRaw & rotInertia); + SpatialInertiaRaw(const SpatialInertiaRaw & other); + + /** + * Helper constructor-like function that takes mass, center of mass + * and the rotational inertia expressed in the center of mass. + * + */ + void fromRotationalInertiaWrtCenterOfMass(const double mass, const PositionRaw & com, const RotationalInertiaRaw & rotInertia); + + + /** multiplication operator + * + * overloading happens on proper classes + * + */ + + + /** + * Getter functions + * + * \note for preserving consistency, no setters are implemented.. + * if you want to modify a spatial inertia create a new one, + * and assign it to the spatial inertia that you want modify. + * Given that no memory allocation happens it should be still + * efficient. + */ + double getMass() const; + PositionRaw getCenterOfMass() const; + const RotationalInertiaRaw& getRotationalInertiaWrtFrameOrigin() const; + RotationalInertiaRaw getRotationalInertiaWrtCenterOfMass() const; + + + /** + * Function to combine the rigid body inertia of two different rigid bodies, + * giving the rigid body inertia of of the rigid body obtanined by welding the two bodies. + */ + static SpatialInertiaRaw combine(const SpatialInertiaRaw& op1, const SpatialInertiaRaw& op2); + + /** + * Multiplication function + * + */ + SpatialForceVector multiply(const SpatialMotionVector & op) const; + + /** reset to zero (i.e. the inertia of body with zero mass) the SpatialInertia */ + void zero(); + }; +} + +#endif + diff --git a/src/core/include/iDynTree/SpatialMomentum.h b/src/core/include/iDynTree/SpatialMomentum.h new file mode 100644 index 00000000000..4be72563529 --- /dev/null +++ b/src/core/include/iDynTree/SpatialMomentum.h @@ -0,0 +1,39 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_SPATIALMOMENTUM_H +#define IDYNTREE_SPATIALMOMENTUM_H + + +#include + +namespace iDynTree +{ + /** + * Class representing a spatial momentum, + * i.e. a 6D combination of linear and angular momentum. + * + * \ingroup iDynTreeCore + * + */ + class SpatialMomentum: public SpatialForceVector + { + public: + /** + * Default constructor. + * The data is not reset to the zero for perfomance reason. + * Please initialize the data in the class before any use. + */ + inline SpatialMomentum() {} + SpatialMomentum(const LinMomentum & _linearVec3, const AngMomentum & _angularVec3); + SpatialMomentum(const SpatialForceVector & other); + SpatialMomentum(const SpatialMomentum & other); + + // overloaded operators + SpatialMomentum operator+(const SpatialMomentum &other) const; + SpatialMomentum operator-(const SpatialMomentum &other) const; + SpatialMomentum operator-() const; + }; +} + +#endif diff --git a/src/core/include/iDynTree/SpatialMotionVector.h b/src/core/include/iDynTree/SpatialMotionVector.h new file mode 100644 index 00000000000..17147e66f60 --- /dev/null +++ b/src/core/include/iDynTree/SpatialMotionVector.h @@ -0,0 +1,138 @@ +// 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 + +#include +#include +#include + +namespace iDynTree +{ + class SpatialForceVector; + class Transform; + class Dummy {}; + + /** + * Class providing the raw coordinates for any motion spatial vector + * (i.e. vector form of an element of se(3)). + * + * \ingroup iDynTreeCore + * + * A motion spatial vector can be used to to describe a twist, twist acceleration + * or their derivatives. + * + * A generic motion spatial vector can also be used to store the logarithm of + * an iDynTree::Transform (i.e. an element of SE(3)). + * + * 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. + * + * \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. + */ + + class SpatialMotionVector: public SpatialVector + { + public: + /** + * constructors + */ + inline SpatialMotionVector() {} + SpatialMotionVector(const LinearMotionVector3 & _linearVec3, const AngularMotionVector3 & _angularVec3); + SpatialMotionVector(const SpatialMotionVector & other); + SpatialMotionVector(const SpatialVector & other); + + /** + * Multiplication for a scalar. + * Mainly used if SpatialMotionVector is used to represent a motion subspace. + */ + SpatialMotionVector operator*(const double scalar) const; + +/** + * Cross product between two 6D motion vectors + * \f$ V_1 = \begin{bmatrix} v_1 \\ \omega_1 \end{bmatrix} \f$ + * and + * \f$ V_2 = \begin{bmatrix} v_2 \\ \omega_2 \end{bmatrix} \f$ + * + * Returns: + * \f[ + * V_1 \times V_2 = + * \begin{bmatrix} + * v_1 \times \omega_2 + \omega_1 \times v_2 \\ + * \omega_1 \times \omega_2 + * \end{bmatrix} + * \f] + */ + SpatialMotionVector cross(const SpatialMotionVector& other) const; + + /** + * Cross product between a 6D motion vector \f$ V = \begin{bmatrix} v \\ \omega \end{bmatrix} \f$ and + * a 6D force vector \f$ F = \begin{bmatrix} f \\ \mu \end{bmatrix} \f$. + * + * Returns: + * \f[ + * V \bar{\times}^* F = + * \begin{bmatrix} + * \omega \times f \\ + * v \times f + \omega \times \mu + * \end{bmatrix} + * \f] + */ + SpatialForceVector cross(const SpatialForceVector& other) const; + + /** + * Cross product matrices + */ + + /** + * If this object is \f$ V = \begin{bmatrix} v \\ \omega \end{bmatrix} \f$, + * return the 6x6 matrix \f$ V\times \f$ + * such that, if U is a SpatialMotionVector : + * \f[ + * (V \times) U = V\texttt{.cross}(U) + * \f] + * + * The returned matrix is then the following one: + * \f[ + * V \times = + * \begin{bmatrix} + * \omega \times & v \times \\ + * 0_{3\times3} & \omega \times + * \end{bmatrix} + * \f] + */ + Matrix6x6 asCrossProductMatrix() const; + + /** + * If this object is \f$ V = \begin{bmatrix} v \\ \omega \end{bmatrix} \f$, return the 6x6 matrix \f$ V\times \f$ + * such that, if F is a SpatialForceVector : + * \f[ + * (V \bar{\times}^*) F = V\texttt{.cross}(F) + * \f] + * + The returned matrix is then the following one: + * \f[ + * V \bar{\times}^* = + * \begin{bmatrix} + * \omega \times & 0_{3\times3} \\ + * v \times & \omega \times + * \end{bmatrix} + * \f] + */ + Matrix6x6 asCrossProductMatrixWrench() const; + + + /** + * Exp mapping between a generic element of se(3) (iDynTree::SpatialMotionVector) + * to the corresponding element of SE(3) (iDynTree::Transform). + */ + Transform exp() const; + }; + +} + +#endif diff --git a/src/core/include/iDynTree/SpatialVector.h b/src/core/include/iDynTree/SpatialVector.h new file mode 100644 index 00000000000..32fe52c8e0b --- /dev/null +++ b/src/core/include/iDynTree/SpatialVector.h @@ -0,0 +1,401 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_SPATIAL_VECTOR_H +#define IDYNTREE_SPATIAL_VECTOR_H + +#include "Position.h" +#include "Rotation.h" +#include "Utils.h" +#include + + +#include +#include + +namespace iDynTree +{ + class SpatialMotionVector; + class SpatialForceVector; + class Position; + class Rotation; + + /** + * Traits class for SpatialMotionVector and SpatialForceVector classes + */ + template + class SpatialMotionForceVectorT_traits {}; + + template <> + class SpatialMotionForceVectorT_traits + { + public: + typedef LinearMotionVector3 LinearVector3Type; + typedef AngularMotionVector3 AngularVector3Type; + }; + + template <> + class SpatialMotionForceVectorT_traits + { + public: + typedef LinearForceVector3 LinearVector3Type; + typedef AngularForceVector3 AngularVector3Type; + }; + + /** + * Helper structure for dual space definition + */ + template struct DualSpace {}; + + template <> struct DualSpace {typedef SpatialForceVector Type;}; + + template <> struct DualSpace {typedef SpatialMotionVector Type;}; + + /** + * Class providing an interface to any spatial motion or force vector, which provides + * raw coordinates. + * + * \ingroup iDynTreeCore + * + * A motion spatial vector can be used to describe twist, spatial acceleration, + * and derivatives. + * + * A force spatial vector can be used to describe spatial momentum, wrench, + * and derivatives. + * + * \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. + */ +#define SPATIALVECTOR_TEMPLATE_HDR \ +template + +#define SPATIALVECTOR_INSTANCE_HDR \ +SpatialVector + + SPATIALVECTOR_TEMPLATE_HDR + class SpatialVector + { + public: + typedef typename SpatialMotionForceVectorT_traits::LinearVector3Type LinearVector3T; + typedef typename SpatialMotionForceVectorT_traits::AngularVector3Type AngularVector3T; + typedef typename DualSpace::Type DualVectorT; + + protected: + LinearVector3T linearVec3; + AngularVector3T angularVec3; + static const unsigned int linearOffset = 0; + static const unsigned int angularOffset = 3; + static const unsigned int totalSize = 6; + + public: + /** + * constructors + */ + SpatialVector(); + SpatialVector(const LinearVector3T & _linearVec3, const AngularVector3T & _angularVec3); + SpatialVector(const SpatialVector & other); + SpatialVector(iDynTree::Span other); + + /** + * Vector accessors, getters, setters + */ + LinearVector3T & getLinearVec3(); + AngularVector3T & getAngularVec3(); + const LinearVector3T & getLinearVec3() const; + const AngularVector3T & getAngularVec3() const; + void setLinearVec3(const LinearVector3T & _linearVec3); + void setAngularVec3(const AngularVector3T & _angularVec3); + + /** + * Vector element accessors, getters, setters + */ + double operator()(const unsigned int index) const; // No input checking. + double& operator()(const unsigned int index); // No input checking. + double getVal(const unsigned int index) const; // Perform boundary checking + bool setVal(const unsigned int index, const double new_el); // Perform boundary checking + unsigned int size() const; + void zero(); + + /** + * Geometric operations + */ + const DerivedSpatialVecT changePoint(const Position & newPoint); + const DerivedSpatialVecT changeCoordFrame(const Rotation & newCoordFrame); + static DerivedSpatialVecT compose(const DerivedSpatialVecT & op1, const DerivedSpatialVecT & op2); + static DerivedSpatialVecT inverse(const DerivedSpatialVecT & op); + + /** + * dot product + */ + double dot(const DualVectorT & other) const; + + /** + * overloaded operators + */ + DerivedSpatialVecT operator+(const DerivedSpatialVecT &other) const; + DerivedSpatialVecT operator-(const DerivedSpatialVecT &other) const; + DerivedSpatialVecT operator-() const; + + /** + * constructor helpers + */ + static DerivedSpatialVecT Zero(); + + /** + * Conversion to basic vector. + */ + Vector6 asVector() const; + + /** @name Output helpers. + * Output helpers. + */ + ///@{ + std::string toString() const; + + std::string reservedToString() const; + ///@} + + + }; + + + + /** + *==================================================================================== + * SpatialVector Method definitions + */ + + // constructors + SPATIALVECTOR_TEMPLATE_HDR + SPATIALVECTOR_INSTANCE_HDR::SpatialVector(): + linearVec3(), + angularVec3() + { + } + + SPATIALVECTOR_TEMPLATE_HDR + SPATIALVECTOR_INSTANCE_HDR::SpatialVector(const LinearVector3T & _linearVec3, + const AngularVector3T & _angularVec3): + linearVec3(_linearVec3), + angularVec3(_angularVec3) + { + } + + SPATIALVECTOR_TEMPLATE_HDR + SPATIALVECTOR_INSTANCE_HDR::SpatialVector(const SpatialVector & other): + linearVec3(other.getLinearVec3()), + angularVec3(other.getAngularVec3()) + { + } + + SPATIALVECTOR_TEMPLATE_HDR + SPATIALVECTOR_INSTANCE_HDR::SpatialVector(Span other) + { + if( other.size() != totalSize) + { + reportError("SpatialVector","constructor","input vector does not have the right size"); + this->zero(); + } + else + { + linearVec3 = LinearVector3T(other.subspan(0, 3)); + angularVec3 = AngularVector3T(other.subspan(3, 3)); + } + } + + // Vector accessors, Getters, setters + SPATIALVECTOR_TEMPLATE_HDR + typename SPATIALVECTOR_INSTANCE_HDR::LinearVector3T & SPATIALVECTOR_INSTANCE_HDR::getLinearVec3() + { + return this->linearVec3; + } + + SPATIALVECTOR_TEMPLATE_HDR + typename SPATIALVECTOR_INSTANCE_HDR::AngularVector3T & SPATIALVECTOR_INSTANCE_HDR::getAngularVec3() + { + return this->angularVec3; + } + + SPATIALVECTOR_TEMPLATE_HDR + const typename SPATIALVECTOR_INSTANCE_HDR::LinearVector3T & SPATIALVECTOR_INSTANCE_HDR::getLinearVec3() const + { + return this->linearVec3; + } + + SPATIALVECTOR_TEMPLATE_HDR + const typename SPATIALVECTOR_INSTANCE_HDR::AngularVector3T & SPATIALVECTOR_INSTANCE_HDR::getAngularVec3() const + { + return this->angularVec3; + } + + SPATIALVECTOR_TEMPLATE_HDR + void SPATIALVECTOR_INSTANCE_HDR::setLinearVec3(const LinearVector3T & _linearVec3) + { + // set linear component + this->linearVec3 = _linearVec3; + } + + SPATIALVECTOR_TEMPLATE_HDR + void SPATIALVECTOR_INSTANCE_HDR::setAngularVec3(const AngularVector3T & _angularVec3) + { + // set angular component + this->angularVec3 = _angularVec3; + } + + // Vector element accessors, getters, setters + SPATIALVECTOR_TEMPLATE_HDR + double SPATIALVECTOR_INSTANCE_HDR::operator()(const unsigned int index) const + { + assert(index < SpatialVector::totalSize); + return (indexlinearVec3(index) : this->angularVec3(index-3)); + } + + SPATIALVECTOR_TEMPLATE_HDR + double& SPATIALVECTOR_INSTANCE_HDR::operator()(const unsigned int index) + { + assert(index < SpatialVector::totalSize); + return (indexlinearVec3(index) : this->angularVec3(index-3)); + } + + SPATIALVECTOR_TEMPLATE_HDR + double SPATIALVECTOR_INSTANCE_HDR::getVal(const unsigned int index) const + { + if( index >= SpatialVector::totalSize ) + { + reportError("VectorFixSize","getVal","index out of bounds"); + return 0.0; + } + return (*this)(index); + } + + SPATIALVECTOR_TEMPLATE_HDR + bool SPATIALVECTOR_INSTANCE_HDR::setVal(const unsigned int index, const double new_el) + { + if( index >= SpatialVector::totalSize ) + { + reportError("VectorFixSize","getVal","index out of bounds"); + return false; + } + (*this)(index) = new_el; + + return true; + } + + SPATIALVECTOR_TEMPLATE_HDR + unsigned int SPATIALVECTOR_INSTANCE_HDR::size() const + { + return SpatialVector::totalSize; + } + + SPATIALVECTOR_TEMPLATE_HDR + void SPATIALVECTOR_INSTANCE_HDR::zero() + { + for(unsigned int i=0; i < SpatialVector::totalSize; i++ ) + { + (*this)(i) = 0.0; + } + } + + + + // Geometric operations + SPATIALVECTOR_TEMPLATE_HDR + const DerivedSpatialVecT SPATIALVECTOR_INSTANCE_HDR::changePoint(const Position & newPoint) + { + return newPoint.changePointOf(*this); + } + + SPATIALVECTOR_TEMPLATE_HDR + const DerivedSpatialVecT SPATIALVECTOR_INSTANCE_HDR::changeCoordFrame(const Rotation & newCoordFrame) + { + return newCoordFrame.changeCoordFrameOf(*this); + } + + SPATIALVECTOR_TEMPLATE_HDR + DerivedSpatialVecT SPATIALVECTOR_INSTANCE_HDR::compose(const DerivedSpatialVecT & op1, const DerivedSpatialVecT & op2) + { + return DerivedSpatialVecT(op1.getLinearVec3()+op2.getLinearVec3(), + op1.getAngularVec3()+op2.getAngularVec3()); + } + + SPATIALVECTOR_TEMPLATE_HDR + DerivedSpatialVecT SPATIALVECTOR_INSTANCE_HDR::inverse(const DerivedSpatialVecT & op) + { + return DerivedSpatialVecT(-op.getLinearVec3(), + -op.getAngularVec3()); + } + + // dot product + SPATIALVECTOR_TEMPLATE_HDR + double SPATIALVECTOR_INSTANCE_HDR::dot(const DualVectorT & other) const + { + return (this->getLinearVec3().dot(other.getLinearVec3()) + + this->getAngularVec3().dot(other.getAngularVec3())); + } + + // overloaded operators + SPATIALVECTOR_TEMPLATE_HDR + DerivedSpatialVecT SPATIALVECTOR_INSTANCE_HDR::operator+(const DerivedSpatialVecT &other) const + { + return compose(*this, other); + } + + SPATIALVECTOR_TEMPLATE_HDR + DerivedSpatialVecT SPATIALVECTOR_INSTANCE_HDR::operator-(const DerivedSpatialVecT &other) const + { + return compose(*this, inverse(other)); + } + + SPATIALVECTOR_TEMPLATE_HDR + DerivedSpatialVecT SPATIALVECTOR_INSTANCE_HDR::operator-() const + { + return inverse(*this); + } + + // constructor helpers + SPATIALVECTOR_TEMPLATE_HDR + DerivedSpatialVecT SPATIALVECTOR_INSTANCE_HDR::Zero() + { + DerivedSpatialVecT ret; + ret.linearVec3.zero(); + ret.angularVec3.zero(); + return ret; + } + + SPATIALVECTOR_TEMPLATE_HDR + Vector6 SPATIALVECTOR_INSTANCE_HDR::asVector() const + { + Vector6 ret; + ret.data()[0] = linearVec3.data()[0]; + ret.data()[1] = linearVec3.data()[1]; + ret.data()[2] = linearVec3.data()[2]; + ret.data()[3] = angularVec3.data()[0]; + ret.data()[4] = angularVec3.data()[1]; + ret.data()[5] = angularVec3.data()[2]; + return ret; + } + + + SPATIALVECTOR_TEMPLATE_HDR + std::string SPATIALVECTOR_INSTANCE_HDR::toString() const + { + std::stringstream ss; + + ss << linearVec3.toString() << " " + << angularVec3.toString(); + ss << std::endl; + + return ss.str(); + } + + SPATIALVECTOR_TEMPLATE_HDR + std::string SPATIALVECTOR_INSTANCE_HDR::reservedToString() const + { + return this->toString(); + } + +#undef SPATIALVECTOR_TEMPLATE_HDR +#undef SPATIALVECTOR_INSTANCE_HDR +} + +#endif /* IDYNTREE_SPATIAL_VECTOR_H */ diff --git a/src/core/include/iDynTree/TestUtils.h b/src/core/include/iDynTree/TestUtils.h new file mode 100644 index 00000000000..d55e47bfcfe --- /dev/null +++ b/src/core/include/iDynTree/TestUtils.h @@ -0,0 +1,403 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_TEST_UTILS_H +#define IDYNTREE_TEST_UTILS_H + +#include + +#include + +#include +#include +#include +#include +#include + +#include +#include + + +namespace iDynTree +{ + class Transform; + class SpatialMotionVector; + class SpatialForceVector; + class Axis; + class SpatialForceVector; + class SpatialMotionVector; + class SpatialInertia; + class Position; + class Rotation; + +#define ASSERT_IS_TRUE(prop) iDynTree::assertTrue(prop,__FILE__,__LINE__) +#define ASSERT_IS_FALSE(prop) iDynTree::assertTrue(!(prop),__FILE__,__LINE__) +#define ASSERT_EQUAL_STRING(val1,val2) iDynTree::assertStringAreEqual(val1,val2,iDynTree::DEFAULT_TOL,__FILE__,__LINE__) +#define ASSERT_EQUAL_DOUBLE(val1,val2) iDynTree::assertDoubleAreEqual(val1,val2,iDynTree::DEFAULT_TOL,__FILE__,__LINE__) +#define ASSERT_EQUAL_DOUBLE_TOL(val1,val2,tol) iDynTree::assertDoubleAreEqual(val1,val2,tol,__FILE__,__LINE__) +#define ASSERT_EQUAL_VECTOR(val1,val2) assertVectorAreEqual(val1,val2,iDynTree::DEFAULT_TOL,__FILE__,__LINE__) +#define ASSERT_EQUAL_VECTOR_TOL(val1,val2,tol) assertVectorAreEqual(val1,val2,tol,__FILE__,__LINE__) +#define ASSERT_EQUAL_VECTOR_REL_TOL(val1,val2,relTol,minAbsTol) assertVectorAreEqualWithRelativeTol(val1,val2,relTol,minAbsTol,__FILE__,__LINE__) +#define ASSERT_EQUAL_SPATIAL_MOTION(val1,val2) assertSpatialMotionAreEqual(val1,val2,iDynTree::DEFAULT_TOL,__FILE__,__LINE__) +#define ASSERT_EQUAL_SPATIAL_FORCE(val1,val2) assertSpatialForceAreEqual(val1,val2,iDynTree::DEFAULT_TOL,__FILE__,__LINE__) +#define ASSERT_EQUAL_SPATIAL_FORCE_TOL(val1,val2,tol) assertSpatialForceAreEqual(val1,val2,tol,__FILE__,__LINE__) +#define ASSERT_EQUAL_MATRIX(val1,val2) assertMatrixAreEqual(val1,val2,iDynTree::DEFAULT_TOL,__FILE__,__LINE__) +#define ASSERT_EQUAL_MATRIX_TOL(val1,val2,tol) assertMatrixAreEqual(val1,val2,tol,__FILE__,__LINE__) +#define ASSERT_EQUAL_TRANSFORM(val1,val2) assertTransformsAreEqual(val1,val2,iDynTree::DEFAULT_TOL,__FILE__,__LINE__) +#define ASSERT_EQUAL_TRANSFORM_TOL(val1,val2,tol) assertTransformsAreEqual(val1,val2,tol,__FILE__,__LINE__) + + + struct TestMatrixMismatch { + bool match; + double expected; + double realElement; + + TestMatrixMismatch(bool _match, double _expected, double _realElement) + : match(_match), expected(_expected), realElement(_realElement) {} + }; + + void assertStringAreEqual(const std::string & val1, const std::string & val2, double tol = DEFAULT_TOL, std::string file="", int line=-1); + + + void assertDoubleAreEqual(const double & val1, const double & val2, double tol = DEFAULT_TOL, std::string file="", int line=-1); + + /** + * Assert that two transforms are equal, and + * exit with EXIT_FAILURE if they are not. + * + */ + void assertTransformsAreEqual(const Transform & trans1, const Transform & trans2, double tol = DEFAULT_TOL, std::string file="", int line=-1); + + /** + * Assert that two spatial motion vectors are equal, + * and exit with EXIT_FAILURE if they are not. + * + */ + void assertSpatialMotionAreEqual(const SpatialMotionVector & t1, const SpatialMotionVector & t2, double tol = DEFAULT_TOL, std::string file="", int line=-1); + + /** + * Assert that two spatial force vectors are equal, + * and exit with EXIT_FAILURE if they are not. + * + */ + void assertSpatialForceAreEqual(const SpatialForceVector & f1, const SpatialForceVector & f2, double tol = DEFAULT_TOL, std::string file="", int line=-1); + + void assertTrue(bool prop,std::string file="", int line=-1); + + /** + * Get random bool + */ + bool getRandomBool(); + + /** + * Get a random double between min and max . + */ + double getRandomDouble(double min=0.0, double max=1.0); + + /** + * Get a random integer between min and max (included). + * For example a dice could be simulated with getRandomInteger(1,6); + */ + int getRandomInteger(int min, int max); + + /** + * Fill a vector with random double. + */ + template + void getRandomVector(VectorType & vec, double min=0.0, double max=1.0) + { + for(unsigned int i=0; i + void getRandomMatrix(MatrixType & mat) + { + for(unsigned int i=0; i + void printVector(std::string /*name*/, const VectorType& vec) + { + for(unsigned int i=0; i < vec.size(); i++ ) + { + std::cerr << vec(i) << "\n"; + } + std::cerr << "\n"; + } + + /** + * Helper for printing difference of two vectors + */ + template + void printVectorDifference(std::string name, const VectorType1& vec1, const VectorType2& vec2) + { + std::cerr << name << " : \n"; + size_t minSize = vec1.size(); + + if( vec2.size() < minSize ) + { + minSize = vec2.size(); + } + + for(unsigned int i=0; i < minSize; i++ ) + { + std::cerr << vec1(i) - vec2(i) << " ( " << (vec1(i) - vec2(i))/std::max(vec1(i),vec2(i)) << " ) " << "\n"; + } + } + + /** + * Helper for printing the patter of wrong elements + * in between two vectors + */ + inline void printVectorWrongElements(std::string name, std::vector & correctElems) + { + std::cerr << name << " ( . match, X mismatch): \n"; + + for(unsigned int i=0; i < correctElems.size(); i++ ) + { + if( correctElems[i] ) + { + std::cerr << "." << "\n"; + } + else + { + std::cerr << "X" << "\n"; + } + } + } + + /** + * Helper for printing the patter of wrong elements + * in between two matrix + */ + inline void printMatrixWrongElements(std::string name, std::vector< std::vector > & correctElems) + { +#ifndef _WIN32 +#define IDYNTREE_MATCH_CHARACTER "\u2714" +#else +#define IDYNTREE_MATCH_CHARACTER "o" +#endif + std::cerr << name << "( " IDYNTREE_MATCH_CHARACTER " match, (expected,got:error) mismatch): \n"; + + size_t rows = correctElems.size(); + size_t cols = correctElems[0].size(); + for(unsigned int row=0; row < rows; row++ ) + { + for( unsigned int col = 0; col < cols; col++ ) + { + if( correctElems[row][col].match ) + { + std::cerr << IDYNTREE_MATCH_CHARACTER; + } + else + { + std::cerr << "(" << correctElems[row][col].expected << "," << correctElems[row][col].realElement + << ":" << correctElems[row][col].realElement - correctElems[row][col].expected << ")"; + } + + std::cerr << " "; + } + + std::cerr << "\n"; + } + } + + /** + * Helper for printing the patter of wrong elements + * in between two matrix + */ + template + void printMatrixPercentageError(const MatrixType1& mat1, const MatrixType2& mat2) + { + size_t rows = mat1.rows(); + size_t cols = mat2.cols(); + for(unsigned int row=0; row < rows; row++ ) + { + for( unsigned int col = 0; col < cols; col++ ) + { + double mat1el = mat1(row,col); + double mat2el = mat2(row,col); + double percentageError = std::abs(mat1el-mat2el)/std::max(mat1el,mat2el); + std::cerr << std::fixed << std::setprecision(3) << percentageError << " "; + } + + std::cerr << "\n"; + } + } + + + /** + * Assert that two vectors are equal, and + * exit with EXIT_FAILURE if they are not. + */ + template + void assertVectorAreEqual(const VectorType1& vec1, const VectorType2& vec2, double tol, std::string file, int line) + { + if( vec1.size() != vec2.size() ) + { + std::cerr << file << ":" << line << " : assertVectorAreEqual failure: vec1 has size " << vec1.size() + << " while vec2 has size " << vec2.size() << std::endl; + exit(EXIT_FAILURE); + } + + std::vector correctElements(vec1.size(),true); + bool checkCorrect = true; + + for( unsigned int i = 0; i < vec1.size(); i++ ) + { + if( !(fabs(vec1(i)-vec2(i)) < tol) ) + { + checkCorrect = false; + correctElements[i] = false; + } + } + + if( !checkCorrect ) + { + std::cerr << file << ":" << line << " : assertVectorAreEqual failure: " << std::endl; + printVector("vec1",vec1); + printVector("vec2",vec2); + printVectorDifference("vec1-vec2",vec1,vec2); + printVectorWrongElements("wrong el:",correctElements); + exit(EXIT_FAILURE); + } + } + + /** + * Assert that two vectors are equal, and exit with EXIT_FAILURE if they are not. + * + * The tolerance passed in this function is a relative tolerance on the max element of the comparison, i.e. + * absoluteTol = max(relativeTol*max(val1,val2), minAbsoluteTol) + */ + template + void assertVectorAreEqualWithRelativeTol(const VectorType1& vec1, const VectorType2& vec2, double relativeTol, double minAbsoluteTol, std::string file, int line) + { + if( vec1.size() != vec2.size() ) + { + std::cerr << file << ":" << line << " : assertVectorAreEqualWithRelativeTol failure: vec1 has size " << vec1.size() + << " while vec2 has size " << vec2.size() << std::endl; + exit(EXIT_FAILURE); + } + + std::vector correctElements(vec1.size(),true); + bool checkCorrect = true; + + for( unsigned int i = 0; i < vec1.size(); i++ ) + { + double absoluteTol = std::max(relativeTol*std::max(std::abs(vec1(i)), std::abs(vec2(i))), minAbsoluteTol); + if( fabs(vec1(i)-vec2(i)) >= absoluteTol ) + { + checkCorrect = false; + correctElements[i] = false; + } + } + + if( !checkCorrect ) + { + std::cerr << file << ":" << line << " : assertVectorAreEqualWithRelativeTol failure: " << std::endl; + printVector("vec1",vec1); + printVector("vec2",vec2); + printVectorDifference("vec1-vec2",vec1,vec2); + printVectorWrongElements("wrong el:",correctElements); + exit(EXIT_FAILURE); + } + } + + /** + * Assert that two matrices are equal, and + * exit with EXIT_FAILURE if they are not. + * + */ + template + void assertMatrixAreEqual(const MatrixType1& mat1, const MatrixType2& mat2, double tol, std::string file, int line) + { + if( mat1.rows() != mat2.rows() || + mat2.cols() != mat1.cols() ) + { + std::cerr << file << ":" << line << " : assertMatrixAreEqual failure: mat1 has size " << mat1.rows() << " " << mat1.cols() + << " while mat2 has size " << mat2.rows() << " " << mat2.cols() << std::endl; + exit(EXIT_FAILURE); + } + + std::vector< std::vector > correctElements(mat2.rows(), std::vector(mat1.cols(), TestMatrixMismatch(true, 0, 0)) ); + bool checkCorrect = true; + + for( unsigned int row = 0; row < mat2.rows(); row++ ) + { + for( unsigned int col = 0; col < mat2.cols(); col++ ) + { + if( fabs(mat1(row,col)-mat2(row,col)) >= tol ) + { + checkCorrect = false; + correctElements[row][col].match = false; + correctElements[row][col].expected = mat1(row,col); + correctElements[row][col].realElement = mat2(row,col); + } + } + } + + if( !checkCorrect ) + { + std::cerr << file << ":" << line << " : assertMatrixAreEqual failure with tol " << tol << " : " << std::endl; + printMatrixWrongElements("wrong el:",correctElements); + //std::cerr << "percentage error : " << std::endl; + //printMatrixPercentageError(mat1,mat2); + exit(EXIT_FAILURE); + } + + } + + +} + +#endif diff --git a/src/core/include/iDynTree/Transform.h b/src/core/include/iDynTree/Transform.h new file mode 100644 index 00000000000..05c79815a97 --- /dev/null +++ b/src/core/include/iDynTree/Transform.h @@ -0,0 +1,538 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_TRANSFORM_H +#define IDYNTREE_TRANSFORM_H + +#include +#include +#include + +#include + +namespace iDynTree +{ + class Position; + class Rotation; + class Wrench; + class Twist; + class SpatialMomentum; + class SpatialAcc; + class SpatialMotionVector; + class SpatialForceVector; + class SpatialInertia; + class ArticulatedBodyInertia; + + /** + * Class representation the relative displacement between two different frames. + * + * \ingroup iDynTreeCore + * + * \image html transform.svg + * + * + * This class is designed to be an easy to use proxy to perform change of frame of + * expression for iDynTree::Position, iDynTree::Twist, iDynTree::Wrench and other data + * structure in \ref iDynTreeCore. For this reason the class is called "Transform", because it will be mainly + * used to transform quantities between frames. + * + * Given that this class it may used to represent homogeneous transform matrix as well as adjoint + * matrix, no raw access to the underline storage ( data() method ) is provided, because it does not + * have a canonical representation. Instead, access is given to the two elements of a transform: + * The position vector \f${}^{\texttt{refOrient}} p_{\texttt{refPoint},\texttt{point}}\f$ and + * the rotation matrix \f${}^{\texttt{refOrient}} R_{\texttt{orient}}\f$. + * + * We will indicate this tranform as \f$( {}^{\texttt{refOrient}} p_{\texttt{refPoint},\texttt{point}} , {}^{\texttt{refOrient}} R_{\texttt{orient}} )\f$ + * + * The frame whose origin is the reference point and whose orientation the reference orientation is often + * indicated as \f$ \texttt{refFrame} = (\texttt{refPoint},\texttt{refOrient}) \f$. + * + * Similarly the frame whose origin is the point and whose orientation is the orientation is indicated + * as \f$ \texttt{frame} = (\texttt{point},\texttt{orient}) \f$. + * + * This transform object can be obtained as the \f${}^{\texttt{refFrame}} H_{\texttt{frame}}\f$ 4x4 homogeneous matrix + * using the asHomogeneousTransform method, or as the \f${}^{\texttt{refFrame}} X_{\texttt{frame}}\f$ 6x6 adjoint matrix using the + * asAdjointTransform . + * + * + * + */ + class Transform + { + protected: + /** + * \brief The position part of the transform. + * + * The 3d vector \f${}^{\texttt{refOrient}} p_{\texttt{refPoint},\texttt{point}}\f$, + * that is the vector between the point and the + * reference point, pointing toward the point and expressed + * in the reference orientation frame. + */ + Position pos; + + /** + * \brief The rotation part of the transform + * + * Set the rotation matrix \f${}^{\texttt{refOrient}} R_{\texttt{orient}} \in \mathbb{R}^{3 \times 3}\f$, + * that is the rotation matrix that takes + * a 3d vector expressed in the orientationFrame and transform it + * in a 3d vector expressed in the reference orientation frame. + */ + Rotation rot; + + public: + /** + * Default constructor. + * The data is not reset to the default for perfomance reason. + * Please initialize the data in the class before any use. + */ + Transform(); + + /** + * Constructor from a rotation and a point + */ + Transform(const Rotation & _rot, const Position & origin); + + /** + * Constructor from a Matrix4x4 object. It is equivalent of calling fromHomogeneousTransform() + * @param transform The input homogeneous matrix + */ + Transform(const Matrix4x4 & transform); + + /** + * Copy constructor: create a Transform from another Transform. + */ + Transform(const Transform & other); + + /** + * Set rotation and translation from a iDynTree::Matrix4x4 object + * @param transform The input homogeneous matrix + */ + void fromHomogeneousTransform(const Matrix4x4& transform); + + /** + * Assigment operator + * + * @param other the rhs + * + * @return *this equal to the other object + */ + Transform& operator=(const Transform& other); + + /** + * Get the rotation part of the transform + * + * Get the rotation matrix \f${}^{\texttt{refOrient}} R_{\texttt{orient}} \in \mathbb{R}^{3 \times 3}\f$, + * that is the rotation matrix that takes + * a 3d vector expressed in the orientationFrame and transform it + * in a 3d vector expressed in the reference orientation frame. + */ + const Rotation & getRotation() const; + + /** + * \brief Get the translation part of the transform + * + * Get 3d vector \f${}^{\texttt{refOrient}} p_{\texttt{refPoint},\texttt{point}}\f$, + * that is the vector between the point and the + * reference point, pointing toward the point and expressed + * in the reference orientation frame. + */ + const Position & getPosition() const; + + /** + * Set the rotation part of the transform + * + * Set the rotation matrix \f${}^{\texttt{refOrient}} R_{\texttt{orient}} \in \mathbb{R}^{3 \times 3}\f$, + * that is the rotation matrix that takes + * a 3d vector expressed in the orientationFrame and transform it + * in a 3d vector expressed in the reference orientation frame. + */ + void setRotation(const Rotation & rotation); + + /** + * \brief Set the translation part of the transform + * + * Set 3d vector \f${}^{\texttt{refOrient}} p_{\texttt{refPoint},\texttt{point}}\f$, + * that is the vector between the point and the + * reference point, pointing toward the point and expressed + * in the reference orientation frame. + */ + void setPosition(const Position & position); + + // geometric operations on 3x1 vectors (positions and rotations and homogemeous tranform) + static Transform compose(const Transform & op1, const Transform & op2); + static Transform inverse2(const Transform & trans); + + /** + * \name Overloaded operators. + * + * This operators are used to change the frame in which a quantity is + * expressed from the \f$\texttt{frame}\f$ to the \f$\texttt{refFrame}\f$ of + * the transform. + */ + ///@{ + + /** + * \brief Combine two transforms. + * + * If this object is + * \f[ + * (p,R) = ( {}^{\texttt{refOrient}} p_{\texttt{refPoint},\texttt{point}} , {}^{\texttt{refOrient}} R_{\texttt{orient}} ) + * \f] + * and the argument is + * \f[ + * (p',R') = ( {}^{\texttt{orient}} p_{\texttt{point},\texttt{newPoint}} , {}^{\texttt{orient}} R_{\texttt{newOrient}} ) + * \f]. + * + * The resulting transform will be: + * \f[ + * (p+Rp', RR') = ( {}^{\texttt{refOrient}} p_{\texttt{refPoint},\texttt{newPoint}} , {}^{\texttt{refOrient}} R_{\texttt{newOrient}} ) + * \f]. + * + * Notice that this is equivalent to multiply the associated homogemeous matrices: + * \f[ + * {}^{\texttt{refFrame}} H_{\texttt{newFrame}} = {}^{\texttt{refFrame}} H_{\texttt{frame}} {}^{\texttt{frame}} H_{\texttt{newFrame}} + * \f] + * or the associated adjoint matrices : + * \f[ + * {}^{\texttt{refFrame}} X_{\texttt{newFrame}} = {}^{\texttt{refFrame}} X_{\texttt{frame}} {}^{\texttt{frame}} X_{\texttt{newFrame}} + * \f] + */ + Transform operator*(const Transform & other) const; + + /** + * Return the inverse of this Transform. + * + * If this object is + * \f[ + * (p,R) = ( {}^{\texttt{refOrient}} p_{\texttt{refPoint},\texttt{point}} , {}^{\texttt{refOrient}} R_{\texttt{orient}} ) + * \f] + * this function will return: + * \f[ + * (-R^\top p , R^\top) = ( {}^{\texttt{orient}} p_{\texttt{point},\texttt{refPoint}} , {}^{\texttt{orient}} R_{\texttt{refOrient}} ) + * \f] + * + */ + Transform inverse() const; + + /** + * Change reference frame of a point. + * + * If this object is + * \f[ + * (p,R) = ( {}^{\texttt{refOrient}} p_{\texttt{refPoint},\texttt{point}} , {}^{\texttt{refOrient}} R_{\texttt{orient}} ) + * \f] + * + * And the Position argument represent a point: + * \f[ + * p' = {}^{\texttt{orient}} p_{\texttt{point},\texttt{newPoint}} + * \f] + * + * The result of the operation is: + * \f[ + * Rp' + p = {}^{\texttt{refOrient}} p_{\texttt{refPoint},\texttt{newPoint}} + * \f] + * + */ + Position operator*(const Position & other) const; + + /** + * Change frame in which a Wrench is expressed. + * + * If this object is + * \f[ + * (p,R) = ( {}^{\texttt{refOrient}} p_{\texttt{refPoint},\texttt{point}} , {}^{\texttt{refOrient}} R_{\texttt{orient}} ) + * \f] + * + * And the argument is a wrench: + * \f[ + * {}_{\texttt{frame}} F = + * \begin{bmatrix} + * f \\ \tau + * \end{bmatrix} + * \f] + * + * The result of this operation is : + * \f[ + * {}_{\texttt{refFrame}} F + * = + * {}_{\texttt{refFrame}}X^{\texttt{frame}} + * {}_{\texttt{frame}} F + * = + * \begin{bmatrix} + * R & + * 0_{3\times3} \\ + * p \times R & + * R + * \end{bmatrix} + * \begin{bmatrix} + * f \\ \tau + * \end{bmatrix} + * = + \begin{bmatrix} + * Rf \\ p \times R f + R\tau + * \end{bmatrix} + * \f] + */ + Wrench operator*(const Wrench & other) const; + + /** + * Change the frame in which a Twist is expressed. + * + * If this object is + * \f[ + * (p,R) = ( {}^{\texttt{refOrient}} p_{\texttt{refPoint},\texttt{point}} , {}^{\texttt{refOrient}} R_{\texttt{orient}} ) + * \f] + * + * And the argument is a twist: + * \f[ + * {}^{\texttt{frame}} V = + * \begin{bmatrix} + * v \\ \omega + * \end{bmatrix} + * \f] + * + * The result of this operation is : + * \f[ + * {}^{\texttt{refFrame}} V + * = + * {}^{\texttt{refFrame}}X_{\texttt{frame}} + * {}^{\texttt{frame}} V + * = + * \begin{bmatrix} + * R & + * p \times R\\ + * 0_{3\times3} & + * R + * \end{bmatrix} + * \begin{bmatrix} + * v \\ \omega + * \end{bmatrix} + * = + \begin{bmatrix} + * R v + p \times R \omega \\ R\omega + * \end{bmatrix} + * \f] + */ + Twist operator*(const Twist & other) const; + + /** + * Change the frame in which a SpatialMomentum is expressed. + * + * Check the operator*(const Wrench & other) documentation + * for the mathematical details. + */ + SpatialMomentum operator*(const SpatialMomentum & other) const; + + /** + * Change the frame in which a SpatialAcc is expressed. + * + * Check the operator*(const Twist & other) documentation + * for the mathematical details. + */ + SpatialAcc operator*(const SpatialAcc & other) const; + + /** + * Change the frame in which a SpatialMotionVector is expressed. + * + * Check the operator*(const Twist & other) documentation + * for the mathematical details. + */ + SpatialMotionVector operator*(const SpatialMotionVector & other) const; + + /** + * Change the frame in which a SpatialForceVector is expressed. + * + * Check the operator*(const Wrench & other) documentation + * for the mathematical details. + */ + SpatialForceVector operator*(const SpatialForceVector & other) const; + + /** + * Change the frame in which a SpatialInertia is expressed. + * + */ + SpatialInertia operator*(const SpatialInertia & other) const; + + /** + * Change the frame in which a ArticulatedBodyInertia is expressed. + * + */ + ArticulatedBodyInertia operator*(const ArticulatedBodyInertia & other) const; + + + /** + * Change the frame in which a Direction is expressed. + * + * If this object is + * \f[ + * (p,R) = ( {}^{\texttt{refOrient}} p_{\texttt{refPoint},\texttt{point}} , {}^{\texttt{refOrient}} R_{\texttt{orient}} ) + * \f] + * + * And the argument is a direction represented by a unit norm 3d vector : + * \f[ + * {}^{\texttt{orient}} d + * \f] + * + * The result of this operation is: + * \f[ + * {}^{\texttt{refOrient}} d = R {}^{\texttt{orient}} d + * \f] + * + */ + Direction operator*(const Direction & other) const; + + /** + * Change the frame in which a Axis is expressed. + * + * If this object is + * \f[ + * (p,R) = ( {}^{\texttt{refOrient}} p_{\texttt{refPoint},\texttt{point}} , {}^{\texttt{refOrient}} R_{\texttt{orient}} ) + * \f] + * + * And the argument is an axis, specified by the axis origin and the axis direction: + * \f[ + * {}^{\texttt{frame}} A = ({}^{\texttt{orient}} p_{\texttt{point},\texttt{axisOrigin}} , {}^{\texttt{orient}} d) = (p',d) + * \f] + * + * The result of this operation is: + * \f[ + * {}^{\texttt{refFrame}} A = ({}^{\texttt{refOrient}} p_{\texttt{refPoint},\texttt{axisOrigin}} , {}^{\texttt{refOrient}} d) = ( Rp' + p , Rd) + * \f] + * + */ + Axis operator*(const Axis & other) const; + ///@} + + /** + * Return an identity transfom + * + * Set the rotation to the identity and the translation to 0 : + * \f[ + * (0_{3 \times 1}, I_{3 \times 3}) + * \f] + * + * @return an identity transform. + */ + static Transform Identity(); + + /** + * @name Raw data accessors. + * + * For several applications it may be useful to access the fransform + * contents in the raw form of homogeneous matrix or an adjoint matrix. + */ + ///@{ + + /** + * Return the 4x4 homogeneous matrix representing the transform. + * + * The returned matrix is the one with this structure: + * + * \f[ + * {}^{\texttt{refFrame}} H_{\texttt{frame}} = + * \begin{bmatrix} + * {}^{\texttt{refOrient}} R_{\texttt{orient}} & {}^{\texttt{refOrient}} p_{\texttt{refPoint},\texttt{point}} \\ + * 0_{1\times3} & 1 + * \end{bmatrix} + * \f] + * Where \f${}^{\texttt{refOrient}} R_{\texttt{orient}} \in \mathbb{R}^{3 \times 3}\f$ is the rotation matrix that takes + * a 3d vector expressed in the orientationFrame and transform it + * in a 3d vector expressed in the reference orientation frame. + * + * \f${}^{\texttt{refOrient}} p_{\texttt{refPoint},\texttt{point}} \in \mathbb{R}^3\f$ + * is the vector between the point and the + * reference point, pointing toward the point and expressed + * in the reference orientation frame. + * + */ + Matrix4x4 asHomogeneousTransform() const; + + /** + * Return the 6x6 adjoint matrix representing this transform. + * + * The returned matrix is the one with this structure: + * + * \f[ + * {}^{\texttt{refFrame}} X_{\texttt{frame}} = + * \begin{bmatrix} + * R & + * p \times R \\ + * 0_{3\times3} & + * R + * \end{bmatrix} + * \f] + * + * Where \f$R = {}^{\texttt{refOrient}} R_{\texttt{orient}} \in \mathbb{R}^{3\times3}\f$ + * is the rotation matrix that takes + * a 3d vector expressed in the orientationFrame and transform it + * in a 3d vector expressed in the reference orientation frame. + * + * \f$p = p_{\texttt{refPoint},\texttt{point}} \in \mathbb{R}^3\f$ + * is the vector between the point and the + * reference point, pointing toward the point and expressed + * in the reference orientation frame \f$p \times\f$ is the skew simmetric + * matrix such that \f$(p \times) v = p \times v\f$ . + * + * \warning Note that in iDynTree the matrix are stored + * in row major order, and the 6d quantites are + * serialized in linear/angular order. + * + */ + Matrix6x6 asAdjointTransform() const; + + /** + * Return the 6x6 adjoint matrix (for wrench) representing this transform. + * + * The returned matrix is the one with this structure: + * + * \f[ + * {}_{\texttt{refFrame}} X^{\texttt{frame}} = + * \begin{bmatrix} + * R & + * 0_{3\times3} \\ + * p \times R & + * R + * \end{bmatrix} + * \f] + * + * Where \f$R = {}^{\texttt{refOrient}} R_{\texttt{orient}} \in \mathbb{R}^{3\times3}\f$ + * is the rotation matrix that takes + * a 3d vector expressed in the orientationFrame and transform it + * in a 3d vector expressed in the reference orientation frame. + * + * \f$p = p_{\texttt{refPoint},\texttt{point}} \in \mathbb{R}^3\f$ + * is the vector between the point and the + * reference point, pointing toward the point and expressed + * in the reference orientation frame \f$p \times\f$ is the skew simmetric + * matrix such that \f$(p \times) v = p \times v\f$ . + * + * \warning Note that in iDynTree the matrix are stored + * in row major order, and the 6d quantites are + * serialized in linear/angular order. + * + */ + Matrix6x6 asAdjointTransformWrench() const; + + /* + * Exp mapping between a generic element of se(3) (iDynTree::SpatialMotionVector) + * to the corresponding element of SE(3) (iDynTree::Transform). + */ + SpatialMotionVector log() const; + + ///@} + + /** + * @name Output helpers. + * Function to print out the Transform. + */ + ///@{ + std::string toString() const; + + std::string reservedToString() const; + ///@} + + }; +} + +#endif diff --git a/src/core/include/iDynTree/TransformDerivative.h b/src/core/include/iDynTree/TransformDerivative.h new file mode 100644 index 00000000000..1f41fc04651 --- /dev/null +++ b/src/core/include/iDynTree/TransformDerivative.h @@ -0,0 +1,258 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_TRANSFORM_DERIVATIVE_H +#define IDYNTREE_TRANSFORM_DERIVATIVE_H + +#include +#include + +namespace iDynTree +{ + class Transform; + class ArticulatedBodyInertia; + class SpatialForceVector; + + /** + * Class representing the derivative of Transform object + * + * \ingroup iDynTreeCore + * + * \note Even if most methods documentation are written + * assuming that this class represents the derivative + * of a Transform with respect to time, it can be used + * to represent a derivative with respect to an arbitrary + * variable. + * + */ + class TransformDerivative + { + protected: + /** + * \brief The derivative of the translation part of a Transform + */ + Vector3 posDerivative; + + /** + * \brief The derivative of the rotation part of a Transform + */ + Matrix3x3 rotDerivative; + + public: + /** + * Default constructor. + * The data is not reset to the default for perfomance reasons. + * Please initialize the data in the class before any use. + */ + TransformDerivative() {} + + /** + * Constructor from a Matrix3x3 and a Vector3 . + * The matrix represent the derivative of a rotation matrix, + * while the vector the derivative of the position part of the tranform. + */ + TransformDerivative(const Matrix3x3 & _rotDeriv, const Vector3 & posDeriv); + + /** + * Copy constructor: create a TransformDerivative from another TransformDerivative. + */ + TransformDerivative(const TransformDerivative & other); + + /** + * Default destructor. + */ + ~TransformDerivative() {} + + /** + * Get the derivative of the rotation part of the transform + * + */ + const Matrix3x3 & getRotationDerivative() const; + + /** + * Get the derivative of the translation part of the transform + * + */ + const Vector3 & getPositionDerivative() const; + + /** + * Set the derivative of the rotation part of the transform + * + */ + void setRotationDerivative(const Matrix3x3 & rotationDerivative); + + /** + * \brief Set the derivative of translation part of the transform + * + */ + void setPositionDerivative(const Vector3 & positionDerivative); + + /** + * Return a zero transfom derivative + * + * @return an zero transform derivative. + */ + static TransformDerivative Zero(); + + /** + * @name Raw data accessors. + * + * For several applications it may be useful to access the fransform + * contents in the raw form of homogeneous matrix or an adjoint matrix. + */ + ///@{ + + /** + * Return the derivative of the 4x4 homogeneous matrix representing the transform. + * + * The returned matrix is the one with this structure: + * + * \f[ + * {}^{\texttt{refFrame}} \dot{H}_{\texttt{frame}} = + * \begin{bmatrix} + * {}^{\texttt{refOrient}} \dot{R}_{\texttt{orient}} & {}^{\texttt{refOrient}} \dot{p}_{\texttt{refPoint},\texttt{point}} \\ + * 0_{1\times3} & 1 + * \end{bmatrix} + * \f] + * + * For details on this notation, check the Transform::asHomogeneousTransform() method . + * + */ + Matrix4x4 asHomogeneousTransformDerivative() const; + + /** + * Return the derivative of the 6x6 adjoint matrix representing this transform. + * + * The returned matrix is the one with this structure: + * + * \f[ + * {}^{\texttt{refFrame}} X_{\texttt{frame}} = + * \begin{bmatrix} + * \dot{R} & + * \dot{p} \times R + p \times \dot{R} \\ + * 0_{3\times3} & + * \dot{R} + * \end{bmatrix} + * \f] + * + * Where \f$R = {}^{\texttt{refOrient}} R_{\texttt{orient}} \in \mathbb{R}^{3\times3}\f$ + * is the rotation matrix that takes + * a 3d vector expressed in the orientationFrame and transform it + * in a 3d vector expressed in the reference orientation frame. + * + * \f$p = p_{\texttt{refPoint},\texttt{point}} \in \mathbb{R}^3\f$ + * is the vector between the point and the + * reference point, pointing toward the point and expressed + * in the reference orientation frame \f$p \times\f$ is the skew simmetric + * matrix such that \f$(p \times) v = p \times v\f$ . + * + * Given that the TransformDerivative object contains only \f$ \dot{R} \f$ and + * \f$ \dot{p} \f$, you need to provide \f$ R \f$ and \f$ p \f$ to this method + * by passing the relative Transform object. + * + * \warning Note that in iDynTree the matrix are stored + * in row major order, and the 6d quantites are + * serialized in linear/angular order. + * + */ + Matrix6x6 asAdjointTransformDerivative(const Transform & transform) const; + + /** + * Return the 6x6 adjoint matrix (for wrench) representing this transform. + * + * The returned matrix is the one with this structure: + * + * \f[ + * {}_{\texttt{refFrame}} X^{\texttt{frame}} = + * \begin{bmatrix} + * \dot{R} & + * 0_{3\times3} \\ + * \dot{p} \times R + p \times \dot{R} & + * \dot{R} + * \end{bmatrix} + * \f] + * + * Where \f$R = {}^{\texttt{refOrient}} R_{\texttt{orient}} \in \mathbb{R}^{3\times3}\f$ + * is the rotation matrix that takes + * a 3d vector expressed in the orientationFrame and transform it + * in a 3d vector expressed in the reference orientation frame. + * + * \f$p = p_{\texttt{refPoint},\texttt{point}} \in \mathbb{R}^3\f$ + * is the vector between the point and the + * reference point, pointing toward the point and expressed + * in the reference orientation frame \f$p \times\f$ is the skew simmetric + * matrix such that \f$(p \times) v = p \times v\f$ . + * + * Given that the TransformDerivative object contains only \f$ \dot{R} \f$ and + * \f$ \dot{p} \f$, you need to provide \f$ R \f$ and \f$ p \f$ to this method + * by passing the relative Transform object. + * + * \warning Note that in iDynTree the matrix are stored + * in row major order, and the 6d quantites are + * serialized in linear/angular order. + * + */ + Matrix6x6 asAdjointTransformWrenchDerivative(const Transform & transform) const; + + /** + * Multiply a transform derivative for a transform. + * + * This operation is useful to compose the derivative of a transform + * for a constant transform. + * + * If this transform derivative represent the derivative of a transform + * with respect to time \f$ ~^a \dot{H}_b \f$, and we have a constant + * transform \f$ ~^b H_c \f$, then this operation returns the derivative + * of the transform \f$ ~^a H_c \f$ : + * \f[ + * ~^a \dot{H}_c = ~^a \dot{H}_b ~^b H_c + * \f] + * + * \warning the otherTransform must be a quantity that does not depend + * on the derivation variable. + */ + TransformDerivative operator*(const Transform & otherTransform) const; + + /** + * Get the derivative of the inverse of the transform. + * + * If this TransformDerivative is \f$ ~^a \dot{H}_b \f$ and the + * transform argument is \f$ ~^a H_b \f$, returns the derivative of + * the inverse transform \f$ ~^b {H}_a \f$ computed as: + * \f[ + * ~^b \dot{H}_a = - ~^b {H}_a ~^a \dot{H}_b ~^b H_a + * \f] + * + */ + TransformDerivative derivativeOfInverse(const Transform & transform) const; + + /** + * Given a articulated inertia \f$I\f$ (other argument), this TransformDerivative \f$ ~^a\dot{X}_b \f$ + * and a transform \f$ ~^a{X}_b \f$ return the articulated body inertia: + * \f[ + * ~_a \dot{X}^b Ia ~^a X_b + ~_a {X}^b Ia ~^a \dot{X}_b + * \f] + */ + ArticulatedBodyInertia transform(const Transform & transform, ArticulatedBodyInertia& other); + + /** + * Given a SpatialForceVector \f$F\f$ (other argument), this TransformDerivative \f$ ~^a\dot{X}_b \f$ + * and a transform \f$ ~^a{X}_b \f$ return the spatial force vector: + * \f[ + * ~_a \dot{X}^b F + * \f] + */ + SpatialForceVector transform(const Transform & transform, SpatialForceVector& other); + + /** + * Given a SpatialMotionVector \f$V\f$ (other argument), this TransformDerivative \f$ ~^a\dot{X}_b \f$ + * and a transform \f$ ~^a{X}_b \f$ return the spatial force vector: + * \f[ + * ~^a \dot{X}_b F + * \f] + */ + SpatialMotionVector transform(const Transform & transform, SpatialMotionVector& other); + }; +} + +#endif diff --git a/src/core/include/iDynTree/Triplets.h b/src/core/include/iDynTree/Triplets.h new file mode 100644 index 00000000000..f840d46445c --- /dev/null +++ b/src/core/include/iDynTree/Triplets.h @@ -0,0 +1,174 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_TRIPLETS_H +#define IDYNTREE_TRIPLETS_H + +#include +#include + +#include +#include +#include + + +namespace iDynTree { + class Triplet; + class Triplets; + + class MatrixDynSize; + + template + class SparseMatrix; + + struct IndexRange; +} + +class iDynTree::Triplet +{ +public: + + static bool rowMajorCompare(const iDynTree::Triplet& a, const iDynTree::Triplet &b); + static bool columnMajorCompare(const iDynTree::Triplet& a, const iDynTree::Triplet &b); + + Triplet(std::size_t row, std::size_t column, double value); + + bool operator<(const iDynTree::Triplet&) const; + bool operator==(const iDynTree::Triplet&) const; + + std::size_t row; + std::size_t column; + double value; +}; + +class iDynTree::Triplets +{ + //Understand if vector is the best solution + std::vector m_triplets; + +public: + + void reserve(std::size_t size); + void clear(); + + template + inline void addSubMatrix(std::size_t startingRow, + std::size_t startingColumn, + const MatrixFixSize &matrix) + { + //if enough memory has been reserved this should be a noop + m_triplets.reserve(m_triplets.size() + (rows * cols)); + for (std::size_t row = 0; row < rows; ++row) { + for (std::size_t col = 0; col < cols; ++col) { + m_triplets.push_back(Triplet(startingRow + row, startingColumn + col, matrix(row, col))); + } + } + } + + void addSubMatrix(std::size_t startingRow, + std::size_t startingColumn, + const MatrixDynSize&); + + template + void addSubMatrix(std::size_t startingRow, + std::size_t startingColumn, + const SparseMatrix& matrix) + { + //if enough memory has been reserved this should be a noop + m_triplets.reserve(m_triplets.size() + matrix.numberOfNonZeros()); + + for (typename SparseMatrix::const_iterator it(matrix.begin()); + it != matrix.end(); ++it) { + m_triplets.push_back(Triplet(startingRow + it->row, + startingColumn + it->column, + it->value)); + } + } + + inline void addDiagonalMatrix(IndexRange startingRow, + IndexRange startingColumn, + double value) + { + addDiagonalMatrix(startingRow.offset, + startingColumn.offset, + value, + startingRow.size); + } + + void addDiagonalMatrix(std::size_t startingRow, + std::size_t startingColumn, + double value, + std::size_t diagonalMatrixSize); + + void pushTriplet(const Triplet& triplet); + + template + inline void setSubMatrix(std::size_t startingRow, + std::size_t startingColumn, + const MatrixFixSize &matrix) + { + //if enough memory has been reserved this should be a noop + m_triplets.reserve(m_triplets.size() + (rows * cols)); + for (std::size_t row = 0; row < rows; ++row) { + for (std::size_t col = 0; col < cols; ++col) { + setTriplet(Triplet(startingRow + row, startingColumn + col, matrix(row, col))); + } + } + } + + void setSubMatrix(std::size_t startingRow, + std::size_t startingColumn, + const MatrixDynSize&); + + template + void setSubMatrix(std::size_t startingRow, + std::size_t startingColumn, + const SparseMatrix& matrix) + { + //if enough memory has been reserved this should be a noop + m_triplets.reserve(m_triplets.size() + matrix.numberOfNonZeros()); + + for (typename SparseMatrix::const_iterator it(matrix.begin()); + it != matrix.end(); ++it) { + setTriplet(Triplet(startingRow + it->row, + startingColumn + it->column, + it->value)); + } + } + + inline void setDiagonalMatrix(IndexRange startingRow, + IndexRange startingColumn, + double value) + { + setDiagonalMatrix(startingRow.offset, + startingColumn.offset, + value, + startingRow.size); + } + + void setDiagonalMatrix(std::size_t startingRow, + std::size_t startingColumn, + double value, + std::size_t diagonalMatrixSize); + + + void setTriplet(const Triplet& triplet); + + bool isEmpty() const; + std::size_t size() const; + + std::string description() const; + + typedef std::vector::iterator iterator; + typedef std::vector::const_iterator const_iterator; + + //for now simply return the vector iterator and do not implement our own iterator + const_iterator begin() const; + iterator begin(); + const_iterator end() const; + iterator end(); + +}; + + +#endif /* end of include guard: IDYNTREE_TRIPLETS_H */ diff --git a/src/core/include/iDynTree/Twist.h b/src/core/include/iDynTree/Twist.h new file mode 100644 index 00000000000..251ca80caf3 --- /dev/null +++ b/src/core/include/iDynTree/Twist.h @@ -0,0 +1,42 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_TWIST_H +#define IDYNTREE_TWIST_H + + +#include + +namespace iDynTree +{ + class SpatialAcc; + class SpatialMomentum; + class Wrench; + + /** + * Class representing a twist, i.e. a 6D combination of linear an angular velocity. + * + * + * \ingroup iDynTreeCore + */ + class Twist: public SpatialMotionVector + { + public: + Twist(); + Twist(const LinVelocity & _linearVec3, const AngVelocity & _angularVec3); + Twist(const SpatialMotionVector& other); + Twist(const Twist& other); + + /** overloaded operators **/ + Twist operator+(const Twist &other) const; + Twist operator-(const Twist &other) const; + Twist operator-() const; + + /** overloaded cross products */ + SpatialAcc operator*(const Twist &other) const; + Wrench operator*(const SpatialMomentum &other) const; + + }; +} + +#endif diff --git a/src/core/include/iDynTree/Utils.h b/src/core/include/iDynTree/Utils.h new file mode 100644 index 00000000000..4aa7923a680 --- /dev/null +++ b/src/core/include/iDynTree/Utils.h @@ -0,0 +1,129 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_UTILS_H +#define IDYNTREE_UTILS_H + +#include + +/** + * \brief Macro to suppress unused variable warnings + * + * see http://stackoverflow.com/a/4851173 + */ +#define IDYNTREE_UNUSED(var) ((void)var) + + +// Note: we set IDYNTREE_DEPRECATED to nothing when compiling with SWIG has some +// SWIG has some problems with this attributes, so until we use a recent SWIG version +// we disabled them for SWIG +/** + * \brief Macro to deprecate functions and methods + * + * see https://blog.samat.io/2017/02/27/Deprecating-functions-and-methods-in-Cplusplus/ + */ +#if defined(SWIG) +#define IDYNTREE_DEPRECATED +#define IDYNTREE_DEPRECATED_WITH_MSG(msg) +// Workaround for SWIG problems with GenerateExportHeader-generated code +#define IDYNTREE_CORE_EXPORT +#else +#define IDYNTREE_DEPRECATED [[deprecated]] +#define IDYNTREE_DEPRECATED_WITH_MSG(msg) [[deprecated(msg)]] +#include "CoreExport.h" +#endif + + +namespace iDynTree +{ + IDYNTREE_CORE_EXPORT extern int UNKNOWN; + + /// Default tolerance for methods with a tolerance, setted to 1e-10 + IDYNTREE_CORE_EXPORT extern double DEFAULT_TOL; + + /** + * Function embedding the semantic checks + * + * This function can throw an exception if the semantic check detects an error (returns False). + */ + void assertWoAbort(const char * semCheck, const char * file, const char* func, int line); + + /** + * Helper class for semantic checking. + * + * Return true if two values are equal, or if one of the two is unknown + * All negative values are used for represent an unknown value. + */ + bool checkEqualOrUnknown(const int op1, const int op2); + + /** + * Helper function for reporting error if the semantic check fails. + * + */ + void reportError(const char * className, const char* methodName, const char * errorMessage); + + /** + * Call report error if condition is true + */ + bool reportErrorIf(bool condition, const char * className_methodName, const char * errorMessage); + + /** + * Helper function for reporting warnings in iDynTree + * + */ + void reportWarning(const char * className, const char* methodName, const char * errorMessage); + + /** + * Helper function for reporting information messages in iDynTree + * + */ + void reportInfo(const char* className, const char* methodName, const char* message); + + /** + * Helper function for reporting debug messages in iDynTree + * + */ + void reportDebug(const char* className, const char* methodName, const char* message); + + /** + * Convert a double from degrees to radians. + */ + double deg2rad(const double valueInDeg); + + /** + * Convert a double from radians to degree. + */ + double rad2deg(const double valueInRad); + + /** + * Simple structure describing a range of rows or columns in a vector or a matrix. + * The offset attributes indicates the index of the first element of the range, while + * the size indicates the size of the range. + * + * Example: offset = 2, size = 3 measn the elements 2 3 4 . + */ + struct IndexRange + { + std::ptrdiff_t offset; + std::ptrdiff_t size; + + bool isValid() const; + static IndexRange InvalidRange(); + }; + + /** + * Enum describing the possible matrix storage ordering + */ + enum MatrixStorageOrdering { + RowMajor, /*!< Row Major ordering, i.e. matrix is serialized row by row */ + ColumnMajor /*!< Column Major ordering, i.e. matrix is serialized row by column */ + }; + + /** + * Check whether two doubles are equal given a tolerance tol. + */ + bool checkDoublesAreEqual(const double & val1, const double & val2, double tol = DEFAULT_TOL); + +} + +#endif diff --git a/src/core/include/iDynTree/VectorDynSize.h b/src/core/include/iDynTree/VectorDynSize.h new file mode 100644 index 00000000000..85d7f9ed540 --- /dev/null +++ b/src/core/include/iDynTree/VectorDynSize.h @@ -0,0 +1,269 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_DYNAMIC_SIZE_VECTOR_H +#define IDYNTREE_DYNAMIC_SIZE_VECTOR_H + +#if !defined(SWIG_VERSION) || SWIG_VERSION >= 0x030000 +#include +#endif + +#include + +namespace iDynTree +{ + /** + * Class providing a simple form of vector with dynamic size. + * It is designed to provide seamless integration with SWIG. + * + * \ingroup iDynTreeCore + */ + class VectorDynSize + { + protected: + /** + * Storage for the VectorDynSize + * + * Pointer to an area of capacity() doubles, managed by this class. + */ + double * m_data; + + /** + * Size of the vector. + */ + std::size_t m_size; + + /** + * The buffer to which m_data is pointing is m_capacity*sizeof(double). + */ + std::size_t m_capacity; + + /** + * Set the capacity of the vector, resizing the buffer pointed by m_data. + */ + void changeCapacityAndCopyData(const std::size_t _newCapacity); + + public: + /** + * Default constructor: initialize the size of the array to zero. + */ + VectorDynSize(); + + /** + * Constructor from the size, all the element assigned to 0 + * + * @param _size the desired size of the array. + * + * \warning performs dynamic memory allocation operations + */ + VectorDynSize(std::size_t _size); + + /** + * Constructor from a C-style array. + * + * Build + * + * \warning performs dynamic memory allocation operations + */ + VectorDynSize(const double * in_data, const std::size_t in_size); + + /** + * Copy constructor + * \warning performs dynamic memory allocation operations + */ + VectorDynSize(const VectorDynSize& vec); + +#if !defined(SWIG_VERSION) || SWIG_VERSION >= 0x030000 + + /** + * Constructor from an iDynTree::Span + * + * @param vec span representing a vector + * + * \warning performs dynamic memory allocation operations + */ + VectorDynSize(iDynTree::Span vec); +#endif + + /** + * Denstructor + * + * \warning performs dynamic memory allocation operations + */ + virtual ~VectorDynSize(); + + /** + * Copy assignment operator. + * + * The vector will be resize to match the + * size of the argument, and the data will + * then be copied. + * + * \warning performs dynamic memory allocation operations + */ + VectorDynSize & operator=(const VectorDynSize& vec); + +#if !defined(SWIG_VERSION) || SWIG_VERSION >= 0x030000 + /** + * Copy assignment operator. + * + * The vector will be resize to match the + * size of the argument, and the data will + * then be copied. + * + * \warning performs dynamic memory allocation operations + */ + VectorDynSize & operator=(iDynTree::Span vec); +#endif + /** + * @name Vector interface methods. + * Methods exposing a vector-like interface to PositionRaw. + */ + ///@{ + double operator()(const std::size_t index) const; + + double& operator()(const std::size_t index); + + double operator[](const std::size_t index) const; + + double& operator[](const std::size_t index); + + double getVal(const std::size_t index) const; + + bool setVal(const std::size_t index, const double new_el); + + /** + * Returns a const iterator to the beginning of the vector + * @note At the moment iterator is implemented as a pointer, it may change in the future. + * For this reason it should not be used as a pointer to the data, use data() instead. + */ + const double* begin() const noexcept; + + /** + * Returns a const iterator to the beginning of the vector + * @note At the moment iterator is implemented as a pointer, it may change in the future. + * For this reason it should not be used as a pointer to the data, use data() instead. + */ + const double* end() const noexcept; + + /** + * Returns a const iterator to the beginning of the vector + * @note At the moment iterator is implemented as a pointer, it may change in the future. + * For this reason it should not be used as a pointer to the data, use data() instead. + */ + const double* cbegin() const noexcept; + + /** + * Returns a const iterator to the beginning of the vector + * @note At the moment iterator is implemented as a pointer, it may change in the future. + * For this reason it should not be used as a pointer to the data, use data() instead. + */ + const double* cend() const noexcept; + + /** + * Returns a iterator to the beginning of the vector + * @note At the moment iterator is implemented as a pointer, it may change in the future. + * For this reason it should not be used as a pointer to the data, use data() instead. + */ + double* begin() noexcept; + + /** + * Returns a iterator to the beginning of the vector + * @note At the moment iterator is implemented as a pointer, it may change in the future. + * For this reason it should not be used as a pointer to the data, use data() instead. + */ + double* end() noexcept; + + std::size_t size() const; + + ///@} + + /** + * Raw data accessor + * + * @return a const pointer to a vector of size() doubles + */ + const double * data() const; + + /** + * Raw data accessor + * + * @return a pointer to a vector of size() doubles + */ + double * data(); + + /** + * Assign all element of the vector to 0. + */ + void zero(); + + /** + * Increase the capacity of the vector preserving content. + * + * @param newCapacity the new capacity of the vector + * \warning performs dynamic memory allocation operations if newCapacity > capacity() + * \note if newCapacity <= capacity(), this method does nothing and the capacity will remain unchanged. + */ + void reserve(const std::size_t newCapacity); + + /** + * Change the size of the vector preserving old content. + * + * @param newSize the new size of the vector + * \warning performs dynamic memory allocation operations if newSize > capacity() + */ + void resize(const std::size_t newSize); + + /** + * Change the capacity of the vector to match the size. + * + * \warning performs dynamic memory allocation operations if size() != capacity() + */ + void shrink_to_fit(); + + /** + * The buffer pointed by data() has size capacity()*sizeof(double) + */ + size_t capacity() const; + + /** + * Assume that buf is pointing to + * a buffer of size() doubles, and fill + * it with the content of this vector. + * + * @param buf pointer to the buffer to fill + * + * @todo provide this for all matrix types + * + * \warning use this function only if you are + * an expert C user + */ + void fillBuffer(double * buf) const; + +#if !defined(SWIG_VERSION) || SWIG_VERSION >= 0x030000 + /** Typedefs to enable make_span. + */ + ///@{ + typedef double value_type; + + typedef std::allocator allocator_type; + + typedef typename std::allocator_traits>::pointer pointer; + + typedef typename std::allocator_traits>::const_pointer const_pointer; + ///@} +#endif + + /** @name Output helpers. + * Output helpers. + */ + ///@{ + std::string toString() const; + + std::string reservedToString() const; + ///@} + + }; +} + +#endif /* IDYNTREE_VECTOR_DYN_SIZE_H */ diff --git a/src/core/include/iDynTree/VectorFixSize.h b/src/core/include/iDynTree/VectorFixSize.h new file mode 100644 index 00000000000..eab009cc358 --- /dev/null +++ b/src/core/include/iDynTree/VectorFixSize.h @@ -0,0 +1,400 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_VECTOR_FIX_SIZE_H +#define IDYNTREE_VECTOR_FIX_SIZE_H + +#include +#if !defined(SWIG_VERSION) || SWIG_VERSION >= 0x030000 +#include +#endif +#include +#include +#include +#include +#include + +namespace iDynTree +{ + /** + * Class providing a simple vector of N elements. + * The size of the vector is known at compile time, + * and it enables to avoid using dynamic memory allocation. + * + * \ingroup iDynTreeCore + */ + template class VectorFixSize + { + protected: + /** + * Storage for the VectorDynSize + * + * Array of VecSize doubles. + */ + double m_data[VecSize]; + + public: + /** + * Default constructor. + * The data is not reset to 0 for perfomance reason. + * Please initialize the data in the vector before any use. + */ + VectorFixSize(); + + /** + * Constructor from a C-style array. + * + * Print an error an build a vector full of zeros if in_size is not size(). + */ + VectorFixSize(const double * in_data, const std::size_t in_size); + +#if !defined(SWIG_VERSION) || SWIG_VERSION >= 0x030000 + /** + * Constructor from an iDynTree::Span + * + * Print an error an build a vector full of zeros if in_size is not size(). + */ + VectorFixSize(iDynTree::Span vec); +#endif + + /** + * @name Vector interface methods. + * Methods exposing a vector-like interface to VectorFixSize. + */ + ///@{ + double operator()(const std::size_t index) const; + + double& operator()(const std::size_t index); + + double operator[](const std::size_t index) const; + + double& operator[](const std::size_t index); + + double getVal(const std::size_t index) const; + + bool setVal(const std::size_t index, const double new_el); + + /** + * Returns a const iterator to the beginning of the vector + * @note At the moment iterator is implemented as a pointer, it may change in the future. + * For this reason it should not be used as a pointer to the data, use data() instead. + */ + const double* begin() const noexcept; + + /** + * Returns a const iterator to the beginning of the vector + * @note At the moment iterator is implemented as a pointer, it may change in the future. + * For this reason it should not be used as a pointer to the data, use data() instead. + */ + const double* end() const noexcept; + + /** + * Returns a const iterator to the beginning of the vector + * @note At the moment iterator is implemented as a pointer, it may change in the future. + * For this reason it should not be used as a pointer to the data, use data() instead. + */ + const double* cbegin() const noexcept; + + /** + * Returns a const iterator to the beginning of the vector + * @note At the moment iterator is implemented as a pointer, it may change in the future. + * For this reason it should not be used as a pointer to the data, use data() instead. + */ + const double* cend() const noexcept; + + /** + * Returns a iterator to the beginning of the vector + * @note At the moment iterator is implemented as a pointer, it may change in the future. + * For this reason it should not be used as a pointer to the data, use data() instead. + */ + double* begin() noexcept; + + /** + * Returns a iterator to the beginning of the vector + * @note At the moment iterator is implemented as a pointer, it may change in the future. + * For this reason it should not be used as a pointer to the data, use data() instead. + */ + double* end() noexcept; + + std::size_t size() const; + + ///@} + +#if !defined(SWIG_VERSION) || SWIG_VERSION >= 0x030000 + /** + * Copy assignment operator for spans. + * + * Checks that dimensions are matching through an assert. + * + */ + VectorFixSize & operator=(iDynTree::Span vec); +#endif + + /** + * Raw data accessor + * + * @return a const pointer to a vector of size() doubles + */ + const double * data() const; + + /** + * Raw data accessor + * + * @return a pointer to a vector of size() doubles + */ + double * data(); + + /** + * Assign all element of the vector to 0. + */ + void zero(); + + /** + * Assume that buf is pointing to + * a buffer of size() doubles, and fill + * it with the content of this vector. + * + * @param buf pointer to the buffer to fill + * + * @todo provide this for all matrix types + * + * \warning use this function only if you are + * an expert C user + */ + void fillBuffer(double * buf) const; + + /** + * @name Output helpers. + * Output helpers. + */ + ///@{ + std::string toString() const; + + std::string reservedToString() const; + ///@} + +#if !defined(SWIG_VERSION) || SWIG_VERSION >= 0x030000 + /** Typedefs to enable make_span. + */ + ///@{ + typedef double value_type; + + typedef std::allocator allocator_type; + + typedef typename std::allocator_traits>::pointer pointer; + + typedef typename std::allocator_traits>::const_pointer const_pointer; + ///@} +#endif + + }; + + //Implementation + template + VectorFixSize::VectorFixSize() + { + + } + + + template + VectorFixSize::VectorFixSize(const double* in_data, + const std::size_t in_size) + { + if( in_size != VecSize ) + { + reportError("VectorFixSize","constructor","input vector does not have the right number of elements"); + this->zero(); + } + else + { + memcpy(this->m_data,in_data,sizeof(double)*VecSize); + } + } + +#if !defined(SWIG_VERSION) || SWIG_VERSION >= 0x030000 + + template + VectorFixSize::VectorFixSize(iDynTree::Span vec) + : VectorFixSize::VectorFixSize(vec.data(), vec.size()) + {} + +#endif + + template + void VectorFixSize::zero() + { + for(std::size_t i=0; i < VecSize; i++ ) + { + this->m_data[i] = 0.0; + } + } + + + template + double* VectorFixSize::data() + { + return this->m_data; + } + + template + const double* VectorFixSize::data() const + { + return this->m_data; + } + + template + const double* VectorFixSize::begin() const noexcept + { + return this->m_data; + } + + template + const double* VectorFixSize::end() const noexcept + { + return this->m_data + VecSize; + } + + template + const double* VectorFixSize::cbegin() const noexcept + { + return this->m_data; + } + + template + const double* VectorFixSize::cend() const noexcept + { + return this->m_data + VecSize; + } + + template + double *VectorFixSize::begin() noexcept + { + return this->m_data; + } + + template + double* VectorFixSize::end() noexcept + { + return this->m_data + VecSize; + } + + template + std::size_t VectorFixSize::size() const + { + return VecSize; + } + +#if !defined(SWIG_VERSION) || SWIG_VERSION >= 0x030000 + template + VectorFixSize & VectorFixSize::operator=(iDynTree::Span vec) { + assert(VecSize == vec.size()); + std::memcpy(this->m_data, vec.data(), VecSize*sizeof(double)); + return *this; + } +#endif + + template + double VectorFixSize::operator()(const std::size_t index) const + { + assert(index < VecSize); + return this->m_data[index]; + } + + template + double & VectorFixSize::operator()(const std::size_t index) + { + assert(index < VecSize); + return this->m_data[index]; + } + + template + double VectorFixSize::operator[](const std::size_t index) const + { + assert(index < VecSize); + return this->m_data[index]; + } + + template + double & VectorFixSize::operator[](const std::size_t index) + { + assert(index < VecSize); + return this->m_data[index]; + } + + template + double VectorFixSize::getVal(const std::size_t index) const + { + if( index >= this->size() ) + { + reportError("VectorFixSize","getVal","index out of bounds"); + return 0.0; + } + + return this->m_data[index]; + } + + template + bool VectorFixSize::setVal(const std::size_t index, const double new_el) + { + if( index >= this->size() ) + { + reportError("VectorFixSize","getVal","index out of bounds"); + return false; + } + + this->m_data[index] = new_el; + + return true; + } + + template + void VectorFixSize::fillBuffer(double* buf) const + { + for(std::size_t i=0; i < this->size(); i++ ) + { + buf[i] = this->m_data[i]; + } + } + + template + std::string VectorFixSize::toString() const + { + std::stringstream ss; + + for(std::size_t i=0; i < this->size(); i++ ) + { + ss << this->m_data[i] << " "; + } + + return ss.str(); + } + + template + std::string VectorFixSize::reservedToString() const + { + std::stringstream ss; + + for(std::size_t i=0; i < this->size(); i++ ) + { + ss << this->m_data[i] << " "; + } + + return ss.str(); + } + + // Explicit instantiations + // The explicit instantiations are the only ones that can be used in the API + // and the only ones that users are supposed to manipulate manipulate + // Add all the explicit instantiation that can be useful, but remember to add + // them also in the iDynTree.i SWIG file + typedef VectorFixSize<2> Vector2; + typedef VectorFixSize<3> Vector3; + typedef VectorFixSize<4> Vector4; + typedef VectorFixSize<6> Vector6; + typedef VectorFixSize<10> Vector10; + typedef VectorFixSize<16> Vector16; + +} + +#endif /* IDYNTREE_VECTOR_FIX_SIZE_H */ diff --git a/src/core/include/iDynTree/Wrench.h b/src/core/include/iDynTree/Wrench.h new file mode 100644 index 00000000000..4763f7a46d3 --- /dev/null +++ b/src/core/include/iDynTree/Wrench.h @@ -0,0 +1,32 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_WRENCH_H +#define IDYNTREE_WRENCH_H + +#include + +namespace iDynTree +{ + /** + * Class representing a wrench, i.e. a 6D combination of linear force an angular torque. + * + * \ingroup iDynTreeCore + * + */ + class Wrench: public SpatialForceVector + { + public: + inline Wrench() {} + Wrench(const Force & _linearVec3, const Torque & _angularVec3); + Wrench(const SpatialForceVector & other); + Wrench(const Wrench & other); + + // overloaded operators + Wrench operator+(const Wrench &other) const; + Wrench operator-(const Wrench &other) const; + Wrench operator-() const; + }; +} + +#endif diff --git a/src/core/src/ArticulatedBodyInertia.cpp b/src/core/src/ArticulatedBodyInertia.cpp index 8dec7f508eb..0f09f80f4bf 100644 --- a/src/core/src/ArticulatedBodyInertia.cpp +++ b/src/core/src/ArticulatedBodyInertia.cpp @@ -2,12 +2,12 @@ // SPDX-License-Identifier: BSD-3-Clause -#include -#include -#include -#include +#include +#include +#include +#include -#include +#include #include diff --git a/src/core/src/Axis.cpp b/src/core/src/Axis.cpp index c960d4d0693..6bbf27a55b8 100644 --- a/src/core/src/Axis.cpp +++ b/src/core/src/Axis.cpp @@ -1,11 +1,11 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include diff --git a/src/core/src/ClassicalAcc.cpp b/src/core/src/ClassicalAcc.cpp index 79de71fee2c..daa9f8993f2 100644 --- a/src/core/src/ClassicalAcc.cpp +++ b/src/core/src/ClassicalAcc.cpp @@ -1,10 +1,10 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include -#include -#include -#include +#include +#include +#include +#include #include diff --git a/src/core/src/CubicSpline.cpp b/src/core/src/CubicSpline.cpp index e02a7346e98..ca8c6aee151 100644 --- a/src/core/src/CubicSpline.cpp +++ b/src/core/src/CubicSpline.cpp @@ -1,8 +1,8 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include -#include +#include +#include #include #include #include diff --git a/src/core/src/Direction.cpp b/src/core/src/Direction.cpp index 2890e5e8ff8..c0dd07b323c 100644 --- a/src/core/src/Direction.cpp +++ b/src/core/src/Direction.cpp @@ -1,9 +1,9 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include +#include -#include +#include #include diff --git a/src/core/src/GeomVector3.cpp b/src/core/src/GeomVector3.cpp index 30b567e3ce7..82b761c237d 100644 --- a/src/core/src/GeomVector3.cpp +++ b/src/core/src/GeomVector3.cpp @@ -2,9 +2,9 @@ // SPDX-License-Identifier: BSD-3-Clause -#include +#include -#include +#include #include diff --git a/src/core/src/InertiaNonLinearParametrization.cpp b/src/core/src/InertiaNonLinearParametrization.cpp index 63cd87c19f1..1fa7584b0eb 100644 --- a/src/core/src/InertiaNonLinearParametrization.cpp +++ b/src/core/src/InertiaNonLinearParametrization.cpp @@ -1,12 +1,12 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include -#include +#include +#include #include -#include +#include namespace iDynTree { diff --git a/src/core/src/MatrixDynSize.cpp b/src/core/src/MatrixDynSize.cpp index 6e4d589f577..a643a516d5a 100644 --- a/src/core/src/MatrixDynSize.cpp +++ b/src/core/src/MatrixDynSize.cpp @@ -1,8 +1,8 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include -#include +#include +#include #include #include #include diff --git a/src/core/src/Position.cpp b/src/core/src/Position.cpp index a1fe22a4df5..b9db130703a 100644 --- a/src/core/src/Position.cpp +++ b/src/core/src/Position.cpp @@ -1,15 +1,15 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include -#include -#include -#include -#include -#include -#include - -#include +#include +#include +#include +#include +#include +#include +#include + +#include #include #include diff --git a/src/core/src/PositionRaw.cpp b/src/core/src/PositionRaw.cpp index f3513615f22..d13a6ff29c6 100644 --- a/src/core/src/PositionRaw.cpp +++ b/src/core/src/PositionRaw.cpp @@ -2,11 +2,11 @@ // SPDX-License-Identifier: BSD-3-Clause -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include #include diff --git a/src/core/src/PrivateUtils.cpp b/src/core/src/PrivateUtils.cpp index 0d5c96eafa5..9ac5e6d77b8 100644 --- a/src/core/src/PrivateUtils.cpp +++ b/src/core/src/PrivateUtils.cpp @@ -1,7 +1,7 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include +#include namespace iDynTree { diff --git a/src/core/src/Rotation.cpp b/src/core/src/Rotation.cpp index f39f04b248d..c030961bb8f 100644 --- a/src/core/src/Rotation.cpp +++ b/src/core/src/Rotation.cpp @@ -1,19 +1,19 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include diff --git a/src/core/src/RotationRaw.cpp b/src/core/src/RotationRaw.cpp index f10224bfc9e..74905b63cfb 100644 --- a/src/core/src/RotationRaw.cpp +++ b/src/core/src/RotationRaw.cpp @@ -2,13 +2,13 @@ // SPDX-License-Identifier: BSD-3-Clause -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include #include #include diff --git a/src/core/src/RotationalInertiaRaw.cpp b/src/core/src/RotationalInertiaRaw.cpp index 93357235649..fe2fcc4705d 100644 --- a/src/core/src/RotationalInertiaRaw.cpp +++ b/src/core/src/RotationalInertiaRaw.cpp @@ -1,8 +1,8 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include -#include +#include +#include #include #include diff --git a/src/core/src/SO3Utils.cpp b/src/core/src/SO3Utils.cpp index 95ff69d2910..d0d1241b6af 100644 --- a/src/core/src/SO3Utils.cpp +++ b/src/core/src/SO3Utils.cpp @@ -2,9 +2,9 @@ // SPDX-License-Identifier: BSD-3-Clause -#include -#include -#include +#include +#include +#include #include #include diff --git a/src/core/src/SpatialAcc.cpp b/src/core/src/SpatialAcc.cpp index 36f294c6cdc..36ac9b461d4 100644 --- a/src/core/src/SpatialAcc.cpp +++ b/src/core/src/SpatialAcc.cpp @@ -1,8 +1,8 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include -#include +#include +#include namespace iDynTree { diff --git a/src/core/src/SpatialForceVector.cpp b/src/core/src/SpatialForceVector.cpp index 3c10d8e717b..f0166ef17d3 100644 --- a/src/core/src/SpatialForceVector.cpp +++ b/src/core/src/SpatialForceVector.cpp @@ -1,10 +1,10 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include -#include +#include +#include -#include +#include #include diff --git a/src/core/src/SpatialInertia.cpp b/src/core/src/SpatialInertia.cpp index a1d68d1744c..b79bbad083d 100644 --- a/src/core/src/SpatialInertia.cpp +++ b/src/core/src/SpatialInertia.cpp @@ -1,15 +1,15 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include #include -#include +#include #include #include diff --git a/src/core/src/SpatialInertiaRaw.cpp b/src/core/src/SpatialInertiaRaw.cpp index b1dc3fe04f1..489323771e9 100644 --- a/src/core/src/SpatialInertiaRaw.cpp +++ b/src/core/src/SpatialInertiaRaw.cpp @@ -2,12 +2,12 @@ // SPDX-License-Identifier: BSD-3-Clause -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include #include diff --git a/src/core/src/SpatialMomentum.cpp b/src/core/src/SpatialMomentum.cpp index 7e1a7fb4580..175c3553723 100644 --- a/src/core/src/SpatialMomentum.cpp +++ b/src/core/src/SpatialMomentum.cpp @@ -1,8 +1,8 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include -#include +#include +#include namespace iDynTree { diff --git a/src/core/src/SpatialMotionVector.cpp b/src/core/src/SpatialMotionVector.cpp index 26f117270a1..dabb0e676f6 100644 --- a/src/core/src/SpatialMotionVector.cpp +++ b/src/core/src/SpatialMotionVector.cpp @@ -1,14 +1,14 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include -#include -#include -#include -#include - -#include -#include +#include +#include +#include +#include +#include + +#include +#include #include diff --git a/src/core/src/TestUtils.cpp b/src/core/src/TestUtils.cpp index 0a51c2dbc2f..dc9221edd70 100644 --- a/src/core/src/TestUtils.cpp +++ b/src/core/src/TestUtils.cpp @@ -1,14 +1,14 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include #include diff --git a/src/core/src/Transform.cpp b/src/core/src/Transform.cpp index 79810047ce4..119873ce8d7 100644 --- a/src/core/src/Transform.cpp +++ b/src/core/src/Transform.cpp @@ -1,21 +1,21 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include #include diff --git a/src/core/src/TransformDerivative.cpp b/src/core/src/TransformDerivative.cpp index 0166e80b9cd..f3d6cbcf1cd 100644 --- a/src/core/src/TransformDerivative.cpp +++ b/src/core/src/TransformDerivative.cpp @@ -1,13 +1,13 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include -#include -#include +#include +#include +#include #include -#include +#include typedef Eigen::Matrix Matrix3dRowMajor; diff --git a/src/core/src/Twist.cpp b/src/core/src/Twist.cpp index 7c84a0ab15a..8d52d5203c8 100644 --- a/src/core/src/Twist.cpp +++ b/src/core/src/Twist.cpp @@ -1,11 +1,11 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include namespace iDynTree { diff --git a/src/core/src/Utils.cpp b/src/core/src/Utils.cpp index f52acac3393..f0525d78315 100644 --- a/src/core/src/Utils.cpp +++ b/src/core/src/Utils.cpp @@ -1,7 +1,7 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include +#include #include diff --git a/src/core/src/VectorDynSize.cpp b/src/core/src/VectorDynSize.cpp index e1eab861473..ebb7bcb9812 100644 --- a/src/core/src/VectorDynSize.cpp +++ b/src/core/src/VectorDynSize.cpp @@ -1,8 +1,8 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include -#include +#include +#include #include diff --git a/src/core/src/Wrench.cpp b/src/core/src/Wrench.cpp index b52c257e8ed..91d384d6164 100644 --- a/src/core/src/Wrench.cpp +++ b/src/core/src/Wrench.cpp @@ -1,8 +1,8 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include -#include +#include +#include namespace iDynTree { diff --git a/src/core/tests/ArticulatedBodyInertiaUnitTest.cpp b/src/core/tests/ArticulatedBodyInertiaUnitTest.cpp index a0e3b14e5cb..82db7350492 100644 --- a/src/core/tests/ArticulatedBodyInertiaUnitTest.cpp +++ b/src/core/tests/ArticulatedBodyInertiaUnitTest.cpp @@ -1,17 +1,17 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include diff --git a/src/core/tests/AxisUnitTest.cpp b/src/core/tests/AxisUnitTest.cpp index fe6cdb34b31..905dcdb9d41 100644 --- a/src/core/tests/AxisUnitTest.cpp +++ b/src/core/tests/AxisUnitTest.cpp @@ -1,14 +1,14 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include -#include -#include -#include -#include -#include - -#include +#include +#include +#include +#include +#include +#include + +#include #include #include diff --git a/src/core/tests/CubicSplineUnitTest.cpp b/src/core/tests/CubicSplineUnitTest.cpp index c00775b9cfe..5d0941ff7c8 100644 --- a/src/core/tests/CubicSplineUnitTest.cpp +++ b/src/core/tests/CubicSplineUnitTest.cpp @@ -1,9 +1,9 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include "iDynTree/Core/CubicSpline.h" -#include "iDynTree/Core/TestUtils.h" -#include "iDynTree/Core/VectorFixSize.h" +#include "iDynTree/CubicSpline.h" +#include "iDynTree/TestUtils.h" +#include "iDynTree/VectorFixSize.h" #include using namespace iDynTree; diff --git a/src/core/tests/DirectionUnitTest.cpp b/src/core/tests/DirectionUnitTest.cpp index 9d3c20bb4bc..1f86220a194 100644 --- a/src/core/tests/DirectionUnitTest.cpp +++ b/src/core/tests/DirectionUnitTest.cpp @@ -1,9 +1,9 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include -#include -#include +#include +#include +#include #include diff --git a/src/core/tests/EigenHelpersUnitTest.cpp b/src/core/tests/EigenHelpersUnitTest.cpp index 40994ad7374..28d05bbbb06 100644 --- a/src/core/tests/EigenHelpersUnitTest.cpp +++ b/src/core/tests/EigenHelpersUnitTest.cpp @@ -1,8 +1,8 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include -#include +#include +#include #include using namespace iDynTree; diff --git a/src/core/tests/EigenSparseHelpersUnitTest.cpp b/src/core/tests/EigenSparseHelpersUnitTest.cpp index 87a2d3432df..01c1fdaf1ee 100644 --- a/src/core/tests/EigenSparseHelpersUnitTest.cpp +++ b/src/core/tests/EigenSparseHelpersUnitTest.cpp @@ -1,9 +1,9 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include -#include -#include +#include +#include +#include #include #include diff --git a/src/core/tests/ExpLogUnitTest.cpp b/src/core/tests/ExpLogUnitTest.cpp index 30662f36236..59a694e2ed5 100644 --- a/src/core/tests/ExpLogUnitTest.cpp +++ b/src/core/tests/ExpLogUnitTest.cpp @@ -1,11 +1,11 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include #include diff --git a/src/core/tests/MatrixDynSizeUnitTest.cpp b/src/core/tests/MatrixDynSizeUnitTest.cpp index eb96093ba0f..4f481b1c49a 100644 --- a/src/core/tests/MatrixDynSizeUnitTest.cpp +++ b/src/core/tests/MatrixDynSizeUnitTest.cpp @@ -1,8 +1,8 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include -#include +#include +#include using namespace iDynTree; diff --git a/src/core/tests/MatrixViewUnitTest.cpp b/src/core/tests/MatrixViewUnitTest.cpp index 97f0ffe60cd..86ca691bb58 100644 --- a/src/core/tests/MatrixViewUnitTest.cpp +++ b/src/core/tests/MatrixViewUnitTest.cpp @@ -1,10 +1,10 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include -#include -#include -#include +#include +#include +#include +#include using namespace iDynTree; diff --git a/src/core/tests/PrivateUtilsUnitTest.cpp b/src/core/tests/PrivateUtilsUnitTest.cpp index 23407d5323e..56e151a3c70 100644 --- a/src/core/tests/PrivateUtilsUnitTest.cpp +++ b/src/core/tests/PrivateUtilsUnitTest.cpp @@ -1,11 +1,11 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include #include diff --git a/src/core/tests/RotationUnitTest.cpp b/src/core/tests/RotationUnitTest.cpp index c8d236455a4..bbe2a1e6e92 100644 --- a/src/core/tests/RotationUnitTest.cpp +++ b/src/core/tests/RotationUnitTest.cpp @@ -1,10 +1,10 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include +#include -#include -#include +#include +#include #include #include diff --git a/src/core/tests/SO3UtilsUnitTest.cpp b/src/core/tests/SO3UtilsUnitTest.cpp index db65a3cf140..2dee63ae935 100644 --- a/src/core/tests/SO3UtilsUnitTest.cpp +++ b/src/core/tests/SO3UtilsUnitTest.cpp @@ -2,10 +2,10 @@ // SPDX-License-Identifier: BSD-3-Clause -#include -#include -#include -#include +#include +#include +#include +#include #include #include diff --git a/src/core/tests/SpanUnitTest.cpp b/src/core/tests/SpanUnitTest.cpp index af8a7ceb3a6..5e6d3f9c309 100644 --- a/src/core/tests/SpanUnitTest.cpp +++ b/src/core/tests/SpanUnitTest.cpp @@ -19,8 +19,8 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include -#include +#include +#include #include diff --git a/src/core/tests/SparseMatrixUnitTest.cpp b/src/core/tests/SparseMatrixUnitTest.cpp index a2cbb0aadd3..fa42429adbc 100644 --- a/src/core/tests/SparseMatrixUnitTest.cpp +++ b/src/core/tests/SparseMatrixUnitTest.cpp @@ -1,9 +1,9 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include -#include -#include +#include +#include +#include #include diff --git a/src/core/tests/SpatialAccUnitTest.cpp b/src/core/tests/SpatialAccUnitTest.cpp index 854d83c1d51..43505d776f2 100644 --- a/src/core/tests/SpatialAccUnitTest.cpp +++ b/src/core/tests/SpatialAccUnitTest.cpp @@ -1,13 +1,13 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include #include diff --git a/src/core/tests/SpatialInertiaUnitTest.cpp b/src/core/tests/SpatialInertiaUnitTest.cpp index a13ff9d1ea9..3513816e146 100644 --- a/src/core/tests/SpatialInertiaUnitTest.cpp +++ b/src/core/tests/SpatialInertiaUnitTest.cpp @@ -1,17 +1,17 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include diff --git a/src/core/tests/SpatialToEigenCompilationErrorTest.cpp b/src/core/tests/SpatialToEigenCompilationErrorTest.cpp index 38f28592833..a2a99f3cc22 100644 --- a/src/core/tests/SpatialToEigenCompilationErrorTest.cpp +++ b/src/core/tests/SpatialToEigenCompilationErrorTest.cpp @@ -1,10 +1,10 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include -#include -#include -#include +#include +#include +#include +#include #include using namespace iDynTree; diff --git a/src/core/tests/TransformFromMatrix4x4UnitTest.cpp b/src/core/tests/TransformFromMatrix4x4UnitTest.cpp index d1528c9754e..49b769c362d 100644 --- a/src/core/tests/TransformFromMatrix4x4UnitTest.cpp +++ b/src/core/tests/TransformFromMatrix4x4UnitTest.cpp @@ -1,12 +1,12 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include #include #include diff --git a/src/core/tests/TwistUnitTest.cpp b/src/core/tests/TwistUnitTest.cpp index d1eb8f18229..9ee8745d28b 100644 --- a/src/core/tests/TwistUnitTest.cpp +++ b/src/core/tests/TwistUnitTest.cpp @@ -1,10 +1,10 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include -#include -#include -#include +#include +#include +#include +#include #include diff --git a/src/core/tests/VectorDynSizeUnitTest.cpp b/src/core/tests/VectorDynSizeUnitTest.cpp index cc1588b1794..be40bd86b4a 100644 --- a/src/core/tests/VectorDynSizeUnitTest.cpp +++ b/src/core/tests/VectorDynSizeUnitTest.cpp @@ -1,9 +1,9 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include -#include -#include +#include +#include +#include using namespace iDynTree; diff --git a/src/core/tests/WrenchUnitTest.cpp b/src/core/tests/WrenchUnitTest.cpp index 32c6ab20dcf..aef4fa6c1be 100644 --- a/src/core/tests/WrenchUnitTest.cpp +++ b/src/core/tests/WrenchUnitTest.cpp @@ -1,11 +1,11 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include diff --git a/src/estimation/CMakeLists.txt b/src/estimation/CMakeLists.txt index ef9289785d6..8d9b16abfb4 100644 --- a/src/estimation/CMakeLists.txt +++ b/src/estimation/CMakeLists.txt @@ -2,22 +2,22 @@ # SPDX-License-Identifier: BSD-3-Clause -set(IDYNTREE_ESTIMATION_HEADERS include/iDynTree/Estimation/BerdyHelper.h - include/iDynTree/Estimation/ExternalWrenchesEstimation.h - include/iDynTree/Estimation/ExtWrenchesAndJointTorquesEstimator.h - include/iDynTree/Estimation/SimpleLeggedOdometry.h - include/iDynTree/Estimation/BerdySparseMAPSolver.h - include/iDynTree/Estimation/SchmittTrigger.h - include/iDynTree/Estimation/ContactStateMachine.h - include/iDynTree/Estimation/BipedFootContactClassifier.h - include/iDynTree/Estimation/GravityCompensationHelpers.h - include/iDynTree/Estimation/ExtendedKalmanFilter.h - include/iDynTree/Estimation/AttitudeEstimator.h - include/iDynTree/Estimation/AttitudeMahonyFilter.h - include/iDynTree/Estimation/AttitudeQuaternionEKF.h - include/iDynTree/Estimation/KalmanFilter.h ) - -set(IDYNTREE_ESTIMATION_PRIVATE_INCLUDES include/iDynTree/Estimation/AttitudeEstimatorUtils.h) +set(IDYNTREE_ESTIMATION_HEADERS include/iDynTree/BerdyHelper.h + include/iDynTree/ExternalWrenchesEstimation.h + include/iDynTree/ExtWrenchesAndJointTorquesEstimator.h + include/iDynTree/SimpleLeggedOdometry.h + include/iDynTree/BerdySparseMAPSolver.h + include/iDynTree/SchmittTrigger.h + include/iDynTree/ContactStateMachine.h + include/iDynTree/BipedFootContactClassifier.h + include/iDynTree/GravityCompensationHelpers.h + include/iDynTree/ExtendedKalmanFilter.h + include/iDynTree/AttitudeEstimator.h + include/iDynTree/AttitudeMahonyFilter.h + include/iDynTree/AttitudeQuaternionEKF.h + include/iDynTree/KalmanFilter.h ) + +set(IDYNTREE_ESTIMATION_PRIVATE_INCLUDES include/iDynTree/AttitudeEstimatorUtils.h) set(IDYNTREE_ESTIMATION_SOURCES src/BerdyHelper.cpp src/ExternalWrenchesEstimation.cpp @@ -46,7 +46,7 @@ add_library(iDynTree::${libraryname} ALIAS ${libraryname}) target_include_directories(${libraryname} PUBLIC "$" "$") -target_link_libraries(${libraryname} PUBLIC idyntree-core idyntree-model idyntree-sensors idyntree-modelio +target_link_libraries(${libraryname} PUBLIC idyntree-core idyntree-model idyntree-modelio PRIVATE Eigen3::Eigen) target_compile_options(${libraryname} PRIVATE ${IDYNTREE_WARNING_FLAGS}) @@ -59,8 +59,8 @@ install(TARGETS ${libraryname} RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT bin LIBRARY DESTINATION "${CMAKE_INSTALL_LIBDIR}" COMPONENT shlib ARCHIVE DESTINATION "${CMAKE_INSTALL_LIBDIR}" COMPONENT lib - PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/iDynTree/Estimation - PRIVATE_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/iDynTree/Estimation/impl) + PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/iDynTree + PRIVATE_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/iDynTree/impl) set_property(GLOBAL APPEND PROPERTY ${VARS_PREFIX}_TARGETS ${libraryname}) @@ -75,6 +75,10 @@ if(CMAKE_VERSION VERSION_GREATER 3.17) PROPERTIES DEPRECATION "Do not use deprecated target iDynTree::idyntree-modelio-urdf, use iDynTree::idyntree-modelio instead.") endif() +# Install deprecated headers +install(DIRECTORY include/iDynTree/Estimation + DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/iDynTree) + if(IDYNTREE_COMPILE_TESTS) add_subdirectory(tests) endif(IDYNTREE_COMPILE_TESTS) diff --git a/src/estimation/include/iDynTree/AttitudeEstimator.h b/src/estimation/include/iDynTree/AttitudeEstimator.h new file mode 100644 index 00000000000..eae14a080d1 --- /dev/null +++ b/src/estimation/include/iDynTree/AttitudeEstimator.h @@ -0,0 +1,189 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_ATTITUDEESTIMATOR_H +#define IDYNTREE_ATTITUDEESTIMATOR_H + +#include +#include + +#include + +namespace iDynTree +{ + typedef iDynTree::Vector3 LinearAccelerometerMeasurements; + typedef iDynTree::Vector3 GyroscopeMeasurements; + typedef iDynTree::Vector3 MagnetometerMeasurements; + + typedef iDynTree::Vector4 UnitQuaternion; + typedef iDynTree::Vector3 RPY; + + /** @struct state internal state of the estimator + * @var state::m_orientation + * orientation estimate in \f$ \mathbb{R}^4 \f$ quaternion representation + * @var state::m_orientation + * angular velocity estimate in \f$ \mathbb{R}^3 \f$ + * @var state::m_orientation + * gyroscope bias estimate in \f$ \mathbb{R}^3 \f$ + */ + struct AttitudeEstimatorState + { + iDynTree::UnitQuaternion m_orientation; + iDynTree::Vector3 m_angular_velocity; + iDynTree::Vector3 m_gyroscope_bias; + }; + + /** + * @class IAttitudeEstimator generic interface for attitude estimator classes + * + * The aim is to implement different attitude estimators as a block that takes IMU measurements + * as inputs and gives attitude estimates as outputs. This way the underlying implementation is abstracted + * and the user only has to set a few parameters and run the estimator. + * + * The general procedure to use the estimators would be, + * - instantiate the filter, + * - set initial internal state + * - in a loop, + * - update the filter with measurements + * - propagate the states + * + * However, additional methods to set and get parameters for the filter might be available with respect to the filters. + * + * The internal state of the estimator is described as \f$ X = \begin{bmatrix} {^A}q_B \\ {^B}\Omega_{A,B} \\ {^B}b \end{bmatrix}^T \f$ + * \f$ {^A}q_B \in \mathbb{R}^4 \f$ is the quaternion representing the orientation of a body(IMU) frame with respect to an inertial frame , + * \f$ {^B}\Omega_{A,B} \in \mathbb{R}^3 \f$ is the angular velocity of a body(IMU) frame with respect to an inertial frame, expressed in the body frame and + * \f$ {^B}b \in \mathbb{R}^3 \f$ is the gyroscope bias expressed in the body frame. + * + */ + class IAttitudeEstimator + { + public: + virtual ~IAttitudeEstimator(); + + /** + * @brief Update the filter with accelerometer and gyroscope measurements + * + * @param[in] linAccMeas proper (body acceleration - gravity) classical acceleration of the origin of the body frame B expressed in frame B + * @param[in] gyroMeas angular velocity of body frame B with respect to an inertial fram A, expressed in frame B + * + * @note consider the current behavior of our system does not use magnetometer measurements and is calling this method to update measurements. + * Then, if we decide to turn the flag use_magnetometer_measurements to true, this will not guarantee that the magnetometer measurements + * will be used. The magnetometer measurements will be used only if we replace this function call with the other overlaoded function considering + * the magnetometer measurements. + * + * @return true/false if successful/not + */ + virtual bool updateFilterWithMeasurements(const iDynTree::LinearAccelerometerMeasurements& linAccMeas, + const iDynTree::GyroscopeMeasurements& gyroMeas) = 0; + + /** + * @brief Update the filter with accelerometer, gyroscope and magnetometer measurements + * + * @param[in] linAccMeas proper (body acceleration - gravity) classical acceleration of the origin of the body frame B expressed in frame B + * @param[in] gyroMeas angular velocity of body frame B with respect to an inertial fram A, expressed in frame B + * @param[in] magMeas magnetometer measurements expressed in frame B + * + * @return true/false if successful/not + */ + virtual bool updateFilterWithMeasurements(const iDynTree::LinearAccelerometerMeasurements& linAccMeas, + const iDynTree::GyroscopeMeasurements& gyroMeas, + const iDynTree::MagnetometerMeasurements& magMeas) = 0; + + /** + * @brief Propagate the states and associated uncertainties through properly defined propagation functions + * The underlying implementation depends on the type of filter being implemented. + * + * @return true/false if successful/not + */ + virtual bool propagateStates() = 0; + + /** + * @brief Get orientation of the body with respect to inertial frame, in rotation matrix form + * If we denote \f$ A \f$ as inertial frame and \f$ B \f$ as the frame attached to the body, + * then this method gives us \f$ {^A}R_B \f$ as the rotation matrix + * @param[out] rot Rotation matrix + * @return true/false if successful/not + */ + virtual bool getOrientationEstimateAsRotationMatrix(iDynTree::Rotation& rot) = 0; + + /** + * @brief Get orientation of the body with respect to inertial frame, in unit quaternion form + * If we denote \f$ A \f$ as inertial frame and \f$ B \f$ as the frame attached to the body, + * then this method gives us \f$ {^A}q_B as the quaternion \f$ + * + * @note quaternion has the form (real, imaginary) and is normalized + * @note Usually a rotation matrix can be described using two quaternions due to its double-connectedness property + * Depending on the specific filter, the initial state and the trajectory of the system, we could obtain + * one quaternion or the other(opposite spin), depending on the system dynamics. + * + * @param[out] q UnitQuaternion + * @return true/false if successful/not + */ + virtual bool getOrientationEstimateAsQuaternion(iDynTree::UnitQuaternion& q) = 0; + + /** + * @brief Get orientation of the body with respect to inertial frame, in Euler's RPY form + * If we denote \f$ A \f$ as inertial frame and \f$ B \f$ as the frame attached to the body, + * then this method gives us the RPY 3d vector of Euler Angles when composed together gives us \f$ {^A}R_B \f$ as the rotation matrix + * where \f$ {^A}R_B = Rot_z(yaw)Rot_y(pitch)Rot_x(roll)\f$. + * For more details about the range of the RPY Euler angles, please refer the documentation of + * GetRPY() + * + * @param[out] rpy 3D vector containing roll pitch yaw angles + * @return true/false if successful/not + */ + virtual bool getOrientationEstimateAsRPY(iDynTree::RPY& rpy) = 0; + + /** + * @brief Get dimension of the state vector + * @return size_t size of state vector + */ + virtual size_t getInternalStateSize() const = 0; + + /** + * @brief Get internal state of the estimator + * The internal state of the estimator is described as \f$ X = \begin{bmatrix} {^A}q_B \\ {^B}\Omega_{A,B} \\ {^B}b \end{bmatrix}^T \f$ + * \f$ {^A}q_B \in \mathbb{R}^4 \f$ is the quaternion representing the orientation of a body(IMU) frame with respect to an inertial frame , + * \f$ {^B}\Omega_{A,B} \in \mathbb{R}^3 \f$ is the angular velocity of a body(IMU) frame with respect to an inertial frame, expressed in the body frame and + * \f$ {^B}b \in \mathbb{R}^3 \f$ is the gyroscope bias expressed in the body frame. + * The default internal state of the estimator would be \f$ X = \begin{bmatrix} 1.0 \\ 0_{1 \times 3} \\ 0_{1 \times 3} \\ 0_{1 \times 3} \end{bmatrix}^T \f$ + * @param[out] stateBuffer Span object as reference of the container where state vector should be copied to + * @return true/false if successful/not + */ + virtual bool getInternalState(const iDynTree::Span & stateBuffer) const = 0; + + /** + * @brief Get initial internal state of the estimator + * The internal state of the estimator is described as \f$ X = \begin{bmatrix} {^A}q_B \\ {^B}\Omega_{A,B} \\ {^B}b \end{bmatrix}^T \f$ + * \f$ {^A}q_B \in \mathbb{R}^4 \f$ is the quaternion representing the orientation of a body(IMU) frame with respect to an inertial frame , + * \f$ {^B}\Omega_{A,B} \in \mathbb{R}^3 \f$ is the angular velocity of a body(IMU) frame with respect to an inertial frame, expressed in the body frame and + * \f$ {^B}b \in \mathbb{R}^3 \f$ is the gyroscope bias expressed in the body frame. + * The default internal state of the estimator would be \f$ X = \begin{bmatrix} 1.0 \\ 0_{1 \times 3} \\ 0_{1 \times 3} \\ 0_{1 \times 3} \end{bmatrix}^T \f$ + * @param[out] stateBuffer Span object as reference of the container where state vector should be copied to + * @return true/false if successful/not + */ + virtual bool getDefaultInternalInitialState(const iDynTree::Span & stateBuffer) const = 0; + + /** + * @brief set internal state of the estimator. + * The internal state of the estimator is described as \f$ X = \begin{bmatrix} {^A}q_B \\ {^B}\Omega_{A,B} \\ {^B}b \end{bmatrix}^T \f$ + * \f$ {^A}q_B \in \mathbb{R}^4 \f$ is the quaternion representing the orientation of a body(IMU) frame with respect to an inertial frame , + * \f$ {^B}\Omega_{A,B} \in \mathbb{R}^3 \f$ is the angular velocity of a body(IMU) frame with respect to an inertial frame, expressed in the body frame and + * \f$ {^B}b \in \mathbb{R}^3 \f$ is the gyroscope bias expressed in the body frame. + * @param[in] stateBuffer Span object as reference of the container from which the internal state vector should be assigned. The size of the buffer should be 10. + * @return true/false if successful/not + */ + virtual bool setInternalState(const iDynTree::Span & stateBuffer) = 0; + + /** + * @brief set the initial orientation for the internal state of the estimator. + * The initial orientation for the internal state of the estimator is described as \f$ {^A}q_B \f$ + * \f$ {^A}q_B \in \mathbb{R}^4 \f$ is the quaternion representing the orientation of a body(IMU) frame with respect to an inertial frame , + * @param[in] stateBuffer Span object as reference of the container from which the inital orientaiton for internal state vector should be assigned. The size of the buffer should be 4. + * @return true/false if successful/not + */ + virtual bool setInternalStateInitialOrientation(const iDynTree::Span& orientationBuffer) = 0; + }; + +} +#endif diff --git a/src/estimation/include/iDynTree/AttitudeEstimatorUtils.h b/src/estimation/include/iDynTree/AttitudeEstimatorUtils.h new file mode 100644 index 00000000000..b8fe587c871 --- /dev/null +++ b/src/estimation/include/iDynTree/AttitudeEstimatorUtils.h @@ -0,0 +1,187 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_ATTITUDE_ESTIMATOR_UTILS_H +#define IDYNTREE_ATTITUDE_ESTIMATOR_UTILS_H + +#include +#include +#define _USE_MATH_DEFINES +#include +#include + +/** + * + * @brief check a valid measurement + * @param[in] a vector3 + * @return bool true/false + */ +bool checkValidMeasurement(const iDynTree::Vector3& in, const std::string& measurement_type, bool check_also_zero_vector); + +/** + * + * @brief get unit vector + * @param[in] a vector3 + * @return bool false if input vector has zero norm + */ +bool getUnitVector(const iDynTree::Vector3& in, iDynTree::Vector3& out); + +/** + * + * @brief checks if vector has NaN values + * any element of vector is NaN implies a NaN vector + * @param[in] vec vector3 + * @return bool true/false + */ +bool isVectorNaN(const iDynTree::Vector3& vec); + +/** + * + * @brief checks if vector is a zero vector + * all elements of vector are zero implies a zero vector + * @param[in] vec vector3 + * @return bool true/false + */ +bool isZeroVector(const iDynTree::Vector3& vec); + +/** + * + * @brief computes the cross vector of two 3D vectors + * @param[in] a 3D vector + * @param[in] b 3D vector + * @return iDynTree::Vector3 + */ +iDynTree::Vector3 crossVector(const iDynTree::Vector3& a, const iDynTree::Vector3& b); + +/** + * @brief computes \f$ 3 \times 3 \f$ skew-symmetric matrix (\f$ \mathbb{so}(3) \f$ space) for a given 3d vector (\f$ \mathbb{R}^3 \f$ space) + * + * @param[in] omega 3d vector (usually angular velocity) + * @return iDynTree::Matrix3x3 + */ +iDynTree::Matrix3x3 mapR3Toso3(const iDynTree::Vector3& omega); + +/** + * @brief checks if the \f$ 3 \times 3 \f$ matrix is skew-symmetric + * \f[ S + S^T = 0 \f] + * @param[in] S \f$ 3 \times 3 \f$ matrix + * @return bool true/false + */ +bool checkSkewSymmetricity(const iDynTree::Matrix3x3& S); + +/** + * @brief computes 3D vector (\f$ \mathbb{R}^3 \f$ space) from a skew symmetric matrix (\f$ \mathbb{so}(3) \$ space) + * + * @param[in] S \f$ 3 \times 3 \f$ matrix + * @return iDynTree::Vector3 + */ +iDynTree::Vector3 mapso3ToR3(const iDynTree::Matrix3x3& S); + +/** + * @brief computes scalar dot product of two 3-d vectors + * Maps (\f$ \mathbb{R}^n \f$ space) to (\f$ \mathbb{R} \f$ space) through its dual vector + * @param[in] a 3D vector (not passed as reference, but as a copy to avoid changes in source due to in-place manipulation) + * @param[in] b 3D vector + * @return double + */ +double innerProduct(const iDynTree::Vector3 a, const iDynTree::Vector3& b); + +/** + * @brief real part of quaternion, + * \f$ s \f$ in \f$ s + i v_1 + i v_2 + i v_3 \f$ + * + * @param[in] q 4d vector or quaternion + * @return double + */ +double realPartOfQuaternion(const iDynTree::UnitQuaternion& q); + +/** + * @brief imaginary part of quaternion + * \f$ v \f$ in \f$ s + i v_1 + i v_2 + i v_3 \f$ + * @param[in] q 4d vector or quaternion + * @return iDynTree::Vector3 + */ +iDynTree::Vector3 imaginaryPartOfQuaternion(const iDynTree::UnitQuaternion& q); + +/** + * @brief composition operator - quaternion multiplication + * + * @param[in] q1 4d vector or quaternion + * @param[in] q2 4d vector or quaternion + * @return iDynTree::UnitQuaternion + */ +iDynTree::UnitQuaternion composeQuaternion(const iDynTree::UnitQuaternion& q1, const iDynTree::UnitQuaternion& q2); + +/** + * @brief computes the matrix map of quaternion left multiplication \f$ q1 \circ q2 = q1q2 \f$ + * in opposition to the right multiplication \f$ q1 \circ q2 = q2q1 \f$ + * @param[in] x 4d vector or quaternion q1 + * @return iDynTree::Matrix4x4 + */ +iDynTree::Matrix4x4 mapofYQuaternionToXYQuaternion(const iDynTree::UnitQuaternion &x); + + +/** + * @brief composition operator - quaternion multiplication + * this method is faster than composeQuaternion() + * @param[in] q1 4d vector or quaternion + * @param[in] q2 4d vector or quaternion + * @return iDynTree::UnitQuaternion + */ +iDynTree::UnitQuaternion composeQuaternion2(const iDynTree::UnitQuaternion &q1, const iDynTree::UnitQuaternion &q2); + + +/** + * @brief computes pure quaternion given a 3d vector (uually angular velocity) + * + * @param[in] bodyFixedFrameVelocityInInertialFrame 3d vector + * @return iDynTree::UnitQuaternion + */ +iDynTree::UnitQuaternion pureQuaternion(const iDynTree::Vector3& bodyFixedFrameVelocityInInertialFrame); + + +/** + * @brief exponential map for quaternion - maps angular velocities to quaternion + * + * \f$ \text{exp}(\omega) = \begin{bmatrix} \text{cos}(\frac{||\omega||}{2}) \\ \text{sin}(\frac{||\omega||}{2})\frac{\omega}{||\omega||} \end{bmatrix} \f$ + * + * @param[in] omega angular velocity + * @return iDynTree::UnitQuaternion + */ +inline iDynTree::UnitQuaternion expQuaternion(iDynTree::Vector3 omega) +{ + iDynTree::UnitQuaternion q; + q.zero(); + q(0) = 1.0; + using iDynTree::toEigen; + double norm{toEigen(omega).norm()}; + + if (norm == 0) + { + return q; + } + + double c = std::cos(norm/2); + double s = std::sin(norm/2)/norm; + + q(0) = c; + q(1) = omega(0)*s; + q(2) = omega(1)*s; + q(3) = omega(2)*s; + + return q; +} + +template +inline bool check_are_almost_equal(const T& x, const T& y, int units_in_last_place) +{ + if (!( std::abs(x- y) <= std::numeric_limits::epsilon()*std::max(std::abs(x), std::abs(y))*units_in_last_place)) + { + return false; + } + + return true; +} + +#endif + diff --git a/src/estimation/include/iDynTree/AttitudeMahonyFilter.h b/src/estimation/include/iDynTree/AttitudeMahonyFilter.h new file mode 100644 index 00000000000..7e59d694a7c --- /dev/null +++ b/src/estimation/include/iDynTree/AttitudeMahonyFilter.h @@ -0,0 +1,154 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef ATTITUDE_MAHONY_FILTER_H +#define ATTITUDE_MAHONY_FILTER_H + +#include +#include + +namespace iDynTree +{ + +/** +* @struct AttitudeMahonyFilterParameters Parameters to set up the quaternion EKF +* @var AttitudeMahonyFilterParameters::time_step_in_seconds +* discretization time step in seconds, default value: \f$ 0.001 s \f$ +* @var AttitudeMahonyFilterParameters::kp +* Mahony \f$ K_p \f$ gain over the correction from IMU measurements, default value: \f$ 1.0 \f$ +* @var AttitudeMahonyFilterParameters::ki +* Mahony \f$ K_i \f$ gain over the gyro bias evolution, default value: \f$ 1.0 \f$ +* @var AttitudeMahonyFilterParameters::use_magenetometer_measurements +* flag to enable the use of magnetometer measurement for yaw correction, default value: false +* @var AttitudeMahonyFilterParameters::confidence_magnetometer_measurements +* confidence on magnetometer measurements, default value: \f$ 0.0 \f$ +*/ +struct AttitudeMahonyFilterParameters { + double time_step_in_seconds{0.001}; + double kp{1.0}; + double ki{1.0}; + bool use_magnetometer_measurements{false}; + double confidence_magnetometer_measurements{0.0}; +}; + + +/** + * @class AttitudeMahonyFilter Implements an explicit passive complementary filter on quaternion groups + * described in the paper Non-linear complementary filters on SO3 groups + * + * The filter is used to estimate the states \f$ X = \begin{bmatrix} {^A}q_B \\ {^B}\Omega_{A,B} \\ {^B}b \end{bmatrix}^T \f$ + * where \f$ {^A}q_B \in \mathbb{R}^4 \f$ is the quaternion representing the orientation of a body(IMU) frame with respect to an inertial frame , + * \f$ {^B}\Omega_{A,B} \in \mathbb{R}^3 \f$ is the angular velocity of a body(IMU) frame with respect to an inertial frame, expressed in the body frame and + * \f$ {^B}b \in \mathbb{R}^3 \f$ is the gyroscope bias expressed in the body frame. + * @note: we will drop the subscripts and superscripts in the rest of the documentation for convenience + * + * The discretized dynamics of the filter is implemented in the propagateStates() method and is described by the following equations, + * \f$ q_{k+1} = q_{k} + \Delta t \frac{1}{2}q_{k} \circ \begin{bmatrix} 0 \\ {\Omega_y}_{k+1} - b_k + K_p \omega_{mes_{k+1}}\end{bmatrix}\f$ + * \f$ \Omega_{k+1} = {\Omega_y}_{k+1} - b_k \f$ + * \f$ b_{k+1} = b_k - K_i \Delta t \frac{1}{2} \omega_{mes_{k+1}} \f$ + * + * The updateFilterWithMeasurements() uses the recent IMU measurements to compute the term \f$ \omega_{mes} \f$ which gives the vectorial from accelerometer and magnetometer measurements + * \f$ \omega_{mes} = -(\Sigma{n}{i=1} \frac{k_i}{2} (v_i \hat{v}_i^T - \hat{v}_i v_i^T) )^{\vee} \f$ + * where \f$ v_i \f$ is the normalized accelerometer or magnetometer measurement, + * \f$ \hat{v_i} \f$ is the vector obtained from the orientation estimated combined with gravity direction or absolute magnetic field direction, for e.g, \f$ \hat{v_acc} = {^w}R_b^T e_3 \f$ + * and \f$ k_i \f$ is the confidence weight on the i-th measurement. In our case, i = 1 or 2. + * + * The usage of the attitude estimator can be as follows, + * - After instantiation, the parameters of the filter can be set using the individual parameter methods or the struct method. + * - The filter state can be initialized by calling the setInternalState() method + * - Once initialized, the following filter methods can be run in a loop to get the orientation estimates, + * - updateFilterWithMeasurements() method to pass the recent measurements to the filter + * - propagateStates() method to propagate the states through the system dynamics and correcting using the updated measurements + * - getInternalState() or getOrientationEstimate*() methods to get the entire state estimate or only the attitude estimated in desired representation + */ +class AttitudeMahonyFilter : public IAttitudeEstimator +{ +public: + AttitudeMahonyFilter(); + + /** + * @brief set flag to use magnetometer measurements + * @param[in] flag enable/disable magnetometer measurements + */ + void useMagnetoMeterMeasurements(bool flag); + + /** + * @brief set the confidence weights on magenetometer measurements, if used + * @param[in] confidence can take values between \f$ [0, 1] \f$ + */ + void setConfidenceForMagnetometerMeasurements(double confidence); + + /** + * @brief set the Kp gain + * @param[in] kp gain + */ + void setGainkp(double kp); + + /** + * @brief set the Ki gain + * @param[in] ki gain + */ + void setGainki(double ki); + + /** + * @brief set discretization time step in seconds + * @param[in] timestepInSeconds time step + */ + void setTimeStepInSeconds(double timestepInSeconds); + + /** + * @brief Set the gravity direction assumed by the filter (for computing orientation vectorial from accelerometer) + * @param[in] gravity_dir gravity direction + */ + void setGravityDirection(const iDynTree::Direction& gravity_dir); + + /** + * @brief Set filter parameters with the struct members. + * This does not reset the internal state. + * @param[in] params object of AttitudeMahonyFilterParameters passed as a const reference + * @return true/false if successful/not + */ + bool setParameters(const AttitudeMahonyFilterParameters& params) + { + m_params_mahony = params; + return true; + } + + /** + * @brief Get filter parameters as a struct. + * @param[out] params object of AttitudeMahonyFilterParameters passed as reference + */ + void getParameters(AttitudeMahonyFilterParameters& params) {params = m_params_mahony;} + + bool updateFilterWithMeasurements(const iDynTree::LinearAccelerometerMeasurements& linAccMeas, + const iDynTree::GyroscopeMeasurements& gyroMeas) override; + bool updateFilterWithMeasurements(const iDynTree::LinearAccelerometerMeasurements& linAccMeas, + const iDynTree::GyroscopeMeasurements& gyroMeas, + const iDynTree::MagnetometerMeasurements& magMeas) override; + bool propagateStates() override; + bool getOrientationEstimateAsRotationMatrix(iDynTree::Rotation& rot) override; + bool getOrientationEstimateAsQuaternion(iDynTree::UnitQuaternion& q) override; + bool getOrientationEstimateAsRPY(iDynTree::RPY& rpy) override; + size_t getInternalStateSize() const override; + bool getInternalState(const iDynTree::Span & stateBuffer) const override; + bool getDefaultInternalInitialState(const iDynTree::Span & stateBuffer) const override; + bool setInternalState(const iDynTree::Span & stateBuffer) override; + bool setInternalStateInitialOrientation(const iDynTree::Span& orientationBuffer) override; + +protected: + AttitudeMahonyFilterParameters m_params_mahony; ///< struct holding the Mahony filter parameters + AttitudeEstimatorState m_state_mahony, m_initial_state_mahony; +private: + iDynTree::Rotation m_orientationInSO3; ///< orientation estimate as rotation matrix \f$ {^A}R_B \f$ where \f$ A \f$ is inertial frame and \f$ B \f$ is the frame attached to the body + iDynTree::RPY m_orientationInRPY; ///< orientation estimate as a 3D vector in RPY representation, where \f$ {^A}R_B = Rot_z(yaw)Rot_y(pitch)Rot_x(roll) \f$ + + iDynTree::Vector3 m_omega_mes; ///< vectorial estimate from accelerometer and magnetometer measurements, \f$ \omega_{mes} \in \mathbb{R}^3 \f$, notation from the paper Non linear complementary filters on the special orthogonal group + iDynTree::GyroscopeMeasurements m_Omega_y; ///< gyroscope measurement, \f$ \Omega_{y} \in \mathbb{R}^3 \f$, notation from the paper Non linear complementary filters on the special orthogonal group + + iDynTree::Direction m_gravity_direction; ///< direction of the gravity vector expressed in the inertial frame denoted by \f$ A \f$, default set to \f$ e_3 = \begin{bmatrix} 0 & 0 & 1.0 \end{bmatrix}^T \f$ + iDynTree::Direction m_earth_magnetic_field_direction; ///< direction of absolute magnetic field expressed in the inertial frame denoted by \f$ A \f$, default set to \f$ {^A}m = \begin{bmatrix} 0 & 0 & 1.0 \end{bmatrix}^T \f$ +}; + +} + +#endif diff --git a/src/estimation/include/iDynTree/AttitudeQuaternionEKF.h b/src/estimation/include/iDynTree/AttitudeQuaternionEKF.h new file mode 100644 index 00000000000..b08a2087e43 --- /dev/null +++ b/src/estimation/include/iDynTree/AttitudeQuaternionEKF.h @@ -0,0 +1,329 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef ATTITUDE_QUATERNION_EKF_H +#define ATTITUDE_QUATERNION_EKF_H + +#include +#include +#include + +namespace iDynTree +{ + const unsigned int output_dimensions_with_magnetometer = 4; ///< dimension of \f$ \mathbb{R}^3 \times \mathbb{R} \f$ accelerometer measurements and magnetometer yaw measurement + const unsigned int output_dimensions_without_magnetometer = 3; ///< dimension of \f$ \mathbb{R}^3 \f$ accelerometer measurements + const unsigned int input_dimensions = 3; ///< dimension of \f$ \mathbb{R}^3 \f$ gyroscope measurements + + /** + * @struct AttitudeQuaternionEKFParameters Parameters to set up the quaternion EKF + * @var AttitudeQuaternionEKFParameters::time_step_in_seconds + * discretization time step in seconds, default value: \f$ 0.01 s \f$ + * @var AttitudeQuaternionEKFParameters::bias_correlation_time_factor + * time factor modeling how fast the bias can vary, default value: \f$ 0.01 \f$ + * @var AttitudeQuaternionEKFParameters::accelerometer_noise_variance + * measurement noise covariance \f$ \sigma_{acc}^{2} \f$ of accelerometer measurement, default value: \f$ 0.001 \f$ + * @var AttitudeQuaternionEKFParameters::magnetometer_noise_variance + * measurement noise covariance \f$ \sigma_{mag}^{2} \f$ of magnetometer measurement, default value: \f$ 0.001 \f$ + * @var AttitudeQuaternionEKFParameters::gyroscope_noise_variance + * system noise covariance \f$ \sigma_{gyro}^{2} \f$ of gyroscope measurement, since it is the input to the system, default value: \f$ 0.001 \f$ + * @var AttitudeQuaternionEKFParameters::gyro_bias_noise_variance + * system noise covariance \f$ \sigma_{gyrobias}^{2} \f$ of gyroscope bias estimate, default value: \f$ 0.0001 \f$ + * @var AttitudeQuaternionEKFParameters::initial_orientation_error_variance + * initial state covariance \f$ \sigma_{q_0}^{2} \f$ of orientation, default value: \f$ 10 \f$ + * @var AttitudeQuaternionEKFParameters::initial_ang_vel_error_variance + * initial state covariance \f$ \sigma_{\omega_0}^{2} \f$ of angular velocity, default value: \f$ 10 \f$ + * @var AttitudeQuaternionEKFParameters::initial_gyro_bias_error_variance + * measurement noise covariance \f$ \sigma_{acc}^{2} \f$ of gyroscope bias, default value: \f$ 10 \f$ + * @var AttitudeQuaternionEKFParameters::use_magnetometer_measurements + * flag to enable the use of magnetometer measurement for yaw correction, default value: false + */ + struct AttitudeQuaternionEKFParameters { + double time_step_in_seconds{0.01}; + double bias_correlation_time_factor{0.01}; + + // measurement noise - zero mean, and a given variance + double accelerometer_noise_variance{0.001}; + double magnetometer_noise_variance{0.001}; + + // process noise - zero mean, and a given covariance + double gyroscope_noise_variance{0.001}; + double gyro_bias_noise_variance{0.0001}; + + double initial_orientation_error_variance{10}; + double initial_ang_vel_error_variance{10}; + double initial_gyro_bias_error_variance{10}; + + bool use_magnetometer_measurements{false}; + }; + + /** + * @class AttitudeQuaternionEKF implements a Quaternion based Discrete Extended Kalman Filter fusing IMU measurements, + * to give estimates of orientation, angular velocity and gyroscope bias + * + * It follows the implementation detailed in + * Quaternion Based Extended Kalman Filter, slides by Michael Stohmeier + * The filter is used to estimate the states \f$ X = \begin{bmatrix} {^A}q_B \\ {^B}\Omega_{A,B} \\ {^B}b \end{bmatrix}^T \f$ + * where \f$ {^A}q_B \in \mathbb{R}^4 \f$ is the quaternion representing the orientation of a body(IMU) frame with respect to an inertial frame , + * \f$ {^B}\Omega_{A,B} \in \mathbb{R}^3 \f$ is the angular velocity of a body(IMU) frame with respect to an inertial frame, expressed in the body frame and + * \f$ {^B}b \in \mathbb{R}^3 \f$ is the gyroscope bias expressed in the body frame. + * @note: we will drop the subscripts and superscripts in the rest of the documentation for convenience + * + * Discretized dynamics during the prediction step, + * \f[ \hat{{x}}_{k+1} = \begin{bmatrix} q_{k} \otimes \text{exp}(\omega \Delta T) \\ y_{gyro_{k}} - b_{k} \\ (1 - \lambda_{b} \Delta t)b_k \end{bmatrix} \f] + * + * Measurement model for accelerometer is given as, + * \f[ h_{acc}(\hat{x}_{k+1}) = \begin{bmatrix} 2(q_1q_3 - q_0q_2) \\ 2(q_2q_3 - q_0q_1) \\ q_0^2 - q_1^2 - q_2^2 + q_3^2 \end{bmatrix} \f] + * obtained from \f$ {^w}R_b^T e_3 \f$ of the assumed gravity direction. + * + * Measurement model for magnetometer measurement is given as, + * \f[ h_{mag}(\hat{x}_{k+1}) = atan2( 2(q_0q_3 + q_1q_2),1 - 2(q_2^2 + q_3^2) ) \f] + * + * The linearized system propogation and measurement model is obtained by computing Jacobins F and H with respect to the state. + * + * The zero mean, additive Gaussian noise can be set using the covariance matrices which will be used during predict and update steps. + * + * The propagateStates() method is called to set the input vector for the EKF, then ekfPredict() is called to propagate the state through the propagation function f() + * and propate the state covariance using the Jacobian F. + * + * The updateFilterWithMeasurements() is called to set the measurement vector for the EKF, and then ekfUpdate is used to correct the state estimate and its covariance + * using the measurement model function h() and the measurement Jacobian H. + * + * The usage of the QEKF should follow the decribed procedure below, + * - instantiate the filter + * - set parameters + * - call initializeFilter() (this is necessary for resizing the buffers, the user should call this method after setting parameters) + * - use setInternalState() to set initial state (The filter will throw an error, if this is not called atleast once, this enforces the user to set intial state) + * - Once initialized, the following filter methods can be run in a loop to get the orientation estimates, + * - propagateStates() method to propagate the states and covariance + * - updateFilterWithMeasurements() method to correct the predicted states and covariance + * - getInternalState() or getOrientationEstimate*() methods to get the entire state estimate or only the attitude estimated in desired representation + * + * @warning calling the method useMagnetometerMeasurements() while the estimator is running, will reset the filter, reinitialize the filter to resize buffers + * and sets the previous estiamted state as the inital state. + * @note calling other set parameter methods does not reset the filter, since they are not associated with changing the output dimensions + * + */ + class AttitudeQuaternionEKF : public IAttitudeEstimator, + public DiscreteExtendedKalmanFilterHelper + { + public: + AttitudeQuaternionEKF(); + + /** + * @brief Get filter parameters as a struct. + * @param[out] params object of AttitudeQuaternionEKFParameters passed as reference + */ + void getParameters(AttitudeQuaternionEKFParameters& params) {params = m_params_qekf;} + + /** + * @brief Set filter parameters with the struct members. + * This resets filter since it also calls useMagnetometerMeasurements(flag) + * (if the use_magnetometer_measurements flag has been toggled). + * @param[in] params object of AttitudeQuaternionEKFParameters passed as a const reference + * @return true/false if successful/not + */ + void setParameters(const AttitudeQuaternionEKFParameters& params) + { + m_params_qekf = params; + useMagnetometerMeasurements(params.use_magnetometer_measurements); + } + + /** + * @brief Set the gravity direction assumed by the filter + * This affects the measurement model function h() and Jacobian H + * @param[in] gravity_dir gravity direction + */ + void setGravityDirection(const iDynTree::Direction& gravity_dir); + + /** + * @brief set discretization time step in seconds + * @param[in] time_step_in_seconds time step + */ + void setTimeStepInSeconds(double time_step_in_seconds) {m_params_qekf.time_step_in_seconds = time_step_in_seconds; } + + /** + * @brief set bias correlation time factor + * @param[in] bias_correlation_time_factor time factor for bias evolution + */ + void setBiasCorrelationTimeFactor(double bias_correlation_time_factor) { m_params_qekf.bias_correlation_time_factor = bias_correlation_time_factor; } + + /** + * @brief set flag to use magnetometer measurements + * @param[in] use_magnetometer_measurements enable/disable magnetometer measurements + * @note calling this method with the flag same as current flag value will not change anything, + * meanwhile a new flag setting will reset the filter, reinitialize the filter and + * set the previous state as filter's initial state and previous state covariance as filter's intial state covariance + */ + bool useMagnetometerMeasurements(bool use_magnetometer_measurements); + + /** + * @brief prepares the measurement noise covariance matrix and calls ekfSetMeasurementNoiseMeanAndCovariance() + * measurement noise depends only on accelerometer xyz (and magnetometer z) + * @note the noise has zero mean (basically passes a zero vector with covariance matrix) + * @param[in] acc variance for accelerometer measurements + * @param[in] mag variance for magnetometer measurements + * @return true/false if successful/not + */ + bool setMeasurementNoiseVariance(double acc, double mag); + + /** + * @brief prepares the system noise covariance matrix and calls ekfSetSystemNoiseMeanAndCovariance() + * process noise depends on gyro measurement and gyro bias estimate - since gyro measurement is passed as input + * @note the noise has zero mean (basically passes a zero vector with covariance matrix) + * measurement noise depends only on accelerometer xyz (and magnetometer z) + * @param[in] gyro variance for gyroscope measurements + * @param[in] gyro_bias variance for gyroscope bias estimates + * @return true/false if successful/not + */ + bool setSystemNoiseVariance(double gyro, double gyro_bias); + + /** + * @brief prepares the state covariance matrix and calls ekfSetStateCovariance() + * @param[in] orientation_var variance for intial orientation state estimate + * @param[in] ang_vel_var variance for initial angular velocity state estimate + * @param[in] gyro_bias_var variance for intial gyro bias state estimate + * @return true/false if successful/not + */ + bool setInitialStateCovariance(double orientation_var, double ang_vel_var, double gyro_bias_var); + + /** + * @brief intializes the filter by resizing buffers and setting parameters + * - sets state, output and input dimensions for the ekf + * - resizes internal buffers + * - calls ekfInit() + * - sets system noise, measurement noise and initial state covariance + * - if successful sets initialized flag to true + * @return true/false if successful/not + */ + bool initializeFilter(); + + + bool updateFilterWithMeasurements(const iDynTree::LinearAccelerometerMeasurements& linAccMeas, + const iDynTree::GyroscopeMeasurements& gyroMeas) override; + bool updateFilterWithMeasurements(const iDynTree::LinearAccelerometerMeasurements& linAccMeas, + const iDynTree::GyroscopeMeasurements& gyroMeas, + const iDynTree::MagnetometerMeasurements& magMeas) override; + bool propagateStates() override; + bool getOrientationEstimateAsRotationMatrix(iDynTree::Rotation& rot) override; + bool getOrientationEstimateAsQuaternion(iDynTree::UnitQuaternion& q) override; + bool getOrientationEstimateAsRPY(iDynTree::RPY& rpy) override; + size_t getInternalStateSize() const override; + bool getInternalState(const iDynTree::Span & stateBuffer) const override; + bool getDefaultInternalInitialState(const iDynTree::Span & stateBuffer) const override; + bool setInternalState(const iDynTree::Span & stateBuffer) override; + bool setInternalStateInitialOrientation(const iDynTree::Span& orientationBuffer) override; + + protected: + AttitudeEstimatorState m_state_qekf, m_initial_state_qekf; + AttitudeQuaternionEKFParameters m_params_qekf; ///< struct holding the QEKF parameters + + + private: + /** + * discrete system propagation \f$ f(X, u) = f(X, y_gyro) \f$ + * where \f$ X = \begin{bmatrix} q_0 & q_1 & q_2 & q_3 & \omega_x & \omega_y & \omega_z & b_x & b_y & b_z \end{bmatrix}^T \f$ + * \f$ u = \begin{bmatrix} {y_{gyro}}_x & {y_{gyro}}_y & {y_{gyro}}_z \end{bmatrix}^T \f$ + * \f$ f(X, u) = \begin{bmatrix} q_{k} \otimes \text{exp}(\omega \Delta T) \\ y_{gyro} - b \\ (1 - \lambda_{b} \Delta t)b \end{bmatrix}\f$ + */ + bool ekf_f(const iDynTree::VectorDynSize& x_k, + const iDynTree::VectorDynSize& u_k, + iDynTree::VectorDynSize& xhat_k_plus_one) override; + + /** + * discrete measurement prediction + * where \f$ h(X) = \begin{bmatrix} h_{acc}(X) & h_{mag}(X) \end{bmatrix}^T \f$ + * \f$ h_{acc}(X) = R^T \begin{bmatrix} 0 \\ 0 \\ -1 \end{bmatrix} \f$ + * \f$ h_{mag}(X) = atan2(tan(yaw))\f$ + */ + bool ekf_h(const iDynTree::VectorDynSize& xhat_k_plus_one, + iDynTree::VectorDynSize& zhat_k_plus_one) override; + + /** + * @brief Describes the system Jacobian necessary for the propagation of predicted state covariance + * The analytical Jacobian describing the partial derivative of the system propagation with respect to the state + * @param[in] x system state + * @param[out] F system Jacobian + * @return bool true/false if successful or not + */ + bool ekfComputeJacobianF(iDynTree::VectorDynSize& x, iDynTree::MatrixDynSize& F) override; + bool ekfComputeJacobianF(iDynTree::VectorDynSize& x, iDynTree::VectorDynSize& u, iDynTree::MatrixDynSize& F) override; + + /** + * @brief Describes the measurement Jacobian necessary for computing Kalman gain and updating the predicted state and its covariance + * The analytical Jacobian describing the partial derivative of the measurement model with respect to the state + * @param[in] x system state + * @param[out] H measurement Jacobian + * @return bool true/false if successful or not + */ + bool ekfComputeJacobianH(iDynTree::VectorDynSize& x, iDynTree::MatrixDynSize& H) override; + + /** @brief prepares the system noise covariance matrix using internal struct params + * system model is as good as gyroscope measurement and bias estimate + * system noise covariance can be descibed as \f$ Q = F_u U {F_u}^T \f$ + * where \f$ F = \begin{bmatrix} \frac{\partial f}{\partial y_gyro} & \frac{\partial f}{ \partial x_gyrobias} \end{bmatrix} \f$ + * \f$ = \begin{bmatrix} 0_{4 \times 3} & 0_{4 \times 3} \\ I_{3 \times 3} & 0_{3 \times 3} \\ 0_{3 \times 3} & I_{3 \times 3} \end{bmatrix}\f$ + * \f$ U = diag(\begin{bmatrix} \sigma_{gyro}^{2} I_{3 \times 3} & \sigma_{gyrobias}^{2} I_{3 \times 3} \end{bmatrix}) \f$ + * @param[in] Q matrix container as reference + */ + void prepareSystemNoiseCovarianceMatrix(iDynTree::MatrixDynSize &Q); + + /** @brief prepares the measurement noise covariance matrix using internal struct parameters + * measurement noise depends only on accelerometer measurement along x-,y- and z- directions + * along with magnetometer z-direction if included + * measurement noise covariance can be descibed as, + * \f$ R = \begin{bmatrix} \sigma_{acc}^{2} I_{3 \times 3} & 0_{3 \times 1} \\ 0_{1 \times 3} & \sigma_{mag}^{2}\f$ + * if magnetometer measurements is also considered. In case of magnetometer measurements not being considered, it is reduced + * to the \f$ 3 \times 3 \f$ matrix + * @param[in] R matrix container as reference + */ + void prepareMeasurementNoiseCovarianceMatrix(iDynTree::MatrixDynSize &R); + + /** + * @brief serializes the state struct to state x of VectorDynSize + */ + void serializeStateVector(); + + /** + * @brief deserializes state x of VectorDynSize to the state struct + */ + void deserializeStateVector(); + + /** + * @brief serializes the accelerometer and magenetometer measurements into y vector + * since DiscreteExtendedKalmanFilter expects a VectorDynSize including all necessary measurements + */ + void serializeMeasurementVector(); + + + /** + * @brief serializes measurements, calls ekfUpdate step and + * gets state estimate corrected by measurements + * + * @return true/false, if successful/not + */ + bool callEkfUpdate(); + + iDynTree::Rotation m_orientationInSO3; ///< orientation estimate as rotation matrix \f$ {^A}R_B \f$ where \f$ A \f$ is inertial frame and \f$ B \f$ is the frame attached to the body + iDynTree::RPY m_orientationInRPY; ///< orientation estimate as a 3D vector in RPY representation, where \f$ {^A}R_B = Rot_z(yaw)Rot_y(pitch)Rot_x(roll) \f$ + + iDynTree::GyroscopeMeasurements m_Omega_y; ///< 3d gyroscope measurement giving angular velocity of body wrt inertial frame, expressed in body frame + iDynTree::LinearAccelerometerMeasurements m_Acc_y; ///< 3d accelerometer measurement giving proper classical acceleration expressed in body frame + double m_Mag_y; ///< magnetometer yaw measurement expressed in body frame + + iDynTree::VectorDynSize m_x; ///< state vector for the EKF - orientation, angular velocity, gyro bias + iDynTree::VectorDynSize m_y; ///< measurement vector for the EKF - accelerometer (and magnetometer yaw) + iDynTree::VectorDynSize m_u; ///< input vector for the EKF - gyroscope measurement + + size_t m_state_size; ///< state dimensions + size_t m_output_size; ///< output dimensions + size_t m_input_size; ///< input dimensions + bool m_initialized{false}; ///< flag to check if QEKF is initialized + + iDynTree::Matrix4x4 m_Id4; ///< \f$ 4 \times 4 \f$ identity matrix + iDynTree::Matrix3x3 m_Id3; ///< \f$ 3 \times 3 \f$ identity matrix + iDynTree::Direction m_gravity_direction; ///< direction of the gravity vector expressed in the inertial frame denoted by \f$ A \f$, default set to \f$ e_3 = \begin{bmatrix} 0 & 0 & 1.0 \end{bmatrix}^T \f$ + }; + +} + +#endif diff --git a/src/estimation/include/iDynTree/BerdyHelper.h b/src/estimation/include/iDynTree/BerdyHelper.h new file mode 100644 index 00000000000..39d936458df --- /dev/null +++ b/src/estimation/include/iDynTree/BerdyHelper.h @@ -0,0 +1,783 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_BERDY_HELPER_H +#define IDYNTREE_BERDY_HELPER_H + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include + +#include + +#include + +namespace iDynTree +{ + +/** + * Enumeration of the Berdy variants implemented + * in this class. + * + * @warning This class is still in active development, and so API interface can change between iDynTree versions. + * \ingroup iDynTreeExperimental + */ +enum BerdyVariants +{ + /** + * Original version of Berdy, as described in: + * + * Latella, C.; Kuppuswamy, N.; Romano, F.; Traversaro, S.; Nori, F. + * Whole-Body Human Inverse Dynamics with Distributed Micro-Accelerometers, Gyros and Force Sensing. Sensors 2016, 16, 727. + * http://www.mdpi.com/1424-8220/16/5/727 + * + * The original version of Berdy is assuming that the joint numbering is a regular ordering of links and joints. + * For this reason the serialization of link/joints quantities follows the one defined in the traversal. + * + * Furthremore, this version assumes that all joints have 1 Degree of freedom, so it does not work for models + * with fixed joints. + */ + ORIGINAL_BERDY_FIXED_BASE = 0, + + /** + * Modified version of Berdy + * for accounting for free floating dynamics and removing the NET_INT_AND_EXT_WRENCHES_ON_LINK_WITHOUT_GRAV from the dynamic variables. + * + */ + BERDY_FLOATING_BASE = 1, + + /** + * Modified version of floating base Berdy + * for accounting centroidal dynamics constraints towards estimating the external link wrench independently of the internal joint torque estimates. + */ + BERDY_FLOATING_BASE_NON_COLLOCATED_EXT_WRENCHES = 2 + +}; + +/** + * Enumeration describing the dynamic variables types (link acceleration, net wrenches, joint wrenches, joint torques, joint acceleration) + * used in Berdy + * + * @warning This class is still in active development, and so API interface can change between iDynTree versions. + * \ingroup iDynTreeExperimental + */ +enum BerdyDynamicVariablesTypes +{ + /*!< \f$ a_i \f$ + * Note that is is the **spatial** proper acceleration expressed, + * i.e. the time derivative of the left-trivialized velocity minus the gravity expressed in body frame. + **/ + LINK_BODY_PROPER_ACCELERATION, + /*!< \f$ f^B_i \f$ */ + NET_INT_AND_EXT_WRENCHES_ON_LINK_WITHOUT_GRAV, + /*!< \f$ f_i \f$ */ + JOINT_WRENCH, + /*!< \f$ \tau_i \f$ */ + DOF_TORQUE, + /*!< \f$ f^x_i \f$ */ + NET_EXT_WRENCH, + /*!< \f$ \ddot{q}_i \f$ */ + DOF_ACCELERATION, + /*!< + * This is the classical proper acceleration, + * i.e. the time derivative of the mixed velocity of the body frame minus the gravity expressed in body frame. + * In Traversaro's PhD thesis this is called sensor proper acceleration. + * This is the necessary for avoiding dependencies on the linear velocity of the base in the floating variant of + * Berdy. + */ + LINK_BODY_PROPER_CLASSICAL_ACCELERATION +}; + +/** + * Enumeration describing the possible sensor types implemented in Berdy. + * + * Note that the concept of "sensor" in Berdy estimation is more general that + * just a physical sensor mounted on the robot: for example it can include + * the information that a link is fixed to the ground (i.e. its angular velocity, + * angular and linear acceleration are zero) even if this information is not coming + * from an actual physical sensors. For this reason we do not use directly + * the iDynTree::SensorTypes enum, even if we reserve the first 1000 elements o + * of this enum to be compatibile with the iDynTree::SensorTypes enum. + * Enum values duplicates between BerdySensorTypes and SensorTypes are append a + * _SENSOR suffix to avoid problems when wrapping such enum wit SWIG. + * + * @warning This class is still in active development, and so API interface can change between iDynTree versions. + * \ingroup iDynTreeExperimental + */ +enum BerdySensorTypes +{ + SIX_AXIS_FORCE_TORQUE_SENSOR = SIX_AXIS_FORCE_TORQUE, + ACCELEROMETER_SENSOR = ACCELEROMETER, + GYROSCOPE_SENSOR = GYROSCOPE, + THREE_AXIS_ANGULAR_ACCELEROMETER_SENSOR = THREE_AXIS_ANGULAR_ACCELEROMETER, + THREE_AXIS_FORCE_TORQUE_CONTACT_SENSOR = THREE_AXIS_FORCE_TORQUE_CONTACT, + DOF_ACCELERATION_SENSOR = 1000, + DOF_TORQUE_SENSOR = 1001, + NET_EXT_WRENCH_SENSOR = 1002, + /** + * Non-physical sensor that measures the wrench trasmitted by a joint. + */ + JOINT_WRENCH_SENSOR = 1003, + + /** + * Non-physical sensor that holds the value of Rate of Change of Momentum (RCM) of the system + * for centroidal dynamics constraint + */ + RCM_SENSOR = 1004 +}; + +bool isLinkBerdyDynamicVariable(const BerdyDynamicVariablesTypes dynamicVariableType); +bool isJointBerdyDynamicVariable(const BerdyDynamicVariablesTypes dynamicVariableType); +bool isDOFBerdyDynamicVariable(const BerdyDynamicVariablesTypes dynamicVariableType); + +/** + * Options of the BerdyHelper class. + * + * Documentation of each option is provided as usual Doxygen documentation. + * Default values for each options are specified in the contstructor. + * + * @warning This class is still in active development, and so API interface can change between iDynTree versions. + * \ingroup iDynTreeExperimental + */ +struct BerdyOptions +{ +public: + BerdyOptions() : berdyVariant(ORIGINAL_BERDY_FIXED_BASE), + includeAllNetExternalWrenchesAsDynamicVariables(true), + includeAllJointAccelerationsAsSensors(true), + includeAllJointTorquesAsSensors(false), + includeAllNetExternalWrenchesAsSensors(true), + includeFixedBaseExternalWrench(false), + baseLink("") + { + } + + /** + * Type of berdy variant implemented. + * + * For description of each type of variant + * check the BerdyVariants enum documentation. + * + * Default value: ORIGINAL_BERDY_FIXED_BASE . + */ + BerdyVariants berdyVariant; + + /** + * If true, include the net external wrenches in the dynamic variables vector. + * + * Default value: true . + * + * \note the Net effect of setting this to zero is to consider all the external + * wrenches to be equal to 0. If berdyVariant is ORIGINAL_BERDY_FIXED_BASE, + * the "net" external wrenches does not include the wrench at the base. + * + * \note if berdyVariant is BERDY_FLOATING_BASE, this option cannot be set to false. + */ + bool includeAllNetExternalWrenchesAsDynamicVariables; + + /** + * If true, include the joint accelerations in the sensors vector. + * + * Default value: true . + */ + bool includeAllJointAccelerationsAsSensors; + + /** + * If true, include the joint torques in the sensors vector. + * + * Default value: false . + */ + bool includeAllJointTorquesAsSensors; + + /** + * If true, include the net external wrenches in the sensors vector. + * It is not compatible with the use of includeAllNetExternalWrenchesAsDynamicVariables set to false. + * + * Default value: true . + */ + bool includeAllNetExternalWrenchesAsSensors; + + /** + * If includeNetExternalWrenchesAsSensors is true and the + * variant is ORIGINAL_BERDY_FIXED_BASE, if this is + * true the external wrench acting on the base fixed link + * is included in the sensors. + * + * Default value : false . + */ + bool includeFixedBaseExternalWrench; + + /** + * Vector of joint names for which we assume that a virtual + * measurement of the wrench trasmitted on the joint is available. + * + * \note This measurements are tipically used only for debug, actual + * internal wrenches are tipically measured using a SIX_AXIS_FORCE_TORQUE_SENSOR . + */ + std::vector jointOnWhichTheInternalWrenchIsMeasured; + + /** + * Name of the link which will be considered as a base link for Berdy computations. + * + * \note If the string is empty the default base link of the model will be used. + */ + std::string baseLink; + + /** + * Check that the options are not self-contradicting. + */ + bool checkConsistency(); +}; + + +//Unfortunately some sensors used in berdy are not proper sensors. +//I cannot use the Sensor class which has almost all the information needed +/** + * Structure which describes the essential information about a sensor used in berdy + * A sensor is identified by the pair (type, id) + * + * @warning This class is still in active development, and so API interface can change between iDynTree versions. + * \ingroup iDynTreeExperimental + */ +struct BerdySensor { + iDynTree::BerdySensorTypes type; /*!< type of the sensor */ + std::string id; /*!< ID of the sensor */ + iDynTree::IndexRange range; /*!< Range of the sensor + * (starting location in the measurements equations + * and number of measurements equations associated with the sensor) */ + + /** + * Overload of equality operator + * + * Two sensors are considered equals if they have the same type and id + * @param sensor the sensor to which the current sensor is compared to + * @return true if the two sensors are equal. False otherwise + */ + bool operator==(const struct BerdySensor& sensor) const; + + bool operator<(const struct BerdySensor& sensor) const; +}; + +struct BerdyDynamicVariable { + iDynTree::BerdyDynamicVariablesTypes type; /*!< type of the dynamic variable */ + std::string id; /*!< ID of the dynamic variable */ + iDynTree::IndexRange range; /*!< Range of the dynamic variable + * (starting location in the dynamic equations + * and number of equations associated with the variable) */ + + /** + * Overload of equality operator + * + * Two variables are considered equals if they have the same type and id + * @param variable the variable to which the current variable is compared to + * @return true if the two variables are equal. False otherwise + */ + bool operator==(const struct BerdyDynamicVariable& variable) const; + + bool operator<(const struct BerdyDynamicVariable& variable) const; +}; + +/** + * \brief Helper class for computing Berdy matrices. + * + * Berdy refers to a class for algorithms to compute + * the Maximum A Posteriori (MAP) estimation of the dynamic variables + * of a multibody model (accelerations, external forces, joint torques) + * assuming the knowledge the measurements of an arbitrary set of sensors and + * of the kinematics and inertial characteristics of the model. + * + * The MAP estimation is computed using a sparse matrix representation of the multibody + * dynamics Newton-Euler equations, originally described in: + * + * Latella, C.; Kuppuswamy, N.; Romano, F.; Traversaro, S.; Nori, F. + * Whole-Body Human Inverse Dynamics with Distributed Micro-Accelerometers, Gyros and Force Sensing. + * Sensors 2016, 16, 727. + * http://www.mdpi.com/1424-8220/16/5/727 + * + * Nori F, Kuppuswamy N, Traversaro S. + * Simultaneous state and dynamics estimation in articulated structures. + * In Intelligent Robots and Systems (IROS), 2015 IEEE/RSJ International Conference on 2015 Sep 28 (pp. 3380-3386). IEEE. + * http://ieeexplore.ieee.org/xpl/articleDetails.jsp?arnumber=7353848 + * + * + * This helper class implements two different variants of the Berdy dynamics representation, identified + * with the BerdyVariants enum: + * * The ORIGINAL_BERDY_FIXED_BASE is the original BERDY representation described in the papers, + * that was assuming a fixed base model with all the joints with 1-DOF . + * * The BERDY_FLOATING_BASE is a variant in which the model is assumed to be floating, + * does not need the position or linear velocity of the floating base and supports + * joints with an arbitrary number of DOF . + * + * The sensors supported by this class are the one contained in the SensorsList representation, + * which can be loaded directly from an URDF representation, see https://github.com/robotology/idyntree/blob/master/doc/model_loading.md#sensor-extensions . + * Using this representation the following sensors can be loaded: + * * Internal Six-Axis Force Torque sensors + * * Gyroscopes + * * Accelerometers + * Support for joint torques/acceleration or external wrenches measurements still needs to be added. + * + * \note The dynamics representation of Berdy is highly dependent on the link assumed to be the floating + * base of the robot, even in the case that BERDY_FLOATING_BASE is used. The assumed traversal (i.e. + * which link is the floating base and how the link are visited) is accessible with the dynamicTraversal() method. + * + * @warning This class is still in active development, and so API interface can change between iDynTree versions. + * \ingroup iDynTreeExperimental + */ +class BerdyHelper +{ + /** + * Model used in this class. + */ + Model m_model; + + /** + * Sensors used in this class + */ + SensorsList m_sensors; + + /** + * Traversal used for the dynamics computations + */ + Traversal m_dynamicsTraversal; + + /** + * Caches of traversals used for kinematic computations. + */ + LinkTraversalsCache m_kinematicTraversals; + + /** + * False initially, true after a valid model and sensors have been loaded. + */ + bool m_areModelAndSensorsValid; + + /** + * False initially, true after updateKinematics was successfully called. + */ + bool m_kinematicsUpdated; + + /** + * Options of the current berdy variant used. + */ + BerdyOptions m_options; + + size_t m_nrOfDynamicalVariables; + size_t m_nrOfDynamicEquations; + size_t m_nrOfSensorsMeasurements; + + + /** + * Joint positions + */ + JointPosDoubleArray m_jointPos; + + /** + * Joint velocities + */ + JointDOFsDoubleArray m_jointVel; + + + /** + * Link body velocities (i.e. 6D velocity of the link, expressed + * in the link frame orientation and with respect to the link origin). + */ + LinkVelArray m_linkVels; + + /** + * Gravity expressed in the base link frame, used only + * if getBerdyVariant is equal to ORIGINAL_BERDY_FIXED_BASE. + */ + Vector3 m_gravity; + SpatialAcc m_gravity6D; + + std::vector m_sensorsOrdering; /*!< Sensor ordering. Created on init */ + std::vector m_dynamicVariablesOrdering; /*!< Dynamic variable ordering. Created on init */ + + /** + * Helpers method for initialization. + */ + bool initOriginalBerdyFixedBase(); + bool initBerdyFloatingBase(); + bool initSensorsMeasurements(); + + IndexRange getRangeOriginalBerdyFixedBase(const BerdyDynamicVariablesTypes dynamicVariableType, const TraversalIndex idx) const; + + /** + * Ranges for specific dynamics equations + */ + + // Dynamic equations for ORIGINAL_BERDY_FIXED_BASE + IndexRange getRangeLinkProperAccDynEqFixedBase(const LinkIndex idx) const; + IndexRange getRangeLinkNetTotalWrenchDynEqFixedBase(const LinkIndex idx) const; + IndexRange getRangeJointWrenchDynEqFixedBase(const JointIndex idx) const; + IndexRange getRangeDOFTorqueDynEqFixedBase(const DOFIndex idx) const; + + // Dynamic equations for BERDY_FLOATING_BASE + IndexRange getRangeAccelerationPropagationFloatingBase(const LinkIndex idx) const; + IndexRange getRangeNewtonEulerEquationsFloatingBase(const LinkIndex idx) const; + + // Y matrices computation + bool computeBerdySensorMatrices(SparseMatrix& Y, VectorDynSize& bY); + bool computeBerdySensorsMatricesFromModel(SparseMatrix& Y, VectorDynSize& bY); + + // D matrices computation + bool computeBerdyDynamicsMatricesFixedBase(SparseMatrix& D, VectorDynSize& bD); + bool computeBerdyDynamicsMatricesFloatingBase(SparseMatrix& D, VectorDynSize& bD); + + // Helper method + Matrix6x1 getBiasTermJointAccelerationPropagation(IJointConstPtr joint, + const LinkIndex parentLinkIdx, + const LinkIndex childLinkIdx, + const Transform &child_X_parent); + void cacheSensorsOrdering(); + void cacheDynamicVariablesOrderingFixedBase(); + void cacheDynamicVariablesOrderingFloatingBase(); + bool serializeDynamicVariablesFixedBase(LinkProperAccArray & properAccs, + LinkNetTotalWrenchesWithoutGravity & netTotalWrenchesWithoutGrav, + LinkNetExternalWrenches & netExtWrenches, + LinkInternalWrenches & linkJointWrenches, + JointDOFsDoubleArray & jointTorques, + JointDOFsDoubleArray & jointAccs, + VectorDynSize& d); + bool serializeDynamicVariablesFloatingBase(LinkProperAccArray & properAccs, + LinkNetTotalWrenchesWithoutGravity & netTotalWrenchesWithoutGrav, + LinkNetExternalWrenches & netExtWrenches, + LinkInternalWrenches & linkJointWrenches, + JointDOFsDoubleArray & jointTorques, + JointDOFsDoubleArray & jointAccs, + VectorDynSize& d); + + bool serializeDynamicVariablesNonCollocatedWrenches(LinkNetExternalWrenches& netExtWrenches, + VectorDynSize& d); + /** + * Helper for mapping sensors measurements to the Y vector. + */ + struct { + size_t dofAccelerationOffset; + size_t dofTorquesOffset; + size_t netExtWrenchOffset; + size_t jointWrenchOffset; + size_t rcmOffset; + } berdySensorTypeOffsets; + + /** + * Helper of additional sensors. + */ + struct { + /** + * List of joint wrench sensors. + */ + std::vector wrenchSensors; + /** + * Mapping between jndIx in wrenchSensor and + */ + std::vector jntIdxToOffset; + } berdySensorsInfo; + + /** + * Buffer for sensor serialization. + */ + VectorDynSize realSensorMeas; + + Triplets matrixDElements; + Triplets matrixYElements; + + /** + * Transform between the frame in which the external net wrench measurements are expressed + * and the link frames. + */ + std::vector m_link_H_externalWrenchMeasurementFrame; + + /* + * LinkPositions variable containing the transforms between the base and the model links + * + */ + LinkPositions base_H_links; + + +public: + /** + * Constructor + */ + BerdyHelper(); + + /** + * Access the model. + */ + Model& model(); + + /** + * Access the sensors. + */ + SensorsList& sensors(); + + /** + * Acces the traveral used for the dynamics computations (const version) + */ + const Traversal& dynamicTraversal() const; + + /** + * Access the model (const version). + */ + const Model& model() const; + + /** + * Access the model (const version). + */ + const SensorsList& sensors() const; + + /** + * Returns if the helper is valid. + * The helper is valid if the model and the sensors have been loaded + * @return true if the helper is valid. False otherwise + */ + bool isValid() const; + + /** + * Init the class + * + * @param[in] model The used model. + * @param[in] sensors The used sensors. + * @param[in] options The used options, check BerdyOptions docs. + * @return true if all went well, false otherwise. + */ + bool init(const Model& model, + const SensorsList& sensors, + const BerdyOptions options=BerdyOptions()); + + /** + * Get currently used options. + */ + BerdyOptions getOptions() const; + + /** + * Get the number of columns of the D matrix. + * This depends on the Berdy variant selected. + */ + size_t getNrOfDynamicVariables() const; + + /** + * Get the number of dynamics equations used in the Berdy + * equations + */ + size_t getNrOfDynamicEquations() const; + + /** + * Get the number of sensors measurements. + */ + size_t getNrOfSensorsMeasurements() const; + + /** + * Resize and set to zero Berdy matrices. + */ + bool resizeAndZeroBerdyMatrices(SparseMatrix& D, VectorDynSize &bD, + SparseMatrix& Y, VectorDynSize &bY); + + /** + * Resize and set to zero Berdy matrices. + * + */ + bool resizeAndZeroBerdyMatrices(MatrixDynSize & D, VectorDynSize & bD, + MatrixDynSize & Y, VectorDynSize & bY); + + /** + * Get Berdy matrices + */ + bool getBerdyMatrices(SparseMatrix& D, VectorDynSize &bD, + SparseMatrix& Y, VectorDynSize &bY); + + /** + * Get Berdy matrices + * + * \note internally this function uses sparse matrices + * Prefer the use of resizeAndZeroBerdyMatrices(SparseMatrix &, VectorDynSize &, SparseMatrix &, VectorDynSize &) + */ + bool getBerdyMatrices(MatrixDynSize & D, VectorDynSize & bD, + MatrixDynSize & Y, VectorDynSize & bY); + + /** + * Return the internal ordering of the sensors + * + * Measurements are expected to respect the internal sensors ordering + * Use this function to obtain the sensors ordering. + * + * @return the sensors ordering + */ + const std::vector& getSensorsOrdering() const; + + /** + * Get the range of the specified sensor in + */ + IndexRange getRangeSensorVariable(const SensorType type, const unsigned int sensorIdx) const; + IndexRange getRangeDOFSensorVariable(const BerdySensorTypes sensorType, const DOFIndex idx) const; + IndexRange getRangeJointSensorVariable(const BerdySensorTypes sensorType, const JointIndex idx) const; + IndexRange getRangeLinkSensorVariable(const BerdySensorTypes sensorType, const LinkIndex idx) const; + IndexRange getRangeRCMSensorVariable(const BerdySensorTypes sensorType) const; + + /** + * Ranges of dynamic variables + */ + IndexRange getRangeLinkVariable(const BerdyDynamicVariablesTypes dynamicVariableType, const LinkIndex idx) const; + IndexRange getRangeJointVariable(const BerdyDynamicVariablesTypes dynamicVariableType, const JointIndex idx) const; + IndexRange getRangeDOFVariable(const BerdyDynamicVariablesTypes dynamicVariableType, const DOFIndex idx) const; + + const std::vector& getDynamicVariablesOrdering() const; + + /** + * Serialized dynamic variables from the separate buffers + */ + bool serializeDynamicVariables(LinkProperAccArray & properAccs, + LinkNetTotalWrenchesWithoutGravity & netTotalWrenchesWithoutGrav, + LinkNetExternalWrenches & netExtWrenches, + LinkInternalWrenches & linkJointWrenches, + JointDOFsDoubleArray & jointTorques, + JointDOFsDoubleArray & jointAccs, + VectorDynSize& d); + /** + * Serialize sensor variable from separate buffers. + */ + bool serializeSensorVariables(SensorsMeasurements & sensMeas, + LinkNetExternalWrenches & netExtWrenches, + JointDOFsDoubleArray & jointTorques, + JointDOFsDoubleArray & jointAccs, + LinkInternalWrenches & linkJointWrenches, + SpatialForceVector & rcm, // Rate of Change of Momentum (RCM) + VectorDynSize & y); + + + /** + * Debug function: + * + * \note This method has been added for debug, and could be removed soon. + */ + bool serializeDynamicVariablesComputedFromFixedBaseRNEA(JointDOFsDoubleArray & jointAccs, + LinkNetExternalWrenches & netExtWrenches, + VectorDynSize& d); + + /** + * Extract the joint torques from the dynamic variables + */ + bool extractJointTorquesFromDynamicVariables(const VectorDynSize& d, + const VectorDynSize& jointPos, + VectorDynSize& jointTorques) const; + + /** + * Extract the net external force-torques from the dynamic variables + */ + bool extractLinkNetExternalWrenchesFromDynamicVariables(const VectorDynSize& d, + LinkNetExternalWrenches& netExtWrenches) const; + + /** + * @name Methods to submit the input data for dynamics computations. + */ + //@{ + + /** + * Set the kinematic information necessary for the dynamics estimation using the + * angular velocity information of a floating frame. + * + * \note This method cannot be used if the selected BerdyVariant is ORIGINAL_BERDY_FIXED_BASE. + * \note we not require to give the linear velocity of floating base because the dynamics equations + * are invariant with respect to an offset in linear velocity. This convenient to avoid + * any dependency on any prior floating base estimation. + * + * @param[in] jointPos the position of the joints of the model. + * @param[in] jointVel the velocities of the joints of the model. + * @param[in] floatingFrame the index of the frame for which kinematic information is provided. + * @param[in] angularVel angular velocity (wrt to any inertial frame) of the specified floating frame, + * expressed in the specified floating frame orientation. + * @return true if all went ok, false otherwise. + */ + bool updateKinematicsFromFloatingBase(const JointPosDoubleArray & jointPos, + const JointDOFsDoubleArray & jointVel, + const FrameIndex & floatingFrame, + const Vector3 & angularVel); + + /** + * Set the kinematic information necessary for the dynamics estimation assuming that a + * given frame is not accelerating with respect to the inertial frame. + * + * @param[in] jointPos the position of the joints of the model. + * @param[in] jointVel the velocities of the joints of the model. + * @param[in] fixedFrame the index of the frame that is not accelerating with respect to the inertial frame. + * @param[in] gravity the gravity acceleration vector, expressed in the specified fixed frame. + * + * \note gravity is used only if selected BerdyVariant is ORIGINAL_BERDY_FIXED_BASE. + * + * @return true if all went ok, false otherwise. + */ + bool updateKinematicsFromFixedBase(const JointPosDoubleArray & jointPos, + const JointDOFsDoubleArray & jointVel, + const FrameIndex & fixedFrame, + const Vector3 & gravity); + + /** + * Set the kinematic information necessary for the dynamics estimation assuming that a + * given baseframe (specified by the m_dynamicsTraversal) is not accelerating with respect to the inertial frame. + * + * @param[in] jointPos the position of the joints of the model. + * @param[in] jointVel the velocities of the joints of the model. + * @param[in] gravity the gravity acceleration vector, expressed in the specified fixed frame. + * + * \note gravity is used only if selected BerdyVariant is ORIGINAL_BERDY_FIXED_BASE. + * + * @return true if all went ok, false otherwise. + * + * @note this is equivalent to + * @code updateKinematicsFromFixedBase(jointPos,jointVel,m_dynamicsTraversal.getBaseLink()->getIndex(),gravity); + * @endcode + * + */ + bool updateKinematicsFromTraversalFixedBase(const JointPosDoubleArray & jointPos, + const JointDOFsDoubleArray & jointVel, + const Vector3 & gravity); + + //@} + + /** + * Set/get the transformation link_H_contact between the link frame and the frame in which the measured net ext wrench is expressed. + * + * The default value is the identity. This is extremly useful to correctly tune the variances when only a subset of the external + * net wrench is known (for example when it is known that the external net wrench is a pure force on a point different from the + * link frame. + * + * \note This will only change the frame in which the measurent equation of the net external wrench is expressed, + * not how the Newton-Euler equation of the link are expressed or how the net ext wrenches are serialized in + * the LinkNetExternalWrenches class. + */ + ///@{ + bool setNetExternalWrenchMeasurementFrame(const LinkIndex lnkIndex, const Transform& link_H_contact); + bool getNetExternalWrenchMeasurementFrame(const LinkIndex lnkIndex, Transform& link_H_contact) const; + ///@} + + + /** + * @name Methods to get informations on the serialization used. + */ + //@{ + + /** + * Get a human readable description of the elements of the dynamic variables. + */ + //std::string getDescriptionOfDynamicVariables(); + + //@} + + + +}; + + +} + +#endif diff --git a/src/estimation/include/iDynTree/BerdySparseMAPSolver.h b/src/estimation/include/iDynTree/BerdySparseMAPSolver.h new file mode 100644 index 00000000000..5678c2781f6 --- /dev/null +++ b/src/estimation/include/iDynTree/BerdySparseMAPSolver.h @@ -0,0 +1,75 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_BERDY_SPARSEMAPSOLVER_H +#define IDYNTREE_BERDY_SPARSEMAPSOLVER_H + +#include +#include +#include + +namespace iDynTree { + + class BerdyHelper; + class VectorDynSize; + class JointPosDoubleArray; + class JointDOFsDoubleArray; + + template + class SparseMatrix; + + + /** + * @warning This class is still in active development, and so API interface can change between iDynTree versions. + * \ingroup iDynTreeExperimental + */ + class BerdySparseMAPSolver + { + + class BerdySparseMAPSolverPimpl; + BerdySparseMAPSolverPimpl* m_pimpl; + + public: + BerdySparseMAPSolver(BerdyHelper& berdyHelper); + ~BerdySparseMAPSolver(); + + void setDynamicsConstraintsPriorCovariance(const iDynTree::SparseMatrix & covariance); + void setDynamicsRegularizationPriorCovariance(const iDynTree::SparseMatrix& covariance); + void setDynamicsRegularizationPriorExpectedValue(const iDynTree::VectorDynSize& expectedValue); + void setMeasurementsPriorCovariance(const iDynTree::SparseMatrix& covariance); + + const iDynTree::SparseMatrix& dynamicsConstraintsPriorCovarianceInverse() const; // Sigma_D^-1 + iDynTree::SparseMatrix& dynamicsConstraintsPriorCovarianceInverse(); // Sigma_D^-1 + const iDynTree::SparseMatrix& dynamicsRegularizationPriorCovarianceInverse() const; // Sigma_d^-1 + iDynTree::SparseMatrix& dynamicsRegularizationPriorCovarianceInverse(); // Sigma_d^-1 + const iDynTree::VectorDynSize& dynamicsRegularizationPriorExpectedValue() const; // mu_d + iDynTree::VectorDynSize& dynamicsRegularizationPriorExpectedValue(); // mu_d + const iDynTree::SparseMatrix& measurementsPriorCovarianceInverse() const; // Sigma_y^-1 + iDynTree::SparseMatrix& measurementsPriorCovarianceInverse(); // Sigma_y^-1 + + bool isValid(); + + bool initialize(); + + void updateEstimateInformationFixedBase(const JointPosDoubleArray& jointsConfiguration, + const JointDOFsDoubleArray& jointsVelocity, + const FrameIndex fixedFrame, + const Vector3& gravityInFixedFrame, + const VectorDynSize& measurements); + + void updateEstimateInformationFloatingBase(const JointPosDoubleArray& jointsConfiguration, + const JointDOFsDoubleArray& jointsVelocity, + const FrameIndex floatingFrame, + const Vector3& bodyAngularVelocityOfSpecifiedFrame, + const VectorDynSize& measurements); + + bool doEstimate(); + + void getLastEstimate(iDynTree::VectorDynSize& lastEstimate) const; + const iDynTree::VectorDynSize& getLastEstimate() const; + + + }; +} + +#endif /* end of include guard: IDYNTREE_BERDY_SPARSEMAPSOLVER_H */ diff --git a/src/estimation/include/iDynTree/BipedFootContactClassifier.h b/src/estimation/include/iDynTree/BipedFootContactClassifier.h new file mode 100644 index 00000000000..302ac4547d2 --- /dev/null +++ b/src/estimation/include/iDynTree/BipedFootContactClassifier.h @@ -0,0 +1,142 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause +#ifndef IDYNTREE_BIPED_FOOT_CONTACT_CLASSIFIER_H +#define IDYNTREE_BIPED_FOOT_CONTACT_CLASSIFIER_H + +#include +#include "ContactStateMachine.h" + + +namespace iDynTree +{ + /** + * Enumeration of switching pattern + */ + enum SwitchingPattern + { + /** + * Switching active foot between left and right at every double stance + */ + ALTERNATE_CONTACT, + + /** + * Setting active foot to the one that made contact most recently + * @warning this pattern remains unimplemented in the current version of this class + * using this pattern will always return left foot as the active foot + */ + LATEST_ACTIVE_CONTACT, + + /** + * Fixing active foot to the default foot defined by the user + * @warning this pattern remains unimplemented in the current version of this class + * using this pattern will always return left foot as the active foot + */ + DEFAULT_CONTACT + }; + + /* Foot Contact Classifier class for determining the primary foot in contact + * with the ground surface. Contains contact state machines for each foot: left and right, + * and implements the Schmitt Trigger thresholding on each foot to determine the primary + * foot which is in contact according to a switching pattern criteria. + * The switching pattern is primarily used to change the frames of reference for the Legged Odometry (LO) + * while one of the two foot breaks contact with the ground surface, since LO assumes that atleast one + * foot is in contact with ground at any instant of time. However, it is also used to assign + * the primary foot in case of double stance. + * + * In the current version of this class, switching logic is implemented only for + * ALTERNATE_CONTACT patterns vastly considered during walking tasks. This class is aimed + * to be extended towards other switching patterns like LATEST_ACTIVE_CONTACT, DEFAULT_CONTACT + * for implementation along side different task based controllers. + */ + class BipedFootContactClassifier + { + public: + /** + * Enumeration of foot in contact + */ + enum contactFoot + { + LEFT_FOOT, // 0 + RIGHT_FOOT, // 1 + UNKNOWN_FOOT // 2 + }; + + /** + * Constructor + * @params leftFootSchmittParams const ref to struct containing parameters for the left foot schmitt trigger device + * @params rightFootSchmittParams const ref to struct containing parameters for the right foot schmitt trigger device + */ + BipedFootContactClassifier(const SchmittParams& leftFootSchmittParams, const SchmittParams& rightFootSchmittParams); + + /** + * Updates the contact state machine for both the foot, determines the current state + * and detects foot transition for setting the primary foot(active foot) + * @param currentTime time + * @param leftNormalForce z-component of the force acting on the left foot + * @param rightNormalForce z-component of the force acting on the right foot + */ + void updateFootContactState(double currentTime, double leftFootNormalForce, double rightFootNormalForce); + + /** + * Get the primary foot + * @return contactFoot left, right or unknown + */ + contactFoot getPrimaryFoot() { return m_primaryFoot; } + + /** + * Get left foot contact state + * @return true if in contact, false otherwise + */ + bool getLeftFootContactState() { return m_leftFootContactState; } + + /** + * Get right foot contact state + * @return true if in contact, false otherwise + */ + bool getRightFootContactState() { return m_rightFootContactState; } + + /** + * set switching pattern to be considered for determining primary foot + * @warning no default value is considered, remember to call this method after instantiation + * @param pattern switching pattern + */ + void setContactSwitchingPattern(SwitchingPattern pattern) { m_pattern = pattern; } + + /** + * set primary foot + * This method was mainly intended to be called by an external process + * to set the primary foot in the initial setting, before any contact is broken. + * In case it is set to UNKNOWN_FOOT, it waits for the foot normal force measurements (checks left first and then right), + * and activates corresponding foot, handled in the detectTransitions() method. + * @param foot primary foot + */ + void setPrimaryFoot(contactFoot foot) { m_primaryFoot = foot; } + + // unique pointer to contact state machine for left foot + std::unique_ptr m_leftFootContactClassifier; + + // unique pointer to contact state machine for right foot + std::unique_ptr m_rightFootContactClassifier; + + private: + /** + * Determine the primary foot depending on the switching pattern + * compares the contact transition modes on each foot and + * sets the primary foot accordingly + */ + void detectFeetTransition(); + + // active/primary foot + contactFoot m_primaryFoot; + + // left foot contact state + bool m_leftFootContactState; + + // right foot contact state + bool m_rightFootContactState; + + // switching pattern + SwitchingPattern m_pattern; + }; +} +#endif \ No newline at end of file diff --git a/src/estimation/include/iDynTree/ContactStateMachine.h b/src/estimation/include/iDynTree/ContactStateMachine.h new file mode 100644 index 00000000000..57b5f38a4cf --- /dev/null +++ b/src/estimation/include/iDynTree/ContactStateMachine.h @@ -0,0 +1,126 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_CONTACTSTATEMACHINE_H +#define IDYNTREE_CONTACTSTATEMACHINE_H +#include +#include +#include "SchmittTrigger.h" + +namespace iDynTree +{ + /** + * struct to hold schmitt trigger device parameters + */ + struct SchmittParams + { + double stableTimeContactMake; + double stableTimeContactBreak; + double contactMakeForceThreshold; + double contactBreakForceThreshold; + }; + + /** + * Contact State Machine class for binary contact state detection + * Contains a Schmitt Trigger device for updating the contact states + * using the contact normal force acting on the contact link and + * Determines contact transitions using simple binary switching logic + * + * Can be used to determine stable contacts, contact breaking and contact making. + * The parameters to the Schmitt Trigger are passed as a struct and is the Schmitt + * Trigger is instantiated in the class constructor. + * + * NOTE: There are no default parameters to the Schmitt Trigger. These parameters are set through + * the constructor during instantiation. + * + * This class does not exactly abstract the Schmitt Trigger class. Schmitt Trigger methods are still accessible through + * the m_contactSchmitt object. This class uses a generic Schmitt Trigger object and augments its functionality + * specific to physical contacts based scenarios. + */ + class ContactStateMachine + { + public: + /** + * Enumeration of contact transitions + */ + enum contactTransition + { + /** + * previous state: off contact, current state: off contact + */ + STABLE_OFFCONTACT, // 0 + + /** + * previous state: on contact, current state: on contact + */ + STABLE_ONCONTACT, // 1 + + /** + * previous state: on contact, current state: off contact + */ + CONTACT_BREAK, // 2 + + /** + * previous state: off contact, current state: on contact + */ + CONTACT_MAKE, // 3 + + /** + * Unknown transition + */ + UNKNOWN_TRANSITION = -1 + }; + + /** + * Constructor + * @param s const reference to a struct containing schmitt trigger device parameters + */ + ContactStateMachine(const SchmittParams& s); + + /** + * Calls schmitt trigger device update + * @param currentTime time + * @param contactNormalForce normal force acting on the contact link in consideration + */ + void contactMeasurementUpdate(double currentTime, double contactNormalForce); + + /** + * Calls schmitt trigger device reset + */ + void resetDevice() { m_contactSchmitt.get()->resetDevice(); } + + /** + * Get current contact state + * @return true, if in contact, false otherwise + */ + bool contactState() { return m_currentState; } + + /** + * Determines contact transitions using simple binary switching logic + * @return contactTransition enumerated value + */ + contactTransition contactTransitionMode(); + + /** + * Get time of last contact state update + * @return time + */ + double lastUpdateTime(); + + /** + * unique pointer to the schmitt trigger device + */ + std::unique_ptr m_contactSchmitt; + private: + // previous contact state + bool m_previousState; + + // current contact state + bool m_currentState; + + // transtion mode based on previous and current contact states + int m_tranisitionMode; + + }; +} +#endif diff --git a/src/estimation/include/iDynTree/Estimation/AttitudeEstimator.h b/src/estimation/include/iDynTree/Estimation/AttitudeEstimator.h index c886556d29a..0968afe1465 100644 --- a/src/estimation/include/iDynTree/Estimation/AttitudeEstimator.h +++ b/src/estimation/include/iDynTree/Estimation/AttitudeEstimator.h @@ -1,189 +1,13 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef IDYNTREE_ATTITUDEESTIMATOR_H -#define IDYNTREE_ATTITUDEESTIMATOR_H +#ifndef IDYNTREE_ESTIMATION_ATTITUDEESTIMATOR_H +#define IDYNTREE_ESTIMATION_ATTITUDEESTIMATOR_H -#include -#include - -#include - -namespace iDynTree -{ - typedef iDynTree::Vector3 LinearAccelerometerMeasurements; - typedef iDynTree::Vector3 GyroscopeMeasurements; - typedef iDynTree::Vector3 MagnetometerMeasurements; - - typedef iDynTree::Vector4 UnitQuaternion; - typedef iDynTree::Vector3 RPY; - - /** @struct state internal state of the estimator - * @var state::m_orientation - * orientation estimate in \f$ \mathbb{R}^4 \f$ quaternion representation - * @var state::m_orientation - * angular velocity estimate in \f$ \mathbb{R}^3 \f$ - * @var state::m_orientation - * gyroscope bias estimate in \f$ \mathbb{R}^3 \f$ - */ - struct AttitudeEstimatorState - { - iDynTree::UnitQuaternion m_orientation; - iDynTree::Vector3 m_angular_velocity; - iDynTree::Vector3 m_gyroscope_bias; - }; - - /** - * @class IAttitudeEstimator generic interface for attitude estimator classes - * - * The aim is to implement different attitude estimators as a block that takes IMU measurements - * as inputs and gives attitude estimates as outputs. This way the underlying implementation is abstracted - * and the user only has to set a few parameters and run the estimator. - * - * The general procedure to use the estimators would be, - * - instantiate the filter, - * - set initial internal state - * - in a loop, - * - update the filter with measurements - * - propagate the states - * - * However, additional methods to set and get parameters for the filter might be available with respect to the filters. - * - * The internal state of the estimator is described as \f$ X = \begin{bmatrix} {^A}q_B \\ {^B}\Omega_{A,B} \\ {^B}b \end{bmatrix}^T \f$ - * \f$ {^A}q_B \in \mathbb{R}^4 \f$ is the quaternion representing the orientation of a body(IMU) frame with respect to an inertial frame , - * \f$ {^B}\Omega_{A,B} \in \mathbb{R}^3 \f$ is the angular velocity of a body(IMU) frame with respect to an inertial frame, expressed in the body frame and - * \f$ {^B}b \in \mathbb{R}^3 \f$ is the gyroscope bias expressed in the body frame. - * - */ - class IAttitudeEstimator - { - public: - virtual ~IAttitudeEstimator(); - - /** - * @brief Update the filter with accelerometer and gyroscope measurements - * - * @param[in] linAccMeas proper (body acceleration - gravity) classical acceleration of the origin of the body frame B expressed in frame B - * @param[in] gyroMeas angular velocity of body frame B with respect to an inertial fram A, expressed in frame B - * - * @note consider the current behavior of our system does not use magnetometer measurements and is calling this method to update measurements. - * Then, if we decide to turn the flag use_magnetometer_measurements to true, this will not guarantee that the magnetometer measurements - * will be used. The magnetometer measurements will be used only if we replace this function call with the other overlaoded function considering - * the magnetometer measurements. - * - * @return true/false if successful/not - */ - virtual bool updateFilterWithMeasurements(const iDynTree::LinearAccelerometerMeasurements& linAccMeas, - const iDynTree::GyroscopeMeasurements& gyroMeas) = 0; - - /** - * @brief Update the filter with accelerometer, gyroscope and magnetometer measurements - * - * @param[in] linAccMeas proper (body acceleration - gravity) classical acceleration of the origin of the body frame B expressed in frame B - * @param[in] gyroMeas angular velocity of body frame B with respect to an inertial fram A, expressed in frame B - * @param[in] magMeas magnetometer measurements expressed in frame B - * - * @return true/false if successful/not - */ - virtual bool updateFilterWithMeasurements(const iDynTree::LinearAccelerometerMeasurements& linAccMeas, - const iDynTree::GyroscopeMeasurements& gyroMeas, - const iDynTree::MagnetometerMeasurements& magMeas) = 0; - - /** - * @brief Propagate the states and associated uncertainties through properly defined propagation functions - * The underlying implementation depends on the type of filter being implemented. - * - * @return true/false if successful/not - */ - virtual bool propagateStates() = 0; - - /** - * @brief Get orientation of the body with respect to inertial frame, in rotation matrix form - * If we denote \f$ A \f$ as inertial frame and \f$ B \f$ as the frame attached to the body, - * then this method gives us \f$ {^A}R_B \f$ as the rotation matrix - * @param[out] rot Rotation matrix - * @return true/false if successful/not - */ - virtual bool getOrientationEstimateAsRotationMatrix(iDynTree::Rotation& rot) = 0; - - /** - * @brief Get orientation of the body with respect to inertial frame, in unit quaternion form - * If we denote \f$ A \f$ as inertial frame and \f$ B \f$ as the frame attached to the body, - * then this method gives us \f$ {^A}q_B as the quaternion \f$ - * - * @note quaternion has the form (real, imaginary) and is normalized - * @note Usually a rotation matrix can be described using two quaternions due to its double-connectedness property - * Depending on the specific filter, the initial state and the trajectory of the system, we could obtain - * one quaternion or the other(opposite spin), depending on the system dynamics. - * - * @param[out] q UnitQuaternion - * @return true/false if successful/not - */ - virtual bool getOrientationEstimateAsQuaternion(iDynTree::UnitQuaternion& q) = 0; - - /** - * @brief Get orientation of the body with respect to inertial frame, in Euler's RPY form - * If we denote \f$ A \f$ as inertial frame and \f$ B \f$ as the frame attached to the body, - * then this method gives us the RPY 3d vector of Euler Angles when composed together gives us \f$ {^A}R_B \f$ as the rotation matrix - * where \f$ {^A}R_B = Rot_z(yaw)Rot_y(pitch)Rot_x(roll)\f$. - * For more details about the range of the RPY Euler angles, please refer the documentation of - * GetRPY() - * - * @param[out] rpy 3D vector containing roll pitch yaw angles - * @return true/false if successful/not - */ - virtual bool getOrientationEstimateAsRPY(iDynTree::RPY& rpy) = 0; - - /** - * @brief Get dimension of the state vector - * @return size_t size of state vector - */ - virtual size_t getInternalStateSize() const = 0; - - /** - * @brief Get internal state of the estimator - * The internal state of the estimator is described as \f$ X = \begin{bmatrix} {^A}q_B \\ {^B}\Omega_{A,B} \\ {^B}b \end{bmatrix}^T \f$ - * \f$ {^A}q_B \in \mathbb{R}^4 \f$ is the quaternion representing the orientation of a body(IMU) frame with respect to an inertial frame , - * \f$ {^B}\Omega_{A,B} \in \mathbb{R}^3 \f$ is the angular velocity of a body(IMU) frame with respect to an inertial frame, expressed in the body frame and - * \f$ {^B}b \in \mathbb{R}^3 \f$ is the gyroscope bias expressed in the body frame. - * The default internal state of the estimator would be \f$ X = \begin{bmatrix} 1.0 \\ 0_{1 \times 3} \\ 0_{1 \times 3} \\ 0_{1 \times 3} \end{bmatrix}^T \f$ - * @param[out] stateBuffer Span object as reference of the container where state vector should be copied to - * @return true/false if successful/not - */ - virtual bool getInternalState(const iDynTree::Span & stateBuffer) const = 0; - - /** - * @brief Get initial internal state of the estimator - * The internal state of the estimator is described as \f$ X = \begin{bmatrix} {^A}q_B \\ {^B}\Omega_{A,B} \\ {^B}b \end{bmatrix}^T \f$ - * \f$ {^A}q_B \in \mathbb{R}^4 \f$ is the quaternion representing the orientation of a body(IMU) frame with respect to an inertial frame , - * \f$ {^B}\Omega_{A,B} \in \mathbb{R}^3 \f$ is the angular velocity of a body(IMU) frame with respect to an inertial frame, expressed in the body frame and - * \f$ {^B}b \in \mathbb{R}^3 \f$ is the gyroscope bias expressed in the body frame. - * The default internal state of the estimator would be \f$ X = \begin{bmatrix} 1.0 \\ 0_{1 \times 3} \\ 0_{1 \times 3} \\ 0_{1 \times 3} \end{bmatrix}^T \f$ - * @param[out] stateBuffer Span object as reference of the container where state vector should be copied to - * @return true/false if successful/not - */ - virtual bool getDefaultInternalInitialState(const iDynTree::Span & stateBuffer) const = 0; - - /** - * @brief set internal state of the estimator. - * The internal state of the estimator is described as \f$ X = \begin{bmatrix} {^A}q_B \\ {^B}\Omega_{A,B} \\ {^B}b \end{bmatrix}^T \f$ - * \f$ {^A}q_B \in \mathbb{R}^4 \f$ is the quaternion representing the orientation of a body(IMU) frame with respect to an inertial frame , - * \f$ {^B}\Omega_{A,B} \in \mathbb{R}^3 \f$ is the angular velocity of a body(IMU) frame with respect to an inertial frame, expressed in the body frame and - * \f$ {^B}b \in \mathbb{R}^3 \f$ is the gyroscope bias expressed in the body frame. - * @param[in] stateBuffer Span object as reference of the container from which the internal state vector should be assigned. The size of the buffer should be 10. - * @return true/false if successful/not - */ - virtual bool setInternalState(const iDynTree::Span & stateBuffer) = 0; +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif - /** - * @brief set the initial orientation for the internal state of the estimator. - * The initial orientation for the internal state of the estimator is described as \f$ {^A}q_B \f$ - * \f$ {^A}q_B \in \mathbb{R}^4 \f$ is the quaternion representing the orientation of a body(IMU) frame with respect to an inertial frame , - * @param[in] stateBuffer Span object as reference of the container from which the inital orientaiton for internal state vector should be assigned. The size of the buffer should be 4. - * @return true/false if successful/not - */ - virtual bool setInternalStateInitialOrientation(const iDynTree::Span& orientationBuffer) = 0; - }; +#include -} #endif diff --git a/src/estimation/include/iDynTree/Estimation/AttitudeEstimatorUtils.h b/src/estimation/include/iDynTree/Estimation/AttitudeEstimatorUtils.h index 0e16539a015..8b5d86f9191 100644 --- a/src/estimation/include/iDynTree/Estimation/AttitudeEstimatorUtils.h +++ b/src/estimation/include/iDynTree/Estimation/AttitudeEstimatorUtils.h @@ -1,187 +1,14 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef IDYNTREE_ATTITUDE_ESTIMATOR_UTILS_H -#define IDYNTREE_ATTITUDE_ESTIMATOR_UTILS_H +#ifndef IDYNTREE_ESTIMATION_ATTITUDE_ESTIMATOR_UTILS_H +#define IDYNTREE_ESTIMATION_ATTITUDE_ESTIMATOR_UTILS_H -#include -#include -#define _USE_MATH_DEFINES -#include -#include - -/** - * - * @brief check a valid measurement - * @param[in] a vector3 - * @return bool true/false - */ -bool checkValidMeasurement(const iDynTree::Vector3& in, const std::string& measurement_type, bool check_also_zero_vector); - -/** - * - * @brief get unit vector - * @param[in] a vector3 - * @return bool false if input vector has zero norm - */ -bool getUnitVector(const iDynTree::Vector3& in, iDynTree::Vector3& out); - -/** - * - * @brief checks if vector has NaN values - * any element of vector is NaN implies a NaN vector - * @param[in] vec vector3 - * @return bool true/false - */ -bool isVectorNaN(const iDynTree::Vector3& vec); - -/** - * - * @brief checks if vector is a zero vector - * all elements of vector are zero implies a zero vector - * @param[in] vec vector3 - * @return bool true/false - */ -bool isZeroVector(const iDynTree::Vector3& vec); - -/** - * - * @brief computes the cross vector of two 3D vectors - * @param[in] a 3D vector - * @param[in] b 3D vector - * @return iDynTree::Vector3 - */ -iDynTree::Vector3 crossVector(const iDynTree::Vector3& a, const iDynTree::Vector3& b); - -/** - * @brief computes \f$ 3 \times 3 \f$ skew-symmetric matrix (\f$ \mathbb{so}(3) \f$ space) for a given 3d vector (\f$ \mathbb{R}^3 \f$ space) - * - * @param[in] omega 3d vector (usually angular velocity) - * @return iDynTree::Matrix3x3 - */ -iDynTree::Matrix3x3 mapR3Toso3(const iDynTree::Vector3& omega); - -/** - * @brief checks if the \f$ 3 \times 3 \f$ matrix is skew-symmetric - * \f[ S + S^T = 0 \f] - * @param[in] S \f$ 3 \times 3 \f$ matrix - * @return bool true/false - */ -bool checkSkewSymmetricity(const iDynTree::Matrix3x3& S); - -/** - * @brief computes 3D vector (\f$ \mathbb{R}^3 \f$ space) from a skew symmetric matrix (\f$ \mathbb{so}(3) \$ space) - * - * @param[in] S \f$ 3 \times 3 \f$ matrix - * @return iDynTree::Vector3 - */ -iDynTree::Vector3 mapso3ToR3(const iDynTree::Matrix3x3& S); - -/** - * @brief computes scalar dot product of two 3-d vectors - * Maps (\f$ \mathbb{R}^n \f$ space) to (\f$ \mathbb{R} \f$ space) through its dual vector - * @param[in] a 3D vector (not passed as reference, but as a copy to avoid changes in source due to in-place manipulation) - * @param[in] b 3D vector - * @return double - */ -double innerProduct(const iDynTree::Vector3 a, const iDynTree::Vector3& b); - -/** - * @brief real part of quaternion, - * \f$ s \f$ in \f$ s + i v_1 + i v_2 + i v_3 \f$ - * - * @param[in] q 4d vector or quaternion - * @return double - */ -double realPartOfQuaternion(const iDynTree::UnitQuaternion& q); - -/** - * @brief imaginary part of quaternion - * \f$ v \f$ in \f$ s + i v_1 + i v_2 + i v_3 \f$ - * @param[in] q 4d vector or quaternion - * @return iDynTree::Vector3 - */ -iDynTree::Vector3 imaginaryPartOfQuaternion(const iDynTree::UnitQuaternion& q); - -/** - * @brief composition operator - quaternion multiplication - * - * @param[in] q1 4d vector or quaternion - * @param[in] q2 4d vector or quaternion - * @return iDynTree::UnitQuaternion - */ -iDynTree::UnitQuaternion composeQuaternion(const iDynTree::UnitQuaternion& q1, const iDynTree::UnitQuaternion& q2); - -/** - * @brief computes the matrix map of quaternion left multiplication \f$ q1 \circ q2 = q1q2 \f$ - * in opposition to the right multiplication \f$ q1 \circ q2 = q2q1 \f$ - * @param[in] x 4d vector or quaternion q1 - * @return iDynTree::Matrix4x4 - */ -iDynTree::Matrix4x4 mapofYQuaternionToXYQuaternion(const iDynTree::UnitQuaternion &x); - - -/** - * @brief composition operator - quaternion multiplication - * this method is faster than composeQuaternion() - * @param[in] q1 4d vector or quaternion - * @param[in] q2 4d vector or quaternion - * @return iDynTree::UnitQuaternion - */ -iDynTree::UnitQuaternion composeQuaternion2(const iDynTree::UnitQuaternion &q1, const iDynTree::UnitQuaternion &q2); - - -/** - * @brief computes pure quaternion given a 3d vector (uually angular velocity) - * - * @param[in] bodyFixedFrameVelocityInInertialFrame 3d vector - * @return iDynTree::UnitQuaternion - */ -iDynTree::UnitQuaternion pureQuaternion(const iDynTree::Vector3& bodyFixedFrameVelocityInInertialFrame); - - -/** - * @brief exponential map for quaternion - maps angular velocities to quaternion - * - * \f$ \text{exp}(\omega) = \begin{bmatrix} \text{cos}(\frac{||\omega||}{2}) \\ \text{sin}(\frac{||\omega||}{2})\frac{\omega}{||\omega||} \end{bmatrix} \f$ - * - * @param[in] omega angular velocity - * @return iDynTree::UnitQuaternion - */ -inline iDynTree::UnitQuaternion expQuaternion(iDynTree::Vector3 omega) -{ - iDynTree::UnitQuaternion q; - q.zero(); - q(0) = 1.0; - using iDynTree::toEigen; - double norm{toEigen(omega).norm()}; - - if (norm == 0) - { - return q; - } - - double c = std::cos(norm/2); - double s = std::sin(norm/2)/norm; - - q(0) = c; - q(1) = omega(0)*s; - q(2) = omega(1)*s; - q(3) = omega(2)*s; - - return q; -} - -template -inline bool check_are_almost_equal(const T& x, const T& y, int units_in_last_place) -{ - if (!( std::abs(x- y) <= std::numeric_limits::epsilon()*std::max(std::abs(x), std::abs(y))*units_in_last_place)) - { - return false; - } +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif - return true; -} +#include #endif diff --git a/src/estimation/include/iDynTree/Estimation/AttitudeMahonyFilter.h b/src/estimation/include/iDynTree/Estimation/AttitudeMahonyFilter.h index cba90384da3..b6d265dd55b 100644 --- a/src/estimation/include/iDynTree/Estimation/AttitudeMahonyFilter.h +++ b/src/estimation/include/iDynTree/Estimation/AttitudeMahonyFilter.h @@ -1,154 +1,13 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef ATTITUDE_MAHONY_FILTER_H -#define ATTITUDE_MAHONY_FILTER_H +#ifndef IDYNTREE_ESTIMATION_ATTITUDE_MAHONY_FILTER_H +#define IDYNTREE_ESTIMATION_ATTITUDE_MAHONY_FILTER_H -#include -#include - -namespace iDynTree -{ - -/** -* @struct AttitudeMahonyFilterParameters Parameters to set up the quaternion EKF -* @var AttitudeMahonyFilterParameters::time_step_in_seconds -* discretization time step in seconds, default value: \f$ 0.001 s \f$ -* @var AttitudeMahonyFilterParameters::kp -* Mahony \f$ K_p \f$ gain over the correction from IMU measurements, default value: \f$ 1.0 \f$ -* @var AttitudeMahonyFilterParameters::ki -* Mahony \f$ K_i \f$ gain over the gyro bias evolution, default value: \f$ 1.0 \f$ -* @var AttitudeMahonyFilterParameters::use_magenetometer_measurements -* flag to enable the use of magnetometer measurement for yaw correction, default value: false -* @var AttitudeMahonyFilterParameters::confidence_magnetometer_measurements -* confidence on magnetometer measurements, default value: \f$ 0.0 \f$ -*/ -struct AttitudeMahonyFilterParameters { - double time_step_in_seconds{0.001}; - double kp{1.0}; - double ki{1.0}; - bool use_magnetometer_measurements{false}; - double confidence_magnetometer_measurements{0.0}; -}; - - -/** - * @class AttitudeMahonyFilter Implements an explicit passive complementary filter on quaternion groups - * described in the paper Non-linear complementary filters on SO3 groups - * - * The filter is used to estimate the states \f$ X = \begin{bmatrix} {^A}q_B \\ {^B}\Omega_{A,B} \\ {^B}b \end{bmatrix}^T \f$ - * where \f$ {^A}q_B \in \mathbb{R}^4 \f$ is the quaternion representing the orientation of a body(IMU) frame with respect to an inertial frame , - * \f$ {^B}\Omega_{A,B} \in \mathbb{R}^3 \f$ is the angular velocity of a body(IMU) frame with respect to an inertial frame, expressed in the body frame and - * \f$ {^B}b \in \mathbb{R}^3 \f$ is the gyroscope bias expressed in the body frame. - * @note: we will drop the subscripts and superscripts in the rest of the documentation for convenience - * - * The discretized dynamics of the filter is implemented in the propagateStates() method and is described by the following equations, - * \f$ q_{k+1} = q_{k} + \Delta t \frac{1}{2}q_{k} \circ \begin{bmatrix} 0 \\ {\Omega_y}_{k+1} - b_k + K_p \omega_{mes_{k+1}}\end{bmatrix}\f$ - * \f$ \Omega_{k+1} = {\Omega_y}_{k+1} - b_k \f$ - * \f$ b_{k+1} = b_k - K_i \Delta t \frac{1}{2} \omega_{mes_{k+1}} \f$ - * - * The updateFilterWithMeasurements() uses the recent IMU measurements to compute the term \f$ \omega_{mes} \f$ which gives the vectorial from accelerometer and magnetometer measurements - * \f$ \omega_{mes} = -(\Sigma{n}{i=1} \frac{k_i}{2} (v_i \hat{v}_i^T - \hat{v}_i v_i^T) )^{\vee} \f$ - * where \f$ v_i \f$ is the normalized accelerometer or magnetometer measurement, - * \f$ \hat{v_i} \f$ is the vector obtained from the orientation estimated combined with gravity direction or absolute magnetic field direction, for e.g, \f$ \hat{v_acc} = {^w}R_b^T e_3 \f$ - * and \f$ k_i \f$ is the confidence weight on the i-th measurement. In our case, i = 1 or 2. - * - * The usage of the attitude estimator can be as follows, - * - After instantiation, the parameters of the filter can be set using the individual parameter methods or the struct method. - * - The filter state can be initialized by calling the setInternalState() method - * - Once initialized, the following filter methods can be run in a loop to get the orientation estimates, - * - updateFilterWithMeasurements() method to pass the recent measurements to the filter - * - propagateStates() method to propagate the states through the system dynamics and correcting using the updated measurements - * - getInternalState() or getOrientationEstimate*() methods to get the entire state estimate or only the attitude estimated in desired representation - */ -class AttitudeMahonyFilter : public IAttitudeEstimator -{ -public: - AttitudeMahonyFilter(); - - /** - * @brief set flag to use magnetometer measurements - * @param[in] flag enable/disable magnetometer measurements - */ - void useMagnetoMeterMeasurements(bool flag); - - /** - * @brief set the confidence weights on magenetometer measurements, if used - * @param[in] confidence can take values between \f$ [0, 1] \f$ - */ - void setConfidenceForMagnetometerMeasurements(double confidence); - - /** - * @brief set the Kp gain - * @param[in] kp gain - */ - void setGainkp(double kp); - - /** - * @brief set the Ki gain - * @param[in] ki gain - */ - void setGainki(double ki); - - /** - * @brief set discretization time step in seconds - * @param[in] timestepInSeconds time step - */ - void setTimeStepInSeconds(double timestepInSeconds); - - /** - * @brief Set the gravity direction assumed by the filter (for computing orientation vectorial from accelerometer) - * @param[in] gravity_dir gravity direction - */ - void setGravityDirection(const iDynTree::Direction& gravity_dir); - - /** - * @brief Set filter parameters with the struct members. - * This does not reset the internal state. - * @param[in] params object of AttitudeMahonyFilterParameters passed as a const reference - * @return true/false if successful/not - */ - bool setParameters(const AttitudeMahonyFilterParameters& params) - { - m_params_mahony = params; - return true; - } - - /** - * @brief Get filter parameters as a struct. - * @param[out] params object of AttitudeMahonyFilterParameters passed as reference - */ - void getParameters(AttitudeMahonyFilterParameters& params) {params = m_params_mahony;} - - bool updateFilterWithMeasurements(const iDynTree::LinearAccelerometerMeasurements& linAccMeas, - const iDynTree::GyroscopeMeasurements& gyroMeas) override; - bool updateFilterWithMeasurements(const iDynTree::LinearAccelerometerMeasurements& linAccMeas, - const iDynTree::GyroscopeMeasurements& gyroMeas, - const iDynTree::MagnetometerMeasurements& magMeas) override; - bool propagateStates() override; - bool getOrientationEstimateAsRotationMatrix(iDynTree::Rotation& rot) override; - bool getOrientationEstimateAsQuaternion(iDynTree::UnitQuaternion& q) override; - bool getOrientationEstimateAsRPY(iDynTree::RPY& rpy) override; - size_t getInternalStateSize() const override; - bool getInternalState(const iDynTree::Span & stateBuffer) const override; - bool getDefaultInternalInitialState(const iDynTree::Span & stateBuffer) const override; - bool setInternalState(const iDynTree::Span & stateBuffer) override; - bool setInternalStateInitialOrientation(const iDynTree::Span& orientationBuffer) override; - -protected: - AttitudeMahonyFilterParameters m_params_mahony; ///< struct holding the Mahony filter parameters - AttitudeEstimatorState m_state_mahony, m_initial_state_mahony; -private: - iDynTree::Rotation m_orientationInSO3; ///< orientation estimate as rotation matrix \f$ {^A}R_B \f$ where \f$ A \f$ is inertial frame and \f$ B \f$ is the frame attached to the body - iDynTree::RPY m_orientationInRPY; ///< orientation estimate as a 3D vector in RPY representation, where \f$ {^A}R_B = Rot_z(yaw)Rot_y(pitch)Rot_x(roll) \f$ - - iDynTree::Vector3 m_omega_mes; ///< vectorial estimate from accelerometer and magnetometer measurements, \f$ \omega_{mes} \in \mathbb{R}^3 \f$, notation from the paper Non linear complementary filters on the special orthogonal group - iDynTree::GyroscopeMeasurements m_Omega_y; ///< gyroscope measurement, \f$ \Omega_{y} \in \mathbb{R}^3 \f$, notation from the paper Non linear complementary filters on the special orthogonal group - - iDynTree::Direction m_gravity_direction; ///< direction of the gravity vector expressed in the inertial frame denoted by \f$ A \f$, default set to \f$ e_3 = \begin{bmatrix} 0 & 0 & 1.0 \end{bmatrix}^T \f$ - iDynTree::Direction m_earth_magnetic_field_direction; ///< direction of absolute magnetic field expressed in the inertial frame denoted by \f$ A \f$, default set to \f$ {^A}m = \begin{bmatrix} 0 & 0 & 1.0 \end{bmatrix}^T \f$ -}; +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif -} +#include #endif diff --git a/src/estimation/include/iDynTree/Estimation/AttitudeQuaternionEKF.h b/src/estimation/include/iDynTree/Estimation/AttitudeQuaternionEKF.h index c7da06b1c0d..d2f8d1d9d53 100644 --- a/src/estimation/include/iDynTree/Estimation/AttitudeQuaternionEKF.h +++ b/src/estimation/include/iDynTree/Estimation/AttitudeQuaternionEKF.h @@ -1,329 +1,13 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef ATTITUDE_QUATERNION_EKF_H -#define ATTITUDE_QUATERNION_EKF_H +#ifndef IDYNTREE_ESTIMATION_ATTITUDE_QUATERNION_EKF_H +#define IDYNTREE_ESTIMATION_ATTITUDE_QUATERNION_EKF_H -#include -#include -#include - -namespace iDynTree -{ - const unsigned int output_dimensions_with_magnetometer = 4; ///< dimension of \f$ \mathbb{R}^3 \times \mathbb{R} \f$ accelerometer measurements and magnetometer yaw measurement - const unsigned int output_dimensions_without_magnetometer = 3; ///< dimension of \f$ \mathbb{R}^3 \f$ accelerometer measurements - const unsigned int input_dimensions = 3; ///< dimension of \f$ \mathbb{R}^3 \f$ gyroscope measurements - - /** - * @struct AttitudeQuaternionEKFParameters Parameters to set up the quaternion EKF - * @var AttitudeQuaternionEKFParameters::time_step_in_seconds - * discretization time step in seconds, default value: \f$ 0.01 s \f$ - * @var AttitudeQuaternionEKFParameters::bias_correlation_time_factor - * time factor modeling how fast the bias can vary, default value: \f$ 0.01 \f$ - * @var AttitudeQuaternionEKFParameters::accelerometer_noise_variance - * measurement noise covariance \f$ \sigma_{acc}^{2} \f$ of accelerometer measurement, default value: \f$ 0.001 \f$ - * @var AttitudeQuaternionEKFParameters::magnetometer_noise_variance - * measurement noise covariance \f$ \sigma_{mag}^{2} \f$ of magnetometer measurement, default value: \f$ 0.001 \f$ - * @var AttitudeQuaternionEKFParameters::gyroscope_noise_variance - * system noise covariance \f$ \sigma_{gyro}^{2} \f$ of gyroscope measurement, since it is the input to the system, default value: \f$ 0.001 \f$ - * @var AttitudeQuaternionEKFParameters::gyro_bias_noise_variance - * system noise covariance \f$ \sigma_{gyrobias}^{2} \f$ of gyroscope bias estimate, default value: \f$ 0.0001 \f$ - * @var AttitudeQuaternionEKFParameters::initial_orientation_error_variance - * initial state covariance \f$ \sigma_{q_0}^{2} \f$ of orientation, default value: \f$ 10 \f$ - * @var AttitudeQuaternionEKFParameters::initial_ang_vel_error_variance - * initial state covariance \f$ \sigma_{\omega_0}^{2} \f$ of angular velocity, default value: \f$ 10 \f$ - * @var AttitudeQuaternionEKFParameters::initial_gyro_bias_error_variance - * measurement noise covariance \f$ \sigma_{acc}^{2} \f$ of gyroscope bias, default value: \f$ 10 \f$ - * @var AttitudeQuaternionEKFParameters::use_magnetometer_measurements - * flag to enable the use of magnetometer measurement for yaw correction, default value: false - */ - struct AttitudeQuaternionEKFParameters { - double time_step_in_seconds{0.01}; - double bias_correlation_time_factor{0.01}; - - // measurement noise - zero mean, and a given variance - double accelerometer_noise_variance{0.001}; - double magnetometer_noise_variance{0.001}; - - // process noise - zero mean, and a given covariance - double gyroscope_noise_variance{0.001}; - double gyro_bias_noise_variance{0.0001}; - - double initial_orientation_error_variance{10}; - double initial_ang_vel_error_variance{10}; - double initial_gyro_bias_error_variance{10}; - - bool use_magnetometer_measurements{false}; - }; - - /** - * @class AttitudeQuaternionEKF implements a Quaternion based Discrete Extended Kalman Filter fusing IMU measurements, - * to give estimates of orientation, angular velocity and gyroscope bias - * - * It follows the implementation detailed in - * Quaternion Based Extended Kalman Filter, slides by Michael Stohmeier - * The filter is used to estimate the states \f$ X = \begin{bmatrix} {^A}q_B \\ {^B}\Omega_{A,B} \\ {^B}b \end{bmatrix}^T \f$ - * where \f$ {^A}q_B \in \mathbb{R}^4 \f$ is the quaternion representing the orientation of a body(IMU) frame with respect to an inertial frame , - * \f$ {^B}\Omega_{A,B} \in \mathbb{R}^3 \f$ is the angular velocity of a body(IMU) frame with respect to an inertial frame, expressed in the body frame and - * \f$ {^B}b \in \mathbb{R}^3 \f$ is the gyroscope bias expressed in the body frame. - * @note: we will drop the subscripts and superscripts in the rest of the documentation for convenience - * - * Discretized dynamics during the prediction step, - * \f[ \hat{{x}}_{k+1} = \begin{bmatrix} q_{k} \otimes \text{exp}(\omega \Delta T) \\ y_{gyro_{k}} - b_{k} \\ (1 - \lambda_{b} \Delta t)b_k \end{bmatrix} \f] - * - * Measurement model for accelerometer is given as, - * \f[ h_{acc}(\hat{x}_{k+1}) = \begin{bmatrix} 2(q_1q_3 - q_0q_2) \\ 2(q_2q_3 - q_0q_1) \\ q_0^2 - q_1^2 - q_2^2 + q_3^2 \end{bmatrix} \f] - * obtained from \f$ {^w}R_b^T e_3 \f$ of the assumed gravity direction. - * - * Measurement model for magnetometer measurement is given as, - * \f[ h_{mag}(\hat{x}_{k+1}) = atan2( 2(q_0q_3 + q_1q_2),1 - 2(q_2^2 + q_3^2) ) \f] - * - * The linearized system propogation and measurement model is obtained by computing Jacobins F and H with respect to the state. - * - * The zero mean, additive Gaussian noise can be set using the covariance matrices which will be used during predict and update steps. - * - * The propagateStates() method is called to set the input vector for the EKF, then ekfPredict() is called to propagate the state through the propagation function f() - * and propate the state covariance using the Jacobian F. - * - * The updateFilterWithMeasurements() is called to set the measurement vector for the EKF, and then ekfUpdate is used to correct the state estimate and its covariance - * using the measurement model function h() and the measurement Jacobian H. - * - * The usage of the QEKF should follow the decribed procedure below, - * - instantiate the filter - * - set parameters - * - call initializeFilter() (this is necessary for resizing the buffers, the user should call this method after setting parameters) - * - use setInternalState() to set initial state (The filter will throw an error, if this is not called atleast once, this enforces the user to set intial state) - * - Once initialized, the following filter methods can be run in a loop to get the orientation estimates, - * - propagateStates() method to propagate the states and covariance - * - updateFilterWithMeasurements() method to correct the predicted states and covariance - * - getInternalState() or getOrientationEstimate*() methods to get the entire state estimate or only the attitude estimated in desired representation - * - * @warning calling the method useMagnetometerMeasurements() while the estimator is running, will reset the filter, reinitialize the filter to resize buffers - * and sets the previous estiamted state as the inital state. - * @note calling other set parameter methods does not reset the filter, since they are not associated with changing the output dimensions - * - */ - class AttitudeQuaternionEKF : public IAttitudeEstimator, - public DiscreteExtendedKalmanFilterHelper - { - public: - AttitudeQuaternionEKF(); - - /** - * @brief Get filter parameters as a struct. - * @param[out] params object of AttitudeQuaternionEKFParameters passed as reference - */ - void getParameters(AttitudeQuaternionEKFParameters& params) {params = m_params_qekf;} - - /** - * @brief Set filter parameters with the struct members. - * This resets filter since it also calls useMagnetometerMeasurements(flag) - * (if the use_magnetometer_measurements flag has been toggled). - * @param[in] params object of AttitudeQuaternionEKFParameters passed as a const reference - * @return true/false if successful/not - */ - void setParameters(const AttitudeQuaternionEKFParameters& params) - { - m_params_qekf = params; - useMagnetometerMeasurements(params.use_magnetometer_measurements); - } - - /** - * @brief Set the gravity direction assumed by the filter - * This affects the measurement model function h() and Jacobian H - * @param[in] gravity_dir gravity direction - */ - void setGravityDirection(const iDynTree::Direction& gravity_dir); - - /** - * @brief set discretization time step in seconds - * @param[in] time_step_in_seconds time step - */ - void setTimeStepInSeconds(double time_step_in_seconds) {m_params_qekf.time_step_in_seconds = time_step_in_seconds; } - - /** - * @brief set bias correlation time factor - * @param[in] bias_correlation_time_factor time factor for bias evolution - */ - void setBiasCorrelationTimeFactor(double bias_correlation_time_factor) { m_params_qekf.bias_correlation_time_factor = bias_correlation_time_factor; } - - /** - * @brief set flag to use magnetometer measurements - * @param[in] use_magnetometer_measurements enable/disable magnetometer measurements - * @note calling this method with the flag same as current flag value will not change anything, - * meanwhile a new flag setting will reset the filter, reinitialize the filter and - * set the previous state as filter's initial state and previous state covariance as filter's intial state covariance - */ - bool useMagnetometerMeasurements(bool use_magnetometer_measurements); - - /** - * @brief prepares the measurement noise covariance matrix and calls ekfSetMeasurementNoiseMeanAndCovariance() - * measurement noise depends only on accelerometer xyz (and magnetometer z) - * @note the noise has zero mean (basically passes a zero vector with covariance matrix) - * @param[in] acc variance for accelerometer measurements - * @param[in] mag variance for magnetometer measurements - * @return true/false if successful/not - */ - bool setMeasurementNoiseVariance(double acc, double mag); - - /** - * @brief prepares the system noise covariance matrix and calls ekfSetSystemNoiseMeanAndCovariance() - * process noise depends on gyro measurement and gyro bias estimate - since gyro measurement is passed as input - * @note the noise has zero mean (basically passes a zero vector with covariance matrix) - * measurement noise depends only on accelerometer xyz (and magnetometer z) - * @param[in] gyro variance for gyroscope measurements - * @param[in] gyro_bias variance for gyroscope bias estimates - * @return true/false if successful/not - */ - bool setSystemNoiseVariance(double gyro, double gyro_bias); - - /** - * @brief prepares the state covariance matrix and calls ekfSetStateCovariance() - * @param[in] orientation_var variance for intial orientation state estimate - * @param[in] ang_vel_var variance for initial angular velocity state estimate - * @param[in] gyro_bias_var variance for intial gyro bias state estimate - * @return true/false if successful/not - */ - bool setInitialStateCovariance(double orientation_var, double ang_vel_var, double gyro_bias_var); - - /** - * @brief intializes the filter by resizing buffers and setting parameters - * - sets state, output and input dimensions for the ekf - * - resizes internal buffers - * - calls ekfInit() - * - sets system noise, measurement noise and initial state covariance - * - if successful sets initialized flag to true - * @return true/false if successful/not - */ - bool initializeFilter(); - - - bool updateFilterWithMeasurements(const iDynTree::LinearAccelerometerMeasurements& linAccMeas, - const iDynTree::GyroscopeMeasurements& gyroMeas) override; - bool updateFilterWithMeasurements(const iDynTree::LinearAccelerometerMeasurements& linAccMeas, - const iDynTree::GyroscopeMeasurements& gyroMeas, - const iDynTree::MagnetometerMeasurements& magMeas) override; - bool propagateStates() override; - bool getOrientationEstimateAsRotationMatrix(iDynTree::Rotation& rot) override; - bool getOrientationEstimateAsQuaternion(iDynTree::UnitQuaternion& q) override; - bool getOrientationEstimateAsRPY(iDynTree::RPY& rpy) override; - size_t getInternalStateSize() const override; - bool getInternalState(const iDynTree::Span & stateBuffer) const override; - bool getDefaultInternalInitialState(const iDynTree::Span & stateBuffer) const override; - bool setInternalState(const iDynTree::Span & stateBuffer) override; - bool setInternalStateInitialOrientation(const iDynTree::Span& orientationBuffer) override; - - protected: - AttitudeEstimatorState m_state_qekf, m_initial_state_qekf; - AttitudeQuaternionEKFParameters m_params_qekf; ///< struct holding the QEKF parameters - - - private: - /** - * discrete system propagation \f$ f(X, u) = f(X, y_gyro) \f$ - * where \f$ X = \begin{bmatrix} q_0 & q_1 & q_2 & q_3 & \omega_x & \omega_y & \omega_z & b_x & b_y & b_z \end{bmatrix}^T \f$ - * \f$ u = \begin{bmatrix} {y_{gyro}}_x & {y_{gyro}}_y & {y_{gyro}}_z \end{bmatrix}^T \f$ - * \f$ f(X, u) = \begin{bmatrix} q_{k} \otimes \text{exp}(\omega \Delta T) \\ y_{gyro} - b \\ (1 - \lambda_{b} \Delta t)b \end{bmatrix}\f$ - */ - bool ekf_f(const iDynTree::VectorDynSize& x_k, - const iDynTree::VectorDynSize& u_k, - iDynTree::VectorDynSize& xhat_k_plus_one) override; - - /** - * discrete measurement prediction - * where \f$ h(X) = \begin{bmatrix} h_{acc}(X) & h_{mag}(X) \end{bmatrix}^T \f$ - * \f$ h_{acc}(X) = R^T \begin{bmatrix} 0 \\ 0 \\ -1 \end{bmatrix} \f$ - * \f$ h_{mag}(X) = atan2(tan(yaw))\f$ - */ - bool ekf_h(const iDynTree::VectorDynSize& xhat_k_plus_one, - iDynTree::VectorDynSize& zhat_k_plus_one) override; - - /** - * @brief Describes the system Jacobian necessary for the propagation of predicted state covariance - * The analytical Jacobian describing the partial derivative of the system propagation with respect to the state - * @param[in] x system state - * @param[out] F system Jacobian - * @return bool true/false if successful or not - */ - bool ekfComputeJacobianF(iDynTree::VectorDynSize& x, iDynTree::MatrixDynSize& F) override; - bool ekfComputeJacobianF(iDynTree::VectorDynSize& x, iDynTree::VectorDynSize& u, iDynTree::MatrixDynSize& F) override; - - /** - * @brief Describes the measurement Jacobian necessary for computing Kalman gain and updating the predicted state and its covariance - * The analytical Jacobian describing the partial derivative of the measurement model with respect to the state - * @param[in] x system state - * @param[out] H measurement Jacobian - * @return bool true/false if successful or not - */ - bool ekfComputeJacobianH(iDynTree::VectorDynSize& x, iDynTree::MatrixDynSize& H) override; - - /** @brief prepares the system noise covariance matrix using internal struct params - * system model is as good as gyroscope measurement and bias estimate - * system noise covariance can be descibed as \f$ Q = F_u U {F_u}^T \f$ - * where \f$ F = \begin{bmatrix} \frac{\partial f}{\partial y_gyro} & \frac{\partial f}{ \partial x_gyrobias} \end{bmatrix} \f$ - * \f$ = \begin{bmatrix} 0_{4 \times 3} & 0_{4 \times 3} \\ I_{3 \times 3} & 0_{3 \times 3} \\ 0_{3 \times 3} & I_{3 \times 3} \end{bmatrix}\f$ - * \f$ U = diag(\begin{bmatrix} \sigma_{gyro}^{2} I_{3 \times 3} & \sigma_{gyrobias}^{2} I_{3 \times 3} \end{bmatrix}) \f$ - * @param[in] Q matrix container as reference - */ - void prepareSystemNoiseCovarianceMatrix(iDynTree::MatrixDynSize &Q); - - /** @brief prepares the measurement noise covariance matrix using internal struct parameters - * measurement noise depends only on accelerometer measurement along x-,y- and z- directions - * along with magnetometer z-direction if included - * measurement noise covariance can be descibed as, - * \f$ R = \begin{bmatrix} \sigma_{acc}^{2} I_{3 \times 3} & 0_{3 \times 1} \\ 0_{1 \times 3} & \sigma_{mag}^{2}\f$ - * if magnetometer measurements is also considered. In case of magnetometer measurements not being considered, it is reduced - * to the \f$ 3 \times 3 \f$ matrix - * @param[in] R matrix container as reference - */ - void prepareMeasurementNoiseCovarianceMatrix(iDynTree::MatrixDynSize &R); - - /** - * @brief serializes the state struct to state x of VectorDynSize - */ - void serializeStateVector(); - - /** - * @brief deserializes state x of VectorDynSize to the state struct - */ - void deserializeStateVector(); - - /** - * @brief serializes the accelerometer and magenetometer measurements into y vector - * since DiscreteExtendedKalmanFilter expects a VectorDynSize including all necessary measurements - */ - void serializeMeasurementVector(); - - - /** - * @brief serializes measurements, calls ekfUpdate step and - * gets state estimate corrected by measurements - * - * @return true/false, if successful/not - */ - bool callEkfUpdate(); - - iDynTree::Rotation m_orientationInSO3; ///< orientation estimate as rotation matrix \f$ {^A}R_B \f$ where \f$ A \f$ is inertial frame and \f$ B \f$ is the frame attached to the body - iDynTree::RPY m_orientationInRPY; ///< orientation estimate as a 3D vector in RPY representation, where \f$ {^A}R_B = Rot_z(yaw)Rot_y(pitch)Rot_x(roll) \f$ - - iDynTree::GyroscopeMeasurements m_Omega_y; ///< 3d gyroscope measurement giving angular velocity of body wrt inertial frame, expressed in body frame - iDynTree::LinearAccelerometerMeasurements m_Acc_y; ///< 3d accelerometer measurement giving proper classical acceleration expressed in body frame - double m_Mag_y; ///< magnetometer yaw measurement expressed in body frame - - iDynTree::VectorDynSize m_x; ///< state vector for the EKF - orientation, angular velocity, gyro bias - iDynTree::VectorDynSize m_y; ///< measurement vector for the EKF - accelerometer (and magnetometer yaw) - iDynTree::VectorDynSize m_u; ///< input vector for the EKF - gyroscope measurement - - size_t m_state_size; ///< state dimensions - size_t m_output_size; ///< output dimensions - size_t m_input_size; ///< input dimensions - bool m_initialized{false}; ///< flag to check if QEKF is initialized - - iDynTree::Matrix4x4 m_Id4; ///< \f$ 4 \times 4 \f$ identity matrix - iDynTree::Matrix3x3 m_Id3; ///< \f$ 3 \times 3 \f$ identity matrix - iDynTree::Direction m_gravity_direction; ///< direction of the gravity vector expressed in the inertial frame denoted by \f$ A \f$, default set to \f$ e_3 = \begin{bmatrix} 0 & 0 & 1.0 \end{bmatrix}^T \f$ - }; +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif -} +#include #endif diff --git a/src/estimation/include/iDynTree/Estimation/BerdyHelper.h b/src/estimation/include/iDynTree/Estimation/BerdyHelper.h index f24314aac98..e68ed085bd1 100644 --- a/src/estimation/include/iDynTree/Estimation/BerdyHelper.h +++ b/src/estimation/include/iDynTree/Estimation/BerdyHelper.h @@ -1,783 +1,13 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef IDYNTREE_BERDY_HELPER_H -#define IDYNTREE_BERDY_HELPER_H - -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include - -#include - -#include - -namespace iDynTree -{ - -/** - * Enumeration of the Berdy variants implemented - * in this class. - * - * @warning This class is still in active development, and so API interface can change between iDynTree versions. - * \ingroup iDynTreeExperimental - */ -enum BerdyVariants -{ - /** - * Original version of Berdy, as described in: - * - * Latella, C.; Kuppuswamy, N.; Romano, F.; Traversaro, S.; Nori, F. - * Whole-Body Human Inverse Dynamics with Distributed Micro-Accelerometers, Gyros and Force Sensing. Sensors 2016, 16, 727. - * http://www.mdpi.com/1424-8220/16/5/727 - * - * The original version of Berdy is assuming that the joint numbering is a regular ordering of links and joints. - * For this reason the serialization of link/joints quantities follows the one defined in the traversal. - * - * Furthremore, this version assumes that all joints have 1 Degree of freedom, so it does not work for models - * with fixed joints. - */ - ORIGINAL_BERDY_FIXED_BASE = 0, - - /** - * Modified version of Berdy - * for accounting for free floating dynamics and removing the NET_INT_AND_EXT_WRENCHES_ON_LINK_WITHOUT_GRAV from the dynamic variables. - * - */ - BERDY_FLOATING_BASE = 1, - - /** - * Modified version of floating base Berdy - * for accounting centroidal dynamics constraints towards estimating the external link wrench independently of the internal joint torque estimates. - */ - BERDY_FLOATING_BASE_NON_COLLOCATED_EXT_WRENCHES = 2 - -}; - -/** - * Enumeration describing the dynamic variables types (link acceleration, net wrenches, joint wrenches, joint torques, joint acceleration) - * used in Berdy - * - * @warning This class is still in active development, and so API interface can change between iDynTree versions. - * \ingroup iDynTreeExperimental - */ -enum BerdyDynamicVariablesTypes -{ - /*!< \f$ a_i \f$ - * Note that is is the **spatial** proper acceleration expressed, - * i.e. the time derivative of the left-trivialized velocity minus the gravity expressed in body frame. - **/ - LINK_BODY_PROPER_ACCELERATION, - /*!< \f$ f^B_i \f$ */ - NET_INT_AND_EXT_WRENCHES_ON_LINK_WITHOUT_GRAV, - /*!< \f$ f_i \f$ */ - JOINT_WRENCH, - /*!< \f$ \tau_i \f$ */ - DOF_TORQUE, - /*!< \f$ f^x_i \f$ */ - NET_EXT_WRENCH, - /*!< \f$ \ddot{q}_i \f$ */ - DOF_ACCELERATION, - /*!< - * This is the classical proper acceleration, - * i.e. the time derivative of the mixed velocity of the body frame minus the gravity expressed in body frame. - * In Traversaro's PhD thesis this is called sensor proper acceleration. - * This is the necessary for avoiding dependencies on the linear velocity of the base in the floating variant of - * Berdy. - */ - LINK_BODY_PROPER_CLASSICAL_ACCELERATION -}; - -/** - * Enumeration describing the possible sensor types implemented in Berdy. - * - * Note that the concept of "sensor" in Berdy estimation is more general that - * just a physical sensor mounted on the robot: for example it can include - * the information that a link is fixed to the ground (i.e. its angular velocity, - * angular and linear acceleration are zero) even if this information is not coming - * from an actual physical sensors. For this reason we do not use directly - * the iDynTree::SensorTypes enum, even if we reserve the first 1000 elements o - * of this enum to be compatibile with the iDynTree::SensorTypes enum. - * Enum values duplicates between BerdySensorTypes and SensorTypes are append a - * _SENSOR suffix to avoid problems when wrapping such enum wit SWIG. - * - * @warning This class is still in active development, and so API interface can change between iDynTree versions. - * \ingroup iDynTreeExperimental - */ -enum BerdySensorTypes -{ - SIX_AXIS_FORCE_TORQUE_SENSOR = SIX_AXIS_FORCE_TORQUE, - ACCELEROMETER_SENSOR = ACCELEROMETER, - GYROSCOPE_SENSOR = GYROSCOPE, - THREE_AXIS_ANGULAR_ACCELEROMETER_SENSOR = THREE_AXIS_ANGULAR_ACCELEROMETER, - THREE_AXIS_FORCE_TORQUE_CONTACT_SENSOR = THREE_AXIS_FORCE_TORQUE_CONTACT, - DOF_ACCELERATION_SENSOR = 1000, - DOF_TORQUE_SENSOR = 1001, - NET_EXT_WRENCH_SENSOR = 1002, - /** - * Non-physical sensor that measures the wrench trasmitted by a joint. - */ - JOINT_WRENCH_SENSOR = 1003, - - /** - * Non-physical sensor that holds the value of Rate of Change of Momentum (RCM) of the system - * for centroidal dynamics constraint - */ - RCM_SENSOR = 1004 -}; - -bool isLinkBerdyDynamicVariable(const BerdyDynamicVariablesTypes dynamicVariableType); -bool isJointBerdyDynamicVariable(const BerdyDynamicVariablesTypes dynamicVariableType); -bool isDOFBerdyDynamicVariable(const BerdyDynamicVariablesTypes dynamicVariableType); - -/** - * Options of the BerdyHelper class. - * - * Documentation of each option is provided as usual Doxygen documentation. - * Default values for each options are specified in the contstructor. - * - * @warning This class is still in active development, and so API interface can change between iDynTree versions. - * \ingroup iDynTreeExperimental - */ -struct BerdyOptions -{ -public: - BerdyOptions() : berdyVariant(ORIGINAL_BERDY_FIXED_BASE), - includeAllNetExternalWrenchesAsDynamicVariables(true), - includeAllJointAccelerationsAsSensors(true), - includeAllJointTorquesAsSensors(false), - includeAllNetExternalWrenchesAsSensors(true), - includeFixedBaseExternalWrench(false), - baseLink("") - { - } - - /** - * Type of berdy variant implemented. - * - * For description of each type of variant - * check the BerdyVariants enum documentation. - * - * Default value: ORIGINAL_BERDY_FIXED_BASE . - */ - BerdyVariants berdyVariant; - - /** - * If true, include the net external wrenches in the dynamic variables vector. - * - * Default value: true . - * - * \note the Net effect of setting this to zero is to consider all the external - * wrenches to be equal to 0. If berdyVariant is ORIGINAL_BERDY_FIXED_BASE, - * the "net" external wrenches does not include the wrench at the base. - * - * \note if berdyVariant is BERDY_FLOATING_BASE, this option cannot be set to false. - */ - bool includeAllNetExternalWrenchesAsDynamicVariables; - - /** - * If true, include the joint accelerations in the sensors vector. - * - * Default value: true . - */ - bool includeAllJointAccelerationsAsSensors; - - /** - * If true, include the joint torques in the sensors vector. - * - * Default value: false . - */ - bool includeAllJointTorquesAsSensors; - - /** - * If true, include the net external wrenches in the sensors vector. - * It is not compatible with the use of includeAllNetExternalWrenchesAsDynamicVariables set to false. - * - * Default value: true . - */ - bool includeAllNetExternalWrenchesAsSensors; - - /** - * If includeNetExternalWrenchesAsSensors is true and the - * variant is ORIGINAL_BERDY_FIXED_BASE, if this is - * true the external wrench acting on the base fixed link - * is included in the sensors. - * - * Default value : false . - */ - bool includeFixedBaseExternalWrench; - - /** - * Vector of joint names for which we assume that a virtual - * measurement of the wrench trasmitted on the joint is available. - * - * \note This measurements are tipically used only for debug, actual - * internal wrenches are tipically measured using a SIX_AXIS_FORCE_TORQUE_SENSOR . - */ - std::vector jointOnWhichTheInternalWrenchIsMeasured; - - /** - * Name of the link which will be considered as a base link for Berdy computations. - * - * \note If the string is empty the default base link of the model will be used. - */ - std::string baseLink; - - /** - * Check that the options are not self-contradicting. - */ - bool checkConsistency(); -}; - - -//Unfortunately some sensors used in berdy are not proper sensors. -//I cannot use the Sensor class which has almost all the information needed -/** - * Structure which describes the essential information about a sensor used in berdy - * A sensor is identified by the pair (type, id) - * - * @warning This class is still in active development, and so API interface can change between iDynTree versions. - * \ingroup iDynTreeExperimental - */ -struct BerdySensor { - iDynTree::BerdySensorTypes type; /*!< type of the sensor */ - std::string id; /*!< ID of the sensor */ - iDynTree::IndexRange range; /*!< Range of the sensor - * (starting location in the measurements equations - * and number of measurements equations associated with the sensor) */ - - /** - * Overload of equality operator - * - * Two sensors are considered equals if they have the same type and id - * @param sensor the sensor to which the current sensor is compared to - * @return true if the two sensors are equal. False otherwise - */ - bool operator==(const struct BerdySensor& sensor) const; - - bool operator<(const struct BerdySensor& sensor) const; -}; - -struct BerdyDynamicVariable { - iDynTree::BerdyDynamicVariablesTypes type; /*!< type of the dynamic variable */ - std::string id; /*!< ID of the dynamic variable */ - iDynTree::IndexRange range; /*!< Range of the dynamic variable - * (starting location in the dynamic equations - * and number of equations associated with the variable) */ - - /** - * Overload of equality operator - * - * Two variables are considered equals if they have the same type and id - * @param variable the variable to which the current variable is compared to - * @return true if the two variables are equal. False otherwise - */ - bool operator==(const struct BerdyDynamicVariable& variable) const; - - bool operator<(const struct BerdyDynamicVariable& variable) const; -}; - -/** - * \brief Helper class for computing Berdy matrices. - * - * Berdy refers to a class for algorithms to compute - * the Maximum A Posteriori (MAP) estimation of the dynamic variables - * of a multibody model (accelerations, external forces, joint torques) - * assuming the knowledge the measurements of an arbitrary set of sensors and - * of the kinematics and inertial characteristics of the model. - * - * The MAP estimation is computed using a sparse matrix representation of the multibody - * dynamics Newton-Euler equations, originally described in: - * - * Latella, C.; Kuppuswamy, N.; Romano, F.; Traversaro, S.; Nori, F. - * Whole-Body Human Inverse Dynamics with Distributed Micro-Accelerometers, Gyros and Force Sensing. - * Sensors 2016, 16, 727. - * http://www.mdpi.com/1424-8220/16/5/727 - * - * Nori F, Kuppuswamy N, Traversaro S. - * Simultaneous state and dynamics estimation in articulated structures. - * In Intelligent Robots and Systems (IROS), 2015 IEEE/RSJ International Conference on 2015 Sep 28 (pp. 3380-3386). IEEE. - * http://ieeexplore.ieee.org/xpl/articleDetails.jsp?arnumber=7353848 - * - * - * This helper class implements two different variants of the Berdy dynamics representation, identified - * with the BerdyVariants enum: - * * The ORIGINAL_BERDY_FIXED_BASE is the original BERDY representation described in the papers, - * that was assuming a fixed base model with all the joints with 1-DOF . - * * The BERDY_FLOATING_BASE is a variant in which the model is assumed to be floating, - * does not need the position or linear velocity of the floating base and supports - * joints with an arbitrary number of DOF . - * - * The sensors supported by this class are the one contained in the SensorsList representation, - * which can be loaded directly from an URDF representation, see https://github.com/robotology/idyntree/blob/master/doc/model_loading.md#sensor-extensions . - * Using this representation the following sensors can be loaded: - * * Internal Six-Axis Force Torque sensors - * * Gyroscopes - * * Accelerometers - * Support for joint torques/acceleration or external wrenches measurements still needs to be added. - * - * \note The dynamics representation of Berdy is highly dependent on the link assumed to be the floating - * base of the robot, even in the case that BERDY_FLOATING_BASE is used. The assumed traversal (i.e. - * which link is the floating base and how the link are visited) is accessible with the dynamicTraversal() method. - * - * @warning This class is still in active development, and so API interface can change between iDynTree versions. - * \ingroup iDynTreeExperimental - */ -class BerdyHelper -{ - /** - * Model used in this class. - */ - Model m_model; - - /** - * Sensors used in this class - */ - SensorsList m_sensors; - - /** - * Traversal used for the dynamics computations - */ - Traversal m_dynamicsTraversal; - - /** - * Caches of traversals used for kinematic computations. - */ - LinkTraversalsCache m_kinematicTraversals; - - /** - * False initially, true after a valid model and sensors have been loaded. - */ - bool m_areModelAndSensorsValid; - - /** - * False initially, true after updateKinematics was successfully called. - */ - bool m_kinematicsUpdated; - - /** - * Options of the current berdy variant used. - */ - BerdyOptions m_options; - - size_t m_nrOfDynamicalVariables; - size_t m_nrOfDynamicEquations; - size_t m_nrOfSensorsMeasurements; - - - /** - * Joint positions - */ - JointPosDoubleArray m_jointPos; - - /** - * Joint velocities - */ - JointDOFsDoubleArray m_jointVel; - - - /** - * Link body velocities (i.e. 6D velocity of the link, expressed - * in the link frame orientation and with respect to the link origin). - */ - LinkVelArray m_linkVels; - - /** - * Gravity expressed in the base link frame, used only - * if getBerdyVariant is equal to ORIGINAL_BERDY_FIXED_BASE. - */ - Vector3 m_gravity; - SpatialAcc m_gravity6D; - - std::vector m_sensorsOrdering; /*!< Sensor ordering. Created on init */ - std::vector m_dynamicVariablesOrdering; /*!< Dynamic variable ordering. Created on init */ - - /** - * Helpers method for initialization. - */ - bool initOriginalBerdyFixedBase(); - bool initBerdyFloatingBase(); - bool initSensorsMeasurements(); - - IndexRange getRangeOriginalBerdyFixedBase(const BerdyDynamicVariablesTypes dynamicVariableType, const TraversalIndex idx) const; - - /** - * Ranges for specific dynamics equations - */ - - // Dynamic equations for ORIGINAL_BERDY_FIXED_BASE - IndexRange getRangeLinkProperAccDynEqFixedBase(const LinkIndex idx) const; - IndexRange getRangeLinkNetTotalWrenchDynEqFixedBase(const LinkIndex idx) const; - IndexRange getRangeJointWrenchDynEqFixedBase(const JointIndex idx) const; - IndexRange getRangeDOFTorqueDynEqFixedBase(const DOFIndex idx) const; - - // Dynamic equations for BERDY_FLOATING_BASE - IndexRange getRangeAccelerationPropagationFloatingBase(const LinkIndex idx) const; - IndexRange getRangeNewtonEulerEquationsFloatingBase(const LinkIndex idx) const; - - // Y matrices computation - bool computeBerdySensorMatrices(SparseMatrix& Y, VectorDynSize& bY); - bool computeBerdySensorsMatricesFromModel(SparseMatrix& Y, VectorDynSize& bY); - - // D matrices computation - bool computeBerdyDynamicsMatricesFixedBase(SparseMatrix& D, VectorDynSize& bD); - bool computeBerdyDynamicsMatricesFloatingBase(SparseMatrix& D, VectorDynSize& bD); - - // Helper method - Matrix6x1 getBiasTermJointAccelerationPropagation(IJointConstPtr joint, - const LinkIndex parentLinkIdx, - const LinkIndex childLinkIdx, - const Transform &child_X_parent); - void cacheSensorsOrdering(); - void cacheDynamicVariablesOrderingFixedBase(); - void cacheDynamicVariablesOrderingFloatingBase(); - bool serializeDynamicVariablesFixedBase(LinkProperAccArray & properAccs, - LinkNetTotalWrenchesWithoutGravity & netTotalWrenchesWithoutGrav, - LinkNetExternalWrenches & netExtWrenches, - LinkInternalWrenches & linkJointWrenches, - JointDOFsDoubleArray & jointTorques, - JointDOFsDoubleArray & jointAccs, - VectorDynSize& d); - bool serializeDynamicVariablesFloatingBase(LinkProperAccArray & properAccs, - LinkNetTotalWrenchesWithoutGravity & netTotalWrenchesWithoutGrav, - LinkNetExternalWrenches & netExtWrenches, - LinkInternalWrenches & linkJointWrenches, - JointDOFsDoubleArray & jointTorques, - JointDOFsDoubleArray & jointAccs, - VectorDynSize& d); - - bool serializeDynamicVariablesNonCollocatedWrenches(LinkNetExternalWrenches& netExtWrenches, - VectorDynSize& d); - /** - * Helper for mapping sensors measurements to the Y vector. - */ - struct { - size_t dofAccelerationOffset; - size_t dofTorquesOffset; - size_t netExtWrenchOffset; - size_t jointWrenchOffset; - size_t rcmOffset; - } berdySensorTypeOffsets; - - /** - * Helper of additional sensors. - */ - struct { - /** - * List of joint wrench sensors. - */ - std::vector wrenchSensors; - /** - * Mapping between jndIx in wrenchSensor and - */ - std::vector jntIdxToOffset; - } berdySensorsInfo; - - /** - * Buffer for sensor serialization. - */ - VectorDynSize realSensorMeas; - - Triplets matrixDElements; - Triplets matrixYElements; - - /** - * Transform between the frame in which the external net wrench measurements are expressed - * and the link frames. - */ - std::vector m_link_H_externalWrenchMeasurementFrame; - - /* - * LinkPositions variable containing the transforms between the base and the model links - * - */ - LinkPositions base_H_links; - - -public: - /** - * Constructor - */ - BerdyHelper(); - - /** - * Access the model. - */ - Model& model(); - - /** - * Access the sensors. - */ - SensorsList& sensors(); - - /** - * Acces the traveral used for the dynamics computations (const version) - */ - const Traversal& dynamicTraversal() const; - - /** - * Access the model (const version). - */ - const Model& model() const; - - /** - * Access the model (const version). - */ - const SensorsList& sensors() const; - - /** - * Returns if the helper is valid. - * The helper is valid if the model and the sensors have been loaded - * @return true if the helper is valid. False otherwise - */ - bool isValid() const; - - /** - * Init the class - * - * @param[in] model The used model. - * @param[in] sensors The used sensors. - * @param[in] options The used options, check BerdyOptions docs. - * @return true if all went well, false otherwise. - */ - bool init(const Model& model, - const SensorsList& sensors, - const BerdyOptions options=BerdyOptions()); - - /** - * Get currently used options. - */ - BerdyOptions getOptions() const; - - /** - * Get the number of columns of the D matrix. - * This depends on the Berdy variant selected. - */ - size_t getNrOfDynamicVariables() const; - - /** - * Get the number of dynamics equations used in the Berdy - * equations - */ - size_t getNrOfDynamicEquations() const; - - /** - * Get the number of sensors measurements. - */ - size_t getNrOfSensorsMeasurements() const; - - /** - * Resize and set to zero Berdy matrices. - */ - bool resizeAndZeroBerdyMatrices(SparseMatrix& D, VectorDynSize &bD, - SparseMatrix& Y, VectorDynSize &bY); - - /** - * Resize and set to zero Berdy matrices. - * - */ - bool resizeAndZeroBerdyMatrices(MatrixDynSize & D, VectorDynSize & bD, - MatrixDynSize & Y, VectorDynSize & bY); - - /** - * Get Berdy matrices - */ - bool getBerdyMatrices(SparseMatrix& D, VectorDynSize &bD, - SparseMatrix& Y, VectorDynSize &bY); - - /** - * Get Berdy matrices - * - * \note internally this function uses sparse matrices - * Prefer the use of resizeAndZeroBerdyMatrices(SparseMatrix &, VectorDynSize &, SparseMatrix &, VectorDynSize &) - */ - bool getBerdyMatrices(MatrixDynSize & D, VectorDynSize & bD, - MatrixDynSize & Y, VectorDynSize & bY); - - /** - * Return the internal ordering of the sensors - * - * Measurements are expected to respect the internal sensors ordering - * Use this function to obtain the sensors ordering. - * - * @return the sensors ordering - */ - const std::vector& getSensorsOrdering() const; - - /** - * Get the range of the specified sensor in - */ - IndexRange getRangeSensorVariable(const SensorType type, const unsigned int sensorIdx) const; - IndexRange getRangeDOFSensorVariable(const BerdySensorTypes sensorType, const DOFIndex idx) const; - IndexRange getRangeJointSensorVariable(const BerdySensorTypes sensorType, const JointIndex idx) const; - IndexRange getRangeLinkSensorVariable(const BerdySensorTypes sensorType, const LinkIndex idx) const; - IndexRange getRangeRCMSensorVariable(const BerdySensorTypes sensorType) const; - - /** - * Ranges of dynamic variables - */ - IndexRange getRangeLinkVariable(const BerdyDynamicVariablesTypes dynamicVariableType, const LinkIndex idx) const; - IndexRange getRangeJointVariable(const BerdyDynamicVariablesTypes dynamicVariableType, const JointIndex idx) const; - IndexRange getRangeDOFVariable(const BerdyDynamicVariablesTypes dynamicVariableType, const DOFIndex idx) const; - - const std::vector& getDynamicVariablesOrdering() const; - - /** - * Serialized dynamic variables from the separate buffers - */ - bool serializeDynamicVariables(LinkProperAccArray & properAccs, - LinkNetTotalWrenchesWithoutGravity & netTotalWrenchesWithoutGrav, - LinkNetExternalWrenches & netExtWrenches, - LinkInternalWrenches & linkJointWrenches, - JointDOFsDoubleArray & jointTorques, - JointDOFsDoubleArray & jointAccs, - VectorDynSize& d); - /** - * Serialize sensor variable from separate buffers. - */ - bool serializeSensorVariables(SensorsMeasurements & sensMeas, - LinkNetExternalWrenches & netExtWrenches, - JointDOFsDoubleArray & jointTorques, - JointDOFsDoubleArray & jointAccs, - LinkInternalWrenches & linkJointWrenches, - SpatialForceVector & rcm, // Rate of Change of Momentum (RCM) - VectorDynSize & y); - - - /** - * Debug function: - * - * \note This method has been added for debug, and could be removed soon. - */ - bool serializeDynamicVariablesComputedFromFixedBaseRNEA(JointDOFsDoubleArray & jointAccs, - LinkNetExternalWrenches & netExtWrenches, - VectorDynSize& d); - - /** - * Extract the joint torques from the dynamic variables - */ - bool extractJointTorquesFromDynamicVariables(const VectorDynSize& d, - const VectorDynSize& jointPos, - VectorDynSize& jointTorques) const; - - /** - * Extract the net external force-torques from the dynamic variables - */ - bool extractLinkNetExternalWrenchesFromDynamicVariables(const VectorDynSize& d, - LinkNetExternalWrenches& netExtWrenches) const; - - /** - * @name Methods to submit the input data for dynamics computations. - */ - //@{ - - /** - * Set the kinematic information necessary for the dynamics estimation using the - * angular velocity information of a floating frame. - * - * \note This method cannot be used if the selected BerdyVariant is ORIGINAL_BERDY_FIXED_BASE. - * \note we not require to give the linear velocity of floating base because the dynamics equations - * are invariant with respect to an offset in linear velocity. This convenient to avoid - * any dependency on any prior floating base estimation. - * - * @param[in] jointPos the position of the joints of the model. - * @param[in] jointVel the velocities of the joints of the model. - * @param[in] floatingFrame the index of the frame for which kinematic information is provided. - * @param[in] angularVel angular velocity (wrt to any inertial frame) of the specified floating frame, - * expressed in the specified floating frame orientation. - * @return true if all went ok, false otherwise. - */ - bool updateKinematicsFromFloatingBase(const JointPosDoubleArray & jointPos, - const JointDOFsDoubleArray & jointVel, - const FrameIndex & floatingFrame, - const Vector3 & angularVel); - - /** - * Set the kinematic information necessary for the dynamics estimation assuming that a - * given frame is not accelerating with respect to the inertial frame. - * - * @param[in] jointPos the position of the joints of the model. - * @param[in] jointVel the velocities of the joints of the model. - * @param[in] fixedFrame the index of the frame that is not accelerating with respect to the inertial frame. - * @param[in] gravity the gravity acceleration vector, expressed in the specified fixed frame. - * - * \note gravity is used only if selected BerdyVariant is ORIGINAL_BERDY_FIXED_BASE. - * - * @return true if all went ok, false otherwise. - */ - bool updateKinematicsFromFixedBase(const JointPosDoubleArray & jointPos, - const JointDOFsDoubleArray & jointVel, - const FrameIndex & fixedFrame, - const Vector3 & gravity); - - /** - * Set the kinematic information necessary for the dynamics estimation assuming that a - * given baseframe (specified by the m_dynamicsTraversal) is not accelerating with respect to the inertial frame. - * - * @param[in] jointPos the position of the joints of the model. - * @param[in] jointVel the velocities of the joints of the model. - * @param[in] gravity the gravity acceleration vector, expressed in the specified fixed frame. - * - * \note gravity is used only if selected BerdyVariant is ORIGINAL_BERDY_FIXED_BASE. - * - * @return true if all went ok, false otherwise. - * - * @note this is equivalent to - * @code updateKinematicsFromFixedBase(jointPos,jointVel,m_dynamicsTraversal.getBaseLink()->getIndex(),gravity); - * @endcode - * - */ - bool updateKinematicsFromTraversalFixedBase(const JointPosDoubleArray & jointPos, - const JointDOFsDoubleArray & jointVel, - const Vector3 & gravity); - - //@} - - /** - * Set/get the transformation link_H_contact between the link frame and the frame in which the measured net ext wrench is expressed. - * - * The default value is the identity. This is extremly useful to correctly tune the variances when only a subset of the external - * net wrench is known (for example when it is known that the external net wrench is a pure force on a point different from the - * link frame. - * - * \note This will only change the frame in which the measurent equation of the net external wrench is expressed, - * not how the Newton-Euler equation of the link are expressed or how the net ext wrenches are serialized in - * the LinkNetExternalWrenches class. - */ - ///@{ - bool setNetExternalWrenchMeasurementFrame(const LinkIndex lnkIndex, const Transform& link_H_contact); - bool getNetExternalWrenchMeasurementFrame(const LinkIndex lnkIndex, Transform& link_H_contact) const; - ///@} - - - /** - * @name Methods to get informations on the serialization used. - */ - //@{ - - /** - * Get a human readable description of the elements of the dynamic variables. - */ - //std::string getDescriptionOfDynamicVariables(); - - //@} - - - -}; +#ifndef IDYNTREE_ESTIMATION_BERDY_HELPER_H +#define IDYNTREE_ESTIMATION_BERDY_HELPER_H +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif -} +#include #endif diff --git a/src/estimation/include/iDynTree/Estimation/BerdySparseMAPSolver.h b/src/estimation/include/iDynTree/Estimation/BerdySparseMAPSolver.h index 64f9ebb7303..ecb64f1f9c0 100644 --- a/src/estimation/include/iDynTree/Estimation/BerdySparseMAPSolver.h +++ b/src/estimation/include/iDynTree/Estimation/BerdySparseMAPSolver.h @@ -1,75 +1,14 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef IDYNTREE_BERDY_SPARSEMAPSOLVER_H -#define IDYNTREE_BERDY_SPARSEMAPSOLVER_H +#ifndef IDYNTREE_ESTIMATION_BERDY_SPARSEMAPSOLVER_H +#define IDYNTREE_ESTIMATION_BERDY_SPARSEMAPSOLVER_H -#include -#include -#include +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif -namespace iDynTree { +#include - class BerdyHelper; - class VectorDynSize; - class JointPosDoubleArray; - class JointDOFsDoubleArray; - - template - class SparseMatrix; - - - /** - * @warning This class is still in active development, and so API interface can change between iDynTree versions. - * \ingroup iDynTreeExperimental - */ - class BerdySparseMAPSolver - { - - class BerdySparseMAPSolverPimpl; - BerdySparseMAPSolverPimpl* m_pimpl; - - public: - BerdySparseMAPSolver(BerdyHelper& berdyHelper); - ~BerdySparseMAPSolver(); - - void setDynamicsConstraintsPriorCovariance(const iDynTree::SparseMatrix & covariance); - void setDynamicsRegularizationPriorCovariance(const iDynTree::SparseMatrix& covariance); - void setDynamicsRegularizationPriorExpectedValue(const iDynTree::VectorDynSize& expectedValue); - void setMeasurementsPriorCovariance(const iDynTree::SparseMatrix& covariance); - - const iDynTree::SparseMatrix& dynamicsConstraintsPriorCovarianceInverse() const; // Sigma_D^-1 - iDynTree::SparseMatrix& dynamicsConstraintsPriorCovarianceInverse(); // Sigma_D^-1 - const iDynTree::SparseMatrix& dynamicsRegularizationPriorCovarianceInverse() const; // Sigma_d^-1 - iDynTree::SparseMatrix& dynamicsRegularizationPriorCovarianceInverse(); // Sigma_d^-1 - const iDynTree::VectorDynSize& dynamicsRegularizationPriorExpectedValue() const; // mu_d - iDynTree::VectorDynSize& dynamicsRegularizationPriorExpectedValue(); // mu_d - const iDynTree::SparseMatrix& measurementsPriorCovarianceInverse() const; // Sigma_y^-1 - iDynTree::SparseMatrix& measurementsPriorCovarianceInverse(); // Sigma_y^-1 - - bool isValid(); - - bool initialize(); - - void updateEstimateInformationFixedBase(const JointPosDoubleArray& jointsConfiguration, - const JointDOFsDoubleArray& jointsVelocity, - const FrameIndex fixedFrame, - const Vector3& gravityInFixedFrame, - const VectorDynSize& measurements); - - void updateEstimateInformationFloatingBase(const JointPosDoubleArray& jointsConfiguration, - const JointDOFsDoubleArray& jointsVelocity, - const FrameIndex floatingFrame, - const Vector3& bodyAngularVelocityOfSpecifiedFrame, - const VectorDynSize& measurements); - - bool doEstimate(); - - void getLastEstimate(iDynTree::VectorDynSize& lastEstimate) const; - const iDynTree::VectorDynSize& getLastEstimate() const; - - - }; -} #endif /* end of include guard: IDYNTREE_BERDY_SPARSEMAPSOLVER_H */ diff --git a/src/estimation/include/iDynTree/Estimation/BipedFootContactClassifier.h b/src/estimation/include/iDynTree/Estimation/BipedFootContactClassifier.h index 302ac4547d2..aed316e4699 100644 --- a/src/estimation/include/iDynTree/Estimation/BipedFootContactClassifier.h +++ b/src/estimation/include/iDynTree/Estimation/BipedFootContactClassifier.h @@ -1,142 +1,12 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef IDYNTREE_BIPED_FOOT_CONTACT_CLASSIFIER_H -#define IDYNTREE_BIPED_FOOT_CONTACT_CLASSIFIER_H +#ifndef IDYNTREE_ESTIMATION_BIPED_FOOT_CONTACT_CLASSIFIER_H +#define IDYNTREE_ESTIMATION_BIPED_FOOT_CONTACT_CLASSIFIER_H -#include -#include "ContactStateMachine.h" +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif +#include -namespace iDynTree -{ - /** - * Enumeration of switching pattern - */ - enum SwitchingPattern - { - /** - * Switching active foot between left and right at every double stance - */ - ALTERNATE_CONTACT, - - /** - * Setting active foot to the one that made contact most recently - * @warning this pattern remains unimplemented in the current version of this class - * using this pattern will always return left foot as the active foot - */ - LATEST_ACTIVE_CONTACT, - - /** - * Fixing active foot to the default foot defined by the user - * @warning this pattern remains unimplemented in the current version of this class - * using this pattern will always return left foot as the active foot - */ - DEFAULT_CONTACT - }; - - /* Foot Contact Classifier class for determining the primary foot in contact - * with the ground surface. Contains contact state machines for each foot: left and right, - * and implements the Schmitt Trigger thresholding on each foot to determine the primary - * foot which is in contact according to a switching pattern criteria. - * The switching pattern is primarily used to change the frames of reference for the Legged Odometry (LO) - * while one of the two foot breaks contact with the ground surface, since LO assumes that atleast one - * foot is in contact with ground at any instant of time. However, it is also used to assign - * the primary foot in case of double stance. - * - * In the current version of this class, switching logic is implemented only for - * ALTERNATE_CONTACT patterns vastly considered during walking tasks. This class is aimed - * to be extended towards other switching patterns like LATEST_ACTIVE_CONTACT, DEFAULT_CONTACT - * for implementation along side different task based controllers. - */ - class BipedFootContactClassifier - { - public: - /** - * Enumeration of foot in contact - */ - enum contactFoot - { - LEFT_FOOT, // 0 - RIGHT_FOOT, // 1 - UNKNOWN_FOOT // 2 - }; - - /** - * Constructor - * @params leftFootSchmittParams const ref to struct containing parameters for the left foot schmitt trigger device - * @params rightFootSchmittParams const ref to struct containing parameters for the right foot schmitt trigger device - */ - BipedFootContactClassifier(const SchmittParams& leftFootSchmittParams, const SchmittParams& rightFootSchmittParams); - - /** - * Updates the contact state machine for both the foot, determines the current state - * and detects foot transition for setting the primary foot(active foot) - * @param currentTime time - * @param leftNormalForce z-component of the force acting on the left foot - * @param rightNormalForce z-component of the force acting on the right foot - */ - void updateFootContactState(double currentTime, double leftFootNormalForce, double rightFootNormalForce); - - /** - * Get the primary foot - * @return contactFoot left, right or unknown - */ - contactFoot getPrimaryFoot() { return m_primaryFoot; } - - /** - * Get left foot contact state - * @return true if in contact, false otherwise - */ - bool getLeftFootContactState() { return m_leftFootContactState; } - - /** - * Get right foot contact state - * @return true if in contact, false otherwise - */ - bool getRightFootContactState() { return m_rightFootContactState; } - - /** - * set switching pattern to be considered for determining primary foot - * @warning no default value is considered, remember to call this method after instantiation - * @param pattern switching pattern - */ - void setContactSwitchingPattern(SwitchingPattern pattern) { m_pattern = pattern; } - - /** - * set primary foot - * This method was mainly intended to be called by an external process - * to set the primary foot in the initial setting, before any contact is broken. - * In case it is set to UNKNOWN_FOOT, it waits for the foot normal force measurements (checks left first and then right), - * and activates corresponding foot, handled in the detectTransitions() method. - * @param foot primary foot - */ - void setPrimaryFoot(contactFoot foot) { m_primaryFoot = foot; } - - // unique pointer to contact state machine for left foot - std::unique_ptr m_leftFootContactClassifier; - - // unique pointer to contact state machine for right foot - std::unique_ptr m_rightFootContactClassifier; - - private: - /** - * Determine the primary foot depending on the switching pattern - * compares the contact transition modes on each foot and - * sets the primary foot accordingly - */ - void detectFeetTransition(); - - // active/primary foot - contactFoot m_primaryFoot; - - // left foot contact state - bool m_leftFootContactState; - - // right foot contact state - bool m_rightFootContactState; - - // switching pattern - SwitchingPattern m_pattern; - }; -} #endif \ No newline at end of file diff --git a/src/estimation/include/iDynTree/Estimation/ContactStateMachine.h b/src/estimation/include/iDynTree/Estimation/ContactStateMachine.h index 57b5f38a4cf..0cc337336b4 100644 --- a/src/estimation/include/iDynTree/Estimation/ContactStateMachine.h +++ b/src/estimation/include/iDynTree/Estimation/ContactStateMachine.h @@ -1,126 +1,13 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef IDYNTREE_CONTACTSTATEMACHINE_H -#define IDYNTREE_CONTACTSTATEMACHINE_H -#include -#include -#include "SchmittTrigger.h" +#ifndef IDYNTREE_ESTIMATION_CONTACTSTATEMACHINE_H +#define IDYNTREE_ESTIMATION_CONTACTSTATEMACHINE_H -namespace iDynTree -{ - /** - * struct to hold schmitt trigger device parameters - */ - struct SchmittParams - { - double stableTimeContactMake; - double stableTimeContactBreak; - double contactMakeForceThreshold; - double contactBreakForceThreshold; - }; - - /** - * Contact State Machine class for binary contact state detection - * Contains a Schmitt Trigger device for updating the contact states - * using the contact normal force acting on the contact link and - * Determines contact transitions using simple binary switching logic - * - * Can be used to determine stable contacts, contact breaking and contact making. - * The parameters to the Schmitt Trigger are passed as a struct and is the Schmitt - * Trigger is instantiated in the class constructor. - * - * NOTE: There are no default parameters to the Schmitt Trigger. These parameters are set through - * the constructor during instantiation. - * - * This class does not exactly abstract the Schmitt Trigger class. Schmitt Trigger methods are still accessible through - * the m_contactSchmitt object. This class uses a generic Schmitt Trigger object and augments its functionality - * specific to physical contacts based scenarios. - */ - class ContactStateMachine - { - public: - /** - * Enumeration of contact transitions - */ - enum contactTransition - { - /** - * previous state: off contact, current state: off contact - */ - STABLE_OFFCONTACT, // 0 - - /** - * previous state: on contact, current state: on contact - */ - STABLE_ONCONTACT, // 1 - - /** - * previous state: on contact, current state: off contact - */ - CONTACT_BREAK, // 2 - - /** - * previous state: off contact, current state: on contact - */ - CONTACT_MAKE, // 3 +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif + +#include - /** - * Unknown transition - */ - UNKNOWN_TRANSITION = -1 - }; - - /** - * Constructor - * @param s const reference to a struct containing schmitt trigger device parameters - */ - ContactStateMachine(const SchmittParams& s); - - /** - * Calls schmitt trigger device update - * @param currentTime time - * @param contactNormalForce normal force acting on the contact link in consideration - */ - void contactMeasurementUpdate(double currentTime, double contactNormalForce); - - /** - * Calls schmitt trigger device reset - */ - void resetDevice() { m_contactSchmitt.get()->resetDevice(); } - - /** - * Get current contact state - * @return true, if in contact, false otherwise - */ - bool contactState() { return m_currentState; } - - /** - * Determines contact transitions using simple binary switching logic - * @return contactTransition enumerated value - */ - contactTransition contactTransitionMode(); - - /** - * Get time of last contact state update - * @return time - */ - double lastUpdateTime(); - - /** - * unique pointer to the schmitt trigger device - */ - std::unique_ptr m_contactSchmitt; - private: - // previous contact state - bool m_previousState; - - // current contact state - bool m_currentState; - - // transtion mode based on previous and current contact states - int m_tranisitionMode; - - }; -} #endif diff --git a/src/estimation/include/iDynTree/Estimation/ExtWrenchesAndJointTorquesEstimator.h b/src/estimation/include/iDynTree/Estimation/ExtWrenchesAndJointTorquesEstimator.h index 9ed05a4c083..78d2f40ae77 100644 --- a/src/estimation/include/iDynTree/Estimation/ExtWrenchesAndJointTorquesEstimator.h +++ b/src/estimation/include/iDynTree/Estimation/ExtWrenchesAndJointTorquesEstimator.h @@ -1,366 +1,13 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef IDYNTREE_ESTIMATION_EXTWRENCHESANDJOINTTORQUEESTIMATOR_H -#define IDYNTREE_ESTIMATION_EXTWRENCHESANDJOINTTORQUEESTIMATOR_H +#ifndef IDYNTREE_DEPRECATED_ESTIMATION_EXTWRENCHESANDJOINTTORQUEESTIMATOR_H +#define IDYNTREE_DEPRECATED_ESTIMATION_EXTWRENCHESANDJOINTTORQUEESTIMATOR_H -#include - -#include -#include -#include -#include -#include -#include -#include - -#include - -namespace iDynTree -{ - -/** - * \brief Estimator for external wrenches and joint torques using internal F/T sensors. - * - * This is a class for estimating external wrenches and joint torques using as an input - * the robot velocities and the accelerations and the measurement of internal six axis - * F/T sensors. - * - * The kinematic information (position,velocity and acceleration) necessary for the - * estimation can be provided in two ways: - * * using the updateKinematicsFromFloatingBase - * * using the updateKinematicsFromFixedBase - * - * Note that in both ways there is no need (for the estimation) to provide the - * absolute position and linear velocity of the robot with respect to the inertial frame. - * The effect of gravity is considered by directly using the proper acceleration in the case - * of the floating frame (proper acceleration can be directly measured by an accelerometer) - * or by directly providing the gravity vector in the fixed frame case. - * - * Beside its main goal of estimation of external wrenches and joint torques, the class - * also provide methods that can be useful to calibrate the six-axis FT sensors of the robot. - * These methods are: - * * ExtWrenchesAndJointTorquesEstimator::computeExpectedFTSensorsMeasurements - * * ExtWrenchesAndJointTorquesEstimator::computeSubModelMatrixRelatingFTSensorsMeasuresAndKinematics - * Details of each method can be found in the method documentation - */ -class ExtWrenchesAndJointTorquesEstimator -{ - /** - * Structure variables. - */ - Model m_model; - SubModelDecomposition m_submodels; - SensorsList m_sensors; - bool m_isModelValid; - bool m_isKinematicsUpdated; - - /**< Traveral used for the dynamics computations */ - Traversal m_dynamicTraversal; - - /** - * Vector of Traversal used for the kinematic computations. - * m_kinematicTraversals.getTraversalWithLinkAsBase(l) contains the traversal with base link l . - */ - LinkTraversalsCache m_kinematicTraversals; - - JointPosDoubleArray m_jointPos; - LinkVelArray m_linkVels; - LinkAccArray m_linkProperAccs; - LinkNetExternalWrenches m_linkNetExternalWrenches; - LinkInternalWrenches m_linkIntWrenches; - FreeFloatingGeneralizedTorques m_generalizedTorques; - - estimateExternalWrenchesBuffers m_calibBufs; - estimateExternalWrenchesBuffers m_bufs; - - /** - * Disable copy constructor and copy operator - */ - ExtWrenchesAndJointTorquesEstimator(const ExtWrenchesAndJointTorquesEstimator & /*other*/) {}; - - /** - * Copy operator is forbidden - */ - ExtWrenchesAndJointTorquesEstimator& operator=(const ExtWrenchesAndJointTorquesEstimator &/*other*/) {return *this;}; - -public: - /** - * \brief Constructor. - */ - ExtWrenchesAndJointTorquesEstimator(); - - /** - * \brief Destructor. - */ - ~ExtWrenchesAndJointTorquesEstimator(); - - /** - * \brief Set model and sensors used for the estimation. - * - * @param[in] _model the kinematic and dynamic model used for the estimation. - * @param[in] _sensors the sensor model used for the estimation. - * @return true if all went well (model and sensors are well formed), false otherwise. - */ - bool setModelAndSensors(const Model & _model, const SensorsList & _sensors); - - /** - * Load model and sensors from file. - * - * @deprecated Use iDynTree::ModelLoader::loadModelFromFile and call setModelAndSensors - * on the parsed Model and SensorsList - * - * @param[in] filename path to the file to load. - * @param[in] filetype (optional) explicit definiton of the filetype to load. - * Only "urdf" is supported at the moment. - * @return true if all went well (files were correctly loaded and consistent), false otherwise. - */ - bool loadModelAndSensorsFromFile(const std::string filename, const std::string filetype=""); - - /** - * Load model and sensors from file, specifieng the dof considered for the estimation. - * - * @note this will create e a reduced model only with the joint specified in consideredDOFs and the - * fixed joints in which FT sensor are mounted. - * - * @param[in] filename path to the file to load. - * @param[in] consideredDOFs list of dof to consider in the model. - * @param[in] filetype (optional) explicit definiton of the filetype to load. - * Only "urdf" is supported at the moment. - * @return true if all went well (files were correctly loaded and consistent), false otherwise. - */ - bool loadModelAndSensorsFromFileWithSpecifiedDOFs(const std::string filename, - const std::vector & consideredDOFs, - const std::string filetype=""); - - - /** - * Get used model. - * - * @return the kinematic and dynamic model used for estimation. - */ - const Model & model() const; - - /** - * Get used sensors. - * - * @return the sensor model used for estimation. - */ - const SensorsList & sensors() const; - - /** - * Get the used submodel decomposition. - * - * @return the used submodel decomposition. - */ - const SubModelDecomposition & submodels() const; - - /** - * Set the kinematic information necessary for the force estimation using the - * acceleration and angular velocity information of a floating frame. - * - * \note Tipically the floating frame information comes from an sensor - * containing a gyroscope (providing the angular velocity) and an - * accelerometer (providing the classical proper acceleration). As - * inertial sensors that return angular acceleration exist but are not common, - * the angular acceleration is usually obtained by numerical derivation - * on the angular velocity measure. In some cases it could make sense - * to just neglet the contribution of the floating frame angular acceleration - * (setting it to a zero vector), if its impact to the dynamics is marginal. - * - * @param[in] jointPos the position of the joints of the model. - * @param[in] jointVel the velocities of the joints of the model. - * @param[in] jointAcc the accelerations of the joints of the model. - * @param[in] floatingFrame the index of the frame for which kinematic information is provided. - * @param[in] properClassicalLinearAcceleration proper (actual acceleration-gravity) classical acceleration - * of the origin of the specified frame, - * expressed in the specified frame orientation. - * @param[in] angularVel angular velocity (wrt to an inertial frame) of the specified floating frame, - * expressed in the specified frame orientation. - * @param[in] angularAcc angular acceleration (wrt to an inertial frame) of the specified floating frame , - * expressed in the specified frame orientation. - * @return true if all went ok, false otherwise. - */ - bool updateKinematicsFromFloatingBase(const JointPosDoubleArray & jointPos, - const JointDOFsDoubleArray & jointVel, - const JointDOFsDoubleArray & jointAcc, - const FrameIndex & floatingFrame, - const Vector3 & properClassicalLinearAcceleration, - const Vector3 & angularVel, - const Vector3 & angularAcc); - - /** - * Set the kinematic information necessary for the force estimation assuming that a - * given frame is not accelerating with respect to the inertial frame. - * - * @param[in] jointPos the position of the joints of the model. - * @param[in] jointVel the velocities of the joints of the model. - * @param[in] jointAcc the accelerations of the joints of the model. - * @param[in] fixedFrame the index of the frame that is not accelerating with respect to the inertial frame. - * @param[in] gravity the gravity acceleration vector, expressed in the specified fixed frame. - * @return true if all went ok, false otherwise. - */ - bool updateKinematicsFromFixedBase(const JointPosDoubleArray & jointPos, - const JointDOFsDoubleArray & jointVel, - const JointDOFsDoubleArray & jointAcc, - const FrameIndex & fixedFrame, - const Vector3 & gravity); - - /** - * \brief Predict FT sensors using the knoledge of external wrenches location. - * - * This function is used to estimate the expected measurement of the FT sensors. - * The typical use of this function is to specify only one external unknown wrench - * in the unknowns parameter and then compute the expected measurements of the FT sensors - * using the kinematic information specified with an updateKinematics*** method. - * The location of the single external unknown wrench is the one of the only link - * that is supporting the weight of the robot. - * - * This function can also be used to estimate the FT sensor measurements in case - * two unknown external wrenches are applied on the robot, if some additional - * assumption about the simmetry of the robot configuration, the joint torques and - * the external wrenches can be done. - * - * \warning Before calling this method, either updateKinematicsFromFloatingBase or - * updateKinematicsFromFixedBase must be called. - * - * @param[in] unknowns the unknown external wrenches. - * @param[out] predictedMeasures the estimate measures for the FT sensors. - * @param[out] estimatedContactWrenches the estimated contact wrenches. - * @param[out] estimatedJointTorques the estimated joint torques. - * @return true if all went well, false otherwise. - */ - bool computeExpectedFTSensorsMeasurements(const LinkUnknownWrenchContacts & unknowns, - SensorsMeasurements & predictedMeasures, - LinkContactWrenches & estimatedContactWrenches, - JointDOFsDoubleArray & estimatedJointTorques); - - /** - * \brief For each submodel without any external wrench, computes the equation that relates the FT sensor measures with the kinematics-related known terms. - * - * For each submodel in which there are no external forces, we can write the following equation: - * - * \f[ - * A w = b - * \f] - * Where: - * * \f$w\f$ is the vector of dimension 6*nrOfFTSensors obtained by stacking the FT sensors measures: - * * \f$A\f$ is a matrix of size 6 x 6*nrOfFTSensors that depends on position in space of the FT sensors - * * \f$b\f$ is a vector of size 6 that depends on the position, velocity, acceleration and gravity of each link in the submodel - * - * This function provides an easy way to compute A and B . Typically, these quantities are not used online - * during the estimation of external wrenches or internal torques, but rather as an helper method when calibrating FT sensors. - * - * In the rest of the documentation, we will refer to this quantities: - * * nrOfSubModels (\f$n_{sm}\f$): the number of submodels in which the model is divided, as induced by the FT sensors present in the model. - * * nrOfSubModelsWithoutExtWrenches: the number of submodels on which there is no external wrench - * * nrOfFTSensors (\f$n_{ft}\f$): the number of FT sensors present in the model - * In particular, the value of A and b for a given submodel is the following. First of all, for any submodel $sm$ with no external force, - * we can write (from Equation 4.19 of https://traversaro.github.io/traversaro-phd-thesis/traversaro-phd-thesis.pdf, modified to account - * for all FT sensors in the submodel and to remove external wrenches): - * - * \f[ - * \sum_{s=1}^{n_ft}~\mu_{sm, ft}~\sigma_{sm, ft}~{}_{B_{sm}} X^{ft} \mathrm{f}^{meas}_{ft} = \sum_{L \in \mathbb{L}_{sm}} {}_{B_{sm}} X^{L} {}_L \phi_L - * \f] - * - * where: - * * \f$ \mu_{sm, ft} \f$ is equal to \f$1\f$ if the sensor \f$ft\f$ is attached to the submodel \f$sm\f$, and \f$0\f$ otherwise - * * \f$ \sigma_{sm, ft} \f$ is equal to \f$1\f$ if the sensor \f$ft\f$ is measuring the force applied on submodel \f$sm\f$ or \f$-1\f$ if it is measuring the force that the submodel excerts on its neighbor submodel - * *\f$ {B_{sm}} \f$ is a frame in which this equation is expressed, that for this function it is the base link of the submodel. - * - * - * With this definitions, we can see that A_sm and b_sm for a given submodel \f$sm\f$ can be simply be defined as: - * - * \f$ - * A_{sm} = \begin{bmatrix} \mu_{sm, ft0}~\sigma_{sm, ft(0)}~{}_C X^{ft(0)} & \hdots & \mu_{sm, ft(n_ft-1)}~\sigma_{sm, ft(n_ft-1)}~X^{ft(n_ft-1)} \end{bmatrix} - * \f$ - * - * \f$ - * w_{sm} = \begin{bmatrix} \mathrm{f}^{meas}_{ft(0)} \\ \vdots \\ \mathrm{f}^{meas}_{ft(n_ft-1)} \end{bmatrix} - * \f$ - * - * \f$ - * b_{sm} = \sum_{L \in \mathbb{L}_{sm}} {}_C X^{L} {}^L \phi_L - * \f$ - * - * \warning Before calling this method, either updateKinematicsFromFloatingBase or - * updateKinematicsFromFixedBase must be called. - * - * @param[in] unknowns the unknown external wrenches, that is used to understand the submodels in which no external wrench is present - * @param[out] A vector of nrOfSubModelsWithoutExtWrenches matrices of size 6 x 6*nrOfFTSensors - * @param[out] b vector of nrOfSubModelsWithoutExtWrenches vectors of size 6 - * @param[out] subModelIDs vector of size nrOfSubModelsWithoutExtWrenches of unsigned integers from 0 to nrOfSubModels-1, subModelIDs[i] specifies to which submodel the quantities A[i] - * @param[out] baseLinkIndeces vector of size nrOfSubModelsWithoutExtWrenches of iDynTree::LinkIndex from 0 to nrOfLinks-1, baseLinkIndeces[i] specifies the link in which the equation i is expressed - * @return true if all went well, false otherwise. - */ - bool computeSubModelMatrixRelatingFTSensorsMeasuresAndKinematics(const LinkUnknownWrenchContacts & unknowns, - std::vector& A, - std::vector& b, - std::vector& subModelID, - std::vector& baseLinkIndeces); - - /** - * \brief Estimate the external wrenches and the internal joint torques using the measurement of the internal F/T sensors. - * - * This is the main method of the class. The technique implemented - * in this method is the one described in the paper: - * - * "Contact force estimations using tactile sensors and force/torque sensors" - * - * Del Prete, A., Natale, L., Nori, F., & Metta, G. (2012). - * Contact force estimations using tactile sensors and force/torque sensors. - * URL : http://www.researchgate.net/profile/Andrea_Del_Prete/publication/236152161_Contact_Force_Estimations_Using_Tactile_Sensors_and_Force__Torque_Sensors/links/00b495166e74a5369d000000.pdf - * - * \note There should be at least 6 unknowns variables for each submodel for which the estimation of - * external wrenches is performed (i.e. usually 1 unknown wrench for each submodel). - * If is not the case, undefined results could occur in the estimation. - * - * @param[in] unknowns the unknown external wrenches - * @param[in] ftSensorsMeasures the measurements for the FT sensors. - * @param[out] estimatedContactWrenches the estimated contact wrenches. - * @param[out] estimatedJointTorques the estimated joint torques. - * @return true if all went ok, false otherwise. - */ - bool estimateExtWrenchesAndJointTorques(const LinkUnknownWrenchContacts & unknowns, - const SensorsMeasurements & ftSensorsMeasures, - LinkContactWrenches & estimatedContactWrenches, - JointDOFsDoubleArray & estimatedJointTorques); - - /** - * Check if the kinematics set in the model are the one of a fixed model. - * - * While computing the expected F/T sensors measures, you tipically want the - * model to be still, to reduce the sources of noise . - * - * @param[in] gravityNorm the norm of the gravity (tipically 9.81) against with all the - * proper accelerations are check (for a still model, the proper - * acceleration norm should be close to the gravity norm. - * @param[in] properAccTol tolerance to use for the check on the proper acceleration norm. - * @param[in] verbose true if you want to print debug information, false otherwise. - * @return true if the model is still, false if it is moving or if the kinematics was never setted. - * - * \note This method can be computationally expensive, so in most case it - * may be a better idea to just do the check on the input variables (joint velocities, joint acceleration). - */ - bool checkThatTheModelIsStill(const double gravityNorm, - const double properAccTol, - const double verbose); - - - /** - * Compute the vector of the sum of all the wrenches (both internal and external, excluding gravity) acting on - * link i, expressed (both orientation and point) with respect to the reference frame of link i, - * using the articulated body model and the kinematics information provided by the updateKinematics* methods. - * - * This is tipically computed as I*a+v*(I*v) , where a is the proper acceleration. - * - * @param[out] netWrenches the vector of link net wrenches. - * @return true if all went ok, false otherwise. - */ - bool estimateLinkNetWrenchesWithoutGravity(LinkNetTotalWrenchesWithoutGravity & netWrenches); - -}; +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif -} +#include #endif diff --git a/src/estimation/include/iDynTree/Estimation/ExtendedKalmanFilter.h b/src/estimation/include/iDynTree/Estimation/ExtendedKalmanFilter.h index 0b6c1d6bd81..6e6efe5ff06 100644 --- a/src/estimation/include/iDynTree/Estimation/ExtendedKalmanFilter.h +++ b/src/estimation/include/iDynTree/Estimation/ExtendedKalmanFilter.h @@ -1,320 +1,13 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef EXTENDED_KALMAN_FILTER_H -#define EXTENDED_KALMAN_FILTER_H +#ifndef IDYNTREE_ESTIMATION_EXTENDED_KALMAN_FILTER_H +#define IDYNTREE_ESTIMATION_EXTENDED_KALMAN_FILTER_H -#include -#include -#include -#include - -namespace iDynTree -{ - - /** - * @class DiscreteExtendedKalmanFilterHelper naive base class implementation of discrete EKF with additive Gaussian noise - * - * @warning This class is not stand-alone but can be used meaningfully only by classes deriving from it. - * The system propagation function f() and measurement model function h() are virtual functions to be defined by - * the derived class implementing the EKF. - * Similarly, the Jacobians for the linearized system propagation and measurement model are also virtual functions - * to be implemented by the derived class. - * - * The derived class must must set the size for states, inputs and outputs using ekfSetStateSize(), ekfSetInputSize() and ekfSetOutputSize() methods. - * Then the derived class must call ekfInit() to resize the vectors and matrices for the EKF. - * - * Similarly before running the estimator through a loop, it is recommended to set the initial states and variances for measurements, system dynamics and intial states. - * This is necessary to avoid any NaN values to be propagated. - * - * Once, initialized properly, the filter can be run by - * - calling ekfSetInputVector() to set the control inputs and then calling ekfPredict() at each prediction step, and - * - calling ekfSetMeasurementVector() to set the measurements and then calling ekfUpdate() at each update step - * - * The internal state of the estimator can be obtained by calling ekfGetStates(). - * - * The Discrete Extended Kalman Filter equations implemented in this class are coherent with the ones described in - * Discrete-time predict and update equations section of this article. - * - * The general workflow implementing/inheriting this class would be in the order, - * - call ekfSetInputSize(), ekfSetOutputSize(), ekfSetStateSize() methods - * - call ekfInit() - * - call ekfSetInitialState(), ekfSetStateCovariance() (either done externally later or internally. usually done externally later, however, filter runs properly only if this step is done) - * - call ekfSetSystemNoiseCovariance(), ekfSetMeasurementNoiseCovariance() - * - * once this is setup, - * - in a loop - * - call ekfSetInputVector() then ekfPredict(). Calling ekfGetStates() and ekfGetStateCovariance() at this point will give us the predicted states and its covariance - * - call ekfSetMeasurementVector() then ekfUpdate(). Calling ekfGetStates() and ekfGetStateCovariance() at this point will give us the updated states and its covariance - * @note if we intend to change the state size, input size or output size on the fly, it is crucial to call the ekfInit() method again, since this resizes the buffers accordingly - * failing to do so will result in memory leaks and will cause the program to crash. - * - */ - class DiscreteExtendedKalmanFilterHelper - { - public: - DiscreteExtendedKalmanFilterHelper(); - - /** - * @brief Describes the state propagation for a given dynamical system - * If state of the system is denoted by \f$ x \f$ and the control input by \f$ u \f$, then - * the system dynamics is given as \f$ x_{k+1} = f(x_k, u_k) \f$. - * @note the detail of this function needs to be implemented by the child class - * @param[in] x_k state at current time instant - * @param[in] u_k control input at current time instant - * @param[out] xhat_k_plus_one predicted state without any correction from measurements - * @return bool true/false if successful or not - */ - virtual bool ekf_f(const iDynTree::VectorDynSize& x_k, - const iDynTree::VectorDynSize& u_k, - iDynTree::VectorDynSize& xhat_k_plus_one) = 0; - - /** - * @brief Describes the measurement model of the system, - * i.e., how the measurements can be described as a function of states, - * Given a state of the system described by \f$ x \f$, - * what would be the measurement \f$ z \f$ observed from this state \f$ z_{k+1} = h(\hat{x}_{k+1}) \f$. - * @note the detail of this function needs to be implemented by the child class - * @param[in] xhat_k_plus_one predicted state of next time instant - * @param[out] zhat_k_plus_one predicted measurement of next time instant - * @return bool true/false if successful or not - */ - virtual bool ekf_h(const iDynTree::VectorDynSize& xhat_k_plus_one, - iDynTree::VectorDynSize& zhat_k_plus_one) = 0; - - /** - * @overload - */ - virtual bool ekfComputeJacobianF(iDynTree::VectorDynSize& x, iDynTree::MatrixDynSize& F) = 0; - - /** - * @brief Describes the system Jacobian necessary for the propagation of predicted state covariance - * The analytical Jacobian describing the partial derivative of the system propagation with respect to the state - * and the system propagation with respect to the input - * @note the detail of this function needs to be implemented by the child class - * @param[in] x system state - * @param[in] u system input - * @param[out] F system Jacobian - * @return bool true/false if successful or not - */ - virtual bool ekfComputeJacobianF(iDynTree::VectorDynSize& x, iDynTree::VectorDynSize& u, iDynTree::MatrixDynSize& F) = 0; - - /** - * @brief Describes the measurement Jacobian necessary for computing Kalman gain and updating the predicted state and its covariance - * The analytical Jacobian describing the partial derivative of the measurement model with respect to the state - * @note the detail of this function needs to be implemented by the child class - * @param[in] x system state - * @param[out] H measurement Jacobian - * @return bool true/false if successful or not - */ - virtual bool ekfComputeJacobianH(iDynTree::VectorDynSize& x, iDynTree::MatrixDynSize& H) = 0; - - /** - * @brief Implements the Discrete EKF prediction equation - * described by - * \f$ \hat{x}_{k+1} = f(x_k, u_k) \f$ is given by the ekf_f() method - * \f$ \hat{P}_{k+1} = F_k P_k F_k^T + Q \f$ - * where, \f$ F \mid_{x = x_k} \f$ is given by the ekfComputejacobianF() method - * - * @warning this function can be called only after setting up the filter properly through ekfInit() step - * @note this function should be once called every step, after setting up the input vector using ekfSetInputVector() method - * @warning setting up the input vector everytime before calling the ekfPredict() method is crucial, the prediction step is not performed if this step is skipped - * this is because internally a flag associated to the setting up of input vector is set true by ekfSetInputVector() method which in turn is set false by ekfPredict() method - * @return bool true/false if successful or not - */ - bool ekfPredict(); - - /** - * @brief Implements the Discrete EKF update equation - * described by - * \f$ z_{k+1} = h(\hat{x}_{k+1}) \f$ is given by ekf_h() method - * innovation \f$ \tilde{y}_{k+1} = y_{k+1} - z_{k+1} \f$ - * innovation covariance \f$ S_{k+1} = H_{k+1} \hat{P}_{k+1} H_{k+1}^T + R \f$, where \f$ H \mid_{x = \hat{x}_{k+1}} \f$ is given by ekfComputejacobianH() method - * Kalman gain \f$ K_{k+1} = \hat{P}_{k+1} H_{k+1}^T S_{k+1}^{-1} \f$ - * Updated covariance \f$ P_{k+1} = \hat{P}_{k+1} - (K_{k+1} H \hat{P}_{k+1}) \f$ - * Updated state estimate \f$ x_{k+1} = \hat{x}_{k+1} + K_{k+1} \tilde{y}_{k+1} \f$ - * - * @warning this function can be called only after setting up the filter properly through ekfInit() step - * @note this function should be once called every step, after setting up the measurement vector using ekfSetMeasurementVector() method - * @warning setting up the measurement vector everytime before calling the ekfUpdate() method is crucial, the update step is not performed if this step is skipped - * this is because internally a flag associated to the setting up of measurement vector is set true by ekfSetMeasurementVector() method which in turn is set false by ekfUpdate() method - * @return bool true/false if successful or not - */ - bool ekfUpdate(); - - /** - * @brief Initializes and resizes the internal buffers of this filter - * @warning this is a very crucial method of this class. This needs to be called after setting the input size through ekfSetInputSize(), - * output size through ekfSetOutputSize() and state dimension through ekfSetStateSize(), - * such that the corresponding matrices and vectors will resize themselves to their corresponding dimensions. - * Failing to do so might result in memory leaks and may cause the program to crash - * @return bool true/false if successful or not - */ - bool ekfInit(); - - /** - * @brief Initializes and resizes the internal buffers of this filter - * @warning this is a very crucial method of this class. This method sets the input size through ekfSetInputSize(), - * output size through ekfSetOutputSize() and state dimension through ekfSetStateSize() with the specified parameters, - * such that the corresponding matrices and vectors will resize themselves to their corresponding dimensions. - * Failing to do so might result in memory leaks and may cause the program to crash - * @param[in] state_size state size - * @param[in] input_size input size - * @param[in] output_size output size - * @return bool true/false if successful or not - */ - bool ekfInit(const size_t& state_size, const size_t& input_size, const size_t& output_size); - - /** - * @brief Resets the filter flags - * The filter flags check if the filter was properly initialized, if the initial state was set, - * if the initial state covariance was set. These three flags are crucial for proper setting up of the filter. - * The other flags include the checks on whether the input and measurement vectors were updated at every prediction/update step - */ - void ekfReset(); - - /** - * @brief Resets the filter flags, initializes and resizes internal buffers of the filter, and - * sets initial state, initial state covariance, and system noise and measurement noise covariance matrices - * @warning size of the span for P0 and Q must be of the size (state size*state size), where * is the regular multiplication operator - * @warning size of the span for R must be of the size (ouput size*output size), where * is the regular multiplication operator - * @warning the matrices from the span are built in row-major ordering. - * - * @note this method is particularly useful while working with hybrid systems, - * where the size of the system state or the measurements keep evolving with time - * - */ - bool ekfReset(const size_t& state_size, - const size_t& input_size, - const size_t& output_size, - const iDynTree::Span& x0, - const iDynTree::Span& P0, - const iDynTree::Span& Q, - const iDynTree::Span& R); - - /** - * @brief Set measurement vector at every time step - * the measurement vector size and output size should match - * @param[in] y iDynTree::Span object to access the measurement vector - * @return bool true/false if successful or not - */ - bool ekfSetMeasurementVector(const iDynTree::Span& y); - - /** - * @brief Set input vector at every time step - * the input vector size and input size should match - * @param[in] u iDynTree::Span object to access the input vector - * @return bool true/false if successful or not - */ - bool ekfSetInputVector(const iDynTree::Span& u); - - /** - * @brief Set initial state - * the size of x0 and state size should match - * @param[in] x0 iDynTree::Span object to access the state vector - * @note this method should be called before running the filter - * @return bool true/false if successful or not - */ - bool ekfSetInitialState(const iDynTree::Span& x0); - - /** - * @brief Set initial state covariance matrix - * the size of P and (state size*state size) should match - * @param[in] P iDynTree::Span object to access the state covariance matrix - * @note this method should be called before running the filter - * @warning if this matrix is not initialized properly, then the resulting output will only have NaNs in it - * @return bool true/false if successful or not - */ - bool ekfSetStateCovariance(const iDynTree::Span& P); - - /** - * @brief Set system noise covariance matrix - * the size of Q and (state size*state size) should match - * @param[in] Q iDynTree::Span object to access the system noise covariance matrix - * @note default value is a zero matrix - * @return bool true/false if successful or not - */ - bool ekfSetSystemNoiseCovariance(const iDynTree::Span& Q); - - /** - * @brief Set measurement noise covariance matrix - * the size of R and (output size*output size) should match - * @param[in] R iDynTree::Span object to access the measurement noise covariance matrix - * @note default value is a zero matrix - * @return bool true/false if successful or not - */ - bool ekfSetMeasurementNoiseCovariance(const iDynTree::Span& R); - - /** - * @brief Set the state dimensions - * @param[in] dim_X state size - * @warning this method should be called before calling ekfInit() - * @return bool true/false if successful or not - */ - void ekfSetStateSize(size_t dim_X) { m_dim_X = dim_X; } - - /** - * @brief Set the input dimensions - * @param[in] dim_U input size - * @warning this method should be called before calling ekfInit() - * @return bool true/false if successful or not - */ - void ekfSetInputSize(size_t dim_U) { m_dim_U = dim_U; } - - /** - * @brief Set the ouptut dimensions - * @param[in] dim_Y output size - * @warning this method should be called before calling ekfInit() - * @return bool true/false if successful or not - */ - void ekfSetOutputSize(size_t dim_Y) { m_dim_Y = dim_Y; } - - /** - * @brief Get current internal state of the filter - * the size of x and state size should match - * @param[out] x iDynTree::Span object to copy the internal state vector into - * @return bool true/false if successful or not - */ - bool ekfGetStates(const iDynTree::Span &x) const; - - /** - * @brief Get state covariance matrix - * the size of P and (state size*state size) should match - * @param[out] P iDynTree::Span object to copy the internal state covariance matrix onto - * @return bool true/false if successful or not - */ - bool ekfGetStateCovariance(const iDynTree::Span &P) const; - - protected: - /** - * function template to ignore unused parameters - */ - template - void ignore(T &&) { } - - private: - size_t m_dim_X; ///< state dimension - size_t m_dim_Y; ///< output dimenstion - size_t m_dim_U; ///< input dimension - iDynTree::VectorDynSize m_x; ///< state at time instant k - iDynTree::VectorDynSize m_u; ///< input at time instant k - iDynTree::VectorDynSize m_y; ///< measurements at time instant k - iDynTree::VectorDynSize m_xhat; ///< predicted state at time instant k before updating measurements +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif - iDynTree::MatrixDynSize m_F; ///< System jacobian - iDynTree::MatrixDynSize m_P; ///< State covariance - iDynTree::MatrixDynSize m_Phat; ///< State covariance estimate before updating measurements - iDynTree::MatrixDynSize m_Q; ///< system noise covariance - iDynTree::MatrixDynSize m_H; ///< measurement jacobian - iDynTree::MatrixDynSize m_S; ///< innovation covariance - iDynTree::MatrixDynSize m_K; ///< Kalman gain - iDynTree::MatrixDynSize m_R; ///< measurement noise covariance - bool m_is_initialized{false}; ///< flag to check if filter is properly initialized - bool m_measurement_updated{false}; ///< flag to check if measurement is updated at each update step - bool m_input_updated{false}; ///< flag to check if control input is updated at each prediction step - bool m_initial_state_set{false}; ///< flag to check if the initial state of the filter is set - bool m_initial_state_covariance_set{false}; ///< flag to check if the initial covariance is set properly - }; -} +#include #endif diff --git a/src/estimation/include/iDynTree/Estimation/ExternalWrenchesEstimation.h b/src/estimation/include/iDynTree/Estimation/ExternalWrenchesEstimation.h index 90e29513643..63a730073f5 100644 --- a/src/estimation/include/iDynTree/Estimation/ExternalWrenchesEstimation.h +++ b/src/estimation/include/iDynTree/Estimation/ExternalWrenchesEstimation.h @@ -1,417 +1,13 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef IDYNTREE_ESTIMATION_EXTERNALWRENCHESTIMATION_H -#define IDYNTREE_ESTIMATION_EXTERNALWRENCHESTIMATION_H +#ifndef IDYNTREE_DEPRECATED_ESTIMATION_EXTERNALWRENCHESTIMATION_H +#define IDYNTREE_DEPRECATED_ESTIMATION_EXTERNALWRENCHESTIMATION_H -#include -#include -#include -#include -#include - -#include -#include - -#include - -namespace iDynTree -{ -class Model; -class Traversal; -class SubModelDecomposition; -class LinkContactWrenches; -class SensorsMeasurements; -class SensorsList; -class LinkVelArray; -class LinkAccArray; -class JointPosDoubleArray; -class JointDOFsDoubleArray; - -/** - * Type of a UnknownWrenchContact. - */ -enum UnknownWrenchContactType -{ - /** - * Contact for which the complete wrench is unknown. - */ - FULL_WRENCH, - - /** - * Contact assumed to be a pure force excerted on the contact point. - */ - PURE_FORCE, - - /** - * Contact assumed to be a pure force with a known direction excerted on the contact point - */ - PURE_FORCE_WITH_KNOWN_DIRECTION, - - /** - * The contact forces is assumed to be known. - */ - NO_UNKNOWNS -}; - - -/** - * \brief A contact whose wrench is unknown. - * - * Class representing a contact for which - * the approximate center of the contact surface is known, - * but the wrench is unknown. - * - */ -struct UnknownWrenchContact -{ - /** - * Constructor - */ - UnknownWrenchContact() - {} - - UnknownWrenchContact(const UnknownWrenchContactType _unknownType, - const Position & _contactPoint, - const Direction & _forceDirection = iDynTree::Direction::Default(), - const Wrench & _knownWrench = iDynTree::Wrench(), - const unsigned long & _contactId = 0): unknownType(_unknownType), - contactPoint(_contactPoint), - forceDirection(_forceDirection), - knownWrench(_knownWrench), - contactId(_contactId) - {} - - /** - * Type of the unknown contact. - */ - UnknownWrenchContactType unknownType; - - /** - * Position of the center of the contact, in the link frame. - */ - Position contactPoint; - - /** - * If unknownType is PURE_FORCE_WITH_KNOWN_DIRECTION, - * contains the known direction (in link frame) of the force. - */ - Direction forceDirection; - - /** - * If unknownType is NO_UNKNOWNS, - * contains the value of the contact force, with the orientation of the link frame, - * and w.r.t. to the origin of the link frame, i.e. it ignores the contactPoint attribute. - */ - Wrench knownWrench; - - /** - * Unique id identifing the contact. - * This id is propagated to the contact wrench data structure. - * It is implemented mainly for compatibility with the skinDynLib library. - */ - unsigned long contactId; -}; - -/** - * A set of UnknownWrenchContact for each link, representing all the contacts - * between the model and the external environment whose wrench is unkwnon. - * - */ -class LinkUnknownWrenchContacts -{ -private: - std::vector< std::vector > m_linkUnknownWrenchContacts; - -public: - /** - * Create a LinkWrenches vector, with the size given - * by nrOfLinks . - * - * @param[in] nrOfLinks the size of the vector. - */ - LinkUnknownWrenchContacts(unsigned int nrOfLinks = 0); - LinkUnknownWrenchContacts(const Model & model); - - /** - * Preserving the number of links, remove all the previously added unknowns. - * - */ - void clear(); - - /** - * - * @param[in] nrOfLinks the number of links used to resize - */ - void resize(unsigned int nrOfLinks); - void resize(const Model & model); - - /** - * Get the number of external contacts for a given link. - */ - size_t getNrOfContactsForLink(const LinkIndex linkIndex) const; - - /** - * Set the number of external contacts for a given link. - */ - void setNrOfContactsForLink(const LinkIndex linkIndex, const size_t nrOfContacts); - - /** - * Add a new contact for a link. - */ - void addNewContactForLink(const LinkIndex linkIndex, const UnknownWrenchContact& newContact); - - /** - * Add a new contact for a frame. - * If the specified frame is not the a link frame, the method automatically convert the unknown - * wrench to the relative link frame. - * - * @param[in] model the model class for getting frame information. - * @param[in] frameIndex the index of the frame in which you are expressing the new unknown wrench. - * @param[in] newContact the new unknown wrench to add. - * @return true if all went well, false otherwise - */ - bool addNewContactInFrame(const Model & model, - const FrameIndex frameIndex, - const UnknownWrenchContact& newContact); - - /** - * Add a full wrench unknown at the origin of the specified frame. - * Simplified version of a addNewContactInFrame, in which the contact - * point is the origin of the frame and the unknown type is FULL_WRENCH. - * - * Equivalent to - * addNewContactInFrame(model,frame,UnknownWrenchContact(FULL_WRENCH,Position::Zero())) - * - * @param[in] model the model class for getting frame information. - * @param[in] frameIndex the index of the frame in which you are expressing the new unknown wrench. - * @return true if all went well, false otherwise - */ - bool addNewUnknownFullWrenchInFrameOrigin(const Model& model, - const FrameIndex frameIndex); - - /** - * Get a specific ContactWrench - * - * @param[in] linkIndex the index of the link for which the contact is retrieved - * @param[in] contactIndex a index (between 0 and getNrOfContactsForLink(link)-1 ) identifing the specific contact. - */ - UnknownWrenchContact& contactWrench(const LinkIndex linkIndex, const size_t contactIndex); - - const UnknownWrenchContact& contactWrench(const LinkIndex linkIndex, const size_t contactIndex) const; - - - /** - * Get a human readable description of the LinkUnknownWrenchContacts (for debug) - */ - std::string toString(const Model & model) const; -}; - -struct estimateExternalWrenchesBuffers -{ - estimateExternalWrenchesBuffers(); - estimateExternalWrenchesBuffers(const SubModelDecomposition& subModels); - estimateExternalWrenchesBuffers(const size_t nrOfSubModels, const size_t nrOfLinks); - - /** - * Resize the struct for the number of submodel - */ - void resize(const SubModelDecomposition& subModels); - void resize(const size_t nrOfSubModels, const size_t nrOfLinks); - - size_t getNrOfSubModels() const; - size_t getNrOfLinks() const; - - /** - * Check if the buffer size are consistent with the submodel - * decomposition. - */ - bool isConsistent(const SubModelDecomposition& subModels) const; - - /** - * The problem of external wrenches estimation boils down to - * solve a LS problem in the form argmin_x (Ax-b)^2 . - */ - std::vector A; - std::vector x; - std::vector b; - - /** - * We compute the b term for each subtree - * in a iterative way, so we need a buffer - * to store it for each link - */ - LinkWrenches b_contacts_subtree; - - /** - * We compute the transform between each link - * and the submodel base, for computing the - * A matrices - */ - LinkPositions subModelBase_H_link; -}; - -/** - * \brief Estimate the external contact wrenches using the MultiBody Newton-Euler equations. - * - * This function is used to estimate the external contacts forces **without** using any measurement - * of the internal FT sensors. It is tipically used to get data for calibrating the offset of - * the internal FT sensors. - */ -bool estimateExternalWrenchesWithoutInternalFT(const Model& model, - const Traversal& traversal, - const LinkUnknownWrenchContacts & unknownWrenches, - const JointPosDoubleArray & jointPos, - const LinkVelArray & linkVel, - const LinkAccArray & linkProperAcc, - estimateExternalWrenchesBuffers & bufs, - LinkContactWrenches & outputContactWrenches); - -/** - * \brief Estimate the external wrenches trasmitted by the contacts between the model and the external environment. - * - * This function exploits the measurements of internal FT sensors (whose structure is contained - * in the sensors parameters and which measurements are contained in the ftSensorsMeasurements - * parameters) to compute an estimation of the values of the unknown wrenches specified in the - * unknownWrenches parameter. - * - * @param[in] model the considered model. - * @param[in] subModels a decomposition of the model along the joint of the six axis F/T sensors. - * @param[in] sensors a description of the sensors available in the model. - * @param[in] unknownWrenches a description of the contacts for which the contact wrench is unknown. - * @param[in] linkVel a vector of link twists, expressed w.r.t to the link orientation and the link origin - * @param[in] linkProperAcc a vector of link spatial (in the Featherstone sense) and proper accelerations, expressed w.r.t to the link orientation and the link origin - * @param[in] ftSensorsMeasurements the measurements of the internal six axis F/T sensors. - * @param[out] outputContactWrenches the estimated contact wrenches. - * @return true if all went well (the dimension of the inputs are consistent), false otherwise - * - */ -bool estimateExternalWrenches(const Model& model, - const SubModelDecomposition& subModels, - const SensorsList& sensors, - const LinkUnknownWrenchContacts & unknownWrenches, - const JointPosDoubleArray & jointPos, - const LinkVelArray & linkVel, - const LinkAccArray & linkProperAcc, - const SensorsMeasurements & ftSensorsMeasurements, - estimateExternalWrenchesBuffers & bufs, - LinkContactWrenches & outputContactWrenches); - -/** - * \brief Modified forward kinematics for torque/force estimation. - * - * This is a version of forward kinematics modified to fit the needs - * of joint torques/external wrenches estimation. - * - * There are several difference with respect to the classical - * forward kinematics. - * The first one is that the only inputs necessary related to the base link are - * the base link classical proper acceleration, the base link angular velocity - * and the base link angular acceleration. This is because the dynamics - * of an articulated system does not depend on an offset in linear velocity, and hence - * the estimation of joint torques/external wrenches is not affected by the base link - * linear velocity. This will mean that the link velocitity computed by this - * algorithm are not the velocity of the links with respect to an inertial frame. - * Nevertherless they can still be used for estimation. - * - * There are two main ways in which the base information is computed: one is - * exploiting the knoledge that a link is not moving with respect to an inertial frame: - * in this case the classical proper acceleration boils down to the inverted gravitational - * acceleration, while the angular velocity and angular accelerations are equal to zero. - * The other way is to exploit the measure of an accelerometer and of a gyroscope - * mounted on the base link of the traversal: the accelerometer will then measure - * directly the classical proper acceleration, while the gyroscope will measure the angular velocity. - * The angular acceleration can be computed by numerical derivation, or simply neglected if its - * effect on the estimation is minimal. - * - * - * \param[in] model the input model - * \param[in] traversal the traversal used to propagate the velocity and the proper acceleration - * \param[in] base_classicalProperAcc classical proper acceleration of the base origin - * \param[in] base_angularVel angular velocity of the base link frame - * \param[in] base_angularAcc angular acceleration of the base link frame - * \param[in] jointPos joint positions - * \param[in] jointVel joint velocities - * \param[in] jointAcc joint accelerations - * \param[out] linkVel vector of link twists, expressed in the link frame for both orientation and origin - * \param[out] linkProperAcc vector of link proper spatial acceleration, expressed in the link frame for both orientation and origin - * @return true if all went well, false otherwise - * - */ -bool dynamicsEstimationForwardVelAccKinematics(const Model & model, - const Traversal & traversal, - const Vector3 & base_classicalProperAcc, - const Vector3 & base_angularVel, - const Vector3 & base_angularAcc, - const JointPosDoubleArray & jointPos, - const JointDOFsDoubleArray & jointVel, - const JointDOFsDoubleArray & jointAcc, - LinkVelArray & linkVel, - LinkAccArray & linkProperAcc); - -/** - * \brief Modified forward kinematics for floating basedynamics estimation. - * - * This is a version of velocity forward kinematics modified to fit the needs - * of free floating dynamics estimation. - * - * There are several difference with respect to the classical - * forward kinematics. - * The first one is that the only inputs necessary related to the base link is - * the base link angular velocity. This is because the dynamics - * of an articulated system does not depend on an offset in linear velocity. - * This will mean that the link velocities computed by this - * algorithm are not the velocity of the links with respect to an inertial frame. - * Nevertherless they can still be used for estimation. - * - * There are two main ways in which the base information is computed: one is - * exploiting the knoledge that a link is not moving with respect to an inertial frame: - * in this case the angular velocity is equal to zero. - * The other way is to exploit the measure of a gyroscope - * mounted on the base link of the traversal: the gyroscope will measure the link angular velocity. - * - * - * \param[in] model the input model - * \param[in] traversal the traversal used to propagate the velocity and the proper acceleration - * \param[in] base_angularVel angular velocity of the base link frame - * \param[in] jointPos joint positions - * \param[in] jointVel joint velocities - * \param[out] linkVel vector of link twists, expressed in the link frame for both orientation and origin - * @return true if all went well, false otherwise - */ -bool dynamicsEstimationForwardVelKinematics(const Model & model, - const Traversal & traversal, - const Vector3 & base_angularVel, - const JointPosDoubleArray & jointPos, - const JointDOFsDoubleArray & jointVel, - LinkVelArray & linkVel); - -/** - * \brief Compute the net internal and external wrenches (excluding gravity forces) acting on the links. - * @param[in] model the input model - * @param[in] linkVel a vector of link twists, expressed w.r.t to the link orientation and the link origin - * @param[in] linkProperAcc a vector of link spatial (in the Featherstone sense) and proper accelerations, expressed w.r.t to the link orientation and the link origin - * @param[in] linkNetWrenchesWithoutGravity the vector of the sum of all the wrenches (both internal and external, excluding gravity) acting on link i, expressed (both orientation and point) with respect to the reference frame of link i - */ -bool computeLinkNetWrenchesWithoutGravity(const Model& model, - const LinkVelArray & linkVel, - const LinkAccArray & linkProperAcc, - LinkNetTotalWrenchesWithoutGravity& linkNetWrenchesWithoutGravity); - -/** - * Compute the link contact wrenches from the net external wrenches - * - * If there are more than 6 unknows for link, the problem becomes ill-defined - * and the function just assign all the external wrench to the first contact. - * \todo(traversaro): support arbitrary LinkUnknownWrenchContacts by performing - * a least square fitting, similar to what implemented in - * the estimateExternalWrenches . - */ -bool estimateLinkContactWrenchesFromLinkNetExternalWrenches(const Model& model, - const LinkUnknownWrenchContacts& unknownWrenches, - const LinkNetExternalWrenches& netExtWrenches, - LinkContactWrenches & outputContactWrenches); +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif -} +#include #endif diff --git a/src/estimation/include/iDynTree/Estimation/GravityCompensationHelpers.h b/src/estimation/include/iDynTree/Estimation/GravityCompensationHelpers.h index 919c6d934ca..874a556e07f 100644 --- a/src/estimation/include/iDynTree/Estimation/GravityCompensationHelpers.h +++ b/src/estimation/include/iDynTree/Estimation/GravityCompensationHelpers.h @@ -1,126 +1,14 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef GRAVITY_COMPENSATION_HELPERS_H -#define GRAVITY_COMPENSATION_HELPERS_H +#ifndef IDYNTREE_ESTIMATION_GRAVITY_COMPENSATION_HELPERS_H +#define IDYNTREE_ESTIMATION_GRAVITY_COMPENSATION_HELPERS_H -#include -#include -#include -#include - -#include -#include -#include +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif -#include +#include -namespace iDynTree -{ - /** - * @brief Class computing the gravity compensation torques - * using accelerometer measurements - * - * @note the estimation of the gravity assumes - * the non-gravitational accelerations measured by the - * accelerometer as negligible - * - */ - class GravityCompensationHelper - { - public: - /** - * @brief Default Constructor - */ - GravityCompensationHelper(); - - /** - * @brief Destructor - */ - ~GravityCompensationHelper(); - - /** - * @brief Load model - * @return true if successful, false otherwise - */ - bool loadModel(const iDynTree::Model& _model, const std::string dynamicBase); - - /** - * @brief Set the kinematic information necessary for the gravity torques estimation using - * the proper acceleration coming from an accelerometer - * - * @note the estimation of the gravity assumes the non-gravitational accelerations - * measured by the accelerometer as negligible. - * - * @param[in] jointPos the position of the joints in the model - * @param[in] floatingFrame the frame index for which proper acceleration is provided - * @param[in] properClassicalLinearAcceleration proper (actual acceleration - gravity) - * classical acceleration of the origin of the - * specified frame, expresssed in the specified - * frame orientation - * - * @return true if successful, false otherwise - */ - bool updateKinematicsFromProperAcceleration(const iDynTree::JointPosDoubleArray& jointPos, - const iDynTree::FrameIndex& floatingFrame, - const iDynTree::Vector3& properClassicalLinearAcceleration); - - /** - * @brief Set the kinematic information necessary for the gravity torques estimation using - * the assumed known gravity vector on frame - * - * @note This is implemented as updateKinematicsFromProperAcceleration(jointPos, floatingFrame, -gravity); - * - * @param[in] jointPos the position of the joints in the model - * @param[in] floatingFrame the frame index for which gravity vector is provided - * @param[in] properClassicalLinearAcceleration gravity acceleration of the origin of the - * specified frame, expresssed in the specified - * frame orientation - * - * @return true if successful, false otherwise - */ - bool updateKinematicsFromGravity(const iDynTree::JointPosDoubleArray& jointPos, - const iDynTree::FrameIndex& floatingFrame, - const iDynTree::Vector3& gravity); - - /** - * @brief Get the gravity compensation torques - * @return true if successful, false otherwise - */ - bool getGravityCompensationTorques(iDynTree::JointDOFsDoubleArray& jointTrqs); - - private: - /** - * @brief Helper function for kinematic traversal dynamic allocation - * @param[in] nrOfLinks number of links to be allocated in the traversal - */ - void allocKinematicTraversals(const size_t nrOfLinks); - - /** - * @brief Helper function for kinematic traversal dynamic deallocation - */ - void freeKinematicTraversals(); - - bool m_isModelValid; ///< flag to check validity of the model - bool m_isKinematicsUpdated; ///< flag to check if kinematics of the robot is updated - iDynTree::Model m_model; ///< robot model for gravity compensation estimation - - iDynTree::Traversal m_dynamicTraversal; ///< Traversal used for dynamic computations - - /** - * Vector of Traversal used for the kinematic computations. - * m_kinematicTraversals[l] contains the traversal with base link l . - */ - std::vector m_kinematicTraversals; - - iDynTree::JointPosDoubleArray m_jointPos; ///< joint positions - iDynTree::JointDOFsDoubleArray m_jointDofsZero; ///< zero DOFArrays for joint velocities and joint accelerations - iDynTree::LinkVelArray m_linkVels; ///< link velocities - iDynTree::LinkAccArray m_linkProperAccs; ///< link proper accelerations - iDynTree::LinkNetExternalWrenches m_linkNetExternalWrenchesZero; ///< link external wrenches set to zero - iDynTree::LinkInternalWrenches m_linkIntWrenches; ///< link internal wrenches - iDynTree::FreeFloatingGeneralizedTorques m_generalizedTorques; ///< generalized torques - }; -} #endif diff --git a/src/estimation/include/iDynTree/Estimation/KalmanFilter.h b/src/estimation/include/iDynTree/Estimation/KalmanFilter.h index fbb15db2a94..c72156135c2 100644 --- a/src/estimation/include/iDynTree/Estimation/KalmanFilter.h +++ b/src/estimation/include/iDynTree/Estimation/KalmanFilter.h @@ -1,231 +1,13 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef KALMAN_FILTER_H -#define KALMAN_FILTER_H +#ifndef IDYNTREE_ESTIMATION_KALMAN_FILTER_H +#define IDYNTREE_ESTIMATION_KALMAN_FILTER_H -#include -#include -#include -#include -#include - -namespace iDynTree -{ - /** - * @class DiscreteKalmanFilterHelper Time Invariant Discrete Kalman Filter with additive Gaussian noise - * - * The Kalman filter can be constructed by giving the system design matrices A, B, C and D. - * where A is the state transition matrix, B is the control input matrix, - * C is the output matrix and D the feed through matrix. - * These matrices can be used to describe a stochastic model for a linear dynamical system. - * - * \f[ x_{k+1} = A x_{k} + B u_{k} + w_k \f] - * \f[ y_{k+1} = C x_{k+1} + D u_{k} + v_k \f] - * - * Once the filter is constructed, the system noise and measurement noise covariance matrices can be set. - * The filter can be run, after setting initial state and state covariance matrix. - * The filter init method is called to check if the filter is properly configured and is ready to use. - * - * @warning care must be taken to design these matrices with proper dimensions, so that the filter does not crash - * - * Once the filter is configured, the filter algorithm can be run in a loop by, - * - setting input vector - * - prediction step which propagates the state through the modeled state dynamics - * - setting the measurement vector - * - correct the predicted states with the incoming measurements - * - */ - class DiscreteKalmanFilterHelper - { - public: - DiscreteKalmanFilterHelper(); - - /** - * @brief Describes the state propagation for a given dynamical system - * and the measurement model given the available measurements. - * If state of the system is denoted by \f$ x \f$, the control input by \f$ u \f$ - * and the measurements by \f$ y \f$, then - * the system dynamics is given as \f$ x_{k+1} = A x_{k} + B u_{k} + w_k \f$ - * and the measurement model is given by \f$ y_{k+1} = C x_{k+1} + D u_{k} + v_k \f$ - * - * @note all matrices are assumed to be time-invariant - * @param[in] A state transition matrix - * @param[in] B control input matrix mapping inputs to states - * @param[in] C output matrix mapping states to measurements - * @param[in] D feed through matrix mapping inputs to measurements - * @return bool true/false if filter was constructed successfully or not - */ - bool constructKalmanFilter(const iDynTree::MatrixDynSize& A, - const iDynTree::MatrixDynSize& B, - const iDynTree::MatrixDynSize& C, - const iDynTree::MatrixDynSize& D); - /** - * @overload - */ - bool constructKalmanFilter(const iDynTree::MatrixDynSize& A, - const iDynTree::MatrixDynSize& B, - const iDynTree::MatrixDynSize& C); - /** - * @overload - */ - bool constructKalmanFilter(const iDynTree::MatrixDynSize& A, - const iDynTree::MatrixDynSize& C); - - /** - * @brief Set initial state of the Kalman filter - * @warning this method must be called before calling kfInit() - * @param[in] x0 initial state of dimension \f$ dim_x \times 1 \f$ - * @return bool true/false successful or not - */ - bool kfSetInitialState(const iDynTree::VectorDynSize& x0); - - /** - * @brief Sets the state covariance matrix - * @warning this method must be called before calling kfInit() - * @param[in] P state covariance matrix of dimensions \f$ dim_x \times dim_x \f$ - * @return bool true/false successful or not - */ - bool kfSetStateCovariance(const iDynTree::MatrixDynSize& P); - - /** - * @brief Sets the system noise covariance matrix - * @warning this method must be called before calling kfInit() - * @param[in] Q system noise covariance matrix of dimensions \f$ dim_x \times dim_x \f$ - * @return bool true/false successful or not - */ - bool kfSetSystemNoiseCovariance(const iDynTree::MatrixDynSize& Q); - - /** - * @brief Sets the measurement noise covariance matrix - * @warning this method must be called before calling kfInit() - * @param[in] R measurement covariance matrix of dimensions \f$ dim_y \times dim_y \f$ - * @return bool true/false successful or not - */ - bool kfSetMeasurementNoiseCovariance(const iDynTree::MatrixDynSize& R); - - /** - * @brief This method checks if the filter is properly constructed and configured - * i.e. if initial states and covariance matrices are set. - * @warning this method must be called before calling kfPredict() or kfUpdate() - * @return bool true/false successful or not - */ - bool kfInit(); - - /** - * @brief Set inputs for the Kalman filter - * - * @param[in] u input vector of dimension \f$ dim_u \times 1 \f$ - * @return bool true/false successful or not - */ - bool kfSetInputVector(const iDynTree::VectorDynSize& u); - - /** - * @brief Runs one step of the Discrete Kalman Filter prediction equation - * described by - * \f$ \hat{x}_{k+1} = A x_{k} + B u_{k} \f$ - * \f$ \hat{P}_{k+1} = A_k P_k A_k^T + Q \f$ - * - * @warning this function can be called only after setting up the filter properly through kfInit() step - * @note in case if B matrix is constructed, this function should be once called every step, - * after setting up the input vector using kfSetInputVector() method. - * - * @return bool true/false if successful or not - */ - bool kfPredict(); - - /** - * @brief Set measurements for the Kalman filter - * - * @param[in] y input vector of dimension \f$ dim_y \times 1 \f$ - * @return bool true/false successful or not - */ - bool kfSetMeasurementVector(const iDynTree::VectorDynSize& y); - - /** - * @brief Runs one step of the Discrete Kalman Filter update equation - * described by - * \f$ \tilde{y}_{k+1} = C \hat{x}_{k+1} + D u_{k} \f$ - * \f$ x_{k+1} = \hat{x}_{k+1} + K_{k+1}(\tilde{y}_{k+1} - z_{k+1}) \f$ - * \f$ P_{k+1} = (I - K_{k+1} C) \hat{P}_{k+1} \f$ - * where K is the Kalman gain. - * @warning this function can be called only after setting up the filter properly through kfInit() step - * @note this function should be once called every step, - * after setting up the measurements vector using kfSetMeasurementVector() method. - * - * @return bool true/false if successful or not - */ - bool kfUpdate(); - - /** - * @brief Get system state - * - * @param[out] x system state of dimension \f$ dim_x \times 1 \f$ - * @return bool true/false successful or not - */ - bool kfGetStates(iDynTree::VectorDynSize &x); - - /** - * @brief Get system state covariance - * - * @param[out] P system state covariance matrix of dimension \f$ dim_x \times dim_x \f$ - * @return bool true/false successful or not - */ - bool kfGetStateCovariance(iDynTree::MatrixDynSize &P); - - /** - * @brief Reset Kalman filter - * Resets the Kalman filter and initializes with the internally stored states and - * matrices initially set by the user. - */ - bool kfReset(); - - /** - * @brief Reset Kalman filter with the given arguments - * @warning the system matrices A, B, C and D are unchanged with the reset filter - * - * @param[in] x0 initial state vector of dimensions \f$ dim_x \times 1 \f$ - * @param[in] P state covariance matrix of dimensions \f$ dim_x \times dim_x \f$ - * @param[in] Q system noise covariance matrix of dimensions \f$ dim_x \times dim_x \f$ - * @param[in] R measurement noise covariance matrix of dimensions \f$ dim_y \times dim_y \f$ - */ - bool kfReset(const iDynTree::VectorDynSize& x0, const iDynTree::MatrixDynSize& P0, - const iDynTree::MatrixDynSize& Q, const iDynTree::MatrixDynSize& R); - - private: - size_t m_dim_X; ///< state dimension - size_t m_dim_Y; ///< output dimenstion - size_t m_dim_U; ///< input dimension - - iDynTree::VectorDynSize m_x; ///< state at time instant k - iDynTree::VectorDynSize m_x0; ///< buffer to store initial state - iDynTree::VectorDynSize m_u; ///< input at time instant k - iDynTree::VectorDynSize m_y; ///< measurements at time instant k - - iDynTree::MatrixDynSize m_A; ///< state transition matrix - iDynTree::MatrixDynSize m_B; ///< control input matrix - iDynTree::MatrixDynSize m_C; ///< output matrix - iDynTree::MatrixDynSize m_D; ///< feedthrough matrix - - iDynTree::MatrixDynSize m_P; ///< state covariance matrix - iDynTree::MatrixDynSize m_P0; ///< initial state covariance matrix - iDynTree::MatrixDynSize m_Q; ///< system noise covariance matrix - iDynTree::MatrixDynSize m_R; ///< measurement covariance matrix - - bool m_is_initialized{false}; ///< flag to check if filter is initialized - - bool m_filter_constructed{false}; ///< flag to check if the filter is constructed properly with the A, B, C, D matrices - bool m_initial_state_set{false}; ///< flag to check if initial state is set - bool m_initial_state_covariance_set{false}; ///< flag to check if initial state covariance matrix is set - bool m_measurement_noise_covariance_matrix_set{false}; ///< flag to check if measurement noise covariance matrix is set - bool m_system_noise_covariance_matrix_set{false}; ///< flag to check if system noise covariance matrix is set - - bool m_measurement_updated{false}; ///< flag to check if measurement is updated - bool m_input_updated{false}; ///< flag to check if input is updated +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif - bool m_use_feed_through{false}; ///< toggle to use feed through matrix D - bool m_use_control_input{false}; ///< toggle to use control input matrix B - }; -} +#include #endif diff --git a/src/estimation/include/iDynTree/Estimation/SchmittTrigger.h b/src/estimation/include/iDynTree/Estimation/SchmittTrigger.h index 59f451f9d3b..a6aa8c95674 100644 --- a/src/estimation/include/iDynTree/Estimation/SchmittTrigger.h +++ b/src/estimation/include/iDynTree/Estimation/SchmittTrigger.h @@ -1,175 +1,13 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef IDYNTREE_SCHMITT_TRIGGER_H -#define IDYNTREE_SCHMITT_TRIGGER_H +#ifndef IDYNTREE_ESTIMATION_SCHMITT_TRIGGER_H +#define IDYNTREE_ESTIMATION_SCHMITT_TRIGGER_H -#include -namespace iDynTree -{ - /** - * Schmitt Trigger class for binary state detection - * This device is used to obtain a binary state (ON/OFF) at - * some time instant depending on a value inputted to the device. - * It is initialized with 4 parameters, - * - * | Parameter | Type | Description | - * |:----------------:|:----:|:--------------------------------------------------------------------:| - * |stableOFFTime |double|Time to elapse to switch to OFF state, once low threshold is triggered| - * |stableONTime |double|Time to elapse to switch to ON state, once high threshold is triggered| - * |lowValueThreshold |double| Threshold Value to turn state OFF | - * |highValueThreshold|double| Threshold Value to turn state ON | - * - * The device is first configured with the user-defined parameter settings, but is not activated until - * the first call of updateDevice method. The updateDevice method must be called at every iteration with - * the current time and the value to be compared, for continuously updating the binary state of the device. - * - * For instance, if the current state is OFF and there is a call to updateDevice with current time and an input - * value greater than the highValueThreshold, then a timer is activated and is updated at every call to updateMethod. - * If there are consecutive calls to the updateMethod with input values greater than the highValueThreshold, - * until the timer lapses stableONTime, then the state is switched to ON, otherwise it is remains OFF. - * - * NOTE: Time is specified relative to the process that is instantiating this device. - * Default state is set to ON during instantiation. This can be changed accordingly using the setInitialState method. - * - * NOTE: There are no default parameters to the Schmitt Trigger. These parameters are set through - * the constructor during instantiation. - */ - class SchmittTrigger - { - public: - /** - * Constructor - * @param stableOFFTime time to elapse to switch to OFF state - * @param stableONTime time to elapse to switch to ON state - * @param lowValueThreshold threshold value to turn state OFF - * @param highValueThreshold threshold value to turn state ON - */ - SchmittTrigger(double stableOFFTime, - double stableONTime, - double lowValueThreshold, - double highValueThreshold); - - /** - * Sets the schmitt trigger settings to default values - */ - void resetDevice(); - - /** - * Update the device state with the latest time and input measurement - * @param currentTime current time - * @param rawValue input measurement - */ - void updateDevice(double currentTime, double rawValue); - - /** - * @name Setters - */ - //@{ - - /** - * Configures the Schmitt Trigger parameters - * (can be called after instantiation, to change the parameters) - * @param stableOFFTime time to elapse to switch to OFF state - * @param stableONTime time to elapse to switch to ON state - * @param lowValueThreshold threshold value to turn state OFF - * @param highValueThreshold threshold value to turn state ON - */ - void configure(double stableOFFTime, - double stableONTime, - double lowValueThreshold, - double highValueThreshold); - - /** - * set required time to elapse to switch to OFF state - * @param stableOFFTime - */ - void setStableOFFTime(double stableOFFTime) { m_stableOFFTime = stableOFFTime; } - - /** - * set required time to elapse to switch to ON state - * @param stableONTime - */ - void setStableONTime(double stableONTime) { m_stableONTime = stableONTime; } +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif - /** - * set low threshold value to trigger OFF state detection - * @param lowValueThreshold - */ - void setLowValueThreshold(double lowValueThreshold) { m_lowValueThreshold = lowValueThreshold; } - - /** - * set high threshold value to trigger ON state detection - * @param highValueThreshold - */ - void setHighValueThreshold(double highValueThreshold) { m_highValueThreshold = highValueThreshold; } - - /** - * set initial state - * @param state binary state true/false - */ - void setInitialState(bool state) { m_currentState = state; } - //@} - - /** - * @name Getters - */ - //@{ - - /** - * get current state - * @return binary state true/false - */ - bool getState() { return m_currentState; } - - /** - * get time elapsed since the first update of the device - * @return time - */ - double getElapsedTime() { return m_previousTime; } - //@} - - - /** - * @name Verbose - */ - //@{ - void setVerbose() {m_verbose = 1;} - void unsetVerbose() {m_verbose = 0;} - //@} - private: - - /** - * @name State - */ - //@{ - bool m_currentState; - double m_previousTime; - double m_timer; - //@} - - /** - * @name Device Parameters - */ - //@{ - double m_stableOFFTime; - double m_stableONTime; - double m_highValueThreshold; - double m_lowValueThreshold; - //@} - - /** - * @name Input - */ - //@{ - double m_rawValue; - //@} - - - // Verbose flag - int m_verbose; - - }; -} +#include #endif \ No newline at end of file diff --git a/src/estimation/include/iDynTree/Estimation/SimpleLeggedOdometry.h b/src/estimation/include/iDynTree/Estimation/SimpleLeggedOdometry.h index 6f522d17306..c2e7e54adad 100644 --- a/src/estimation/include/iDynTree/Estimation/SimpleLeggedOdometry.h +++ b/src/estimation/include/iDynTree/Estimation/SimpleLeggedOdometry.h @@ -1,243 +1,13 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef IDYNTREE_SIMPLE_LEGGED_ODOMETRY2_ -#define IDYNTREE_SIMPLE_LEGGED_ODOMETRY2_ +#ifndef IDYNTREE_ESTIMATION_SIMPLE_LEGGED_ODOMETRY2_ +#define IDYNTREE_ESTIMATION_SIMPLE_LEGGED_ODOMETRY2_ -#include -#include -#include -#include -#include -#include +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif -namespace iDynTree -{ +#include -/** - * \ingroup iDynTreeEstimation - * - * A simple legged odometry for a legged robot. - * - * Under the assumption that at least a link of the robot at the time is - * not moving (no slippage), it computes the estimate of the transform - * between a inertial/world frame and the robot floating base. - * - * The algorithm implemented is the following : - * - * -# During initialization the user of the class specifies (through init()): - * * a frame that is rigidly attached to a link that is not moving (`initialFixedFrame`) - * * an optional transform between the desired location of the world and the fixed frame (`world_H_initialFixedFrame`) - * At the beginning, the `world_H_fixed` is the one specified (default is the identity) - * - * -# At this point, the `getWorldFrameTransform(iDynTree::FrameIndex frame_id)` will return the `world_H_frame` - * ( \f${}^{world} H_{frame}\f$ ) transform simply by computing the forward kinematics from the fixed frame - * to the frame specified by `frame_id` : `world_H_frame = world_H_fixed * fixed_H_frame(qj)`, i.e. - * \f${}^{world} H_{frame} = {}^{world} H_{fixed} \cdot {}^{fixed} H_{frame}(q_j)\f$ - * - * -# If the fixed frame changes, we can simply change the frame used as "fixed" (changeFixedLink()), and consistently update the - * `world_H_fixed` transform to be equal to `world_H_new_fixed = world_H_old_fixed * old_fixed_H_new_fixed(qj)`, i.e. - * \f${}^{world} H_{new\_fixed} = {}^{world} H_{old\_fixed} \cdot {}^{old\_fixed} H_{new\_fixed}(q_j)\f$ - * - * -# After the update, the `getWorldFrameTransform(iDynTree::FrameIndex frame_id)` can be obtained as in point 1b . - * - * To reset the location of the world, init can simply be called again. - * - */ -class SimpleLeggedOdometry -{ - /** - * Model used in odometry. - */ - Model m_model; - - /** - * Traversal used for the dynamics computations - */ - Traversal m_traversal; - - /** - * False initially, true after a valid model has been loaded. - */ - bool m_isModelValid; - - /** - * False initially, true after updateKinematics was successfully called. - */ - bool m_kinematicsUpdated; - - /** - * False initially, true after init was successfully called. - */ - bool m_isOdometryInitialized; - - /** - * The link index of the link considered fixed. - */ - LinkIndex m_fixedLinkIndex; - - /** - * Position of each link with respect to the base - * link of the traversal (i.e. output - * of the relative forward kinematics). - */ - LinkPositions m_base_H_link; - - /** - * Transform between the link currently considered fixed - * and the world/inertial frame. - * This is initialized in the init method and update - * by the changeFixedLink method. - */ - Transform m_world_H_fixedLink; - -public: - /** - * Constructor - */ - SimpleLeggedOdometry(); - - /** - * Destructor - */ - ~SimpleLeggedOdometry(); - - /** - * \brief Set model used for the odometry. - * The model is copied inside the class, to be used for the odometry estimation. - * - * @param[in] _model the kinematic and dynamic model used for the estimation. - * @return true if all went well (model is well formed), false otherwise. - */ - bool setModel(const Model & _model); - - /** - * Get used model. - * - * @return the kinematic and dynamic model used for estimation. - */ - const Model & model() const; - - /** - * Set the measured joint positions. - * - * This is used to update the joints positions used by the odometry. - * - */ - bool updateKinematics(JointPosDoubleArray & jointPos); - - /** - * Initialize the odometry. - * This method initializes the world location w.r.t. to a frame - * that is not the frame that is initially assumed fixed. - * For this reason it is necessary to call updateKinematics at least once before calling this method. - * - * @param[in] initialFixedFrame Frame initially assumed to be fixed. - * @param[in] initialFixedFrame_H_world Pose of the world w.r.t. the initial fixed frame (default: identity, i.e. the initialFixedFrame is the world). - * @return true if all went well, false otherwise. - */ - bool init(const std::string & initialFixedFrame, - const Transform initialFixedFrame_H_world = Transform::Identity()); - - /** - * Initialize the odometry. - * This method initializes the world location w.r.t. to a frame - * that is not the frame that is initially assumed fixed. - * For this reason it is necessary to call updateKinematics at least once before calling this method. - * - * @param[in] initialFixedFrameIndex Frame initially assumed to be fixed. - * @param[in] initialFixedFrame_H_world Pose of the world w.r.t. the initial fixed frame (default: identity, i.e. the initialFixedFrame is the world). - * @return true if all went well, false otherwise. - */ - bool init(const FrameIndex initialFixedFrameIndex, - const Transform initialFixedFrame_H_world = Transform::Identity()); - - /** - * Initialize the odometry, specifying separately initial fixed frame and world. - * This method initializes the world location w.r.t. to a frame - * that is not the frame that is initially assumed fixed, - * for this reason it is necessary to call updateKinematics at least once before calling this method. - * - * @param[in] initialFixedFrame Frame initially assumed to be fixed. - * @param[in] initialReferenceFrameForWorld Frame in which the initial world is expressed. - * @param[in] initialReferenceFrame_H_world Pose of the world w.r.t. the initial reference frame (default: identity, i.e. the initialReferenceFrameForWorld is the world). - * @return true if all went well, false otherwise. - */ - bool init(const std::string & initialFixedFrame, - const std::string & initialReferenceFrameForWorld, - const Transform initialReferenceFrame_H_world = Transform::Identity()); - - /** - * Initialize the odometry, specifying separately initial fixed frame and world. - * This method initializes the world location w.r.t. to a frame - * that is not the frame that is initially assumed fixed, - * for this reason it is necessary to call updateKinematics at least once before calling this method. - * - * @param[in] initialFixedFrameIndex Frame initially assumed to be fixed. - * @param[in] initialReferenceFrameIndexForWorld Frame in which the initial world is expressed. - * @param[in] initialReferenceFrame_H_world Pose of the world w.r.t. the initial reference frame (default: identity, i.e. the initialReferenceFrameForWorld is the world). - * @return true if all went well, false otherwise. - */ - bool init(const FrameIndex initialFixedFrameIndex, - const FrameIndex initialReferenceFrameIndexForWorld, - const Transform initialReferenceFrame_H_world = Transform::Identity()); - - /** - * Change the link that the odometry assumes to be fixed with - * respect to the inertial/world frame. - */ - bool changeFixedFrame(const std::string & newFixedFrame); - - /** - * Change the link that the odometry assumes to be fixed - * with respect to the inertial/world frame. - */ - bool changeFixedFrame(const FrameIndex newFixedFrame); - - /** - * Change the link that the odometry assumes to be fixed - * with respect to the inertial/world frame. - * - * \note The position of the external frame is set by the user - */ - bool changeFixedFrame(const std::string & newFixedFrame, - const Transform & world_H_newFixedFrame); - - /** - * Change the link that the odometry assumes to be fixed - * with respect to the inertial/world frame. - * - * \note The position of the external frame is set by the user - */ - bool changeFixedFrame(const FrameIndex newFixedFrame, - const Transform & world_H_newFixedFrame); - - /** - * Get the link currently considered fixed with respect to the inertial frame. - * - * \note This can be diffent from what was set with changeFixedFrame, because - * multiple frames can belong to the same link. - * - * @return the name of the link currently considered fixed. - */ - std::string getCurrentFixedLink(); - - /** - * Get the world_H_link transform for an arbitrary link. - * - * \note this method works only for link, not for arbitrary frames. - */ - iDynTree::Transform getWorldLinkTransform(const LinkIndex frame_index); - - /** - * Get the world_H_frame transform for an arbitrary frame. - * - * \note this method works also for arbitrary frames. - */ - iDynTree::Transform getWorldFrameTransform(const FrameIndex frame_index); -}; - - -} // End namespace iDynTree - -#endif // IDYNTREE_SIMPLE_LEGGED_ODOMETRY2_ +#endif diff --git a/src/estimation/include/iDynTree/ExtWrenchesAndJointTorquesEstimator.h b/src/estimation/include/iDynTree/ExtWrenchesAndJointTorquesEstimator.h new file mode 100644 index 00000000000..fbc5828f11b --- /dev/null +++ b/src/estimation/include/iDynTree/ExtWrenchesAndJointTorquesEstimator.h @@ -0,0 +1,366 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_ESTIMATION_EXTWRENCHESANDJOINTTORQUEESTIMATOR_H +#define IDYNTREE_ESTIMATION_EXTWRENCHESANDJOINTTORQUEESTIMATOR_H + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace iDynTree +{ + +/** + * \brief Estimator for external wrenches and joint torques using internal F/T sensors. + * + * This is a class for estimating external wrenches and joint torques using as an input + * the robot velocities and the accelerations and the measurement of internal six axis + * F/T sensors. + * + * The kinematic information (position,velocity and acceleration) necessary for the + * estimation can be provided in two ways: + * * using the updateKinematicsFromFloatingBase + * * using the updateKinematicsFromFixedBase + * + * Note that in both ways there is no need (for the estimation) to provide the + * absolute position and linear velocity of the robot with respect to the inertial frame. + * The effect of gravity is considered by directly using the proper acceleration in the case + * of the floating frame (proper acceleration can be directly measured by an accelerometer) + * or by directly providing the gravity vector in the fixed frame case. + * + * Beside its main goal of estimation of external wrenches and joint torques, the class + * also provide methods that can be useful to calibrate the six-axis FT sensors of the robot. + * These methods are: + * * ExtWrenchesAndJointTorquesEstimator::computeExpectedFTSensorsMeasurements + * * ExtWrenchesAndJointTorquesEstimator::computeSubModelMatrixRelatingFTSensorsMeasuresAndKinematics + * Details of each method can be found in the method documentation + */ +class ExtWrenchesAndJointTorquesEstimator +{ + /** + * Structure variables. + */ + Model m_model; + SubModelDecomposition m_submodels; + SensorsList m_sensors; + bool m_isModelValid; + bool m_isKinematicsUpdated; + + /**< Traveral used for the dynamics computations */ + Traversal m_dynamicTraversal; + + /** + * Vector of Traversal used for the kinematic computations. + * m_kinematicTraversals.getTraversalWithLinkAsBase(l) contains the traversal with base link l . + */ + LinkTraversalsCache m_kinematicTraversals; + + JointPosDoubleArray m_jointPos; + LinkVelArray m_linkVels; + LinkAccArray m_linkProperAccs; + LinkNetExternalWrenches m_linkNetExternalWrenches; + LinkInternalWrenches m_linkIntWrenches; + FreeFloatingGeneralizedTorques m_generalizedTorques; + + estimateExternalWrenchesBuffers m_calibBufs; + estimateExternalWrenchesBuffers m_bufs; + + /** + * Disable copy constructor and copy operator + */ + ExtWrenchesAndJointTorquesEstimator(const ExtWrenchesAndJointTorquesEstimator & /*other*/) {}; + + /** + * Copy operator is forbidden + */ + ExtWrenchesAndJointTorquesEstimator& operator=(const ExtWrenchesAndJointTorquesEstimator &/*other*/) {return *this;}; + +public: + /** + * \brief Constructor. + */ + ExtWrenchesAndJointTorquesEstimator(); + + /** + * \brief Destructor. + */ + ~ExtWrenchesAndJointTorquesEstimator(); + + /** + * \brief Set model and sensors used for the estimation. + * + * @param[in] _model the kinematic and dynamic model used for the estimation. + * @param[in] _sensors the sensor model used for the estimation. + * @return true if all went well (model and sensors are well formed), false otherwise. + */ + bool setModelAndSensors(const Model & _model, const SensorsList & _sensors); + + /** + * Load model and sensors from file. + * + * @deprecated Use iDynTree::ModelLoader::loadModelFromFile and call setModelAndSensors + * on the parsed Model and SensorsList + * + * @param[in] filename path to the file to load. + * @param[in] filetype (optional) explicit definiton of the filetype to load. + * Only "urdf" is supported at the moment. + * @return true if all went well (files were correctly loaded and consistent), false otherwise. + */ + bool loadModelAndSensorsFromFile(const std::string filename, const std::string filetype=""); + + /** + * Load model and sensors from file, specifieng the dof considered for the estimation. + * + * @note this will create e a reduced model only with the joint specified in consideredDOFs and the + * fixed joints in which FT sensor are mounted. + * + * @param[in] filename path to the file to load. + * @param[in] consideredDOFs list of dof to consider in the model. + * @param[in] filetype (optional) explicit definiton of the filetype to load. + * Only "urdf" is supported at the moment. + * @return true if all went well (files were correctly loaded and consistent), false otherwise. + */ + bool loadModelAndSensorsFromFileWithSpecifiedDOFs(const std::string filename, + const std::vector & consideredDOFs, + const std::string filetype=""); + + + /** + * Get used model. + * + * @return the kinematic and dynamic model used for estimation. + */ + const Model & model() const; + + /** + * Get used sensors. + * + * @return the sensor model used for estimation. + */ + const SensorsList & sensors() const; + + /** + * Get the used submodel decomposition. + * + * @return the used submodel decomposition. + */ + const SubModelDecomposition & submodels() const; + + /** + * Set the kinematic information necessary for the force estimation using the + * acceleration and angular velocity information of a floating frame. + * + * \note Tipically the floating frame information comes from an sensor + * containing a gyroscope (providing the angular velocity) and an + * accelerometer (providing the classical proper acceleration). As + * inertial sensors that return angular acceleration exist but are not common, + * the angular acceleration is usually obtained by numerical derivation + * on the angular velocity measure. In some cases it could make sense + * to just neglet the contribution of the floating frame angular acceleration + * (setting it to a zero vector), if its impact to the dynamics is marginal. + * + * @param[in] jointPos the position of the joints of the model. + * @param[in] jointVel the velocities of the joints of the model. + * @param[in] jointAcc the accelerations of the joints of the model. + * @param[in] floatingFrame the index of the frame for which kinematic information is provided. + * @param[in] properClassicalLinearAcceleration proper (actual acceleration-gravity) classical acceleration + * of the origin of the specified frame, + * expressed in the specified frame orientation. + * @param[in] angularVel angular velocity (wrt to an inertial frame) of the specified floating frame, + * expressed in the specified frame orientation. + * @param[in] angularAcc angular acceleration (wrt to an inertial frame) of the specified floating frame , + * expressed in the specified frame orientation. + * @return true if all went ok, false otherwise. + */ + bool updateKinematicsFromFloatingBase(const JointPosDoubleArray & jointPos, + const JointDOFsDoubleArray & jointVel, + const JointDOFsDoubleArray & jointAcc, + const FrameIndex & floatingFrame, + const Vector3 & properClassicalLinearAcceleration, + const Vector3 & angularVel, + const Vector3 & angularAcc); + + /** + * Set the kinematic information necessary for the force estimation assuming that a + * given frame is not accelerating with respect to the inertial frame. + * + * @param[in] jointPos the position of the joints of the model. + * @param[in] jointVel the velocities of the joints of the model. + * @param[in] jointAcc the accelerations of the joints of the model. + * @param[in] fixedFrame the index of the frame that is not accelerating with respect to the inertial frame. + * @param[in] gravity the gravity acceleration vector, expressed in the specified fixed frame. + * @return true if all went ok, false otherwise. + */ + bool updateKinematicsFromFixedBase(const JointPosDoubleArray & jointPos, + const JointDOFsDoubleArray & jointVel, + const JointDOFsDoubleArray & jointAcc, + const FrameIndex & fixedFrame, + const Vector3 & gravity); + + /** + * \brief Predict FT sensors using the knoledge of external wrenches location. + * + * This function is used to estimate the expected measurement of the FT sensors. + * The typical use of this function is to specify only one external unknown wrench + * in the unknowns parameter and then compute the expected measurements of the FT sensors + * using the kinematic information specified with an updateKinematics*** method. + * The location of the single external unknown wrench is the one of the only link + * that is supporting the weight of the robot. + * + * This function can also be used to estimate the FT sensor measurements in case + * two unknown external wrenches are applied on the robot, if some additional + * assumption about the simmetry of the robot configuration, the joint torques and + * the external wrenches can be done. + * + * \warning Before calling this method, either updateKinematicsFromFloatingBase or + * updateKinematicsFromFixedBase must be called. + * + * @param[in] unknowns the unknown external wrenches. + * @param[out] predictedMeasures the estimate measures for the FT sensors. + * @param[out] estimatedContactWrenches the estimated contact wrenches. + * @param[out] estimatedJointTorques the estimated joint torques. + * @return true if all went well, false otherwise. + */ + bool computeExpectedFTSensorsMeasurements(const LinkUnknownWrenchContacts & unknowns, + SensorsMeasurements & predictedMeasures, + LinkContactWrenches & estimatedContactWrenches, + JointDOFsDoubleArray & estimatedJointTorques); + + /** + * \brief For each submodel without any external wrench, computes the equation that relates the FT sensor measures with the kinematics-related known terms. + * + * For each submodel in which there are no external forces, we can write the following equation: + * + * \f[ + * A w = b + * \f] + * Where: + * * \f$w\f$ is the vector of dimension 6*nrOfFTSensors obtained by stacking the FT sensors measures: + * * \f$A\f$ is a matrix of size 6 x 6*nrOfFTSensors that depends on position in space of the FT sensors + * * \f$b\f$ is a vector of size 6 that depends on the position, velocity, acceleration and gravity of each link in the submodel + * + * This function provides an easy way to compute A and B . Typically, these quantities are not used online + * during the estimation of external wrenches or internal torques, but rather as an helper method when calibrating FT sensors. + * + * In the rest of the documentation, we will refer to this quantities: + * * nrOfSubModels (\f$n_{sm}\f$): the number of submodels in which the model is divided, as induced by the FT sensors present in the model. + * * nrOfSubModelsWithoutExtWrenches: the number of submodels on which there is no external wrench + * * nrOfFTSensors (\f$n_{ft}\f$): the number of FT sensors present in the model + * In particular, the value of A and b for a given submodel is the following. First of all, for any submodel $sm$ with no external force, + * we can write (from Equation 4.19 of https://traversaro.github.io/traversaro-phd-thesis/traversaro-phd-thesis.pdf, modified to account + * for all FT sensors in the submodel and to remove external wrenches): + * + * \f[ + * \sum_{s=1}^{n_ft}~\mu_{sm, ft}~\sigma_{sm, ft}~{}_{B_{sm}} X^{ft} \mathrm{f}^{meas}_{ft} = \sum_{L \in \mathbb{L}_{sm}} {}_{B_{sm}} X^{L} {}_L \phi_L + * \f] + * + * where: + * * \f$ \mu_{sm, ft} \f$ is equal to \f$1\f$ if the sensor \f$ft\f$ is attached to the submodel \f$sm\f$, and \f$0\f$ otherwise + * * \f$ \sigma_{sm, ft} \f$ is equal to \f$1\f$ if the sensor \f$ft\f$ is measuring the force applied on submodel \f$sm\f$ or \f$-1\f$ if it is measuring the force that the submodel excerts on its neighbor submodel + * *\f$ {B_{sm}} \f$ is a frame in which this equation is expressed, that for this function it is the base link of the submodel. + * + * + * With this definitions, we can see that A_sm and b_sm for a given submodel \f$sm\f$ can be simply be defined as: + * + * \f$ + * A_{sm} = \begin{bmatrix} \mu_{sm, ft0}~\sigma_{sm, ft(0)}~{}_C X^{ft(0)} & \hdots & \mu_{sm, ft(n_ft-1)}~\sigma_{sm, ft(n_ft-1)}~X^{ft(n_ft-1)} \end{bmatrix} + * \f$ + * + * \f$ + * w_{sm} = \begin{bmatrix} \mathrm{f}^{meas}_{ft(0)} \\ \vdots \\ \mathrm{f}^{meas}_{ft(n_ft-1)} \end{bmatrix} + * \f$ + * + * \f$ + * b_{sm} = \sum_{L \in \mathbb{L}_{sm}} {}_C X^{L} {}^L \phi_L + * \f$ + * + * \warning Before calling this method, either updateKinematicsFromFloatingBase or + * updateKinematicsFromFixedBase must be called. + * + * @param[in] unknowns the unknown external wrenches, that is used to understand the submodels in which no external wrench is present + * @param[out] A vector of nrOfSubModelsWithoutExtWrenches matrices of size 6 x 6*nrOfFTSensors + * @param[out] b vector of nrOfSubModelsWithoutExtWrenches vectors of size 6 + * @param[out] subModelIDs vector of size nrOfSubModelsWithoutExtWrenches of unsigned integers from 0 to nrOfSubModels-1, subModelIDs[i] specifies to which submodel the quantities A[i] + * @param[out] baseLinkIndeces vector of size nrOfSubModelsWithoutExtWrenches of iDynTree::LinkIndex from 0 to nrOfLinks-1, baseLinkIndeces[i] specifies the link in which the equation i is expressed + * @return true if all went well, false otherwise. + */ + bool computeSubModelMatrixRelatingFTSensorsMeasuresAndKinematics(const LinkUnknownWrenchContacts & unknowns, + std::vector& A, + std::vector& b, + std::vector& subModelID, + std::vector& baseLinkIndeces); + + /** + * \brief Estimate the external wrenches and the internal joint torques using the measurement of the internal F/T sensors. + * + * This is the main method of the class. The technique implemented + * in this method is the one described in the paper: + * + * "Contact force estimations using tactile sensors and force/torque sensors" + * + * Del Prete, A., Natale, L., Nori, F., & Metta, G. (2012). + * Contact force estimations using tactile sensors and force/torque sensors. + * URL : http://www.researchgate.net/profile/Andrea_Del_Prete/publication/236152161_Contact_Force_Estimations_Using_Tactile_Sensors_and_Force__Torque_Sensors/links/00b495166e74a5369d000000.pdf + * + * \note There should be at least 6 unknowns variables for each submodel for which the estimation of + * external wrenches is performed (i.e. usually 1 unknown wrench for each submodel). + * If is not the case, undefined results could occur in the estimation. + * + * @param[in] unknowns the unknown external wrenches + * @param[in] ftSensorsMeasures the measurements for the FT sensors. + * @param[out] estimatedContactWrenches the estimated contact wrenches. + * @param[out] estimatedJointTorques the estimated joint torques. + * @return true if all went ok, false otherwise. + */ + bool estimateExtWrenchesAndJointTorques(const LinkUnknownWrenchContacts & unknowns, + const SensorsMeasurements & ftSensorsMeasures, + LinkContactWrenches & estimatedContactWrenches, + JointDOFsDoubleArray & estimatedJointTorques); + + /** + * Check if the kinematics set in the model are the one of a fixed model. + * + * While computing the expected F/T sensors measures, you tipically want the + * model to be still, to reduce the sources of noise . + * + * @param[in] gravityNorm the norm of the gravity (tipically 9.81) against with all the + * proper accelerations are check (for a still model, the proper + * acceleration norm should be close to the gravity norm. + * @param[in] properAccTol tolerance to use for the check on the proper acceleration norm. + * @param[in] verbose true if you want to print debug information, false otherwise. + * @return true if the model is still, false if it is moving or if the kinematics was never setted. + * + * \note This method can be computationally expensive, so in most case it + * may be a better idea to just do the check on the input variables (joint velocities, joint acceleration). + */ + bool checkThatTheModelIsStill(const double gravityNorm, + const double properAccTol, + const double verbose); + + + /** + * Compute the vector of the sum of all the wrenches (both internal and external, excluding gravity) acting on + * link i, expressed (both orientation and point) with respect to the reference frame of link i, + * using the articulated body model and the kinematics information provided by the updateKinematics* methods. + * + * This is tipically computed as I*a+v*(I*v) , where a is the proper acceleration. + * + * @param[out] netWrenches the vector of link net wrenches. + * @return true if all went ok, false otherwise. + */ + bool estimateLinkNetWrenchesWithoutGravity(LinkNetTotalWrenchesWithoutGravity & netWrenches); + +}; + +} + +#endif diff --git a/src/estimation/include/iDynTree/ExtendedKalmanFilter.h b/src/estimation/include/iDynTree/ExtendedKalmanFilter.h new file mode 100644 index 00000000000..6ee547930f1 --- /dev/null +++ b/src/estimation/include/iDynTree/ExtendedKalmanFilter.h @@ -0,0 +1,320 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef EXTENDED_KALMAN_FILTER_H +#define EXTENDED_KALMAN_FILTER_H + +#include +#include +#include +#include + +namespace iDynTree +{ + + /** + * @class DiscreteExtendedKalmanFilterHelper naive base class implementation of discrete EKF with additive Gaussian noise + * + * @warning This class is not stand-alone but can be used meaningfully only by classes deriving from it. + * The system propagation function f() and measurement model function h() are virtual functions to be defined by + * the derived class implementing the EKF. + * Similarly, the Jacobians for the linearized system propagation and measurement model are also virtual functions + * to be implemented by the derived class. + * + * The derived class must must set the size for states, inputs and outputs using ekfSetStateSize(), ekfSetInputSize() and ekfSetOutputSize() methods. + * Then the derived class must call ekfInit() to resize the vectors and matrices for the EKF. + * + * Similarly before running the estimator through a loop, it is recommended to set the initial states and variances for measurements, system dynamics and intial states. + * This is necessary to avoid any NaN values to be propagated. + * + * Once, initialized properly, the filter can be run by + * - calling ekfSetInputVector() to set the control inputs and then calling ekfPredict() at each prediction step, and + * - calling ekfSetMeasurementVector() to set the measurements and then calling ekfUpdate() at each update step + * + * The internal state of the estimator can be obtained by calling ekfGetStates(). + * + * The Discrete Extended Kalman Filter equations implemented in this class are coherent with the ones described in + * Discrete-time predict and update equations section of this article. + * + * The general workflow implementing/inheriting this class would be in the order, + * - call ekfSetInputSize(), ekfSetOutputSize(), ekfSetStateSize() methods + * - call ekfInit() + * - call ekfSetInitialState(), ekfSetStateCovariance() (either done externally later or internally. usually done externally later, however, filter runs properly only if this step is done) + * - call ekfSetSystemNoiseCovariance(), ekfSetMeasurementNoiseCovariance() + * + * once this is setup, + * - in a loop + * - call ekfSetInputVector() then ekfPredict(). Calling ekfGetStates() and ekfGetStateCovariance() at this point will give us the predicted states and its covariance + * - call ekfSetMeasurementVector() then ekfUpdate(). Calling ekfGetStates() and ekfGetStateCovariance() at this point will give us the updated states and its covariance + * @note if we intend to change the state size, input size or output size on the fly, it is crucial to call the ekfInit() method again, since this resizes the buffers accordingly + * failing to do so will result in memory leaks and will cause the program to crash. + * + */ + class DiscreteExtendedKalmanFilterHelper + { + public: + DiscreteExtendedKalmanFilterHelper(); + + /** + * @brief Describes the state propagation for a given dynamical system + * If state of the system is denoted by \f$ x \f$ and the control input by \f$ u \f$, then + * the system dynamics is given as \f$ x_{k+1} = f(x_k, u_k) \f$. + * @note the detail of this function needs to be implemented by the child class + * @param[in] x_k state at current time instant + * @param[in] u_k control input at current time instant + * @param[out] xhat_k_plus_one predicted state without any correction from measurements + * @return bool true/false if successful or not + */ + virtual bool ekf_f(const iDynTree::VectorDynSize& x_k, + const iDynTree::VectorDynSize& u_k, + iDynTree::VectorDynSize& xhat_k_plus_one) = 0; + + /** + * @brief Describes the measurement model of the system, + * i.e., how the measurements can be described as a function of states, + * Given a state of the system described by \f$ x \f$, + * what would be the measurement \f$ z \f$ observed from this state \f$ z_{k+1} = h(\hat{x}_{k+1}) \f$. + * @note the detail of this function needs to be implemented by the child class + * @param[in] xhat_k_plus_one predicted state of next time instant + * @param[out] zhat_k_plus_one predicted measurement of next time instant + * @return bool true/false if successful or not + */ + virtual bool ekf_h(const iDynTree::VectorDynSize& xhat_k_plus_one, + iDynTree::VectorDynSize& zhat_k_plus_one) = 0; + + /** + * @overload + */ + virtual bool ekfComputeJacobianF(iDynTree::VectorDynSize& x, iDynTree::MatrixDynSize& F) = 0; + + /** + * @brief Describes the system Jacobian necessary for the propagation of predicted state covariance + * The analytical Jacobian describing the partial derivative of the system propagation with respect to the state + * and the system propagation with respect to the input + * @note the detail of this function needs to be implemented by the child class + * @param[in] x system state + * @param[in] u system input + * @param[out] F system Jacobian + * @return bool true/false if successful or not + */ + virtual bool ekfComputeJacobianF(iDynTree::VectorDynSize& x, iDynTree::VectorDynSize& u, iDynTree::MatrixDynSize& F) = 0; + + /** + * @brief Describes the measurement Jacobian necessary for computing Kalman gain and updating the predicted state and its covariance + * The analytical Jacobian describing the partial derivative of the measurement model with respect to the state + * @note the detail of this function needs to be implemented by the child class + * @param[in] x system state + * @param[out] H measurement Jacobian + * @return bool true/false if successful or not + */ + virtual bool ekfComputeJacobianH(iDynTree::VectorDynSize& x, iDynTree::MatrixDynSize& H) = 0; + + /** + * @brief Implements the Discrete EKF prediction equation + * described by + * \f$ \hat{x}_{k+1} = f(x_k, u_k) \f$ is given by the ekf_f() method + * \f$ \hat{P}_{k+1} = F_k P_k F_k^T + Q \f$ + * where, \f$ F \mid_{x = x_k} \f$ is given by the ekfComputejacobianF() method + * + * @warning this function can be called only after setting up the filter properly through ekfInit() step + * @note this function should be once called every step, after setting up the input vector using ekfSetInputVector() method + * @warning setting up the input vector everytime before calling the ekfPredict() method is crucial, the prediction step is not performed if this step is skipped + * this is because internally a flag associated to the setting up of input vector is set true by ekfSetInputVector() method which in turn is set false by ekfPredict() method + * @return bool true/false if successful or not + */ + bool ekfPredict(); + + /** + * @brief Implements the Discrete EKF update equation + * described by + * \f$ z_{k+1} = h(\hat{x}_{k+1}) \f$ is given by ekf_h() method + * innovation \f$ \tilde{y}_{k+1} = y_{k+1} - z_{k+1} \f$ + * innovation covariance \f$ S_{k+1} = H_{k+1} \hat{P}_{k+1} H_{k+1}^T + R \f$, where \f$ H \mid_{x = \hat{x}_{k+1}} \f$ is given by ekfComputejacobianH() method + * Kalman gain \f$ K_{k+1} = \hat{P}_{k+1} H_{k+1}^T S_{k+1}^{-1} \f$ + * Updated covariance \f$ P_{k+1} = \hat{P}_{k+1} - (K_{k+1} H \hat{P}_{k+1}) \f$ + * Updated state estimate \f$ x_{k+1} = \hat{x}_{k+1} + K_{k+1} \tilde{y}_{k+1} \f$ + * + * @warning this function can be called only after setting up the filter properly through ekfInit() step + * @note this function should be once called every step, after setting up the measurement vector using ekfSetMeasurementVector() method + * @warning setting up the measurement vector everytime before calling the ekfUpdate() method is crucial, the update step is not performed if this step is skipped + * this is because internally a flag associated to the setting up of measurement vector is set true by ekfSetMeasurementVector() method which in turn is set false by ekfUpdate() method + * @return bool true/false if successful or not + */ + bool ekfUpdate(); + + /** + * @brief Initializes and resizes the internal buffers of this filter + * @warning this is a very crucial method of this class. This needs to be called after setting the input size through ekfSetInputSize(), + * output size through ekfSetOutputSize() and state dimension through ekfSetStateSize(), + * such that the corresponding matrices and vectors will resize themselves to their corresponding dimensions. + * Failing to do so might result in memory leaks and may cause the program to crash + * @return bool true/false if successful or not + */ + bool ekfInit(); + + /** + * @brief Initializes and resizes the internal buffers of this filter + * @warning this is a very crucial method of this class. This method sets the input size through ekfSetInputSize(), + * output size through ekfSetOutputSize() and state dimension through ekfSetStateSize() with the specified parameters, + * such that the corresponding matrices and vectors will resize themselves to their corresponding dimensions. + * Failing to do so might result in memory leaks and may cause the program to crash + * @param[in] state_size state size + * @param[in] input_size input size + * @param[in] output_size output size + * @return bool true/false if successful or not + */ + bool ekfInit(const size_t& state_size, const size_t& input_size, const size_t& output_size); + + /** + * @brief Resets the filter flags + * The filter flags check if the filter was properly initialized, if the initial state was set, + * if the initial state covariance was set. These three flags are crucial for proper setting up of the filter. + * The other flags include the checks on whether the input and measurement vectors were updated at every prediction/update step + */ + void ekfReset(); + + /** + * @brief Resets the filter flags, initializes and resizes internal buffers of the filter, and + * sets initial state, initial state covariance, and system noise and measurement noise covariance matrices + * @warning size of the span for P0 and Q must be of the size (state size*state size), where * is the regular multiplication operator + * @warning size of the span for R must be of the size (ouput size*output size), where * is the regular multiplication operator + * @warning the matrices from the span are built in row-major ordering. + * + * @note this method is particularly useful while working with hybrid systems, + * where the size of the system state or the measurements keep evolving with time + * + */ + bool ekfReset(const size_t& state_size, + const size_t& input_size, + const size_t& output_size, + const iDynTree::Span& x0, + const iDynTree::Span& P0, + const iDynTree::Span& Q, + const iDynTree::Span& R); + + /** + * @brief Set measurement vector at every time step + * the measurement vector size and output size should match + * @param[in] y iDynTree::Span object to access the measurement vector + * @return bool true/false if successful or not + */ + bool ekfSetMeasurementVector(const iDynTree::Span& y); + + /** + * @brief Set input vector at every time step + * the input vector size and input size should match + * @param[in] u iDynTree::Span object to access the input vector + * @return bool true/false if successful or not + */ + bool ekfSetInputVector(const iDynTree::Span& u); + + /** + * @brief Set initial state + * the size of x0 and state size should match + * @param[in] x0 iDynTree::Span object to access the state vector + * @note this method should be called before running the filter + * @return bool true/false if successful or not + */ + bool ekfSetInitialState(const iDynTree::Span& x0); + + /** + * @brief Set initial state covariance matrix + * the size of P and (state size*state size) should match + * @param[in] P iDynTree::Span object to access the state covariance matrix + * @note this method should be called before running the filter + * @warning if this matrix is not initialized properly, then the resulting output will only have NaNs in it + * @return bool true/false if successful or not + */ + bool ekfSetStateCovariance(const iDynTree::Span& P); + + /** + * @brief Set system noise covariance matrix + * the size of Q and (state size*state size) should match + * @param[in] Q iDynTree::Span object to access the system noise covariance matrix + * @note default value is a zero matrix + * @return bool true/false if successful or not + */ + bool ekfSetSystemNoiseCovariance(const iDynTree::Span& Q); + + /** + * @brief Set measurement noise covariance matrix + * the size of R and (output size*output size) should match + * @param[in] R iDynTree::Span object to access the measurement noise covariance matrix + * @note default value is a zero matrix + * @return bool true/false if successful or not + */ + bool ekfSetMeasurementNoiseCovariance(const iDynTree::Span& R); + + /** + * @brief Set the state dimensions + * @param[in] dim_X state size + * @warning this method should be called before calling ekfInit() + * @return bool true/false if successful or not + */ + void ekfSetStateSize(size_t dim_X) { m_dim_X = dim_X; } + + /** + * @brief Set the input dimensions + * @param[in] dim_U input size + * @warning this method should be called before calling ekfInit() + * @return bool true/false if successful or not + */ + void ekfSetInputSize(size_t dim_U) { m_dim_U = dim_U; } + + /** + * @brief Set the ouptut dimensions + * @param[in] dim_Y output size + * @warning this method should be called before calling ekfInit() + * @return bool true/false if successful or not + */ + void ekfSetOutputSize(size_t dim_Y) { m_dim_Y = dim_Y; } + + /** + * @brief Get current internal state of the filter + * the size of x and state size should match + * @param[out] x iDynTree::Span object to copy the internal state vector into + * @return bool true/false if successful or not + */ + bool ekfGetStates(const iDynTree::Span &x) const; + + /** + * @brief Get state covariance matrix + * the size of P and (state size*state size) should match + * @param[out] P iDynTree::Span object to copy the internal state covariance matrix onto + * @return bool true/false if successful or not + */ + bool ekfGetStateCovariance(const iDynTree::Span &P) const; + + protected: + /** + * function template to ignore unused parameters + */ + template + void ignore(T &&) { } + + private: + size_t m_dim_X; ///< state dimension + size_t m_dim_Y; ///< output dimenstion + size_t m_dim_U; ///< input dimension + iDynTree::VectorDynSize m_x; ///< state at time instant k + iDynTree::VectorDynSize m_u; ///< input at time instant k + iDynTree::VectorDynSize m_y; ///< measurements at time instant k + iDynTree::VectorDynSize m_xhat; ///< predicted state at time instant k before updating measurements + + iDynTree::MatrixDynSize m_F; ///< System jacobian + iDynTree::MatrixDynSize m_P; ///< State covariance + iDynTree::MatrixDynSize m_Phat; ///< State covariance estimate before updating measurements + iDynTree::MatrixDynSize m_Q; ///< system noise covariance + iDynTree::MatrixDynSize m_H; ///< measurement jacobian + iDynTree::MatrixDynSize m_S; ///< innovation covariance + iDynTree::MatrixDynSize m_K; ///< Kalman gain + iDynTree::MatrixDynSize m_R; ///< measurement noise covariance + bool m_is_initialized{false}; ///< flag to check if filter is properly initialized + bool m_measurement_updated{false}; ///< flag to check if measurement is updated at each update step + bool m_input_updated{false}; ///< flag to check if control input is updated at each prediction step + bool m_initial_state_set{false}; ///< flag to check if the initial state of the filter is set + bool m_initial_state_covariance_set{false}; ///< flag to check if the initial covariance is set properly + }; +} + +#endif diff --git a/src/estimation/include/iDynTree/ExternalWrenchesEstimation.h b/src/estimation/include/iDynTree/ExternalWrenchesEstimation.h new file mode 100644 index 00000000000..e80c318673c --- /dev/null +++ b/src/estimation/include/iDynTree/ExternalWrenchesEstimation.h @@ -0,0 +1,417 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_ESTIMATION_EXTERNALWRENCHESTIMATION_H +#define IDYNTREE_ESTIMATION_EXTERNALWRENCHESTIMATION_H + +#include +#include +#include +#include +#include + +#include +#include + +#include + +namespace iDynTree +{ +class Model; +class Traversal; +class SubModelDecomposition; +class LinkContactWrenches; +class SensorsMeasurements; +class SensorsList; +class LinkVelArray; +class LinkAccArray; +class JointPosDoubleArray; +class JointDOFsDoubleArray; + +/** + * Type of a UnknownWrenchContact. + */ +enum UnknownWrenchContactType +{ + /** + * Contact for which the complete wrench is unknown. + */ + FULL_WRENCH, + + /** + * Contact assumed to be a pure force excerted on the contact point. + */ + PURE_FORCE, + + /** + * Contact assumed to be a pure force with a known direction excerted on the contact point + */ + PURE_FORCE_WITH_KNOWN_DIRECTION, + + /** + * The contact forces is assumed to be known. + */ + NO_UNKNOWNS +}; + + +/** + * \brief A contact whose wrench is unknown. + * + * Class representing a contact for which + * the approximate center of the contact surface is known, + * but the wrench is unknown. + * + */ +struct UnknownWrenchContact +{ + /** + * Constructor + */ + UnknownWrenchContact() + {} + + UnknownWrenchContact(const UnknownWrenchContactType _unknownType, + const Position & _contactPoint, + const Direction & _forceDirection = iDynTree::Direction::Default(), + const Wrench & _knownWrench = iDynTree::Wrench(), + const unsigned long & _contactId = 0): unknownType(_unknownType), + contactPoint(_contactPoint), + forceDirection(_forceDirection), + knownWrench(_knownWrench), + contactId(_contactId) + {} + + /** + * Type of the unknown contact. + */ + UnknownWrenchContactType unknownType; + + /** + * Position of the center of the contact, in the link frame. + */ + Position contactPoint; + + /** + * If unknownType is PURE_FORCE_WITH_KNOWN_DIRECTION, + * contains the known direction (in link frame) of the force. + */ + Direction forceDirection; + + /** + * If unknownType is NO_UNKNOWNS, + * contains the value of the contact force, with the orientation of the link frame, + * and w.r.t. to the origin of the link frame, i.e. it ignores the contactPoint attribute. + */ + Wrench knownWrench; + + /** + * Unique id identifing the contact. + * This id is propagated to the contact wrench data structure. + * It is implemented mainly for compatibility with the skinDynLib library. + */ + unsigned long contactId; +}; + +/** + * A set of UnknownWrenchContact for each link, representing all the contacts + * between the model and the external environment whose wrench is unkwnon. + * + */ +class LinkUnknownWrenchContacts +{ +private: + std::vector< std::vector > m_linkUnknownWrenchContacts; + +public: + /** + * Create a LinkWrenches vector, with the size given + * by nrOfLinks . + * + * @param[in] nrOfLinks the size of the vector. + */ + LinkUnknownWrenchContacts(unsigned int nrOfLinks = 0); + LinkUnknownWrenchContacts(const Model & model); + + /** + * Preserving the number of links, remove all the previously added unknowns. + * + */ + void clear(); + + /** + * + * @param[in] nrOfLinks the number of links used to resize + */ + void resize(unsigned int nrOfLinks); + void resize(const Model & model); + + /** + * Get the number of external contacts for a given link. + */ + size_t getNrOfContactsForLink(const LinkIndex linkIndex) const; + + /** + * Set the number of external contacts for a given link. + */ + void setNrOfContactsForLink(const LinkIndex linkIndex, const size_t nrOfContacts); + + /** + * Add a new contact for a link. + */ + void addNewContactForLink(const LinkIndex linkIndex, const UnknownWrenchContact& newContact); + + /** + * Add a new contact for a frame. + * If the specified frame is not the a link frame, the method automatically convert the unknown + * wrench to the relative link frame. + * + * @param[in] model the model class for getting frame information. + * @param[in] frameIndex the index of the frame in which you are expressing the new unknown wrench. + * @param[in] newContact the new unknown wrench to add. + * @return true if all went well, false otherwise + */ + bool addNewContactInFrame(const Model & model, + const FrameIndex frameIndex, + const UnknownWrenchContact& newContact); + + /** + * Add a full wrench unknown at the origin of the specified frame. + * Simplified version of a addNewContactInFrame, in which the contact + * point is the origin of the frame and the unknown type is FULL_WRENCH. + * + * Equivalent to + * addNewContactInFrame(model,frame,UnknownWrenchContact(FULL_WRENCH,Position::Zero())) + * + * @param[in] model the model class for getting frame information. + * @param[in] frameIndex the index of the frame in which you are expressing the new unknown wrench. + * @return true if all went well, false otherwise + */ + bool addNewUnknownFullWrenchInFrameOrigin(const Model& model, + const FrameIndex frameIndex); + + /** + * Get a specific ContactWrench + * + * @param[in] linkIndex the index of the link for which the contact is retrieved + * @param[in] contactIndex a index (between 0 and getNrOfContactsForLink(link)-1 ) identifing the specific contact. + */ + UnknownWrenchContact& contactWrench(const LinkIndex linkIndex, const size_t contactIndex); + + const UnknownWrenchContact& contactWrench(const LinkIndex linkIndex, const size_t contactIndex) const; + + + /** + * Get a human readable description of the LinkUnknownWrenchContacts (for debug) + */ + std::string toString(const Model & model) const; +}; + +struct estimateExternalWrenchesBuffers +{ + estimateExternalWrenchesBuffers(); + estimateExternalWrenchesBuffers(const SubModelDecomposition& subModels); + estimateExternalWrenchesBuffers(const size_t nrOfSubModels, const size_t nrOfLinks); + + /** + * Resize the struct for the number of submodel + */ + void resize(const SubModelDecomposition& subModels); + void resize(const size_t nrOfSubModels, const size_t nrOfLinks); + + size_t getNrOfSubModels() const; + size_t getNrOfLinks() const; + + /** + * Check if the buffer size are consistent with the submodel + * decomposition. + */ + bool isConsistent(const SubModelDecomposition& subModels) const; + + /** + * The problem of external wrenches estimation boils down to + * solve a LS problem in the form argmin_x (Ax-b)^2 . + */ + std::vector A; + std::vector x; + std::vector b; + + /** + * We compute the b term for each subtree + * in a iterative way, so we need a buffer + * to store it for each link + */ + LinkWrenches b_contacts_subtree; + + /** + * We compute the transform between each link + * and the submodel base, for computing the + * A matrices + */ + LinkPositions subModelBase_H_link; +}; + +/** + * \brief Estimate the external contact wrenches using the MultiBody Newton-Euler equations. + * + * This function is used to estimate the external contacts forces **without** using any measurement + * of the internal FT sensors. It is tipically used to get data for calibrating the offset of + * the internal FT sensors. + */ +bool estimateExternalWrenchesWithoutInternalFT(const Model& model, + const Traversal& traversal, + const LinkUnknownWrenchContacts & unknownWrenches, + const JointPosDoubleArray & jointPos, + const LinkVelArray & linkVel, + const LinkAccArray & linkProperAcc, + estimateExternalWrenchesBuffers & bufs, + LinkContactWrenches & outputContactWrenches); + +/** + * \brief Estimate the external wrenches trasmitted by the contacts between the model and the external environment. + * + * This function exploits the measurements of internal FT sensors (whose structure is contained + * in the sensors parameters and which measurements are contained in the ftSensorsMeasurements + * parameters) to compute an estimation of the values of the unknown wrenches specified in the + * unknownWrenches parameter. + * + * @param[in] model the considered model. + * @param[in] subModels a decomposition of the model along the joint of the six axis F/T sensors. + * @param[in] sensors a description of the sensors available in the model. + * @param[in] unknownWrenches a description of the contacts for which the contact wrench is unknown. + * @param[in] linkVel a vector of link twists, expressed w.r.t to the link orientation and the link origin + * @param[in] linkProperAcc a vector of link spatial (in the Featherstone sense) and proper accelerations, expressed w.r.t to the link orientation and the link origin + * @param[in] ftSensorsMeasurements the measurements of the internal six axis F/T sensors. + * @param[out] outputContactWrenches the estimated contact wrenches. + * @return true if all went well (the dimension of the inputs are consistent), false otherwise + * + */ +bool estimateExternalWrenches(const Model& model, + const SubModelDecomposition& subModels, + const SensorsList& sensors, + const LinkUnknownWrenchContacts & unknownWrenches, + const JointPosDoubleArray & jointPos, + const LinkVelArray & linkVel, + const LinkAccArray & linkProperAcc, + const SensorsMeasurements & ftSensorsMeasurements, + estimateExternalWrenchesBuffers & bufs, + LinkContactWrenches & outputContactWrenches); + +/** + * \brief Modified forward kinematics for torque/force estimation. + * + * This is a version of forward kinematics modified to fit the needs + * of joint torques/external wrenches estimation. + * + * There are several difference with respect to the classical + * forward kinematics. + * The first one is that the only inputs necessary related to the base link are + * the base link classical proper acceleration, the base link angular velocity + * and the base link angular acceleration. This is because the dynamics + * of an articulated system does not depend on an offset in linear velocity, and hence + * the estimation of joint torques/external wrenches is not affected by the base link + * linear velocity. This will mean that the link velocitity computed by this + * algorithm are not the velocity of the links with respect to an inertial frame. + * Nevertherless they can still be used for estimation. + * + * There are two main ways in which the base information is computed: one is + * exploiting the knoledge that a link is not moving with respect to an inertial frame: + * in this case the classical proper acceleration boils down to the inverted gravitational + * acceleration, while the angular velocity and angular accelerations are equal to zero. + * The other way is to exploit the measure of an accelerometer and of a gyroscope + * mounted on the base link of the traversal: the accelerometer will then measure + * directly the classical proper acceleration, while the gyroscope will measure the angular velocity. + * The angular acceleration can be computed by numerical derivation, or simply neglected if its + * effect on the estimation is minimal. + * + * + * \param[in] model the input model + * \param[in] traversal the traversal used to propagate the velocity and the proper acceleration + * \param[in] base_classicalProperAcc classical proper acceleration of the base origin + * \param[in] base_angularVel angular velocity of the base link frame + * \param[in] base_angularAcc angular acceleration of the base link frame + * \param[in] jointPos joint positions + * \param[in] jointVel joint velocities + * \param[in] jointAcc joint accelerations + * \param[out] linkVel vector of link twists, expressed in the link frame for both orientation and origin + * \param[out] linkProperAcc vector of link proper spatial acceleration, expressed in the link frame for both orientation and origin + * @return true if all went well, false otherwise + * + */ +bool dynamicsEstimationForwardVelAccKinematics(const Model & model, + const Traversal & traversal, + const Vector3 & base_classicalProperAcc, + const Vector3 & base_angularVel, + const Vector3 & base_angularAcc, + const JointPosDoubleArray & jointPos, + const JointDOFsDoubleArray & jointVel, + const JointDOFsDoubleArray & jointAcc, + LinkVelArray & linkVel, + LinkAccArray & linkProperAcc); + +/** + * \brief Modified forward kinematics for floating basedynamics estimation. + * + * This is a version of velocity forward kinematics modified to fit the needs + * of free floating dynamics estimation. + * + * There are several difference with respect to the classical + * forward kinematics. + * The first one is that the only inputs necessary related to the base link is + * the base link angular velocity. This is because the dynamics + * of an articulated system does not depend on an offset in linear velocity. + * This will mean that the link velocities computed by this + * algorithm are not the velocity of the links with respect to an inertial frame. + * Nevertherless they can still be used for estimation. + * + * There are two main ways in which the base information is computed: one is + * exploiting the knoledge that a link is not moving with respect to an inertial frame: + * in this case the angular velocity is equal to zero. + * The other way is to exploit the measure of a gyroscope + * mounted on the base link of the traversal: the gyroscope will measure the link angular velocity. + * + * + * \param[in] model the input model + * \param[in] traversal the traversal used to propagate the velocity and the proper acceleration + * \param[in] base_angularVel angular velocity of the base link frame + * \param[in] jointPos joint positions + * \param[in] jointVel joint velocities + * \param[out] linkVel vector of link twists, expressed in the link frame for both orientation and origin + * @return true if all went well, false otherwise + */ +bool dynamicsEstimationForwardVelKinematics(const Model & model, + const Traversal & traversal, + const Vector3 & base_angularVel, + const JointPosDoubleArray & jointPos, + const JointDOFsDoubleArray & jointVel, + LinkVelArray & linkVel); + +/** + * \brief Compute the net internal and external wrenches (excluding gravity forces) acting on the links. + * @param[in] model the input model + * @param[in] linkVel a vector of link twists, expressed w.r.t to the link orientation and the link origin + * @param[in] linkProperAcc a vector of link spatial (in the Featherstone sense) and proper accelerations, expressed w.r.t to the link orientation and the link origin + * @param[in] linkNetWrenchesWithoutGravity the vector of the sum of all the wrenches (both internal and external, excluding gravity) acting on link i, expressed (both orientation and point) with respect to the reference frame of link i + */ +bool computeLinkNetWrenchesWithoutGravity(const Model& model, + const LinkVelArray & linkVel, + const LinkAccArray & linkProperAcc, + LinkNetTotalWrenchesWithoutGravity& linkNetWrenchesWithoutGravity); + +/** + * Compute the link contact wrenches from the net external wrenches + * + * If there are more than 6 unknows for link, the problem becomes ill-defined + * and the function just assign all the external wrench to the first contact. + * \todo(traversaro): support arbitrary LinkUnknownWrenchContacts by performing + * a least square fitting, similar to what implemented in + * the estimateExternalWrenches . + */ +bool estimateLinkContactWrenchesFromLinkNetExternalWrenches(const Model& model, + const LinkUnknownWrenchContacts& unknownWrenches, + const LinkNetExternalWrenches& netExtWrenches, + LinkContactWrenches & outputContactWrenches); + +} + +#endif diff --git a/src/estimation/include/iDynTree/GravityCompensationHelpers.h b/src/estimation/include/iDynTree/GravityCompensationHelpers.h new file mode 100644 index 00000000000..8b169164cc7 --- /dev/null +++ b/src/estimation/include/iDynTree/GravityCompensationHelpers.h @@ -0,0 +1,126 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef GRAVITY_COMPENSATION_HELPERS_H +#define GRAVITY_COMPENSATION_HELPERS_H + +#include +#include +#include +#include + +#include +#include +#include + +#include + +namespace iDynTree +{ + /** + * @brief Class computing the gravity compensation torques + * using accelerometer measurements + * + * @note the estimation of the gravity assumes + * the non-gravitational accelerations measured by the + * accelerometer as negligible + * + */ + class GravityCompensationHelper + { + public: + /** + * @brief Default Constructor + */ + GravityCompensationHelper(); + + /** + * @brief Destructor + */ + ~GravityCompensationHelper(); + + /** + * @brief Load model + * @return true if successful, false otherwise + */ + bool loadModel(const iDynTree::Model& _model, const std::string dynamicBase); + + /** + * @brief Set the kinematic information necessary for the gravity torques estimation using + * the proper acceleration coming from an accelerometer + * + * @note the estimation of the gravity assumes the non-gravitational accelerations + * measured by the accelerometer as negligible. + * + * @param[in] jointPos the position of the joints in the model + * @param[in] floatingFrame the frame index for which proper acceleration is provided + * @param[in] properClassicalLinearAcceleration proper (actual acceleration - gravity) + * classical acceleration of the origin of the + * specified frame, expresssed in the specified + * frame orientation + * + * @return true if successful, false otherwise + */ + bool updateKinematicsFromProperAcceleration(const iDynTree::JointPosDoubleArray& jointPos, + const iDynTree::FrameIndex& floatingFrame, + const iDynTree::Vector3& properClassicalLinearAcceleration); + + /** + * @brief Set the kinematic information necessary for the gravity torques estimation using + * the assumed known gravity vector on frame + * + * @note This is implemented as updateKinematicsFromProperAcceleration(jointPos, floatingFrame, -gravity); + * + * @param[in] jointPos the position of the joints in the model + * @param[in] floatingFrame the frame index for which gravity vector is provided + * @param[in] properClassicalLinearAcceleration gravity acceleration of the origin of the + * specified frame, expresssed in the specified + * frame orientation + * + * @return true if successful, false otherwise + */ + bool updateKinematicsFromGravity(const iDynTree::JointPosDoubleArray& jointPos, + const iDynTree::FrameIndex& floatingFrame, + const iDynTree::Vector3& gravity); + + /** + * @brief Get the gravity compensation torques + * @return true if successful, false otherwise + */ + bool getGravityCompensationTorques(iDynTree::JointDOFsDoubleArray& jointTrqs); + + private: + /** + * @brief Helper function for kinematic traversal dynamic allocation + * @param[in] nrOfLinks number of links to be allocated in the traversal + */ + void allocKinematicTraversals(const size_t nrOfLinks); + + /** + * @brief Helper function for kinematic traversal dynamic deallocation + */ + void freeKinematicTraversals(); + + bool m_isModelValid; ///< flag to check validity of the model + bool m_isKinematicsUpdated; ///< flag to check if kinematics of the robot is updated + iDynTree::Model m_model; ///< robot model for gravity compensation estimation + + iDynTree::Traversal m_dynamicTraversal; ///< Traversal used for dynamic computations + + /** + * Vector of Traversal used for the kinematic computations. + * m_kinematicTraversals[l] contains the traversal with base link l . + */ + std::vector m_kinematicTraversals; + + iDynTree::JointPosDoubleArray m_jointPos; ///< joint positions + iDynTree::JointDOFsDoubleArray m_jointDofsZero; ///< zero DOFArrays for joint velocities and joint accelerations + iDynTree::LinkVelArray m_linkVels; ///< link velocities + iDynTree::LinkAccArray m_linkProperAccs; ///< link proper accelerations + iDynTree::LinkNetExternalWrenches m_linkNetExternalWrenchesZero; ///< link external wrenches set to zero + iDynTree::LinkInternalWrenches m_linkIntWrenches; ///< link internal wrenches + iDynTree::FreeFloatingGeneralizedTorques m_generalizedTorques; ///< generalized torques + }; +} +#endif + diff --git a/src/estimation/include/iDynTree/KalmanFilter.h b/src/estimation/include/iDynTree/KalmanFilter.h new file mode 100644 index 00000000000..01ee3898be8 --- /dev/null +++ b/src/estimation/include/iDynTree/KalmanFilter.h @@ -0,0 +1,231 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef KALMAN_FILTER_H +#define KALMAN_FILTER_H + +#include +#include +#include +#include +#include + +namespace iDynTree +{ + /** + * @class DiscreteKalmanFilterHelper Time Invariant Discrete Kalman Filter with additive Gaussian noise + * + * The Kalman filter can be constructed by giving the system design matrices A, B, C and D. + * where A is the state transition matrix, B is the control input matrix, + * C is the output matrix and D the feed through matrix. + * These matrices can be used to describe a stochastic model for a linear dynamical system. + * + * \f[ x_{k+1} = A x_{k} + B u_{k} + w_k \f] + * \f[ y_{k+1} = C x_{k+1} + D u_{k} + v_k \f] + * + * Once the filter is constructed, the system noise and measurement noise covariance matrices can be set. + * The filter can be run, after setting initial state and state covariance matrix. + * The filter init method is called to check if the filter is properly configured and is ready to use. + * + * @warning care must be taken to design these matrices with proper dimensions, so that the filter does not crash + * + * Once the filter is configured, the filter algorithm can be run in a loop by, + * - setting input vector + * - prediction step which propagates the state through the modeled state dynamics + * - setting the measurement vector + * - correct the predicted states with the incoming measurements + * + */ + class DiscreteKalmanFilterHelper + { + public: + DiscreteKalmanFilterHelper(); + + /** + * @brief Describes the state propagation for a given dynamical system + * and the measurement model given the available measurements. + * If state of the system is denoted by \f$ x \f$, the control input by \f$ u \f$ + * and the measurements by \f$ y \f$, then + * the system dynamics is given as \f$ x_{k+1} = A x_{k} + B u_{k} + w_k \f$ + * and the measurement model is given by \f$ y_{k+1} = C x_{k+1} + D u_{k} + v_k \f$ + * + * @note all matrices are assumed to be time-invariant + * @param[in] A state transition matrix + * @param[in] B control input matrix mapping inputs to states + * @param[in] C output matrix mapping states to measurements + * @param[in] D feed through matrix mapping inputs to measurements + * @return bool true/false if filter was constructed successfully or not + */ + bool constructKalmanFilter(const iDynTree::MatrixDynSize& A, + const iDynTree::MatrixDynSize& B, + const iDynTree::MatrixDynSize& C, + const iDynTree::MatrixDynSize& D); + /** + * @overload + */ + bool constructKalmanFilter(const iDynTree::MatrixDynSize& A, + const iDynTree::MatrixDynSize& B, + const iDynTree::MatrixDynSize& C); + /** + * @overload + */ + bool constructKalmanFilter(const iDynTree::MatrixDynSize& A, + const iDynTree::MatrixDynSize& C); + + /** + * @brief Set initial state of the Kalman filter + * @warning this method must be called before calling kfInit() + * @param[in] x0 initial state of dimension \f$ dim_x \times 1 \f$ + * @return bool true/false successful or not + */ + bool kfSetInitialState(const iDynTree::VectorDynSize& x0); + + /** + * @brief Sets the state covariance matrix + * @warning this method must be called before calling kfInit() + * @param[in] P state covariance matrix of dimensions \f$ dim_x \times dim_x \f$ + * @return bool true/false successful or not + */ + bool kfSetStateCovariance(const iDynTree::MatrixDynSize& P); + + /** + * @brief Sets the system noise covariance matrix + * @warning this method must be called before calling kfInit() + * @param[in] Q system noise covariance matrix of dimensions \f$ dim_x \times dim_x \f$ + * @return bool true/false successful or not + */ + bool kfSetSystemNoiseCovariance(const iDynTree::MatrixDynSize& Q); + + /** + * @brief Sets the measurement noise covariance matrix + * @warning this method must be called before calling kfInit() + * @param[in] R measurement covariance matrix of dimensions \f$ dim_y \times dim_y \f$ + * @return bool true/false successful or not + */ + bool kfSetMeasurementNoiseCovariance(const iDynTree::MatrixDynSize& R); + + /** + * @brief This method checks if the filter is properly constructed and configured + * i.e. if initial states and covariance matrices are set. + * @warning this method must be called before calling kfPredict() or kfUpdate() + * @return bool true/false successful or not + */ + bool kfInit(); + + /** + * @brief Set inputs for the Kalman filter + * + * @param[in] u input vector of dimension \f$ dim_u \times 1 \f$ + * @return bool true/false successful or not + */ + bool kfSetInputVector(const iDynTree::VectorDynSize& u); + + /** + * @brief Runs one step of the Discrete Kalman Filter prediction equation + * described by + * \f$ \hat{x}_{k+1} = A x_{k} + B u_{k} \f$ + * \f$ \hat{P}_{k+1} = A_k P_k A_k^T + Q \f$ + * + * @warning this function can be called only after setting up the filter properly through kfInit() step + * @note in case if B matrix is constructed, this function should be once called every step, + * after setting up the input vector using kfSetInputVector() method. + * + * @return bool true/false if successful or not + */ + bool kfPredict(); + + /** + * @brief Set measurements for the Kalman filter + * + * @param[in] y input vector of dimension \f$ dim_y \times 1 \f$ + * @return bool true/false successful or not + */ + bool kfSetMeasurementVector(const iDynTree::VectorDynSize& y); + + /** + * @brief Runs one step of the Discrete Kalman Filter update equation + * described by + * \f$ \tilde{y}_{k+1} = C \hat{x}_{k+1} + D u_{k} \f$ + * \f$ x_{k+1} = \hat{x}_{k+1} + K_{k+1}(\tilde{y}_{k+1} - z_{k+1}) \f$ + * \f$ P_{k+1} = (I - K_{k+1} C) \hat{P}_{k+1} \f$ + * where K is the Kalman gain. + * @warning this function can be called only after setting up the filter properly through kfInit() step + * @note this function should be once called every step, + * after setting up the measurements vector using kfSetMeasurementVector() method. + * + * @return bool true/false if successful or not + */ + bool kfUpdate(); + + /** + * @brief Get system state + * + * @param[out] x system state of dimension \f$ dim_x \times 1 \f$ + * @return bool true/false successful or not + */ + bool kfGetStates(iDynTree::VectorDynSize &x); + + /** + * @brief Get system state covariance + * + * @param[out] P system state covariance matrix of dimension \f$ dim_x \times dim_x \f$ + * @return bool true/false successful or not + */ + bool kfGetStateCovariance(iDynTree::MatrixDynSize &P); + + /** + * @brief Reset Kalman filter + * Resets the Kalman filter and initializes with the internally stored states and + * matrices initially set by the user. + */ + bool kfReset(); + + /** + * @brief Reset Kalman filter with the given arguments + * @warning the system matrices A, B, C and D are unchanged with the reset filter + * + * @param[in] x0 initial state vector of dimensions \f$ dim_x \times 1 \f$ + * @param[in] P state covariance matrix of dimensions \f$ dim_x \times dim_x \f$ + * @param[in] Q system noise covariance matrix of dimensions \f$ dim_x \times dim_x \f$ + * @param[in] R measurement noise covariance matrix of dimensions \f$ dim_y \times dim_y \f$ + */ + bool kfReset(const iDynTree::VectorDynSize& x0, const iDynTree::MatrixDynSize& P0, + const iDynTree::MatrixDynSize& Q, const iDynTree::MatrixDynSize& R); + + private: + size_t m_dim_X; ///< state dimension + size_t m_dim_Y; ///< output dimenstion + size_t m_dim_U; ///< input dimension + + iDynTree::VectorDynSize m_x; ///< state at time instant k + iDynTree::VectorDynSize m_x0; ///< buffer to store initial state + iDynTree::VectorDynSize m_u; ///< input at time instant k + iDynTree::VectorDynSize m_y; ///< measurements at time instant k + + iDynTree::MatrixDynSize m_A; ///< state transition matrix + iDynTree::MatrixDynSize m_B; ///< control input matrix + iDynTree::MatrixDynSize m_C; ///< output matrix + iDynTree::MatrixDynSize m_D; ///< feedthrough matrix + + iDynTree::MatrixDynSize m_P; ///< state covariance matrix + iDynTree::MatrixDynSize m_P0; ///< initial state covariance matrix + iDynTree::MatrixDynSize m_Q; ///< system noise covariance matrix + iDynTree::MatrixDynSize m_R; ///< measurement covariance matrix + + bool m_is_initialized{false}; ///< flag to check if filter is initialized + + bool m_filter_constructed{false}; ///< flag to check if the filter is constructed properly with the A, B, C, D matrices + bool m_initial_state_set{false}; ///< flag to check if initial state is set + bool m_initial_state_covariance_set{false}; ///< flag to check if initial state covariance matrix is set + bool m_measurement_noise_covariance_matrix_set{false}; ///< flag to check if measurement noise covariance matrix is set + bool m_system_noise_covariance_matrix_set{false}; ///< flag to check if system noise covariance matrix is set + + bool m_measurement_updated{false}; ///< flag to check if measurement is updated + bool m_input_updated{false}; ///< flag to check if input is updated + + bool m_use_feed_through{false}; ///< toggle to use feed through matrix D + bool m_use_control_input{false}; ///< toggle to use control input matrix B + }; +} + +#endif diff --git a/src/estimation/include/iDynTree/SchmittTrigger.h b/src/estimation/include/iDynTree/SchmittTrigger.h new file mode 100644 index 00000000000..59f451f9d3b --- /dev/null +++ b/src/estimation/include/iDynTree/SchmittTrigger.h @@ -0,0 +1,175 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_SCHMITT_TRIGGER_H +#define IDYNTREE_SCHMITT_TRIGGER_H + +#include +namespace iDynTree +{ + /** + * Schmitt Trigger class for binary state detection + * This device is used to obtain a binary state (ON/OFF) at + * some time instant depending on a value inputted to the device. + * It is initialized with 4 parameters, + * + * | Parameter | Type | Description | + * |:----------------:|:----:|:--------------------------------------------------------------------:| + * |stableOFFTime |double|Time to elapse to switch to OFF state, once low threshold is triggered| + * |stableONTime |double|Time to elapse to switch to ON state, once high threshold is triggered| + * |lowValueThreshold |double| Threshold Value to turn state OFF | + * |highValueThreshold|double| Threshold Value to turn state ON | + * + * The device is first configured with the user-defined parameter settings, but is not activated until + * the first call of updateDevice method. The updateDevice method must be called at every iteration with + * the current time and the value to be compared, for continuously updating the binary state of the device. + * + * For instance, if the current state is OFF and there is a call to updateDevice with current time and an input + * value greater than the highValueThreshold, then a timer is activated and is updated at every call to updateMethod. + * If there are consecutive calls to the updateMethod with input values greater than the highValueThreshold, + * until the timer lapses stableONTime, then the state is switched to ON, otherwise it is remains OFF. + * + * NOTE: Time is specified relative to the process that is instantiating this device. + * Default state is set to ON during instantiation. This can be changed accordingly using the setInitialState method. + * + * NOTE: There are no default parameters to the Schmitt Trigger. These parameters are set through + * the constructor during instantiation. + */ + class SchmittTrigger + { + public: + /** + * Constructor + * @param stableOFFTime time to elapse to switch to OFF state + * @param stableONTime time to elapse to switch to ON state + * @param lowValueThreshold threshold value to turn state OFF + * @param highValueThreshold threshold value to turn state ON + */ + SchmittTrigger(double stableOFFTime, + double stableONTime, + double lowValueThreshold, + double highValueThreshold); + + /** + * Sets the schmitt trigger settings to default values + */ + void resetDevice(); + + /** + * Update the device state with the latest time and input measurement + * @param currentTime current time + * @param rawValue input measurement + */ + void updateDevice(double currentTime, double rawValue); + + /** + * @name Setters + */ + //@{ + + /** + * Configures the Schmitt Trigger parameters + * (can be called after instantiation, to change the parameters) + * @param stableOFFTime time to elapse to switch to OFF state + * @param stableONTime time to elapse to switch to ON state + * @param lowValueThreshold threshold value to turn state OFF + * @param highValueThreshold threshold value to turn state ON + */ + void configure(double stableOFFTime, + double stableONTime, + double lowValueThreshold, + double highValueThreshold); + + /** + * set required time to elapse to switch to OFF state + * @param stableOFFTime + */ + void setStableOFFTime(double stableOFFTime) { m_stableOFFTime = stableOFFTime; } + + /** + * set required time to elapse to switch to ON state + * @param stableONTime + */ + void setStableONTime(double stableONTime) { m_stableONTime = stableONTime; } + + /** + * set low threshold value to trigger OFF state detection + * @param lowValueThreshold + */ + void setLowValueThreshold(double lowValueThreshold) { m_lowValueThreshold = lowValueThreshold; } + + /** + * set high threshold value to trigger ON state detection + * @param highValueThreshold + */ + void setHighValueThreshold(double highValueThreshold) { m_highValueThreshold = highValueThreshold; } + + /** + * set initial state + * @param state binary state true/false + */ + void setInitialState(bool state) { m_currentState = state; } + //@} + + /** + * @name Getters + */ + //@{ + + /** + * get current state + * @return binary state true/false + */ + bool getState() { return m_currentState; } + + /** + * get time elapsed since the first update of the device + * @return time + */ + double getElapsedTime() { return m_previousTime; } + //@} + + + /** + * @name Verbose + */ + //@{ + void setVerbose() {m_verbose = 1;} + void unsetVerbose() {m_verbose = 0;} + //@} + private: + + /** + * @name State + */ + //@{ + bool m_currentState; + double m_previousTime; + double m_timer; + //@} + + /** + * @name Device Parameters + */ + //@{ + double m_stableOFFTime; + double m_stableONTime; + double m_highValueThreshold; + double m_lowValueThreshold; + //@} + + /** + * @name Input + */ + //@{ + double m_rawValue; + //@} + + + // Verbose flag + int m_verbose; + + }; +} + +#endif \ No newline at end of file diff --git a/src/estimation/include/iDynTree/SimpleLeggedOdometry.h b/src/estimation/include/iDynTree/SimpleLeggedOdometry.h new file mode 100644 index 00000000000..c460ed5b402 --- /dev/null +++ b/src/estimation/include/iDynTree/SimpleLeggedOdometry.h @@ -0,0 +1,243 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_SIMPLE_LEGGED_ODOMETRY2_ +#define IDYNTREE_SIMPLE_LEGGED_ODOMETRY2_ + +#include +#include +#include +#include +#include +#include + +namespace iDynTree +{ + +/** + * \ingroup iDynTreeEstimation + * + * A simple legged odometry for a legged robot. + * + * Under the assumption that at least a link of the robot at the time is + * not moving (no slippage), it computes the estimate of the transform + * between a inertial/world frame and the robot floating base. + * + * The algorithm implemented is the following : + * + * -# During initialization the user of the class specifies (through init()): + * * a frame that is rigidly attached to a link that is not moving (`initialFixedFrame`) + * * an optional transform between the desired location of the world and the fixed frame (`world_H_initialFixedFrame`) + * At the beginning, the `world_H_fixed` is the one specified (default is the identity) + * + * -# At this point, the `getWorldFrameTransform(iDynTree::FrameIndex frame_id)` will return the `world_H_frame` + * ( \f${}^{world} H_{frame}\f$ ) transform simply by computing the forward kinematics from the fixed frame + * to the frame specified by `frame_id` : `world_H_frame = world_H_fixed * fixed_H_frame(qj)`, i.e. + * \f${}^{world} H_{frame} = {}^{world} H_{fixed} \cdot {}^{fixed} H_{frame}(q_j)\f$ + * + * -# If the fixed frame changes, we can simply change the frame used as "fixed" (changeFixedLink()), and consistently update the + * `world_H_fixed` transform to be equal to `world_H_new_fixed = world_H_old_fixed * old_fixed_H_new_fixed(qj)`, i.e. + * \f${}^{world} H_{new\_fixed} = {}^{world} H_{old\_fixed} \cdot {}^{old\_fixed} H_{new\_fixed}(q_j)\f$ + * + * -# After the update, the `getWorldFrameTransform(iDynTree::FrameIndex frame_id)` can be obtained as in point 1b . + * + * To reset the location of the world, init can simply be called again. + * + */ +class SimpleLeggedOdometry +{ + /** + * Model used in odometry. + */ + Model m_model; + + /** + * Traversal used for the dynamics computations + */ + Traversal m_traversal; + + /** + * False initially, true after a valid model has been loaded. + */ + bool m_isModelValid; + + /** + * False initially, true after updateKinematics was successfully called. + */ + bool m_kinematicsUpdated; + + /** + * False initially, true after init was successfully called. + */ + bool m_isOdometryInitialized; + + /** + * The link index of the link considered fixed. + */ + LinkIndex m_fixedLinkIndex; + + /** + * Position of each link with respect to the base + * link of the traversal (i.e. output + * of the relative forward kinematics). + */ + LinkPositions m_base_H_link; + + /** + * Transform between the link currently considered fixed + * and the world/inertial frame. + * This is initialized in the init method and update + * by the changeFixedLink method. + */ + Transform m_world_H_fixedLink; + +public: + /** + * Constructor + */ + SimpleLeggedOdometry(); + + /** + * Destructor + */ + ~SimpleLeggedOdometry(); + + /** + * \brief Set model used for the odometry. + * The model is copied inside the class, to be used for the odometry estimation. + * + * @param[in] _model the kinematic and dynamic model used for the estimation. + * @return true if all went well (model is well formed), false otherwise. + */ + bool setModel(const Model & _model); + + /** + * Get used model. + * + * @return the kinematic and dynamic model used for estimation. + */ + const Model & model() const; + + /** + * Set the measured joint positions. + * + * This is used to update the joints positions used by the odometry. + * + */ + bool updateKinematics(JointPosDoubleArray & jointPos); + + /** + * Initialize the odometry. + * This method initializes the world location w.r.t. to a frame + * that is not the frame that is initially assumed fixed. + * For this reason it is necessary to call updateKinematics at least once before calling this method. + * + * @param[in] initialFixedFrame Frame initially assumed to be fixed. + * @param[in] initialFixedFrame_H_world Pose of the world w.r.t. the initial fixed frame (default: identity, i.e. the initialFixedFrame is the world). + * @return true if all went well, false otherwise. + */ + bool init(const std::string & initialFixedFrame, + const Transform initialFixedFrame_H_world = Transform::Identity()); + + /** + * Initialize the odometry. + * This method initializes the world location w.r.t. to a frame + * that is not the frame that is initially assumed fixed. + * For this reason it is necessary to call updateKinematics at least once before calling this method. + * + * @param[in] initialFixedFrameIndex Frame initially assumed to be fixed. + * @param[in] initialFixedFrame_H_world Pose of the world w.r.t. the initial fixed frame (default: identity, i.e. the initialFixedFrame is the world). + * @return true if all went well, false otherwise. + */ + bool init(const FrameIndex initialFixedFrameIndex, + const Transform initialFixedFrame_H_world = Transform::Identity()); + + /** + * Initialize the odometry, specifying separately initial fixed frame and world. + * This method initializes the world location w.r.t. to a frame + * that is not the frame that is initially assumed fixed, + * for this reason it is necessary to call updateKinematics at least once before calling this method. + * + * @param[in] initialFixedFrame Frame initially assumed to be fixed. + * @param[in] initialReferenceFrameForWorld Frame in which the initial world is expressed. + * @param[in] initialReferenceFrame_H_world Pose of the world w.r.t. the initial reference frame (default: identity, i.e. the initialReferenceFrameForWorld is the world). + * @return true if all went well, false otherwise. + */ + bool init(const std::string & initialFixedFrame, + const std::string & initialReferenceFrameForWorld, + const Transform initialReferenceFrame_H_world = Transform::Identity()); + + /** + * Initialize the odometry, specifying separately initial fixed frame and world. + * This method initializes the world location w.r.t. to a frame + * that is not the frame that is initially assumed fixed, + * for this reason it is necessary to call updateKinematics at least once before calling this method. + * + * @param[in] initialFixedFrameIndex Frame initially assumed to be fixed. + * @param[in] initialReferenceFrameIndexForWorld Frame in which the initial world is expressed. + * @param[in] initialReferenceFrame_H_world Pose of the world w.r.t. the initial reference frame (default: identity, i.e. the initialReferenceFrameForWorld is the world). + * @return true if all went well, false otherwise. + */ + bool init(const FrameIndex initialFixedFrameIndex, + const FrameIndex initialReferenceFrameIndexForWorld, + const Transform initialReferenceFrame_H_world = Transform::Identity()); + + /** + * Change the link that the odometry assumes to be fixed with + * respect to the inertial/world frame. + */ + bool changeFixedFrame(const std::string & newFixedFrame); + + /** + * Change the link that the odometry assumes to be fixed + * with respect to the inertial/world frame. + */ + bool changeFixedFrame(const FrameIndex newFixedFrame); + + /** + * Change the link that the odometry assumes to be fixed + * with respect to the inertial/world frame. + * + * \note The position of the external frame is set by the user + */ + bool changeFixedFrame(const std::string & newFixedFrame, + const Transform & world_H_newFixedFrame); + + /** + * Change the link that the odometry assumes to be fixed + * with respect to the inertial/world frame. + * + * \note The position of the external frame is set by the user + */ + bool changeFixedFrame(const FrameIndex newFixedFrame, + const Transform & world_H_newFixedFrame); + + /** + * Get the link currently considered fixed with respect to the inertial frame. + * + * \note This can be diffent from what was set with changeFixedFrame, because + * multiple frames can belong to the same link. + * + * @return the name of the link currently considered fixed. + */ + std::string getCurrentFixedLink(); + + /** + * Get the world_H_link transform for an arbitrary link. + * + * \note this method works only for link, not for arbitrary frames. + */ + iDynTree::Transform getWorldLinkTransform(const LinkIndex frame_index); + + /** + * Get the world_H_frame transform for an arbitrary frame. + * + * \note this method works also for arbitrary frames. + */ + iDynTree::Transform getWorldFrameTransform(const FrameIndex frame_index); +}; + + +} // End namespace iDynTree + +#endif // IDYNTREE_SIMPLE_LEGGED_ODOMETRY2_ diff --git a/src/estimation/src/AttitudeEstimator.cpp b/src/estimation/src/AttitudeEstimator.cpp index 30834f3b1b9..5af1ec53020 100644 --- a/src/estimation/src/AttitudeEstimator.cpp +++ b/src/estimation/src/AttitudeEstimator.cpp @@ -1,7 +1,7 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include +#include iDynTree::IAttitudeEstimator::~IAttitudeEstimator() diff --git a/src/estimation/src/AttitudeEstimatorUtils.cpp b/src/estimation/src/AttitudeEstimatorUtils.cpp index 88d05fc110d..5fe57165bfd 100644 --- a/src/estimation/src/AttitudeEstimatorUtils.cpp +++ b/src/estimation/src/AttitudeEstimatorUtils.cpp @@ -1,7 +1,7 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include +#include #include #include diff --git a/src/estimation/src/AttitudeMahonyFilter.cpp b/src/estimation/src/AttitudeMahonyFilter.cpp index c624961ebac..b91277644a3 100644 --- a/src/estimation/src/AttitudeMahonyFilter.cpp +++ b/src/estimation/src/AttitudeMahonyFilter.cpp @@ -1,8 +1,8 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include -#include +#include +#include #include iDynTree::Matrix3x3 getMatrixFromVectorVectorMultiplication(iDynTree::Vector3 a, iDynTree::Vector3 b) diff --git a/src/estimation/src/AttitudeQuaternionEKF.cpp b/src/estimation/src/AttitudeQuaternionEKF.cpp index 5697f5e0f2f..f2ee1b594dd 100644 --- a/src/estimation/src/AttitudeQuaternionEKF.cpp +++ b/src/estimation/src/AttitudeQuaternionEKF.cpp @@ -1,8 +1,8 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include -#include +#include +#include void iDynTree::AttitudeQuaternionEKF::serializeStateVector() { diff --git a/src/estimation/src/BerdyHelper.cpp b/src/estimation/src/BerdyHelper.cpp index 6beff693c55..50e9b938793 100644 --- a/src/estimation/src/BerdyHelper.cpp +++ b/src/estimation/src/BerdyHelper.cpp @@ -1,20 +1,20 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include +#include -#include -#include -#include +#include +#include +#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include -#include +#include #include #include diff --git a/src/estimation/src/BerdySparseMAPSolver.cpp b/src/estimation/src/BerdySparseMAPSolver.cpp index 097504add9f..329dea5795f 100644 --- a/src/estimation/src/BerdySparseMAPSolver.cpp +++ b/src/estimation/src/BerdySparseMAPSolver.cpp @@ -1,13 +1,13 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include "iDynTree/Estimation/BerdySparseMAPSolver.h" -#include "iDynTree/Estimation/BerdyHelper.h" - -#include -#include -#include -#include -#include +#include "iDynTree/BerdySparseMAPSolver.h" +#include "iDynTree/BerdyHelper.h" + +#include +#include +#include +#include +#include #include #include diff --git a/src/estimation/src/BipedFootContactClassifier.cpp b/src/estimation/src/BipedFootContactClassifier.cpp index dea33c4c809..172780a9502 100644 --- a/src/estimation/src/BipedFootContactClassifier.cpp +++ b/src/estimation/src/BipedFootContactClassifier.cpp @@ -1,7 +1,7 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include "iDynTree/Estimation/BipedFootContactClassifier.h" +#include "iDynTree/BipedFootContactClassifier.h" namespace iDynTree { diff --git a/src/estimation/src/ContactStateMachine.cpp b/src/estimation/src/ContactStateMachine.cpp index 9d77818839e..51b1ba8ebc8 100644 --- a/src/estimation/src/ContactStateMachine.cpp +++ b/src/estimation/src/ContactStateMachine.cpp @@ -1,7 +1,7 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include "iDynTree/Estimation/ContactStateMachine.h" +#include "iDynTree/ContactStateMachine.h" namespace iDynTree { diff --git a/src/estimation/src/ExtWrenchesAndJointTorquesEstimator.cpp b/src/estimation/src/ExtWrenchesAndJointTorquesEstimator.cpp index 693f407f7a1..6d47c518022 100644 --- a/src/estimation/src/ExtWrenchesAndJointTorquesEstimator.cpp +++ b/src/estimation/src/ExtWrenchesAndJointTorquesEstimator.cpp @@ -1,31 +1,31 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include -#include - -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include - -#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include + +#include #include diff --git a/src/estimation/src/ExtendedKalmanFilter.cpp b/src/estimation/src/ExtendedKalmanFilter.cpp index b60523d3b09..f571e7c61d7 100644 --- a/src/estimation/src/ExtendedKalmanFilter.cpp +++ b/src/estimation/src/ExtendedKalmanFilter.cpp @@ -1,8 +1,8 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include -#include +#include +#include iDynTree::DiscreteExtendedKalmanFilterHelper::DiscreteExtendedKalmanFilterHelper() { diff --git a/src/estimation/src/ExternalWrenchesEstimation.cpp b/src/estimation/src/ExternalWrenchesEstimation.cpp index ff48e76c072..6207c850d95 100644 --- a/src/estimation/src/ExternalWrenchesEstimation.cpp +++ b/src/estimation/src/ExternalWrenchesEstimation.cpp @@ -1,21 +1,21 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include - -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -#include -#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include namespace iDynTree { diff --git a/src/estimation/src/GravityCompensationHelpers.cpp b/src/estimation/src/GravityCompensationHelpers.cpp index 7fb2e09db82..1df31353815 100644 --- a/src/estimation/src/GravityCompensationHelpers.cpp +++ b/src/estimation/src/GravityCompensationHelpers.cpp @@ -1,7 +1,7 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include "iDynTree/Estimation/GravityCompensationHelpers.h" +#include "iDynTree/GravityCompensationHelpers.h" namespace iDynTree { diff --git a/src/estimation/src/KalmanFilter.cpp b/src/estimation/src/KalmanFilter.cpp index 517b48385bc..ea14c246b65 100644 --- a/src/estimation/src/KalmanFilter.cpp +++ b/src/estimation/src/KalmanFilter.cpp @@ -1,7 +1,7 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include +#include iDynTree::DiscreteKalmanFilterHelper::DiscreteKalmanFilterHelper() { diff --git a/src/estimation/src/SchmittTrigger.cpp b/src/estimation/src/SchmittTrigger.cpp index 52843c902d5..6b716a2100f 100644 --- a/src/estimation/src/SchmittTrigger.cpp +++ b/src/estimation/src/SchmittTrigger.cpp @@ -1,7 +1,7 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include "iDynTree/Estimation/SchmittTrigger.h" +#include "iDynTree/SchmittTrigger.h" namespace iDynTree { diff --git a/src/estimation/src/SimpleLeggedOdometry.cpp b/src/estimation/src/SimpleLeggedOdometry.cpp index dd78158cd04..7db6db5c647 100644 --- a/src/estimation/src/SimpleLeggedOdometry.cpp +++ b/src/estimation/src/SimpleLeggedOdometry.cpp @@ -2,15 +2,15 @@ // SPDX-License-Identifier: BSD-3-Clause -#include +#include -#include +#include -#include -#include -#include +#include +#include +#include -#include +#include #include diff --git a/src/estimation/tests/AttitudeEstimatorUnitTest.cpp b/src/estimation/tests/AttitudeEstimatorUnitTest.cpp index c02300f94dd..a56ca83b493 100644 --- a/src/estimation/tests/AttitudeEstimatorUnitTest.cpp +++ b/src/estimation/tests/AttitudeEstimatorUnitTest.cpp @@ -1,11 +1,11 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include #include diff --git a/src/estimation/tests/BerdyHelperUnitTest.cpp b/src/estimation/tests/BerdyHelperUnitTest.cpp index 39a392cbdcf..53d36dececf 100644 --- a/src/estimation/tests/BerdyHelperUnitTest.cpp +++ b/src/estimation/tests/BerdyHelperUnitTest.cpp @@ -1,20 +1,20 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include -#include +#include +#include -#include +#include #include "testModels.h" -#include -#include -#include -#include -#include - -#include -#include +#include +#include +#include +#include +#include + +#include +#include #include #include diff --git a/src/estimation/tests/BerdyMAPSolverUnitTest.cpp b/src/estimation/tests/BerdyMAPSolverUnitTest.cpp index 53a4127ea7f..022a5ab4355 100644 --- a/src/estimation/tests/BerdyMAPSolverUnitTest.cpp +++ b/src/estimation/tests/BerdyMAPSolverUnitTest.cpp @@ -1,13 +1,13 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include +#include -#include +#include -#include -#include -#include +#include +#include +#include #include #include diff --git a/src/estimation/tests/ExtWrenchesAndJointTorquesEstimatorUnitTest.cpp b/src/estimation/tests/ExtWrenchesAndJointTorquesEstimatorUnitTest.cpp index 37151d3706b..922f3d72b8c 100644 --- a/src/estimation/tests/ExtWrenchesAndJointTorquesEstimatorUnitTest.cpp +++ b/src/estimation/tests/ExtWrenchesAndJointTorquesEstimatorUnitTest.cpp @@ -1,13 +1,13 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include -#include +#include +#include #include "testModels.h" -#include -#include +#include +#include using namespace iDynTree; diff --git a/src/estimation/tests/ExternalWrenchesEstimationUnitTest.cpp b/src/estimation/tests/ExternalWrenchesEstimationUnitTest.cpp index ab6a0aae0dc..dfb791de9d9 100644 --- a/src/estimation/tests/ExternalWrenchesEstimationUnitTest.cpp +++ b/src/estimation/tests/ExternalWrenchesEstimationUnitTest.cpp @@ -1,23 +1,23 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include - -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include #include #include diff --git a/src/estimation/tests/KalmanFilterUnitTest.cpp b/src/estimation/tests/KalmanFilterUnitTest.cpp index b591460782b..4aaaed79472 100644 --- a/src/estimation/tests/KalmanFilterUnitTest.cpp +++ b/src/estimation/tests/KalmanFilterUnitTest.cpp @@ -1,8 +1,8 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include -#include +#include +#include #include #include diff --git a/src/estimation/tests/SimpleLeggedOdometryUnitTest.cpp b/src/estimation/tests/SimpleLeggedOdometryUnitTest.cpp index e6b7d2f5425..a8571badb95 100644 --- a/src/estimation/tests/SimpleLeggedOdometryUnitTest.cpp +++ b/src/estimation/tests/SimpleLeggedOdometryUnitTest.cpp @@ -1,12 +1,12 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include +#include #include "testModels.h" -#include -#include +#include +#include using namespace iDynTree; diff --git a/src/high-level/CMakeLists.txt b/src/high-level/CMakeLists.txt index 4dc0645067e..563f5f93296 100644 --- a/src/high-level/CMakeLists.txt +++ b/src/high-level/CMakeLists.txt @@ -17,7 +17,7 @@ add_library(iDynTree::${libraryname} ALIAS ${libraryname}) target_include_directories(${libraryname} PUBLIC "$" "$") -target_link_libraries(${libraryname} PUBLIC idyntree-core idyntree-model idyntree-sensors idyntree-modelio +target_link_libraries(${libraryname} PUBLIC idyntree-core idyntree-model idyntree-modelio PRIVATE Eigen3::Eigen) set_property(TARGET ${libraryname} PROPERTY PUBLIC_HEADER ${IDYNTREE_HIGH_LEVEL_HEADERS}) diff --git a/src/high-level/include/iDynTree/KinDynComputations.h b/src/high-level/include/iDynTree/KinDynComputations.h index 1430093637d..7585dee518f 100644 --- a/src/high-level/include/iDynTree/KinDynComputations.h +++ b/src/high-level/include/iDynTree/KinDynComputations.h @@ -7,15 +7,15 @@ #include #include -#include -#include -#include -#include -#include - -#include -#include -#include +#include +#include +#include +#include +#include + +#include +#include +#include namespace iDynTree { diff --git a/src/high-level/src/KinDynComputations.cpp b/src/high-level/src/KinDynComputations.cpp index ababafeef10..342c2d57358 100644 --- a/src/high-level/src/KinDynComputations.cpp +++ b/src/high-level/src/KinDynComputations.cpp @@ -3,29 +3,29 @@ #include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include #include #include diff --git a/src/high-level/tests/KinDynComputationsMatrixViewAndSpanUnitTest.cpp b/src/high-level/tests/KinDynComputationsMatrixViewAndSpanUnitTest.cpp index 7363424f92f..3312360888a 100644 --- a/src/high-level/tests/KinDynComputationsMatrixViewAndSpanUnitTest.cpp +++ b/src/high-level/tests/KinDynComputationsMatrixViewAndSpanUnitTest.cpp @@ -2,23 +2,23 @@ // SPDX-License-Identifier: BSD-3-Clause #include "testModels.h" -#include +#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include -#include +#include #include -#include -#include -#include +#include +#include +#include -#include +#include namespace Eigen diff --git a/src/high-level/tests/KinDynComputationsUnitTest.cpp b/src/high-level/tests/KinDynComputationsUnitTest.cpp index f40da4f9df1..f1315ac5c51 100644 --- a/src/high-level/tests/KinDynComputationsUnitTest.cpp +++ b/src/high-level/tests/KinDynComputationsUnitTest.cpp @@ -2,26 +2,26 @@ // SPDX-License-Identifier: BSD-3-Clause #include "testModels.h" -#include +#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include -#include +#include #include -#include -#include -#include -#include -#include -#include - -#include +#include +#include +#include +#include +#include +#include + +#include using namespace iDynTree; diff --git a/src/icub/include/iDynTree/iKinConversionsImplementation.h b/src/icub/include/iDynTree/iKinConversionsImplementation.h index 15957cba437..9f26b807a7a 100644 --- a/src/icub/include/iDynTree/iKinConversionsImplementation.h +++ b/src/icub/include/iDynTree/iKinConversionsImplementation.h @@ -5,9 +5,9 @@ #define IDYNTREE_IKIN_CONVERSIONS_IMPLEMENTATION_H #include -#include +#include -#include +#include #include diff --git a/src/icub/include/iDynTree/skinDynLibConversions.h b/src/icub/include/iDynTree/skinDynLibConversions.h index 9c48acdbe96..cdc103ebc74 100644 --- a/src/icub/include/iDynTree/skinDynLibConversions.h +++ b/src/icub/include/iDynTree/skinDynLibConversions.h @@ -7,8 +7,8 @@ #include #include -#include -#include +#include +#include #include #include diff --git a/src/icub/include/iDynTree/skinDynLibConversionsImplementation.h b/src/icub/include/iDynTree/skinDynLibConversionsImplementation.h index e2ce4fee1df..ae67ef1cfb0 100644 --- a/src/icub/include/iDynTree/skinDynLibConversionsImplementation.h +++ b/src/icub/include/iDynTree/skinDynLibConversionsImplementation.h @@ -6,14 +6,14 @@ #include -#include -#include +#include +#include -#include -#include -#include +#include +#include +#include -#include +#include #include diff --git a/src/inverse-kinematics/include/iDynTree/BoundingBoxHelpers.h b/src/inverse-kinematics/include/iDynTree/BoundingBoxHelpers.h index 05a80b63bf7..4fb9a3390bd 100644 --- a/src/inverse-kinematics/include/iDynTree/BoundingBoxHelpers.h +++ b/src/inverse-kinematics/include/iDynTree/BoundingBoxHelpers.h @@ -7,10 +7,10 @@ #include #include -#include -#include -#include -#include +#include +#include +#include +#include #include namespace iDynTree diff --git a/src/inverse-kinematics/include/iDynTree/ConvexHullHelpers.h b/src/inverse-kinematics/include/iDynTree/ConvexHullHelpers.h index 33d4f57358d..b28fd402dd0 100644 --- a/src/inverse-kinematics/include/iDynTree/ConvexHullHelpers.h +++ b/src/inverse-kinematics/include/iDynTree/ConvexHullHelpers.h @@ -8,10 +8,10 @@ #include #include -#include -#include -#include -#include +#include +#include +#include +#include namespace iDynTree diff --git a/src/inverse-kinematics/include/iDynTree/InverseKinematics.h b/src/inverse-kinematics/include/iDynTree/InverseKinematics.h index 3981be6e144..342acae2393 100644 --- a/src/inverse-kinematics/include/iDynTree/InverseKinematics.h +++ b/src/inverse-kinematics/include/iDynTree/InverseKinematics.h @@ -9,7 +9,7 @@ #include #include -#include +#include namespace iDynTree { class VectorDynSize; diff --git a/src/inverse-kinematics/include/private/InverseKinematicsData.h b/src/inverse-kinematics/include/private/InverseKinematicsData.h index b2b809cc0d3..99c37fbc215 100644 --- a/src/inverse-kinematics/include/private/InverseKinematicsData.h +++ b/src/inverse-kinematics/include/private/InverseKinematicsData.h @@ -9,11 +9,11 @@ #include #include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include diff --git a/src/inverse-kinematics/include/private/InverseKinematicsNLP.h b/src/inverse-kinematics/include/private/InverseKinematicsNLP.h index fe13b964ac7..3a8631250a3 100644 --- a/src/inverse-kinematics/include/private/InverseKinematicsNLP.h +++ b/src/inverse-kinematics/include/private/InverseKinematicsNLP.h @@ -5,11 +5,11 @@ #define IDYNTREE_INTERNAL_INVERSEKINEMATICSNLP_H #include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include diff --git a/src/inverse-kinematics/include/private/TransformConstraint.h b/src/inverse-kinematics/include/private/TransformConstraint.h index 71f7401ffd6..0b2cf628f16 100644 --- a/src/inverse-kinematics/include/private/TransformConstraint.h +++ b/src/inverse-kinematics/include/private/TransformConstraint.h @@ -4,7 +4,7 @@ #ifndef IDYNTREE_INTERNAL_TRANSFORM_CONSTRAINT_H #define IDYNTREE_INTERNAL_TRANSFORM_CONSTRAINT_H -#include +#include #include namespace internal { diff --git a/src/inverse-kinematics/src/BoundingBoxHelpers.cpp b/src/inverse-kinematics/src/BoundingBoxHelpers.cpp index f0947891f56..369a3e5c6f6 100644 --- a/src/inverse-kinematics/src/BoundingBoxHelpers.cpp +++ b/src/inverse-kinematics/src/BoundingBoxHelpers.cpp @@ -3,8 +3,8 @@ #include -#include -#include +#include +#include #include diff --git a/src/inverse-kinematics/src/ConvexHullHelpers.cpp b/src/inverse-kinematics/src/ConvexHullHelpers.cpp index aec3362f122..c8e50b83bab 100644 --- a/src/inverse-kinematics/src/ConvexHullHelpers.cpp +++ b/src/inverse-kinematics/src/ConvexHullHelpers.cpp @@ -3,8 +3,8 @@ #include -#include -#include +#include +#include #include diff --git a/src/inverse-kinematics/src/InverseKinematics.cpp b/src/inverse-kinematics/src/InverseKinematics.cpp index a86a1ae59a6..1e2b3a03a28 100644 --- a/src/inverse-kinematics/src/InverseKinematics.cpp +++ b/src/inverse-kinematics/src/InverseKinematics.cpp @@ -6,12 +6,12 @@ #include "InverseKinematicsData.h" #include "TransformConstraint.h" -#include -#include -#include -#include +#include +#include +#include +#include -#include +#include #include #include diff --git a/src/inverse-kinematics/src/InverseKinematicsData.cpp b/src/inverse-kinematics/src/InverseKinematicsData.cpp index cc9ffe38d32..a1eb2ceb915 100644 --- a/src/inverse-kinematics/src/InverseKinematicsData.cpp +++ b/src/inverse-kinematics/src/InverseKinematicsData.cpp @@ -4,12 +4,12 @@ #include "InverseKinematicsData.h" #include "InverseKinematicsNLP.h" #include "TransformConstraint.h" -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include #include #include diff --git a/src/inverse-kinematics/src/InverseKinematicsNLP.cpp b/src/inverse-kinematics/src/InverseKinematicsNLP.cpp index 74f1bd3365e..d212d089fe9 100644 --- a/src/inverse-kinematics/src/InverseKinematicsNLP.cpp +++ b/src/inverse-kinematics/src/InverseKinematicsNLP.cpp @@ -12,7 +12,7 @@ #include "TransformConstraint.h" #include -#include +#include #include #include diff --git a/src/inverse-kinematics/src/TransformConstraint.cpp b/src/inverse-kinematics/src/TransformConstraint.cpp index bdf43b433bd..8e66469519e 100644 --- a/src/inverse-kinematics/src/TransformConstraint.cpp +++ b/src/inverse-kinematics/src/TransformConstraint.cpp @@ -2,7 +2,7 @@ // SPDX-License-Identifier: BSD-3-Clause #include "TransformConstraint.h" -#include +#include namespace internal { namespace kinematics { diff --git a/src/inverse-kinematics/tests/ConvexHullHelpersUnitTest.cpp b/src/inverse-kinematics/tests/ConvexHullHelpersUnitTest.cpp index b2ef5be0216..98caa6a0103 100644 --- a/src/inverse-kinematics/tests/ConvexHullHelpersUnitTest.cpp +++ b/src/inverse-kinematics/tests/ConvexHullHelpersUnitTest.cpp @@ -3,8 +3,8 @@ #include -#include -#include +#include +#include void testConvexHullProjectionConstraint() { diff --git a/src/inverse-kinematics/tests/InverseKinematicsMatrixViewAndSpanUnitTest.cpp b/src/inverse-kinematics/tests/InverseKinematicsMatrixViewAndSpanUnitTest.cpp index e51caff45cf..bfa4ebd7277 100644 --- a/src/inverse-kinematics/tests/InverseKinematicsMatrixViewAndSpanUnitTest.cpp +++ b/src/inverse-kinematics/tests/InverseKinematicsMatrixViewAndSpanUnitTest.cpp @@ -4,12 +4,12 @@ #include #include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include #include "testModels.h" diff --git a/src/inverse-kinematics/tests/InverseKinematicsUnitTest.cpp b/src/inverse-kinematics/tests/InverseKinematicsUnitTest.cpp index f7b4630bc7d..4a5a8404231 100644 --- a/src/inverse-kinematics/tests/InverseKinematicsUnitTest.cpp +++ b/src/inverse-kinematics/tests/InverseKinematicsUnitTest.cpp @@ -4,11 +4,11 @@ #include #include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include "testModels.h" diff --git a/src/inverse-kinematics/tests/iKinVersusLegacyTest.cpp b/src/inverse-kinematics/tests/iKinVersusLegacyTest.cpp index 8c8ecc44e70..f7b61af980c 100644 --- a/src/inverse-kinematics/tests/iKinVersusLegacyTest.cpp +++ b/src/inverse-kinematics/tests/iKinVersusLegacyTest.cpp @@ -6,7 +6,7 @@ #include #include #include -#include +#include #include #include @@ -17,7 +17,7 @@ using namespace yarp::sig; using namespace yarp::math; #include -#include +#include int main(int argc, char **argv) { diff --git a/src/model/CMakeLists.txt b/src/model/CMakeLists.txt index e322439b33a..4219ff41692 100644 --- a/src/model/CMakeLists.txt +++ b/src/model/CMakeLists.txt @@ -2,31 +2,40 @@ # SPDX-License-Identifier: BSD-3-Clause -set(IDYNTREE_MODEL_HEADERS include/iDynTree/Model/ContactWrench.h - include/iDynTree/Model/DenavitHartenberg.h - include/iDynTree/Model/FixedJoint.h - include/iDynTree/Model/ForwardKinematics.h - include/iDynTree/Model/FreeFloatingState.h - include/iDynTree/Model/FreeFloatingMatrices.h - include/iDynTree/Model/IJoint.h - include/iDynTree/Model/Dynamics.h - include/iDynTree/Model/DynamicsLinearization.h - include/iDynTree/Model/DynamicsLinearizationHelpers.h - include/iDynTree/Model/Indices.h - include/iDynTree/Model/Jacobians.h - include/iDynTree/Model/JointState.h - include/iDynTree/Model/LinkTraversalsCache.h - include/iDynTree/Model/Link.h - include/iDynTree/Model/LinkState.h - include/iDynTree/Model/Model.h - include/iDynTree/Model/ModelTransformers.h - include/iDynTree/Model/MovableJointImpl.h - include/iDynTree/Model/RevoluteJoint.h - include/iDynTree/Model/PrismaticJoint.h - include/iDynTree/Model/SolidShapes.h - include/iDynTree/Model/SubModel.h - include/iDynTree/Model/Traversal.h - include/iDynTree/Model/ModelTestUtils.h) +set(IDYNTREE_MODEL_HEADERS include/iDynTree/ContactWrench.h + include/iDynTree/DenavitHartenberg.h + include/iDynTree/FixedJoint.h + include/iDynTree/ForwardKinematics.h + include/iDynTree/FreeFloatingState.h + include/iDynTree/FreeFloatingMatrices.h + include/iDynTree/IJoint.h + include/iDynTree/Dynamics.h + include/iDynTree/DynamicsLinearization.h + include/iDynTree/DynamicsLinearizationHelpers.h + include/iDynTree/Indices.h + include/iDynTree/Jacobians.h + include/iDynTree/JointState.h + include/iDynTree/LinkTraversalsCache.h + include/iDynTree/Link.h + include/iDynTree/LinkState.h + include/iDynTree/Model.h + include/iDynTree/ModelTransformers.h + include/iDynTree/MovableJointImpl.h + include/iDynTree/RevoluteJoint.h + include/iDynTree/PrismaticJoint.h + include/iDynTree/SolidShapes.h + include/iDynTree/SubModel.h + include/iDynTree/Traversal.h + include/iDynTree/ModelTestUtils.h + include/iDynTree/AllSensorsTypes.h + include/iDynTree/Sensors.h + include/iDynTree/SixAxisForceTorqueSensor.h + include/iDynTree/GyroscopeSensor.h + include/iDynTree/AccelerometerSensor.h + include/iDynTree/ThreeAxisAngularAccelerometerSensor.h + include/iDynTree/ThreeAxisForceTorqueContactSensor.h + include/iDynTree/PredictSensorsMeasurements.h + include/iDynTree/ModelSensorsTransformers.h) set(IDYNTREE_MODEL_SOURCES src/ContactWrench.cpp src/DenavitHartenberg.cpp @@ -50,7 +59,15 @@ set(IDYNTREE_MODEL_SOURCES src/ContactWrench.cpp src/PrismaticJoint.cpp src/SolidShapes.cpp src/SubModel.cpp - src/Traversal.cpp) + src/Traversal.cpp + src/Sensors.cpp + src/SixAxisForceTorqueSensor.cpp + src/AccelerometerSensor.cpp + src/GyroscopeSensor.cpp + src/ThreeAxisAngularAccelerometerSensor.cpp + src/ThreeAxisForceTorqueContactSensor.cpp + src/PredictSensorsMeasurements.cpp + src/ModelSensorsTransformers.cpp) SOURCE_GROUP("Source Files" FILES ${IDYNTREE_MODEL_SOURCES}) SOURCE_GROUP("Header Files" FILES ${IDYNTREE_MODEL_HEADERS}) @@ -58,6 +75,7 @@ SOURCE_GROUP("Header Files" FILES ${IDYNTREE_MODEL_HEADERS}) set(libraryname idyntree-model) add_library(${libraryname} ${IDYNTREE_MODEL_SOURCES} ${IDYNTREE_MODEL_HEADERS}) +add_library(iDynTree::${libraryname} ALIAS ${libraryname}) target_include_directories(${libraryname} PUBLIC "$" "$") @@ -92,12 +110,32 @@ install(TARGETS ${libraryname} RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT bin LIBRARY DESTINATION "${CMAKE_INSTALL_LIBDIR}" COMPONENT shlib ARCHIVE DESTINATION "${CMAKE_INSTALL_LIBDIR}" COMPONENT lib - PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/iDynTree/Model - PRIVATE_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/iDynTree/Model/impl) + PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/iDynTree + PRIVATE_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/iDynTree/impl) set_property(GLOBAL APPEND PROPERTY ${VARS_PREFIX}_TARGETS ${libraryname}) - if(IDYNTREE_COMPILE_TESTS) add_subdirectory(tests) endif(IDYNTREE_COMPILE_TESTS) + +# Add deprecated target iDynTree::idyntree-sensors +add_library(idyntree-sensors INTERFACE) +add_library(iDynTree::idyntree-sensors ALIAS idyntree-sensors) +set_target_properties(idyntree-sensors PROPERTIES INTERFACE_LINK_LIBRARIES iDynTree::idyntree-model) +if(CMAKE_VERSION VERSION_GREATER_EQUAL 3.17) + set_target_properties(idyntree-sensors PROPERTIES DEPRECATION "Linking the iDynTree::idyntree-sensors target is deprecated, please link iDynTree::idyntree-model that provides the symbols formerly containd in iDynTree::idyntree-sensors") +endif() +set_property(GLOBAL APPEND PROPERTY ${VARS_PREFIX}_TARGETS idyntree-sensors) +install(TARGETS idyntree-sensors + EXPORT iDynTree + COMPONENT runtime + RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT bin + LIBRARY DESTINATION "${CMAKE_INSTALL_LIBDIR}" COMPONENT shlib + ARCHIVE DESTINATION "${CMAKE_INSTALL_LIBDIR}" COMPONENT lib) + +# Install deprecated headers +install(DIRECTORY include/iDynTree/Model + DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/iDynTree) +install(DIRECTORY include/iDynTree/Sensors + DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/iDynTree) \ No newline at end of file diff --git a/src/sensors/include/iDynTree/Sensors/AccelerometerSensor.h b/src/model/include/iDynTree/AccelerometerSensor.h similarity index 97% rename from src/sensors/include/iDynTree/Sensors/AccelerometerSensor.h rename to src/model/include/iDynTree/AccelerometerSensor.h index cbe483ae4ee..4c18f6ac755 100644 --- a/src/sensors/include/iDynTree/Sensors/AccelerometerSensor.h +++ b/src/model/include/iDynTree/AccelerometerSensor.h @@ -5,7 +5,7 @@ #ifndef ACCELEROMETER_HPP #define ACCELEROMETER_HPP -#include +#include namespace iDynTree { @@ -15,7 +15,7 @@ namespace iDynTree class Twist; } -#include +#include namespace iDynTree { diff --git a/src/model/include/iDynTree/AllSensorsTypes.h b/src/model/include/iDynTree/AllSensorsTypes.h new file mode 100644 index 00000000000..b4fd780813f --- /dev/null +++ b/src/model/include/iDynTree/AllSensorsTypes.h @@ -0,0 +1,13 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_ALL_SENSORS_TYPE_H +#define IDYNTREE_ALL_SENSORS_TYPE_H + +#include +#include +#include +#include +#include + +#endif diff --git a/src/model/include/iDynTree/Centroidal.h b/src/model/include/iDynTree/Centroidal.h new file mode 100644 index 00000000000..6e1136a28fd --- /dev/null +++ b/src/model/include/iDynTree/Centroidal.h @@ -0,0 +1,26 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_CENTROIDAL_H +#define IDYNTREE_CENTROIDAL_H + +#include + +namespace iDynTree +{ + class Model; + class Traversal; + class Transform; + class FreeFloatingPos; + class FreeFloatingVel; + class FreeFloatingAcc; + class LinkPositions; + class LinkVelArray; + class LinkAccArray; + class JointPosDoubleArray; + class MatrixDynSize; + + +} + +#endif diff --git a/src/model/include/iDynTree/ContactWrench.h b/src/model/include/iDynTree/ContactWrench.h new file mode 100644 index 00000000000..29fc56d4334 --- /dev/null +++ b/src/model/include/iDynTree/ContactWrench.h @@ -0,0 +1,150 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_CONTACT_WRENCH_H +#define IDYNTREE_CONTACT_WRENCH_H + +#include +#include + +#include +#include + + +#include + +namespace iDynTree +{ + class Model; + + /** + * \brief A wrench excerted on a link due to an external contact. + * + * The contact wrench is represented by two quantities the contactPoint + * and the contactWrench . + */ + class ContactWrench + { + private: + Position m_contactPoint; + Wrench m_contactWrench; + + /** + * Unique id identifing the contact. + * This id is propagated to the contact wrench data structure. + * It is implemented mainly for compatibility with the skinDynLib library. + */ + std::size_t m_contactId; + + public: + /** + * \brief Position of a point on the contact surface. + * + * This is the position of a point (expressed in the link frame) that lies + * on the contact surface. + * + * Its precise definition is application dependent. + * + * If the ContactWrench class is used to represent a pure force + * applied on a point, the contactPoint is the point in which the + * pure force is applied. + * + * If the ContactWrench class is used to represent contact information + * coming from a tacticle system, the contact point may be the average + * between all the taxels (tactile elements) belonging to the contact. + */ + Position & contactPoint(); + + /** + * \brief Wrench that an external element excert on a link through the contact. + * + * This wrench (expressed with respect to the contactPoint and with the orientation + * of the link frame) is the wrench excerted on the link by an external element. + * + */ + Wrench & contactWrench(); + + std::size_t& contactId(); + + const Position & contactPoint() const; + const Wrench & contactWrench() const; + const std::size_t& contactId() const; + + }; + + /** + * A set of ContactWrench for each link, representing all the contacts + * between the model and the external environment. + * + */ + class LinkContactWrenches + { + private: + std::vector< std::vector > m_linkContactWrenches; + + public: + /** + * Create a LinkWrenches vector, with the size given + * by nrOfLinks . + * + * @param[in] nrOfLinks the size of the vector. + */ + LinkContactWrenches(std::size_t nrOfLinks = 0); + LinkContactWrenches(const Model & model); + + /** + * + * + * @param[in] nrOfLinks the number of links to which resize this object + */ + void resize(std::size_t nrOfLinks); + void resize(const Model & model); + + /** + * Get the number of external contacts for a given link. + */ + size_t getNrOfContactsForLink(const LinkIndex linkIndex) const; + + /** + * Set the number of external contacts for a given link. + */ + void setNrOfContactsForLink(const LinkIndex linkIndex, const size_t nrOfContacts); + + /** + * Get the number of links. + */ + size_t getNrOfLinks() const; + + /** + * Get a specific ContactWrench + * + * @param[in] linkIndex the index of the link for which the contact is retrieved + * @param[in] contactIndex a index (between 0 and getNrOfContactsForLink(link)-1 ) identifing the specific contact. + */ + ContactWrench& contactWrench(const LinkIndex linkIndex, const size_t contactIndex); + + const ContactWrench& contactWrench(const LinkIndex linkIndex, const size_t contactIndex) const; + + /** + * + * \brief Compute the net wrenches vector from the contacts wrenches vector. + * + * This is just a loop that sums all the contact wrenches for every link + * and store the results (expressed in the link frame) in the netWrenches vector. + * + * @return true if all went well, false otherwise. + */ + bool computeNetWrenches(LinkNetExternalWrenches & netWrenches) const; + + + /** + * Get a human readable description of the LinkUnknownWrenchContacts (for debug) + */ + std::string toString(const Model& model) const; + }; + + +} + + +#endif /* IDYNTREE_CONTACT_WRENCH_H */ diff --git a/src/model/include/iDynTree/DenavitHartenberg.h b/src/model/include/iDynTree/DenavitHartenberg.h new file mode 100644 index 00000000000..af677a9e892 --- /dev/null +++ b/src/model/include/iDynTree/DenavitHartenberg.h @@ -0,0 +1,185 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_DENAVIT_HARTENBERG_H +#define IDYNTREE_DENAVIT_HARTENBERG_H + +#include +#include + +#include + +#include + +namespace iDynTree +{ + +/** + * Structure representing + * the four DH parameters of + * a link in a Chain. + */ +struct DHLink +{ + double A; + double D; + double Alpha; + double Offset; + double Min; + double Max; +}; + +/** + * Simple representation of a chain described with + * Denavit-Hartenberg Parameters. + * Directly inspiered by the iCub::iKin::iKinChain class. + */ +class DHChain +{ +private: + Transform H0; + std::vector dhParams; + Transform HN; + std::vector dofNames; + +public: + void setNrOfDOFs(size_t nDofs); + size_t getNrOfDOFs() const; + + void setH0(const iDynTree::Transform & _H0); + const iDynTree::Transform & getH0() const; + + void setHN(const Transform & _HN); + const Transform & getHN() const; + + /** + * Return a reference to the i-th link of the chain. + */ + DHLink & operator() (const size_t i); + + /** + * Return a reference to the i-th link of the chain (const version). + */ + const DHLink & operator() (const size_t i) const; + + /** + * Get the name of the i-th DOF. + */ + std::string getDOFName(size_t dofIdx) const; + + /** + * Get the name of the i-th DOF. + */ + void setDOFName(size_t dofIdx, const std::string &dofName); + + /** + * Create an iDynTree::Model from this DHChain. + * \see CreateModelFromDHChain . + */ + bool toModel(Model & outputModel) const; + + /** + * Create a DHChain from a Model. + * \see ExtractDHChainFromModel . + */ + bool fromModel(const iDynTree::Model & model, + std::string baseFrame, + std::string eeFrame); +}; + +/* + * DH_Craig1989 : constructs a transformationmatrix + * T_link(i-1)_link(i) with the Denavit-Hartenberg convention as + * described in the Craigs book: Craig, J. J.,Introduction to + * Robotics: Mechanics and Control, Addison-Wesley, + * isbn:0-201-10326-5, 1986. + * + * Note that the frame is a redundant way to express the information + * in the DH-convention. + * \verbatim + * Parameters in full : a(i-1),alpha(i-1),d(i),theta(i) + * + * axis i-1 is connected by link i-1 to axis i numbering axis 1 + * to axis n link 0 (immobile base) to link n + * + * link length a(i-1) length of the mutual perpendicular line + * (normal) between the 2 axes. This normal runs from (i-1) to + * (i) axis. + * + * link twist alpha(i-1): construct plane perpendicular to the + * normal project axis(i-1) and axis(i) into plane angle from + * (i-1) to (i) measured in the direction of the normal + * + * link offset d(i) signed distance between normal (i-1) to (i) + * and normal (i) to (i+1) along axis i joint angle theta(i) + * signed angle between normal (i-1) to (i) and normal (i) to + * (i+1) along axis i + * + * First and last joints : a(0)= a(n) = 0 + * alpha(0) = alpha(n) = 0 + * + * PRISMATIC : theta(1) = 0 d(1) arbitrarily + * + * REVOLUTE : theta(1) arbitrarily d(1) = 0 + * + * Not unique : if intersecting joint axis 2 choices for normal + * Frame assignment of the DH convention : Z(i-1) follows axis + * (i-1) X(i-1) is the normal between axis(i-1) and axis(i) + * Y(i-1) follows out of Z(i-1) and X(i-1) + * + * a(i-1) = distance from Z(i-1) to Z(i) along X(i-1) + * alpha(i-1) = angle between Z(i-1) to Z(i) along X(i-1) + * d(i) = distance from X(i-1) to X(i) along Z(i) + * theta(i) = angle between X(i-1) to X(i) along X(i) + * \endverbatim + * + * Function compatible with KDL's Frame::DH_Craig1989 method. + */ +Transform TransformFromDHCraig1989(double a,double alpha,double d,double theta); + +/** + * DH : constructs a transformationmatrix T_link(i-1)_link(i) with + * the Denavit-Hartenberg convention as described in the original + * publictation: Denavit, J. and Hartenberg, R. S., A kinematic + * notation for lower-pair mechanisms based on matrices, ASME + * Journal of Applied Mechanics, 23:215-221, 1955. + * + * Function compatible on KDL's Frame::DH method. + */ +Transform TransformFromDH(double a,double alpha,double d,double theta); + +/** + * Extract a Denavit Hartenberg Chain from a iDynTree::Model. + * In particular you can specify the base frame and the end effector frame + * of the chain. The chain is then extracted using the algorithm found + * in: + * Chapter 3, Spong, Mark W., Seth Hutchinson, and M. Vidyasagar. "Robot modeling and control". 2006. + * (section 3.2.3 of http://www.cs.duke.edu/brd/Teaching/Bio/asmb/current/Papers/chap3-forward-kinematics.pdf) + * + * \note The DH representation supports only revolute and translational + * 1-DOF joints. In this implementation only revolute joints are supported. + */ +bool ExtractDHChainFromModel(const iDynTree::Model & model, + const std::string baseFrame, + const std::string eeFrame, + DHChain & outputChain, + double tolerance = 1e-5); + +/** + * Create an iDynTree::Model from a DHChain. + * + * \note The names of the links will be link0, link1, ... linkN, + * furthermore there are two additional frame for the base and endEffector frames, + * named baseFrame and distalFrame . + * \note The inertia of the links will be set to 1 kg in the origin of the link. + * + * @return true if all went ok, false otherwise. + */ +bool CreateModelFromDHChain(const DHChain & inputChain, + Model & outputModel); + + +} + + +#endif diff --git a/src/model/include/iDynTree/Dynamics.h b/src/model/include/iDynTree/Dynamics.h new file mode 100644 index 00000000000..a6028f2dc0d --- /dev/null +++ b/src/model/include/iDynTree/Dynamics.h @@ -0,0 +1,193 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_INVERSE_DYNAMICS_H +#define IDYNTREE_INVERSE_DYNAMICS_H + +#include + +#include +#include +#include + +namespace iDynTree +{ + class Model; + class Traversal; + class FreeFloatingPos; + class FreeFloatingVel; + class FreeFloatingAcc; + class FreeFloatingGeneralizedTorques; + class FreeFloatingMassMatrix; + class JointDOFsDoubleArray; + class DOFSpatialForceArray; + class DOFSpatialMotionArray; + class SpatialMomentum; + + /** + * \ingroup iDynTreeModel + * + * Compute the total linear and angular momentum of a robot, expressed in the world frame. + * + * @param[in] model the used model, + * @param[in] linkPositions linkPositions(l) contains the world_H_link transform. + * @param[in] linkVels linkVels(l) contains the link l velocity expressed in l frame. + * @param[out] totalMomentum total momentum, expressed in world frame. + * @return true if all went well, false otherwise. + */ + bool ComputeLinearAndAngularMomentum(const Model& model, + const LinkPositions& linkPositions, + const LinkVelArray& linkVels, + SpatialMomentum& totalMomentum); + + /** + * Compute the total momentum derivatitive bias, i.e. the part of the total momentum derivative that does not depend on robot acceleration. + * + * The linear and angular momentum derivative depends on the robot position, velocity and acceleration. + * This function computes the part that do not depend on the robot accelearation. + * + * This function returns the bias of the derivative of the ComputeLinearAndAngularMomentum function. + */ + bool ComputeLinearAndAngularMomentumDerivativeBias(const Model & model, + const LinkPositions& linkPositions, + const LinkVelArray & linkVel, + const LinkAccArray & linkBiasAcc, + Wrench& totalMomentumBias); + + /** + * \ingroup iDynTreeModel + * + * @brief Compute the inverse dynamics, i.e. the generalized torques corresponding to a given set of robot accelerations and external force/torques. + * + * @param[in] model The model used for the computation. + * @param[in] traversal The traversal used for the computation, it defines the used base link. + * @param[in] jointPos The (internal) joint position of the model. + * @param[in] linksVel Vector of left-trivialized velocities for each link of the model (for each link \f$L\f$, the corresponding velocity is \f${}^L \mathrm{v}_{A, L}\f$). + * @param[in] linksProperAcc Vector of left-trivialized proper acceleration for each link of the model + * (for each link \f$L\f$, the corresponding proper acceleration is \f${}^L \dot{\mathrm{v}}_{A, L} - \begin{bmatrix} {}^L R_A {}^A g \\ 0_{3\times1} \end{bmatrix} \f$), where \f$ {}^A g \in \mathbb{R}^3 \f$ is the gravity acceleration expressed in an inertial frame \f$A\f$ . See iDynTree::LinkNetExternalWrenches . + * @param[in] linkExtForces Vector of external 6D force/torques applied to the links. For each link \f$L\f$, the corresponding external force is \f${}_L \mathrm{f}^x_L\f$, i.e. the force that the enviroment applies on the on the link \f$L\f$, expressed in the link frame \f$L\f$. + * @param[out] linkIntWrenches Vector of internal joint force/torques. See iDynTree::LinkInternalWrenches . + * @param[out] baseForceAndJointTorques Generalized torques output. The base element is the residual force on the base (that is equal to zero if the robot acceleration and the external forces provided in LinkNetExternalWrenches were consistent), while the joint part is composed by the joint torques. + */ + bool RNEADynamicPhase(const iDynTree::Model & model, + const iDynTree::Traversal & traversal, + const iDynTree::JointPosDoubleArray & jointPos, + const iDynTree::LinkVelArray & linksVel, + const iDynTree::LinkAccArray & linksProperAcc, + const iDynTree::LinkNetExternalWrenches & linkExtForces, + iDynTree::LinkInternalWrenches & linkIntWrenches, + iDynTree::FreeFloatingGeneralizedTorques & baseForceAndJointTorques); + + /** + * Compute the floating base mass matrix, using the + * composite rigid body algorithm. + * + */ + bool CompositeRigidBodyAlgorithm(const Model& model, + const Traversal& traversal, + const JointPosDoubleArray& jointPos, + LinkCompositeRigidBodyInertias& linkCRBs, + FreeFloatingMassMatrix& massMatrix); + + + /** + * Structure of buffers required by ArticulatedBodyAlgorithm. + * + * As the ArticulatedBodyAlgorithm function needs some internal buffers + * to run, but we don't want to put memory allocation inside the ArticulatedBodyAlgorithm + * function, we put all the internal buffers in this structure. + * + * A convenient resize(Model) function is provided to automatically resize + * the buffers given a Model. + */ + struct ArticulatedBodyAlgorithmInternalBuffers + { + ArticulatedBodyAlgorithmInternalBuffers() {}; + + /** + * Call resize(model); + */ + ArticulatedBodyAlgorithmInternalBuffers(const Model & model); + + /** + * Resize all the buffers to the right size given the model, + * and reset all the buffers to 0. + */ + void resize(const Model& model); + + /** + * Check if the dimension of the buffer is consistent + * with a model (it should be after a call to resize(model) ). + */ + bool isConsistent(const Model& model); + + DOFSpatialMotionArray S; + DOFSpatialForceArray U; + JointDOFsDoubleArray D; + JointDOFsDoubleArray u; + LinkVelArray linksVel; + LinkAccArray linksBiasAcceleration; + LinkAccArray linksAccelerations; + LinkArticulatedBodyInertias linkABIs; + LinkWrenches linksBiasWrench; + + // Debug quantity + //LinkWrenches pa; + }; + + /** + * \ingroup iDynTreeModel + * + * Compute the floating base acceleration of an unconstrianed + * robot, using as input the external forces and the joint torques. + * We follow the algorithm described in Featherstone 2008, modified + * for the floating base case and for handling fixed joints. + * + */ + bool ArticulatedBodyAlgorithm(const Model& model, + const Traversal& traversal, + const FreeFloatingPos& robotPos, + const FreeFloatingVel& robotVel, + const LinkNetExternalWrenches & linkExtWrenches, + const JointDOFsDoubleArray & jointTorques, + ArticulatedBodyAlgorithmInternalBuffers & buffers, + FreeFloatingAcc & robotAcc); + + /** + * \ingroup iDynTreeModel + * + * + * @brief Compute the inverse dynamics of the model as linear function of the inertial parameters. + * + * This function computes the matrix that multiplied by the vector of inertial parameters of a model (see iDynTree::Model::getInertialParameters) + * returns the inverse dynamics generalized torques. In particular it is consistent with the result of the iDynTree::RNEADynamicPhase function, i.e. + * the first six rows of the regressor correspond to the sum of all external force/torques acting on the robot, expressed in the origin + * and with the orientation of the specified referenceFrame, as defined by the referenceFrame_H_link argument. + * + * + * @note The regressor only computes the inverse dynamics generalized torques assuming that the external forces are equal to zero, + * as the contribution of the external forces to the inverse dynamics is indipendent from inertial parameters. + * + * @param[in] model The model used for the computation. + * @param[in] traversal The traversal used for the computation, it defines the used base link. + * @param[in] referenceFrame_H_link Position of each link w.r.t. to given frame D (tipically an inertial frame A, the base frame B or the mixed frame B[A]). For each link \f$L\f$, the corresponding transform is \f${}^D H_L\f$. + * @param[in] linksVel Vector of left-trivialized velocities for each link of the model (for each link \f$L\f$, the corresponding velocity is \f${}^L \mathrm{v}_{A, L}\f$). + * @param[in] linksProperAcc Vector of left-trivialized proper acceleration for each link of the model + * (for each link \f$L\f$, the corresponding proper acceleration is \f${}^L \dot{\mathrm{v}}_{A, L} - \begin{bmatrix} {}^L R_A {}^A g \\ 0_{3\times1} \end{bmatrix} \f$), where \f$ {}^A g \in \mathbb{R}^3 \f$ is the gravity acceleration expressed in an inertial frame \f$A\f$ . + * @param[out] baseForceAndJointTorquesRegressor The (6+model.getNrOfDOFs() X 10*model.getNrOfLinks()) inverse dynamics regressor. + * + */ + bool InverseDynamicsInertialParametersRegressor(const iDynTree::Model & model, + const iDynTree::Traversal & traversal, + const iDynTree::LinkPositions& referenceFrame_H_link, + const iDynTree::LinkVelArray & linksVel, + const iDynTree::LinkAccArray & linksAcc, + iDynTree::MatrixDynSize & baseForceAndJointTorquesRegressor); + + + +} + + +#endif + diff --git a/src/model/include/iDynTree/DynamicsLinearization.h b/src/model/include/iDynTree/DynamicsLinearization.h new file mode 100644 index 00000000000..4a9dad0f56e --- /dev/null +++ b/src/model/include/iDynTree/DynamicsLinearization.h @@ -0,0 +1,102 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_DYNAMICS_LINEARIZATION_H +#define IDYNTREE_DYNAMICS_LINEARIZATION_H + +#include +#include + +#include +#include + +namespace iDynTree +{ + /** + * Structure containing the internal buffers used + * by the ForwardDynamicsLinearization function. + */ + struct ForwardDynamicsLinearizationInternalBuffers + { + ForwardDynamicsLinearizationInternalBuffers() {}; + + /** + * Call resize(model); + */ + ForwardDynamicsLinearizationInternalBuffers(const Model & model); + + /** + * Resize all the buffers to the right size given the model, + * and reset all the buffers to 0. + */ + void resize(const Model& model); + + /** + * Buffers used for computing the ABA algorithm. + */ + ArticulatedBodyAlgorithmInternalBuffers aba; + + /** + * Buffers used for computing the derivative of ABA with respect + * to the joint DOFs position. + */ + std::vector dPos; + + /** + * Buffers used to compute the derivative with respect to the base velocity. + */ + LinkPositions linkPos; + std::vector dVb_linkBiasWrench; + std::vector dVb_u; + std::vector dVb_linkBiasAcceleration; + std::vector dVb_linksAccelerations; + + /** + * Buffer to store the derivative of + * the product + * \f[ + * V_l \bar\times^* M_l V_l + * \f] + * + * with respect to V_l + */ + std::vector dVl_linkLocalBiasWrench; + + + /** + * Buffers used for computing the derivative of ABA with respect + * to the joint DOFs velocities. + */ + std::vector dVel; + }; + + class FreeFloatingStateLinearization : public MatrixDynSize + { + public: + FreeFloatingStateLinearization(); + + FreeFloatingStateLinearization(const Model & model); + + void resize(const Model & model); + }; + + /* + * Compute the left-trivialized linearization, + * as described in [fill with left-trivialized repot when available]. + * + * HIGHLY EXPERIMENTAL FUNCTION, DO NOT USE + */ + bool ForwardDynamicsLinearization(const Model& model, + const Traversal& traversal, + const FreeFloatingPos& robotPos, + const FreeFloatingVel& robotVel, + const LinkNetExternalWrenches & linkExtWrenches, + const JointDOFsDoubleArray & jointTorques, + ForwardDynamicsLinearizationInternalBuffers & bufs, + FreeFloatingAcc & robotAcc, + FreeFloatingStateLinearization & A); + +} + + +#endif \ No newline at end of file diff --git a/src/model/include/iDynTree/DynamicsLinearizationHelpers.h b/src/model/include/iDynTree/DynamicsLinearizationHelpers.h new file mode 100644 index 00000000000..7ff1de5aa0f --- /dev/null +++ b/src/model/include/iDynTree/DynamicsLinearizationHelpers.h @@ -0,0 +1,82 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_DYNAMICS_LINEARIZATION_HELPERS_H +#define IDYNTREE_DYNAMICS_LINEARIZATION_HELPERS_H + +#include + +#include + +namespace iDynTree +{ + /** + * Class representing the derivative + * of a spatial motion vector with respect to + * another spatial motion vector. + */ + class SpatialMotionWrtMotionDerivative: public Matrix6x6 + { + public: + /** + * Equivalent to: + * + * ``` + * SpatialMotionWrtMotionDerivative ret; + * toEigen(ret) = toEigen(ret)*toEigen(transform.asAdjointMatrix()) + * return ret; + * ``` + */ + SpatialMotionWrtMotionDerivative operator*(const Transform & a_X_b); + + }; + + /** + * Equivalent to: + * + * ``` + * SpatialForceWrtMotionDerivative ret; + * toEigen(ret) = toEigen(transform.asAdjointMatrixWrench())*toEigen(ret); + * return ret; + * ``` + */ + SpatialMotionWrtMotionDerivative operator*(const Transform & a_X_b, const SpatialMotionWrtMotionDerivative & op2); + + + /** + * Class representing the derivative + * of a spatial force vector with respect to + * a spatial motion vector. + */ + class SpatialForceWrtMotionDerivative: public Matrix6x6 + { + public: + /** + * Equivalent to: + * + * ``` + * SpatialForceWrtMotionDerivative ret; + * toEigen(ret) = toEigen(ret)*toEigen(transform.asAdjointMatrix()) + * return ret; + * ``` + */ + SpatialForceWrtMotionDerivative operator*(const Transform & a_X_b); + + }; + + + /** + * Equivalent to: + * + * ``` + * SpatialForceWrtMotionDerivative ret; + * toEigen(ret) = toEigen(transform.asAdjointMatrixWrench())*toEigen(ret); + * return ret; + * ``` + */ + SpatialForceWrtMotionDerivative operator*(const Transform & a_X_b, const SpatialForceWrtMotionDerivative & op2); + +} + + +#endif /* IDYNTREE_DYNAMICS_LINEARIZATION_HELPERS_H */ \ No newline at end of file diff --git a/src/model/include/iDynTree/DynamicsUtils.h b/src/model/include/iDynTree/DynamicsUtils.h new file mode 100644 index 00000000000..1acdc8ba401 --- /dev/null +++ b/src/model/include/iDynTree/DynamicsUtils.h @@ -0,0 +1,25 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_DYNAMICS_UTILS_H +#define IDYNTREE_DYNAMICS_UTILS_H + +#include +#include +#include + +namespace iDynTree +{ + /** + * Given a rigid body inertia \f$M\f$ and spatial motion vector \f$V\f$, + * the bias wrench \f$B\f$ of rigid body is defined as: + * \f[ + * B = \times + * \f] + * + */ + biasWrenchVelocityDerivative(SpatialInertia M, SpatialMotionVector V); +} + + +#endif /* IDYNTREE_DYNAMICS_UTILS_H */ \ No newline at end of file diff --git a/src/model/include/iDynTree/FixedJoint.h b/src/model/include/iDynTree/FixedJoint.h new file mode 100644 index 00000000000..dcd6551c10e --- /dev/null +++ b/src/model/include/iDynTree/FixedJoint.h @@ -0,0 +1,189 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_FIXED_JOINT_H +#define IDYNTREE_FIXED_JOINT_H + +#include + +#include +#include + +namespace iDynTree +{ + /** + * Class representing a fixed joint, i.e. a joint that rigidly attaches two links. + * + * \ingroup iDynTreeModel + */ + class FixedJoint : public IJoint + { + private: + JointIndex m_index; + std::size_t m_posCoordsOffset; + std::size_t m_DOFsOffset; + LinkIndex link1; + LinkIndex link2; + Transform link1_X_link2; + Transform link2_X_link1; + + public: + + /** + * Default constructor. + * The joint is initialized with an Identity rest transform. + * You can call setRestTransform to set the rest transform at a + * second stage. + */ + explicit FixedJoint(); + + /** + * Constructor + */ + FixedJoint(const LinkIndex link1, const LinkIndex link2, + const Transform& link1_X_link2); + + /** + * Constructor in which the LinkIndex to which the joint is attached are not specified. + * This constructor is tipically used together with the Model::addJoint or + * Model::addJointAndLink methods, in which the links to which the joint is attached are + * specified by the other arguments of the method. + */ + FixedJoint(const Transform& link1_X_link2); + + /** + * Copy constructor + */ + FixedJoint(const FixedJoint & other); + + /** + * Destructor + */ + virtual ~FixedJoint(); + + // Documentation inherited + virtual IJoint * clone() const; + + // Documentation inherited + virtual unsigned int getNrOfPosCoords() const; + + // Documentation inherited + virtual unsigned int getNrOfDOFs() const; + + // Documentation inherited + virtual void setAttachedLinks(const LinkIndex link1, const LinkIndex link2); + + // Documentation inherited + virtual void setRestTransform(const Transform& link1_X_link2); + + // Documentation inherited + virtual LinkIndex getFirstAttachedLink() const; + + // Documentation inherited + virtual LinkIndex getSecondAttachedLink() const; + + // Documentation inherited + virtual Transform getRestTransform(const LinkIndex child, + const LinkIndex parent) const; + + + // Documentation inherited + virtual const Transform & getTransform(const VectorDynSize & jntPos, + const LinkIndex child, + const LinkIndex parent) const; + + // Documentation inherited + TransformDerivative getTransformDerivative(const VectorDynSize & jntPos, + const LinkIndex child, + const LinkIndex parent, + const int posCoord_i) const; + + // Documentation inherited + virtual SpatialMotionVector getMotionSubspaceVector(int dof_i, + const LinkIndex child, + const LinkIndex parent) const; + + // Documentation inherited + virtual void computeChildPosVelAcc(const VectorDynSize & jntPos, + const VectorDynSize & jntVel, + const VectorDynSize & jntAcc, + LinkPositions & linkPositions, + LinkVelArray & linkVels, + LinkAccArray & linkAccs, + const LinkIndex child, + const LinkIndex parent) const; + + // Documentation inherited + virtual void computeChildVelAcc(const VectorDynSize & jntPos, + const VectorDynSize & jntVel, + const VectorDynSize & jntAcc, + LinkVelArray & linkVels, + LinkAccArray & linkAccs, + const LinkIndex child, const LinkIndex parent) const; + + // Documentation inherited + virtual void computeChildVel(const VectorDynSize & jntPos, + const VectorDynSize & jntVel, + LinkVelArray & linkVels, + const LinkIndex child, + const LinkIndex parent) const; + + // Documentation inherited + virtual void computeChildAcc(const VectorDynSize & jntPos, + const VectorDynSize & jntVel, + const LinkVelArray & linkVels, + const VectorDynSize & jntAcc, + LinkAccArray & linkAccs, + const LinkIndex child, + const LinkIndex parent) const; + + // Documentation inherited + virtual void computeChildBiasAcc(const VectorDynSize & jntPos, + const VectorDynSize & jntVel, + const LinkVelArray & linkVels, + LinkAccArray & linkBiasAccs, + const LinkIndex child, + const LinkIndex parent) const; + + // Documentation inherited + virtual void computeJointTorque(const VectorDynSize & jntPos, const Wrench & internalWrench, + const LinkIndex linkThatAppliesWrench, const LinkIndex linkOnWhichWrenchIsApplied, + VectorDynSize & jntTorques) const; + + // Documentation inherited + virtual void setIndex(JointIndex & _index); + + // Documentation inherited + virtual JointIndex getIndex() const; + + // Documentation inherited + virtual void setPosCoordsOffset(const size_t _index); + + // Documentation inherited + virtual size_t getPosCoordsOffset() const; + + // Documentation inherited + virtual void setDOFsOffset(const size_t _index); + + // Documentation inherited + virtual size_t getDOFsOffset() const; + + // LIMITS METHODS + virtual bool hasPosLimits() const; + virtual bool enablePosLimits(const bool enable); + virtual bool getPosLimits(const size_t _index, double & min, double & max) const; + virtual double getMinPosLimit(const size_t _index) const; + virtual double getMaxPosLimit(const size_t _index) const; + virtual bool setPosLimits(const size_t _index, double & min, double & max); + + // DYNAMICS METHODS + virtual JointDynamicsType getJointDynamicsType() const; + virtual bool setJointDynamicsType(const JointDynamicsType enable); + virtual double getDamping(const size_t _index) const; + virtual double getStaticFriction(const size_t _index) const; + virtual bool setDamping(const size_t _index, double& damping); + virtual bool setStaticFriction(const size_t _index, double& staticFriction); + }; +} + +#endif /* IDYNTREE_FIXED_JOINT_H */ diff --git a/src/model/include/iDynTree/ForwardKinematics.h b/src/model/include/iDynTree/ForwardKinematics.h new file mode 100644 index 00000000000..11dfcd2471b --- /dev/null +++ b/src/model/include/iDynTree/ForwardKinematics.h @@ -0,0 +1,154 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_FORWARD_KINEMATICS_H +#define IDYNTREE_FORWARD_KINEMATICS_H + +#include + +namespace iDynTree +{ + class Model; + class Traversal; + class Transform; + class FreeFloatingPos; + class FreeFloatingVel; + class FreeFloatingAcc; + class LinkPositions; + class LinkVelArray; + class LinkAccArray; + class JointPosDoubleArray; + class SpatialAcc; + class VectorDynSize; + + /** + * \ingroup iDynTreeModel + * + * Given a robot floating base configuration (i.e. world_H_base and the joint positions) + * compute for each link the world_H_link transform. + * + * @param[in] model the used model, + * @param[in] traversal the used traversal, + * @param[in] worldHbase the world_H_base transform, + * @param[in] jointPositions the vector of (internal) joint positions, + * @param[out] linkPositions linkPositions(l) contains the world_H_link transform. + * @return true if all went well, false otherwise. + */ + bool ForwardPositionKinematics(const Model& model, + const Traversal& traversal, + const Transform& worldHbase, + const VectorDynSize& jointPositions, + LinkPositions& linkPositions); + + /** + * Variant of ForwardPositionKinematics that takes in input a FreeFloatingPos + * object instead of a separate couple of (worldHbase,jointPositions) + * + */ + bool ForwardPositionKinematics(const Model & model, + const Traversal & traversal, + const FreeFloatingPos & jointPos, + LinkPositions & linkPos); + + + + /** + * Function that compute the links velocities and accelerations + * given the free floating robot velocities and accelerations. + * + * This function implements what is usually known as the + * "forward pass" of the Recursive Newton Euler algorithm. + */ + bool ForwardVelAccKinematics(const iDynTree::Model & model, + const iDynTree::Traversal & traversal, + const iDynTree::FreeFloatingPos & robotPos, + const iDynTree::FreeFloatingVel & robotVel, + const iDynTree::FreeFloatingAcc & robotAcc, + iDynTree::LinkVelArray & linkVel, + iDynTree::LinkAccArray & linkAcc); + + /** + * Function that compute the links position, velocities and accelerations + * given the free floating robot position, velocities and accelerations. + * + * + */ + bool ForwardPosVelAccKinematics(const iDynTree::Model & model, + const iDynTree::Traversal & traversal, + const iDynTree::FreeFloatingPos & robotPos, + const iDynTree::FreeFloatingVel & robotVel, + const iDynTree::FreeFloatingAcc & robotAcc, + iDynTree::LinkPositions & linkPos, + iDynTree::LinkVelArray & linkVel, + iDynTree::LinkAccArray & linkAcc); + + /** + * Function that compute the links position and velocities and accelerations + * given the free floating robot position and velocities. + * + * + */ + bool ForwardPosVelKinematics(const iDynTree::Model & model, + const iDynTree::Traversal & traversal, + const iDynTree::FreeFloatingPos & robotPos, + const iDynTree::FreeFloatingVel & robotVel, + iDynTree::LinkPositions & linkPos, + iDynTree::LinkVelArray & linkVel); + + /** + * Function that computes the links accelerations + * given the free floating robot velocities and accelerations. + * + * This function implements what is usually known as the acceleration part + * "forward pass" of the Recursive Newton Euler algorithm. + */ + bool ForwardAccKinematics(const Model & model, + const Traversal & traversal, + const FreeFloatingPos & robotPos, + const FreeFloatingVel & robotVel, + const FreeFloatingAcc & robotAcc, + const LinkVelArray & linkVel, + LinkAccArray & linkAcc); + + /** + * Function that computes the links bias accelerations given the free floating robot velocities. + * + * @note This function can also consider a non-zero base acceleration, because when computing + * the bias acc for the MIXED representation, a zero mixed base acceleration is a equivalent + * to a non-zero BODY_FIXED base acceleration. + * + * @param[in] model the used model, + * @param[in] traversal the used traversal, + * @param[in] worldHbase the world_H_base transform, + * @param[in] robotPos the position of the robot, i.e. \f$ ({}^A H_B, s)\f$, + * @param[in] robotVel the velocity of the robot, with the base velocity expressed in BODY_FIXED + * representation i.e. \f$ \nu = \begin{bmatrix} {}^B \mathrm{v}_{A,B} \newline \dot{s} \end{bmatrix} \f$, + * @param[in] baseBiasAcc base bias acceleration with BODY_FIXED rapresentation, useful when the bias acceleration + * is considering the MIXED base acceleration to be zero, + * @param[in] linkVel the velocity of each link of the robot, with velocity expressed with BODY_FIXED + * representation i.e. for each link \f$ \f$ we have \f$ {}^L \mathrm{v}_{A,L} \f$, + * @param[out] linkBiasAcc the bias acceleration of each link of the robot, with the acceleration expressed with BODY_FIXED + * representation. + * + */ + bool ForwardBiasAccKinematics(const Model & model, + const Traversal & traversal, + const FreeFloatingPos & robotPos, + const FreeFloatingVel & robotVel, + const SpatialAcc & baseBiasAcc, + const LinkVelArray & linkVel, + LinkAccArray & linkBiasAcc); + + /** + * Legacy function, will be deprecated, use the variant with an explicit baseBiasAcc value. + */ + bool ForwardBiasAccKinematics(const Model & model, + const Traversal & traversal, + const FreeFloatingPos & robotPos, + const FreeFloatingVel & robotVel, + const LinkVelArray & linkVel, + LinkAccArray & linkBiasAcc); + +} + +#endif diff --git a/src/model/include/iDynTree/FreeFloatingMatrices.h b/src/model/include/iDynTree/FreeFloatingMatrices.h new file mode 100644 index 00000000000..9e789e5423f --- /dev/null +++ b/src/model/include/iDynTree/FreeFloatingMatrices.h @@ -0,0 +1,98 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_FREE_FLOATING_MATRICES_H +#define IDYNTREE_FREE_FLOATING_MATRICES_H + +#include + +namespace iDynTree +{ + +class Model; + +/** + * @brief Possible frame velocity representation convention. + * + * Given a link \f$L\f$ and an absolute frame \f$A\f$, the + * the possible frame velocity representation are the following: + * * `INERTIAL_FIXED_REPRESENTATION` : Velocity representation is \f${}^{A} \mathrm{v}_{A,B}\f$, + * * `BODY_FIXED_REPRESENTATION` : Velocity representation is \f${}^{B} \mathrm{v}_{A,B}\f$, + * * `MIXED_REPRESENTATION` : Velocity representation is \f${}^{B[A]} \mathrm{v}_{A,B}\f$. + * + * See iDynTree::KinDynComputations documentation for more details. + */ +enum FrameVelocityRepresentation +{ + INERTIAL_FIXED_REPRESENTATION, + BODY_FIXED_REPRESENTATION, + MIXED_REPRESENTATION +}; + +/** + * Jacobian of the 6D frame velocity. + */ +class FrameFreeFloatingJacobian : public MatrixDynSize +{ +public: + FrameFreeFloatingJacobian(size_t nrOfDofs=0); + FrameFreeFloatingJacobian(const iDynTree::Model & model); + + void resize(const iDynTree::Model & model); + bool isConsistent(const iDynTree::Model & model) const; + + virtual ~FrameFreeFloatingJacobian(); +}; + +/** + * Jacobian of the 6D momentum. + */ +class MomentumFreeFloatingJacobian : public MatrixDynSize +{ +public: + MomentumFreeFloatingJacobian(size_t nrOfDofs=0); + MomentumFreeFloatingJacobian(const iDynTree::Model & model); + + void resize(const iDynTree::Model & model); + bool isConsistent(const iDynTree::Model & model) const; + + virtual ~MomentumFreeFloatingJacobian(); +}; + + +/** + * Class representing the mass matrix of a Free Floating robot. + * + * + */ +class FreeFloatingMassMatrix : public MatrixDynSize +{ +public: + FreeFloatingMassMatrix(size_t nrOfDofs=0); + + /** + * Constructor from a model, to get the appropriate size of the + * mass matrix. + */ + FreeFloatingMassMatrix(const iDynTree::Model& model); + + /** + * Resize the class to match the dimension of the dofs contained in a Model. + * + * @param model the model from which to get the number and dimension of the joints. + * + * @warning This method wipes the content of the mass matrix. + * + */ + void resize(const iDynTree::Model& model); + + + /** + * Destructor + */ + virtual ~FreeFloatingMassMatrix(); +}; + +} + +#endif diff --git a/src/model/include/iDynTree/FreeFloatingState.h b/src/model/include/iDynTree/FreeFloatingState.h new file mode 100644 index 00000000000..381acbeab3b --- /dev/null +++ b/src/model/include/iDynTree/FreeFloatingState.h @@ -0,0 +1,246 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_FREE_FLOATING_STATE_H +#define IDYNTREE_FREE_FLOATING_STATE_H + +#include +#include +#include + +#include +#include +#include + +#include + +namespace iDynTree +{ + class Model; + + /** + * Class representing the position of a Free Floating robot. + * + * The position of a free floating robot is represented by the + * transform of a base frame with respect to an inertial frame, + * and a vector of joint positions. + * + */ + class FreeFloatingPos + { + private: + Transform m_worldBasePos; + JointPosDoubleArray m_jointPos; + + public: + // Documentation inherited + FreeFloatingPos(); + FreeFloatingPos(const iDynTree::Model& model); + + /** + * Resize the class to match the dimension of the joints contained in a Model. + * + * @param model the model from which to get the number and dimension of the joints. + * + * @warning This method wipes the joint positions if number and dimension of the joints + * of the model passed are different from the one already stored. + * + */ + void resize(const iDynTree::Model& model); + + /** + * Get the base position. + */ + Transform & worldBasePos(); + + /** + * Get the vector of joint positions. + */ + JointPosDoubleArray & jointPos(); + + /** + * Get the base position (const version). + */ + const Transform & worldBasePos() const; + + /** + * Get the vector of joint positions (const version). + */ + const JointPosDoubleArray & jointPos() const; + + /** + * Get the dimension of the joint positions vector. + */ + unsigned int getNrOfPosCoords() const; + + /** + * Destructor + */ + virtual ~FreeFloatingPos(); + }; + + + class FreeFloatingGeneralizedTorques + { + private: + Wrench m_baseWrench; + JointDOFsDoubleArray m_jointTorques; + + public: + // Documentation inherited + FreeFloatingGeneralizedTorques(); + FreeFloatingGeneralizedTorques(const iDynTree::Model& model); + + /** + * Resize the class to match the dimension of the joints contained in a Model. + * + * @param model the model from which to get the number and dimension of the joints. + * + * @warning This method wipes the joint torques if number and dimension of the joints + * of the model passed are different from the one already stored. + * + */ + void resize(const iDynTree::Model& model); + + /** + * Get the base wrench. + */ + Wrench & baseWrench(); + + /** + * Get the joint torques vector. + */ + JointDOFsDoubleArray & jointTorques(); + + /** + * Get the base wrench (const version). + */ + const Wrench & baseWrench() const; + + /** + * Get the joint torques vector (const version). + */ + const JointDOFsDoubleArray & jointTorques() const; + + unsigned int getNrOfDOFs() const; + + /** + * Destructor + */ + virtual ~FreeFloatingGeneralizedTorques(); + }; + + + class FreeFloatingVel + { + private: + Twist m_baseVel; + JointDOFsDoubleArray m_jointVel; + + public: + // Documentation inherited + FreeFloatingVel(); + FreeFloatingVel(const iDynTree::Model& model); + + /** + * Resize the class to match the number of internal DOFs contained in a Model. + * + * @param model the model from which to get the number and dimension of the joints. + * + * @warning This method wipes the joint velocities if number and dimension of the joints + * of the model passed are different from the one already stored. + * + */ + void resize(const iDynTree::Model& model); + + /** + * Get the base acceleration. + */ + Twist & baseVel(); + + /** + * Get the vector of joint accelerations. + */ + JointDOFsDoubleArray & jointVel(); + + /** + * Get the base acceleration (const version). + */ + const Twist & baseVel() const; + + /** + * Get the vector of joint accelerations (const version). + */ + const JointDOFsDoubleArray & jointVel() const; + + /** + * Get the dimension of the joint accelerations vector. + */ + unsigned int getNrOfDOFs() const; + + /** + * Destructor + */ + virtual ~FreeFloatingVel(); + }; + + /** + * Class representing the accelerations of a Free Floating robot. + * + */ + class FreeFloatingAcc + { + private: + SpatialAcc m_baseAcc; + JointDOFsDoubleArray m_jointAcc; + + public: + // Documentation inherited + FreeFloatingAcc(); + FreeFloatingAcc(const iDynTree::Model& model); + + /** + * Resize the class to match the number of internal DOFs contained in a Model. + * + * @param model the model from which to get the number and dimension of the joints. + * + * @warning This method wipes the joint positions if number and dimension of the joints + * of the model passed are different from the one already stored. + * + */ + void resize(const iDynTree::Model& model); + + /** + * Get the base acceleration. + */ + SpatialAcc & baseAcc(); + + /** + * Get the vector of joint accelerations. + */ + JointDOFsDoubleArray & jointAcc(); + + /** + * Get the base acceleration (const version). + */ + const SpatialAcc & baseAcc() const; + + /** + * Get the vector of joint accelerations (const version). + */ + const JointDOFsDoubleArray & jointAcc() const; + + /** + * Get the dimension of the joint accelerations vector. + */ + unsigned int getNrOfDOFs() const; + + /** + * Destructor + */ + virtual ~FreeFloatingAcc(); + }; + +} + +#endif diff --git a/src/sensors/include/iDynTree/Sensors/GyroscopeSensor.h b/src/model/include/iDynTree/GyroscopeSensor.h similarity index 97% rename from src/sensors/include/iDynTree/Sensors/GyroscopeSensor.h rename to src/model/include/iDynTree/GyroscopeSensor.h index 38135a4a1fd..009dcaca354 100644 --- a/src/sensors/include/iDynTree/Sensors/GyroscopeSensor.h +++ b/src/model/include/iDynTree/GyroscopeSensor.h @@ -5,7 +5,7 @@ #ifndef GYROSCOPE_HPP #define GYROSCOPE_HPP -#include +#include namespace iDynTree { @@ -13,7 +13,7 @@ namespace iDynTree typedef AngularMotionVector3 AngVelocity; } -#include +#include #include namespace iDynTree { diff --git a/src/model/include/iDynTree/IJoint.h b/src/model/include/iDynTree/IJoint.h new file mode 100644 index 00000000000..0ffd9913a16 --- /dev/null +++ b/src/model/include/iDynTree/IJoint.h @@ -0,0 +1,463 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_I_JOINT_H +#define IDYNTREE_I_JOINT_H + +#include + +namespace iDynTree +{ + class LinkPositions; + class LinkVelArray; + class LinkAccArray; + class Transform; + class TransformDerivative; + class Wrench; + class Twist; + class VectorDynSize; + class SpatialMotionVector; + + enum JointDynamicsType + { + /** + * NoDynamics: No joint dynamics is assumed for the joint. + * This joint dynamics type does not consider any parameter. + */ + NoJointDynamics = 0, + + /** + * URDFJointDynamics: Dynamics described by the URDF 1.0 specification. + * + * This joint dynamics type consider the following parameters: + * * `Damping` + * * `StaticFriction` + */ + URDFJointDynamics = 1 + }; + + /** + * Interface (i.e. abstract class) exposed by classes that implement a Joint. + * A Joint is the basic representation of the motion allowed between two links. + * + * This interface is mean to be used by kinematics and dynamics algorithm to + * query informations related to a joint and the relations (relative position, + * relative twist, relative acceleration) that it imposes to the connected links. + * + * The design of this class is heavily inspired by the Simbody implementation of joints, + * as described in this article: + * + * Seth, Ajay, et al. "Minimal formulation of joint motion for biomechanisms." + * Nonlinear dynamics 62.1-2 (2010): 291-303. + * + * Other sources of inspiration are RBDL, DART and Featherstone book. + * + * With respect to all this implementation we model the joints as undirected quantities, + * i.e. as object in which information can be queryied in symmetric way with respect to the + * attached links. This mean there is no parent and child link, but the joint is attached + * to two link, and the interface is agnostic with respect to which link the code considers + * as "parent" or "child". + * + * \ingroup iDynTreeModel + */ + class IJoint + { + public: + /** + * Denstructor + * + */ + virtual ~IJoint() = 0; + + /** + * Clone the joint object. + */ + virtual IJoint * clone() const = 0; + + /** + * Get the number of coordinates used to represent + * the position of the joint. + * + * For joints whose configuration is in R^n, + * the number of position coordinates should + * match the number of degrees of freedom of the joint. + * + * @return the number of position coordinates. + */ + virtual unsigned int getNrOfPosCoords() const = 0; + + /** + * Get the number of degrees of freedom of the joint. + * + * This should be a number between 0 (fixed joint) and 6 (free joint). + * + * @return the number of degrees of freedom of the joint. + */ + virtual unsigned int getNrOfDOFs() const = 0; + + + /** + * @name Methods to set joint + * Methods to set joint informations (used when building a model) + */ + //@{ + + /** + * Set the two links at which the joint is attached. + * @param link1 is the first link + * @param link2 is the second link + */ + virtual void setAttachedLinks(const LinkIndex link1, const LinkIndex link2) = 0; + + /** + * Set the transform between the link2 frame and link1 frame at joint position 0 + * (or at the identity configuration element for complex joints). + * + * The link1_T_link2 is transform that transforms a quantity + * expressed in link2 frame in a quantity expressed in the link1 + * frame, when the joint is in the 0 position : + * p_link1 = link1_T_link2*p_link2 . + */ + virtual void setRestTransform(const Transform& link1_X_link2) = 0; + + //@} + + /** + * Get the first link attached to the joint. + */ + virtual LinkIndex getFirstAttachedLink() const = 0; + + /** + * Get the second link attached to the joint. + */ + virtual LinkIndex getSecondAttachedLink() const = 0; + + + /** + * Get the transform between the link parent and the link child at joint position 0 + * (or at the identity configuration element for complex joints). + * Such that: + * p_child = child_H_parent*p_parent + * where p_child is a quantity expressed in the child frame, + * and p_parent is a quantity expressed in the child frame. + */ + virtual Transform getRestTransform(const LinkIndex child, + const LinkIndex parent) const = 0; + + /** + * Get the transform between the parent and the child, such that: + * p_child = child_H_parent*p_parent, + * where p_child is a quantity expressed in the child frame, + * and p_parent is a quantity expressed in the parent frame. + */ + virtual const Transform & getTransform(const VectorDynSize & jntPos, + const LinkIndex child, + const LinkIndex parent) const = 0; + + /** + * Get the derivative of the transform with + * respect to a position coordinate. + * + * In particular, if the selected position coordinate is \f$q\f$, return the derivative: + * \f[ + * \frac{\partial {}^\texttt{child} H_\texttt{parent} }{\partial q} + * \f] + * + * If posCoord_i is not >= 0 and < getNrOfPosCoords(), the returned value is undefined. + * + */ + virtual TransformDerivative getTransformDerivative(const VectorDynSize & jntPos, + const LinkIndex child, + const LinkIndex parent, + const int posCoord_i) const = 0; + + /** + * Get the motion subspace vector corresponding to the i-th + * dof of the joint, i.e. the i-th column of the motion subspace matrix. + * The motion subspace matrix is the matrix that + * maps the joint velocity to the relative twist between the two + * links. + * + * In particular the motion subspace vector of the i-th dof is the S + * vector such that + * v_child = S_{child,parent}*dq_i + child_X_parent*v_parent + * if the velocities associated to all other DOFs of the joint + * are considered zero, where v_child and v_parent are the left-trivialized + * (body) velocities of the link child and parent. + * + * See + * "Modelling, Estimation and Identification of Humanoid Robots Dynamics" + * Silvio Traversaro - Section 3.2 + * https://traversaro.github.io/preprints/traversaro-phd-thesis.pdf + * for more details. + * + * @return the motion subspace vector. + * + * If dof_i is not >= 0 and < getNrOfDOFs(), the returned value is undefined. + * + * \note The motion subspace matrix is also known in literature as hinge matrix, + * hinge map matrix, joint map matrix or joint motion map matrix. + */ + virtual SpatialMotionVector getMotionSubspaceVector(int dof_i, + const LinkIndex child, + const LinkIndex parent) const = 0; + + /** + * Compute the position, velocity and acceleration of link child, + * given the position, velocty and acceleration of link parent and + * the joint position, velocity and acceleration. + * + * The position, velocity and acceleration of link child + * are directly saved in the linkPositions, linkVels and linkAccs arguments. + * + */ + virtual void computeChildPosVelAcc(const VectorDynSize & jntPos, + const VectorDynSize & jntVel, + const VectorDynSize & jntAcc, + LinkPositions & linkPositions, + LinkVelArray & linkVels, + LinkAccArray & linkAccs, + const LinkIndex child, const LinkIndex parent) const = 0; + + /** + * Compute the velocity and acceleration of child, + * given the velocity and acceleration of parent and + * the joint position, velocity and acceleration. + * + */ + virtual void computeChildVelAcc(const VectorDynSize & jntPos, + const VectorDynSize & jntVel, + const VectorDynSize & jntAcc, + LinkVelArray & linkVels, + LinkAccArray & linkAccs, + const LinkIndex child, const LinkIndex parent) const = 0; + + /** + * Compute the velocity of child, + * given the velocity of parent and + * the joint position, velocity. + */ + virtual void computeChildVel(const VectorDynSize & jntPos, + const VectorDynSize & jntVel, + LinkVelArray & linkVels, + const LinkIndex child, + const LinkIndex parent) const = 0; + + /** + * Compute the (body-fixed) acceleration of a child link + * given the (body-fixed) acceleration of the parent + */ + virtual void computeChildAcc(const VectorDynSize & jntPos, + const VectorDynSize & jntVel, + const LinkVelArray & linkVels, + const VectorDynSize & jntAcc, + LinkAccArray & linkAccs, + const LinkIndex child, + const LinkIndex parent) const = 0; + + /** + * Compute the (body-fixed) bias acceleration of a child link + * given the (body-fixed) bias acceleration of the parent + */ + virtual void computeChildBiasAcc(const VectorDynSize & jntPos, + const VectorDynSize & jntVel, + const LinkVelArray & linkVels, + LinkAccArray & linkBiasAccs, + const LinkIndex child, + const LinkIndex parent) const = 0; + + /** + * Compute the internal torque of joint, given the internal wrench that the linkThatAppliesWrench applies + * on the linkOnWhichWrenchIsApplied, expressed in the link frame of the linkOnWhichWrenchIsApplied. + * + * @param[in] jntPos vector of joint positions. + * @param[in] internalWrench internal wrench that the linkThatAppliesWrench applies + * on the linkOnWhichWrenchIsApplied, expressed in the link + * frame of the linkOnWhichWrenchIsApplied + * @param[in] linkThatAppliesWrench link index of the link that applies the considered internal wrench. + * @param[in] linkOnWhichWrenchIsApplied link index of the link on which the considered internal wrench is applied. + * @param[out] jntTorques vector of joint torques. + */ + virtual void computeJointTorque(const VectorDynSize & jntPos, + const Wrench & internalWrench, + const LinkIndex linkThatAppliesWrench, + const LinkIndex linkOnWhichWrenchIsApplied, + VectorDynSize & jntTorques) const = 0; + + /** + * Set the index of the joint in the Model Joint serialization. + * + */ + virtual void setIndex(JointIndex & _index) = 0; + + /** + * Get the index of the joint in the model Joint serialization. + */ + virtual JointIndex getIndex() const = 0; + + + /** + * Set the offset of the position coordinates of this + * joint in the position coordiantes serialization of the model. + */ + virtual void setPosCoordsOffset(const size_t _index) = 0; + + /** + * Get the offset of the position coordinates of + * this joint in the position coordiantes serialization of the model. + */ + virtual size_t getPosCoordsOffset() const = 0; + + /** + * Set the offset of the coordinates of this + * joint in the velocity/acceleration coordiantes serialization of the model. + */ + virtual void setDOFsOffset(const size_t _index) = 0; + + /** + * Get the offset of the position coordinates of + * joint in the velocity/acceleration coordiantes serialization of the model. + */ + virtual size_t getDOFsOffset() const = 0; + + /** + * @name Limit handling methods. + * Methods for handling physical limits of joints. + * + * The model used for limits is rather simple: a joint can have limits (being bounded) + * or not. + * + * In the current version the limits are supported only for simple + * joints in which the velocity is the derivative of the position coordinate, + * and then getNrOfPosCoords() is equal to getNrOfDOFs() . + * The limits for such joints are specified by two constant vectors of dimension getNrOfDOFs(), + * the vector of minimum positions and the vector of maximum positions. + */ + ///@{ + /** + * Method to check if the joint has limits. + * + * @return true if the joints has limits + */ + virtual bool hasPosLimits() const = 0; + + /** + * Method to set if the joint has limits. + * + * @return true if everything went correctly, false otherwise + * (for example if the joint does not support joint position limits) + */ + virtual bool enablePosLimits(const bool enable) = 0; + + /** + * Get min and max position limits of the joint, for the _index dof. + * @param[in] _index index of the dof for which the limit are obtained. + * @return true if everything is correct, false otherwise. + */ + virtual bool getPosLimits(const size_t _index, double & min, double & max) const = 0; + + /** + * Get the min position limit of the joint, bindings-friendly version. + */ + virtual double getMinPosLimit(const size_t _index) const = 0; + + /** + * Get the max position limit of the joint, bindings-friendly version. + */ + virtual double getMaxPosLimit(const size_t _index) const = 0; + + /** + * Set the position limits for a dof the joint. + * + * @note This just sets the internal position limits of the joint. + * To set them as enabled, you need to call the enablePosLimits(true) method. + */ + virtual bool setPosLimits(const size_t _index, double & min, double & max) = 0; + + /** + * @name Joint dynamics methods. + * + * Methods for handling representation of joint dynamics. + * The precise definition of "joint dynamics" is not precisely, as depending on the + * specific application the kind of joint dynamics model can be different, and in some + * case it may be even just instantaneous models (for example, when only the damping is considered). + * + * For the type of joint dynamics supported, see the iDynTree::JointDynamicsType enum documentation. + * + * The joint dynamics model are used in the following contexts: + * * In methods to serialize and deserialize URDF files + * + * The joint dynamics are **not used at all** in classes to compute kinematics and dynamics quantities, + * such as iDynTree::KinDynComputations . + */ + ///@{ + + /** + * Method to get the specific joint dynamics type used for the joint. + * \note: It is assume that all the degrees of freedom of a joint share the same joint dynamics type. + * + * @return the specific joint dynamics type used for the joint. + */ + virtual JointDynamicsType getJointDynamicsType() const = 0; + + /** + * Method to get the specific joint dynamics type used for the joint. + * \note: It is assume that all the degrees of freedom of a joint share the same joint dynamics type. + * + * @return true if everything went correctly, false otherwise + */ + virtual bool setJointDynamicsType(const JointDynamicsType enable) = 0; + + /** + * Set damping parameter of the joint, for the _index dof. + * The damping coefficient is expressed in N∙s/m for a prismatic joint, N∙m∙s/rad for a revolute joint. + * + * This parameter is considered in the following joint dynamics types: + * * `URDFJointDynamics` + * + * @param[in] _index index of the dof for which the dynamic parameters are obtained. + * @return true if everything is correct, false otherwise. + */ + virtual bool setDamping(const size_t _index, double& damping) = 0; + + /** + * Set static friction parameter of the joint, for the _index dof. + * The static friction coefficient is expressed in N for a prismatic joint, N∙m for a revolute joint. + * + * This parameter is considered in the following joint dynamics types: + * * `URDFJointDynamics` + * + * @param[in] _index index of the dof for which the dynamic parameters are obtained. + * @return true if everything is correct, false otherwise. + */ + virtual bool setStaticFriction(const size_t _index, double& staticFriction) = 0; + + /** + * Get the damping coefficient of the joint. + * The unit is N∙s/m for a prismatic joint, N∙m∙s/rad for a revolute joint. + * + * This parameter is considered in the following joint dynamics types: + * * `URDFJointDynamics` + */ + virtual double getDamping(const size_t _index) const = 0; + + /** + * Get the static friction coefficient of the joint. + * The unit is N for a prismatic joint, N∙m for a revolute joint. + * + * This parameter is considered in the following joint dynamics types: + * * `URDFJointDynamics` + */ + virtual double getStaticFriction(const size_t _index) const = 0; + + + ///@} + }; + + typedef IJoint * IJointPtr; + typedef const IJoint * IJointConstPtr; + + +} + +#endif /* IDYNTREE_I_JOINT_H */ diff --git a/src/model/include/iDynTree/Indices.h b/src/model/include/iDynTree/Indices.h new file mode 100644 index 00000000000..d4bfb9d9c4f --- /dev/null +++ b/src/model/include/iDynTree/Indices.h @@ -0,0 +1,40 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_INDICES_H +#define IDYNTREE_INDICES_H + +#include +#include + +// Workaround for SWIG problems with GenerateExportHeader-generated code +#if defined(SWIG) +#define IDYNTREE_MODEL_EXPORT +#else +#include "ModelExport.h" +#endif + +namespace iDynTree +{ + typedef std::ptrdiff_t LinkIndex; + IDYNTREE_MODEL_EXPORT extern LinkIndex LINK_INVALID_INDEX; + IDYNTREE_MODEL_EXPORT extern std::string LINK_INVALID_NAME; + + typedef std::ptrdiff_t JointIndex; + IDYNTREE_MODEL_EXPORT extern std::ptrdiff_t JOINT_INVALID_INDEX; + IDYNTREE_MODEL_EXPORT extern std::string JOINT_INVALID_NAME; + + typedef std::ptrdiff_t DOFIndex; + IDYNTREE_MODEL_EXPORT extern std::ptrdiff_t DOF_INVALID_INDEX; + IDYNTREE_MODEL_EXPORT extern std::string DOF_INVALID_NAME; + + typedef std::ptrdiff_t FrameIndex; + IDYNTREE_MODEL_EXPORT extern std::ptrdiff_t FRAME_INVALID_INDEX; + IDYNTREE_MODEL_EXPORT extern std::string FRAME_INVALID_NAME; + + typedef std::ptrdiff_t TraversalIndex; + IDYNTREE_MODEL_EXPORT extern TraversalIndex TRAVERSAL_INVALID_INDEX; + +} + +#endif /* IDYNTREE_INDICES_H */ diff --git a/src/model/include/iDynTree/Jacobians.h b/src/model/include/iDynTree/Jacobians.h new file mode 100644 index 00000000000..716570aab5f --- /dev/null +++ b/src/model/include/iDynTree/Jacobians.h @@ -0,0 +1,53 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_JACOBIANS_H +#define IDYNTREE_JACOBIANS_H + +#include + +namespace iDynTree +{ + class Model; + class Traversal; + class Transform; + class FreeFloatingPos; + class FreeFloatingVel; + class FreeFloatingAcc; + class LinkPositions; + class LinkVelArray; + class LinkAccArray; + class JointPosDoubleArray; + class MatrixDynSize; + + template + class MatrixView; + + /** + * \ingroup iDynTreeModel + * + * Compute a free floating jacobian + * + * @param[in] model the used model, + * @param[in] traversal the used traversal, + * @param[in] jointPositions the vector of (internal) joint positions, + * @param[in] linkPositions linkPositions(l) contains the world_H_link transform. + * @param[in] linkIndex the index of the link of which we compute the jacobian. + * @param[in] jacobFrame_X_world TODO + * @param[in] baseFrame_X_jacobBaseFrame TODO + * @param[out] jacobian the computed Jacobian + * @return true if all went well, false otherwise. + */ + bool FreeFloatingJacobianUsingLinkPos(const Model& model, + const Traversal& traversal, + const JointPosDoubleArray& jointPositions, + const LinkPositions& linkPositions, + const LinkIndex linkIndex, + const Transform & jacobFrame_X_world, + const Transform & baseFrame_X_jacobBaseFrame, + const MatrixView& jacobian); + + +} + +#endif diff --git a/src/model/include/iDynTree/JointState.h b/src/model/include/iDynTree/JointState.h new file mode 100644 index 00000000000..182b558d7bb --- /dev/null +++ b/src/model/include/iDynTree/JointState.h @@ -0,0 +1,108 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_JOINT_STATE_H +#define IDYNTREE_JOINT_STATE_H + +#include +#include +#include + +#include + +namespace iDynTree +{ + class Model; + class SpatialMotionVector; + + /** + * Class for storing a vector of scalar values, + * one for each internal position coordinate in a model. + */ + class JointPosDoubleArray: public VectorDynSize + { + public: + JointPosDoubleArray(std::size_t nrOfDOFs = 0); + JointPosDoubleArray(const iDynTree::Model & model); + + void resize(std::size_t nrOfDOFs); + void resize(const iDynTree::Model & model); + + bool isConsistent(const iDynTree::Model & model) const; + + ~JointPosDoubleArray(); + }; + + /** + * Class for storing a vector of scalar values, + * one for each internal coordinate in a model. + */ + class JointDOFsDoubleArray: public VectorDynSize + { + public: + JointDOFsDoubleArray(std::size_t nrOfDOFs = 0); + JointDOFsDoubleArray(const iDynTree::Model & model); + + void resize(std::size_t nrOfDOFs); + void resize(const iDynTree::Model & model); + + bool isConsistent(const iDynTree::Model & model) const; + + ~JointDOFsDoubleArray(); + }; + + /** + * Class for storing a vector of spatial force vectors, + * one for each dof in a model. + */ + + /** + * Class for storing a vector of spatial force vectors, + * one for each (internal) dof in a model. + */ + class DOFSpatialForceArray + { + private: + std::vector m_dofSpatialForce; + + public: + DOFSpatialForceArray(std::size_t nrOfDOFs = 0); + DOFSpatialForceArray(const iDynTree::Model & model); + + void resize(const std::size_t nrOfDOFs); + void resize(const iDynTree::Model & model); + + bool isConsistent(const iDynTree::Model & model) const; + + iDynTree::SpatialForceVector & operator()(const size_t dof); + const iDynTree::SpatialForceVector & operator()(const size_t dof) const; + + ~DOFSpatialForceArray(); + }; + + /** + * Class for storing a vector of spatial motion vectors, + * one for each (internal) dof in a model. + */ + class DOFSpatialMotionArray + { + private: + std::vector m_dofSpatialMotion; + + public: + DOFSpatialMotionArray(std::size_t nrOfDOFs = 0); + DOFSpatialMotionArray(const iDynTree::Model & model); + + void resize(const std::size_t nrOfDOFs); + void resize(const iDynTree::Model & model); + + bool isConsistent(const iDynTree::Model & model) const; + + iDynTree::SpatialMotionVector & operator()(const size_t dof); + const iDynTree::SpatialMotionVector & operator()(const size_t dof) const; + + ~DOFSpatialMotionArray(); + }; +} + +#endif diff --git a/src/model/include/iDynTree/Link.h b/src/model/include/iDynTree/Link.h new file mode 100644 index 00000000000..2b8b28e4f03 --- /dev/null +++ b/src/model/include/iDynTree/Link.h @@ -0,0 +1,79 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_LINK_H +#define IDYNTREE_LINK_H + +#include + +#include + + +namespace iDynTree +{ + + /** + * Class that represents a link. + * + * + * \ingroup iDynTreeModel + */ + class Link + { + private: + LinkIndex m_index; + SpatialInertia m_inertia; + + public: + /** + * Costructor + */ + Link(); + + /** + * Low-level accessor to the link inertia. + */ + SpatialInertia & inertia(); + + + /** + * Low-level const accessor to the link inertia. + */ + const SpatialInertia & inertia() const; + + /** + * Set the spatial inertia of the link, + * expressed in the link reference frame + * (i.e. with respect to the link reference + * frame origin and with the orientation + * of the link reference frame). + */ + void setInertia(SpatialInertia & _inertia); + + /** + * Get the spatial inertia of the link, + * expressed in the link reference frame + * (i.e. with respect to the link reference + * frame origin and with the orientation + * of the link reference frame). + * + * @return a reference to the inertia of the link. + */ + const SpatialInertia & getInertia() const; + + /** + * Set the index of the link. + */ + void setIndex(LinkIndex & _index); + + /** + * Get the index of the link. + */ + LinkIndex getIndex() const; + }; + + typedef Link * LinkPtr; + typedef const Link * LinkConstPtr; +} + +#endif /* IDYNTREE_LINK_H */ diff --git a/src/model/include/iDynTree/LinkState.h b/src/model/include/iDynTree/LinkState.h new file mode 100644 index 00000000000..83f39f8becb --- /dev/null +++ b/src/model/include/iDynTree/LinkState.h @@ -0,0 +1,234 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_LINK_STATE_H +#define IDYNTREE_LINK_STATE_H + +#include +#include +#include +#include +#include + +#include + +#include + +namespace iDynTree +{ + class Model; + + class LinkPositions + { + private: + std::vector m_linkPos; + + public: + LinkPositions(std::size_t nrOfLinks = 0); + LinkPositions(const iDynTree::Model & model); + + void resize(std::size_t nrOfLinks); + void resize(const iDynTree::Model & model); + + bool isConsistent(const Model& model) const; + + size_t getNrOfLinks() const; + + iDynTree::Transform & operator()(const LinkIndex link); + const iDynTree::Transform & operator()(const LinkIndex link) const; + + std::string toString(const Model & model) const; + + ~LinkPositions(); + }; + + /** + * Vector of wrenches connected in some way to the link of a model. + * + * It is used to model both the total external wrench + * acting on a link (LinkExternalWrenches), or the internal wrenches + * that a link excerts on his parent (given a Traversal) + * computed as a by product by the dynamic loop of the RNEA ( RNEADynamicPhase ). + * + * In both cases the Wrench corresponding to the link with LinkIndex i + * is always expressed with the orientation of the link frame and with + * respect to the link frame origin. + */ + class LinkWrenches + { + private: + std::vector m_linkWrenches; + + public: + /** + * Create a LinkWrenches vector, with the size given + * by nrOfLinks . + * + * @param[in] nrOfLinks the size of the vector. + */ + LinkWrenches(std::size_t nrOfLinks = 0); + LinkWrenches(const iDynTree::Model & model); + + /** + * Resize the vector to have size nrOfLinks. + * + * @param[in] nrOfLinks new size for the vector + */ + void resize(std::size_t nrOfLinks); + void resize(const iDynTree::Model & model); + + bool isConsistent(const Model& model) const; + size_t getNrOfLinks() const; + + iDynTree::Wrench & operator()(const LinkIndex link); + const iDynTree::Wrench & operator()(const LinkIndex link) const; + + std::string toString(const Model & model) const; + + /** + * Set all the elements to zero. + */ + void zero(); + + ~LinkWrenches(); + }; + + /** + * Vector of the sum of all the external wrenches excerted on each link. + * + * The wrench returned by operator(i) is the sum of all external wrenches + * (thus excluding the wrench applied on the link by other links in the model) + * that the environment applies on the link $i$, expressed ( + * both orientation and point) with respect to the reference frame of link i. + */ + typedef LinkWrenches LinkNetExternalWrenches; + + /** + * Vector of the wrenches acting that a link excert on his parent, + * given a Traversal. + * + * Given a Traversal with base link b, the wrench returned by operator(i) is the wrench + * the parent of link i excerts on link i, expressed (both orientation + * and point) with respect to the reference frame of link i. + */ + typedef LinkWrenches LinkInternalWrenches; + + + /** + * Vector of the sum of all the wrenches (both internal and external, excluding gravity) acting on + * link i, expressed (both orientation and point) with respect to the reference frame of link i. + * + * This is tipically computed as I*a+v*(I*v) , where a is the proper acceleration. + */ + typedef LinkWrenches LinkNetTotalWrenchesWithoutGravity; + + /** + * Class for storing a vector of SpatialInertia objects , one for each link in a model. + */ + class LinkInertias + { + private: + std::vector m_linkInertials; + + public: + LinkInertias(std::size_t nrOfLinks = 0); + LinkInertias(const iDynTree::Model & model); + + void resize(std::size_t nrOfLinks); + void resize(const iDynTree::Model & model); + + bool isConsistent(const Model& model) const; + + iDynTree::SpatialInertia & operator()(const LinkIndex link); + const iDynTree::SpatialInertia & operator()(const LinkIndex link) const; + + ~LinkInertias(); + }; + + typedef LinkInertias LinkCompositeRigidBodyInertias; + + /** + * Class for storing a vector of ArticulatedBodyInertias objects , one for each link in a model. + */ + class LinkArticulatedBodyInertias + { + private: + std::vector m_linkABIs; + + public: + LinkArticulatedBodyInertias(std::size_t nrOfLinks = 0); + LinkArticulatedBodyInertias(const iDynTree::Model & model); + + void resize(std::size_t nrOfLinks); + void resize(const iDynTree::Model & model); + + bool isConsistent(const Model& model) const; + + iDynTree::ArticulatedBodyInertia & operator()(const LinkIndex link); + const iDynTree::ArticulatedBodyInertia & operator()(const LinkIndex link) const; + + ~LinkArticulatedBodyInertias(); + }; + + /** + * Class for storing a vector of twists, one for each link in a model. + */ + class LinkVelArray + { + private: + std::vector m_linkTwist; + + public: + LinkVelArray(std::size_t nrOfLinks = 0); + LinkVelArray(const iDynTree::Model & model); + + void resize(std::size_t nrOfLinks); + void resize(const iDynTree::Model & model); + + bool isConsistent(const Model& model) const; + + size_t getNrOfLinks() const; + + iDynTree::Twist & operator()(const LinkIndex link); + const iDynTree::Twist & operator()(const LinkIndex link) const; + + std::string toString(const Model & model) const; + + ~LinkVelArray(); + }; + + /** + * Class for storing a vector of spatial accelerations, + * one for each link in a model. + */ + class LinkAccArray + { + private: + std::vector m_linkAcc; + + public: + LinkAccArray(std::size_t nrOfLinks = 0); + LinkAccArray(const iDynTree::Model & model); + + void resize(std::size_t nrOfLinks); + void resize(const iDynTree::Model & model); + + bool isConsistent(const Model& model) const; + + iDynTree::SpatialAcc & operator()(const LinkIndex link); + const iDynTree::SpatialAcc & operator()(const LinkIndex link) const; + + std::size_t getNrOfLinks() const; + + std::string toString(const Model & model) const; + + ~LinkAccArray(); + }; + + /** + * Typedef used when the vector is meant to be a vector of link proper accelerations. + */ + typedef LinkAccArray LinkProperAccArray; +} + +#endif /* IDYNTREE_LINK_STATE_H */ diff --git a/src/model/include/iDynTree/LinkTraversalsCache.h b/src/model/include/iDynTree/LinkTraversalsCache.h new file mode 100644 index 00000000000..5807837f4ed --- /dev/null +++ b/src/model/include/iDynTree/LinkTraversalsCache.h @@ -0,0 +1,74 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_MODEL_LINKTRAVERSALSCACHE_H +#define IDYNTREE_MODEL_LINKTRAVERSALSCACHE_H + +#include +#include + +namespace iDynTree { + class Traversal; + class Model; + + class LinkTraversalsCache; +} + +/** + * Link traversal cache, store a traversal for each link in the model. + * + * Class that stores a traversal for each link in the model. + * It actually computes the traversal on the first time that a given traversal + * is requested + */ +class iDynTree::LinkTraversalsCache +{ +private: + std::vector m_linkTraversals; + void deleteTraversals(); + +public: + /** + * Default constructur + */ + LinkTraversalsCache(); + + /** + * Default destructor + */ + ~LinkTraversalsCache(); + + + /** + * Resize the cache to contains the specified number of Traversals + * + * @param nrOfLinks number of traversal + */ + void resize(unsigned int nrOfLinks); + + + /** + * Resize the cache to contains the number of Traversals for the specified model + * + * @param model the model + */ + void resize(const iDynTree::Model& model); + + + /** + * return the traversal for the specified link in the specified model + * + * If the traversal has been cached this function returns the cached traversal + * otherwise it creates the new traversal, caches it and returns it, thus + * performin a memory allocation. + * + * @param model the model + * @param linkIdx the index to be considered as base + * @return the traversal for the specified model link + */ + Traversal& getTraversalWithLinkAsBase(const iDynTree::Model & model, + const iDynTree::LinkIndex linkIdx); +}; + + +#endif /* end of include guard: IDYNTREE_MODEL_LINKTRAVERSALSCACHE_H */ diff --git a/src/model/include/iDynTree/Model.h b/src/model/include/iDynTree/Model.h new file mode 100644 index 00000000000..c4874e9c3ac --- /dev/null +++ b/src/model/include/iDynTree/Model.h @@ -0,0 +1,578 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_MODEL_H +#define IDYNTREE_MODEL_H + +#include +#include +#include + +#include +#include + +#include +#include +#include + +namespace iDynTree +{ + class Traversal; + + struct Neighbor + { + LinkIndex neighborLink; + JointIndex neighborJoint; + }; + + /** + * Class that represents a generic multibody model. + * + * A model is composed by rigid bodies (i.e. links) connected + * by joints. Each joint can have from 0 to 6 degrees of freedom. + * + * Each link has a "link frame" rigidly attached to it. + * Additionally, other rigidly attachable frames can be defined for each link. + * + * The model contains also a serialization for the different elements, + * i.e. a function between: + * * joint names and the integers 0..getNrOfJoints()-1 + * * dof names and the integers 0..getNrOfDOFs()-1 + * * link names and the integers 0..getNrOfLinks()-1 + * * frame names and the integers 0..getNrOfFrames()-1 + * + * For simplicity, these mappings are build when building the model. + * In particular the link and joint indices are assigned when the links + * and joint are added to the model using the `addLink` and `addJoint` + * methods. + * + * The DOF indices are also assigned when the joint is added to the model + * with the addJoint method. For example if a model is composed only of + * 0 or 1 DOF joints and the 1 DOFs joints are added before the 0 DOFs then + * the joint index and dof index for 1 DOF joints will be coincident (this is + * how the URDF parser is actually implemented). For this reason the current + * implementation does not have a concept of DOF explicit identifier, + * i.e. a getDOFName(DOFIndex dofIndex) method does not exist. + * + * The frame indices between 0 and getNrOfLinks()-1 are always assigned to the + * "main" link frame of the link with the same index. The frame indices + * between getNrOfLinks() and getNrOfFrames()-1 are assigned when the additional + * frame is added to the model with the addAdditionalFrameToLink call. All the additional + * frame indices are incremented by 1 whenever a new link is added, to ensure that + * its "link frame" has a frame index in the 0...getNrOfLinks()-1 range. + * + * + * + * \ingroup iDynTreeModel + */ + class Model + { + private: + /** Vector of links. For each link its index indicates its location in this vector */ + std::vector links; + + /** Vector of joints. For each joint its index indicates its location in this vector */ + std::vector joints; + + /** Vector of additional frames. + * The element additionalFrames[frameOffset] will be the link_H_frame transform of the frame with + * FrameIndex getNrOfLinks() + frameOffset . + */ + std::vector additionalFrames; + + /** + * Vector of link indices corresponding to an additional frame. + * The element additionalFrameNames[frameOffset] will be the link_H_frame transform of the frame with + * FrameIndex getNrOfLinks() + frameOffset . + */ + std::vector additionalFramesLinks; + + /** Vector of link names, matches the index of each link to its name. */ + std::vector linkNames; + + /** Vector of joint names, matches the index of each joint to its name. */ + std::vector jointNames; + + /** + * Vector of additional frame names. + * The element frameNames[frameOffset] will be the name of the frame with + * FrameIndex getNrOfLinks() + frameOffset . + */ + std::vector frameNames; + + /** Adjacency lists: match each link index to a list of its neighbors, + and the joint connecting to them. */ + std::vector< std::vector > neighbors; + + /** Vector containing the package directories associated to the model. */ + std::vector packageDirs; + + /** + * Most data structures are not undirected, so we store the original + * root of the tree, to provide a default root for Traversal generation. + */ + LinkIndex defaultBaseLink; + + /** + * Cache number of position coordinaties of the model. + * If all joints are 0 or 1 dofs, this is equal to nrOfDOFs. + */ + unsigned int nrOfPosCoords; + + /** + * Cached number of (internal) DOFs of the model. + * This is just the sum of all the getNrOfDOFs of the joint in the model. + */ + unsigned int nrOfDOFs; + + /** + * Solid shapes used for visualization. + */ + ModelSolidShapes m_visualSolidShapes; + + /** + * Solid shapes used for collision checking. + */ + ModelSolidShapes m_collisionSolidShapes; + + /** + * Copy the structure of the model from another instance of a model. + */ + void copy(const Model & model); + + /** + * Destroy the object, properly deallocating memory. + */ + void destroy(); + + public: + + /** + * Costructor + */ + Model(); + + /** + * Copy costructor + */ + Model(const Model & other); + + /** + * Copy operator + */ + Model& operator=(const Model &other); + + /** + * Copy the model. + * + * Get a copy of the model. + */ + Model copy() const; + + /** + * Destructor + * + */ + virtual ~Model(); + + /** + * Get the number of links in the model. + */ + size_t getNrOfLinks() const; + + /** + * @return a vector containing all the directories of the meshes + */ + const std::vector& getPackageDirs() const; + + void setPackageDirs(const std::vector& packageDirs); + + /** + * Get the name of a link given its index, or + * an LINK_INVALID_NAME string if linkIndex < 0 or >= getNrOfLinks() + */ + std::string getLinkName(const LinkIndex linkIndex) const; + + LinkIndex getLinkIndex(const std::string & linkName) const; + + /** + * \brief Check if a given LinkIndex is valid. + * + * A link index is valid if is different from + * LINK_INVALID_INDEX and 0 =< index < getNrOfLinks()-1 + * + * @return true if the index is valid, false otherwise. + */ + bool isValidLinkIndex(const LinkIndex index) const; + + LinkPtr getLink(const LinkIndex linkIndex); + LinkConstPtr getLink(const LinkIndex linkIndex) const; + + LinkIndex addLink(const std::string & name, const Link & link); + + /** + * Get number of joints in the model. + */ + size_t getNrOfJoints() const; + + /** + * Get the name of a joint given its index, or + * an JOINT_INVALID_NAME if linkIndex < 0 or >= getNrOfLinks() + */ + std::string getJointName(const JointIndex index) const; + + /** + * Get the total mass of the robot + */ + double getTotalMass() const; + + /** + * Get the index of a joint, given a jointName. + * If the jointName is not found in the model, + * return JOINT_INVALID_INDEX . + * + */ + JointIndex getJointIndex(const std::string & jointName) const; + + IJointPtr getJoint(const JointIndex index); + + IJointConstPtr getJoint(const JointIndex index) const; + + /** + * \brief Check if a given JointIndex is valid. + * + * A joint index is valid if is different from + * JOINT_INVALID_INDEX and 0 =< index < getNrOfJoints()-1 + * + * @return true if the index is valid, false otherwise. + */ + bool isValidJointIndex(const JointIndex index) const; + + /** + * Check if a name is already used for a link in the model. + * + * @return true if a name is used by a link in a model, false otherwise. + */ + bool isLinkNameUsed(const std::string linkName) const; + + /** + * Check if a name is already used for a joint in the model. + * + * @return true if a name is used by a joint in a model, false otherwise. + */ + bool isJointNameUsed(const std::string jointName) const; + + /** + * Check if a name is already used for a frame in the model. + * + * \note this function will check the name of the links and the names of the additional frames. + * @return true if a name is used by a frame in a model, false otherwise. + */ + bool isFrameNameUsed(const std::string frameName) const; + + /** + * + * Add a joint to the model. The two links to which the joint is connected + * are specified in the joint itself, throught the appropriate methods. + * @return the JointIndex of the added joint, or JOINT_INVALID_INDEX if + * there was an error in adding the joint. + */ + JointIndex addJoint(const std::string & jointName, IJointConstPtr joint); + + /** + * Add a joint to the model, and specify the two links that are connected + * by the specified joints. The setAttachedLinks of the passed joint + * is called appropriately, to ensure consistency in the model. + * @return the JointIndex of the added joint, or JOINT_INVALID_INDEX if + * there was an error in adding the joint. + */ + JointIndex addJoint(const std::string & link1, const std::string & link2, + const std::string & jointName, IJointConstPtr joint); + + /** + * Add a joint to the model, and add also a link. + * The added joints connects an existing link of the model to the newly + * added link. + * The setAttachedLinks of the passed joint + * is called appropriately, to ensure consistency in the model. + * @return the JointIndex of the added joint, or JOINT_INVALID_INDEX if + * there was an error in adding the joint. + */ + JointIndex addJointAndLink(const std::string & existingLink, + const std::string & jointName, IJointConstPtr joint, + const std::string & newLinkName, Link & newLink); + + /** + * \brief Displace a link by inserting a new link and a new joint. + * Displace a link by replacing it with a new link in the existing joint and insert new joint between the new link and the displaced link. + * Inputs: + * @param[in] existingJoint is where the new link will be inserted. + * @param[in] unmovableLink is the link that was previously connected to the displaced link by the existingJoint. + * @param[in] _unmovable_X_newLink is the transformation matrix from the unmovableLink to the new Link. + * @param[in] jointName is the name of the new joint. + * @param[in] joint is the new joint connecting the new link and the link that was displaced. + * @param[in] newLinkName is the name of the new link. + * @param[in] newLink is the new link that will be attached to the unmovable link using the existingJoint. + * The setAttachedLinks of the new joint + * and the existing joint are edited appropriately, to ensure consistency in the model. + * @return the JointIndex of the added joint, or JOINT_INVALID_INDEX if + * there was an error in adding the joint. + */ + JointIndex insertLinkToExistingJointAndAddJointForDisplacedLink(const std::string & existingJoint, + const std::string & unmovableLink, + const Transform& _unmovableLink_X_newLink, + const std::string & jointName, IJointConstPtr joint, + const std::string & newLinkName, Link & newLink); + + /** + * Get the dimension of the vector used to parametrize the positions of the joints of the robot. + * This number can be obtained by summing the getNrOfPosCoords of all the joints of the model. + * + * \warning This is *not* including the 6 degrees of freedom of the base. + */ + size_t getNrOfPosCoords() const; + + /** + * Get the number of degrees of freedom of the joint of the robot. + * This number can be obtained by summing the getNrOfDOFs of all the joints of the model. + * + * \warning This is *not* including the 6 degrees of freedom of the base. + */ + size_t getNrOfDOFs() const; + + /** + * Get the number of frames in the model. + * + * \note this will return the sum of the number of link + * (as each link has an attached frame) and the total number + * of additional frames. + * + * @return the number of frames in the model. + */ + size_t getNrOfFrames() const; + + /** + * Add an additional frame to a link. + * + * \note This function has signature different from + * addJoint/addLink because the FrameIndex of the + * additional frame are invalidated at each call to + * the addLink. So we don't return the FrameIndex in this + * function, as the model construction should be completed + * before that FrameIndex are stored. + * + * @param[in] linkName the link to which attach the additional frame. + * @param[in] frameName the name of the frame added to the model. + * @param[in] link_H_frame the pose of added frame with respect to the link main frame, + * expressed with a transform with: + * refFrame: the main link frame + * frame: the added frame + * @return true if all went well, false if an error occured. + * + */ + bool addAdditionalFrameToLink(const std::string & linkName, + const std::string & frameName, + iDynTree::Transform link_H_frame); + + /** + * Get the name of a frame given its index. + * + * @param[in] frameIndex the index of the frame whose name is requested. + * @return the name of a frame given its index, or + * a FRAME_INVALID_NAME string if frameIndex < 0 or >= getNrOfFrames() + */ + std::string getFrameName(const FrameIndex frameIndex) const; + + /** + * Get the index of a frame given its name. + * + * @param[in] frameName the name of the frame for which the index is requested. + * @return the index of the frame, of FRAME_INVALID_INDEX if frameName is not + * not a frame name. + */ + FrameIndex getFrameIndex(const std::string & frameName) const; + + /** + * \brief Check if a given FrameIndex is valid. + * + * A frame index is valid if is different from + * FRAME_INVALID_INDEX and 0 =< index < getNrOfFrames()-1 + * + * @return true if the index is valid, false otherwise. + */ + bool isValidFrameIndex(const FrameIndex index) const; + + /** + * Get the tranform of the frame with respect to the main + * frame of the link, returned as link_H_frame tranform + * with refFrame : the link main frame and frame : the + * frame with index frameIndex . + * + * @param[in] frameIndex the index of the frame for which transform is requested. + * @return the link_H_frame transform, or an identity tranform + * if frameIndex < 0 or frameIndex >= getNrOfFrames . + */ + Transform getFrameTransform(const FrameIndex frameIndex) const; + + /** + * Get the link at which the frame with index frameIndex + * is attached. + * + */ + LinkIndex getFrameLink(const FrameIndex frameIndex) const; + + /** + * Get the additional frames of a specified link. + * + * @note The vector of returned frame index is ordered according to the frame index. + * @warning This method searches linearly over all the frames. + * + * @param[in] link a LinkIndex of the specified link, + * @param[out] frames a vector of FrameIndex of the frame indeces attached to the specified link index, + * @return true if the specified link is a valid link, false otherwise. + */ + bool getLinkAdditionalFrames(const LinkIndex lnkIndex, std::vector& frameIndeces) const; + + /** + * Get the nr of neighbors of a given link. + */ + unsigned int getNrOfNeighbors(const LinkIndex link) const; + + /** + * Get the neighbor of a link. neighborIndex should be + * >= 0 and <= getNrOfNeighbors(link) + */ + Neighbor getNeighbor(const LinkIndex link, unsigned int neighborIndex) const; + + /** + * Set the default base link, used for generation of the default traversal. + */ + bool setDefaultBaseLink(const LinkIndex linkIndex); + + /** + * Get the default base link, used for generation of the default traversal. + * + * If no link are present in model, returns LINK_INVALID_INDEX. + * If setDefaultBaseLink was never called but at least a link has been added + * to the model, returns 0 (i.e. the index of the first link added to the model. + * + * @return index of the default base link, if valid + */ + LinkIndex getDefaultBaseLink() const; + + /** + * Compute a Traversal of all the links in the Model, doing a Depth First Search starting + * at the default base. + * + * \warning The traversal computed with this function contains pointers to the joints + * and links present in this model. Whenever these pointers are invalidated, + * for example because a link or a joint is added or the model is copied, + * the traversal need to be recomputed. + * + * \warning this function works only on Models without cycles. + * @param[out] traversal traversal of all links in the model + * @return true if all went well, false otherwise. + */ + bool computeFullTreeTraversal(Traversal & traversal) const; + + /** + * Compute a Traversal of all the links in the Model, doing a Depth First Search starting + * at the given traversalBase. + * + * \warning The traversal computed with this function contains pointers to the joints + * and links present in this model. Whenever these pointers are invalidated, + * for example because a link or a joint is added or the model is copied, + * the traversal need to be recomputed. + *. + * \warning this function works only on Models without cycles. + * @param[out] traversal traversal of all links in the model + * @param[in] traversalBase base (root) link in the traversal + * @return true if all went well, false otherwise + */ + bool computeFullTreeTraversal(Traversal & traversal, const LinkIndex traversalBase) const; + + /** + * Get the inertial parameters of the links of the model in vector forms. + * + * This methods gets the inertial parameters (mass, center + * of mass, 3D inertia matrix) of the links of the robot. + * + * The output vector of inertial parameters must have 10*getNrOfLinks() elements, + * each 10 elements subvector corresponds to the inertial parameters of one link, + * following the serialization induced by the link indices + * (link 0 corresponds to elements 0-9, link 1 to 10-19, etc). + * + * The mapping between the SpatialInertia class and the Vector10 elements is the one + * defined in SpatialInertia::asVector() method. + * + * @param[out] modelInertialParams vector of inertial parameters + * @return true if all went well, false otherwise. + * + */ + bool getInertialParameters(VectorDynSize & modelInertialParams) const; + + /** + * Update the inertial parameters of the links of the model. + * + * This methods modifies the inertial parameters (mass, center + * of mass, 3D inertia matrix) of the links of the robot. + * + * The input vector of inertial parameters must have 10*getNrOfLinks() elements, + * each 10 elements subvector corresponds to the inertial parameters of one link, + * following the serialization induced by the link indices + * (link 0 corresponds to elements 0-9, link 1 to 10-19, etc). + * + * The mapping between the SpatialInertia class and the Vector10 elements is the one + * defined in SpatialInertia::asVector() method. + * + * @note For efficency reason, inertial parameters are not checked for full physical + * consistency before being update. + * + * @param[in] modelInertialParams vector of inertial parameters + * @return true if all went well, false otherwise. + * + */ + bool updateInertialParameters(const VectorDynSize & modelInertialParams); + + /** + * Get the ModelSolidShapes meant for visualization. + * + * @return a reference to ModelSolidShapes meant for visualization. + */ + ModelSolidShapes& visualSolidShapes(); + + /** + * Get the ModelSolidShapes meant for visualization (const version) + * + * @return a reference to ModelSolidShapes meant for visualization. + */ + const ModelSolidShapes& visualSolidShapes() const; + + /** + * Get the ModelSolidShapes meant for collision checking. + * + * @return a reference to ModelSolidShapes meant for visualization. + */ + ModelSolidShapes& collisionSolidShapes(); + + + /** + * Get the ModelSolidShapes meant for collision checking (const version) + * + * @return a reference to ModelSolidShapes meant for visualization. + */ + const ModelSolidShapes& collisionSolidShapes() const; + + /** + * \brief Get a printable representation of the Model. + * + * Useful for debugging. + */ + std::string toString() const; + + }; + + +} + +#endif /* IDYNTREE_LINK_H */ diff --git a/src/model/include/iDynTree/Model/Centroidal.h b/src/model/include/iDynTree/Model/Centroidal.h index f0a9cab637d..1a97bcdbb6b 100644 --- a/src/model/include/iDynTree/Model/Centroidal.h +++ b/src/model/include/iDynTree/Model/Centroidal.h @@ -1,26 +1,14 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef IDYNTREE_CENTROIDAL_H -#define IDYNTREE_CENTROIDAL_H +#ifndef IDYNTREE_MODEL_CENTROIDAL_H +#define IDYNTREE_MODEL_CENTROIDAL_H -#include - -namespace iDynTree -{ - class Model; - class Traversal; - class Transform; - class FreeFloatingPos; - class FreeFloatingVel; - class FreeFloatingAcc; - class LinkPositions; - class LinkVelArray; - class LinkAccArray; - class JointPosDoubleArray; - class MatrixDynSize; +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif +#include -} #endif diff --git a/src/model/include/iDynTree/Model/ContactWrench.h b/src/model/include/iDynTree/Model/ContactWrench.h index 6361b4de298..df03818ccfe 100644 --- a/src/model/include/iDynTree/Model/ContactWrench.h +++ b/src/model/include/iDynTree/Model/ContactWrench.h @@ -1,150 +1,13 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef IDYNTREE_CONTACT_WRENCH_H -#define IDYNTREE_CONTACT_WRENCH_H +#ifndef IDYNTREE_MODEL_CONTACT_WRENCH_H +#define IDYNTREE_MODEL_CONTACT_WRENCH_H -#include -#include +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif -#include -#include +#include - -#include - -namespace iDynTree -{ - class Model; - - /** - * \brief A wrench excerted on a link due to an external contact. - * - * The contact wrench is represented by two quantities the contactPoint - * and the contactWrench . - */ - class ContactWrench - { - private: - Position m_contactPoint; - Wrench m_contactWrench; - - /** - * Unique id identifing the contact. - * This id is propagated to the contact wrench data structure. - * It is implemented mainly for compatibility with the skinDynLib library. - */ - std::size_t m_contactId; - - public: - /** - * \brief Position of a point on the contact surface. - * - * This is the position of a point (expressed in the link frame) that lies - * on the contact surface. - * - * Its precise definition is application dependent. - * - * If the ContactWrench class is used to represent a pure force - * applied on a point, the contactPoint is the point in which the - * pure force is applied. - * - * If the ContactWrench class is used to represent contact information - * coming from a tacticle system, the contact point may be the average - * between all the taxels (tactile elements) belonging to the contact. - */ - Position & contactPoint(); - - /** - * \brief Wrench that an external element excert on a link through the contact. - * - * This wrench (expressed with respect to the contactPoint and with the orientation - * of the link frame) is the wrench excerted on the link by an external element. - * - */ - Wrench & contactWrench(); - - std::size_t& contactId(); - - const Position & contactPoint() const; - const Wrench & contactWrench() const; - const std::size_t& contactId() const; - - }; - - /** - * A set of ContactWrench for each link, representing all the contacts - * between the model and the external environment. - * - */ - class LinkContactWrenches - { - private: - std::vector< std::vector > m_linkContactWrenches; - - public: - /** - * Create a LinkWrenches vector, with the size given - * by nrOfLinks . - * - * @param[in] nrOfLinks the size of the vector. - */ - LinkContactWrenches(std::size_t nrOfLinks = 0); - LinkContactWrenches(const Model & model); - - /** - * - * - * @param[in] nrOfLinks the number of links to which resize this object - */ - void resize(std::size_t nrOfLinks); - void resize(const Model & model); - - /** - * Get the number of external contacts for a given link. - */ - size_t getNrOfContactsForLink(const LinkIndex linkIndex) const; - - /** - * Set the number of external contacts for a given link. - */ - void setNrOfContactsForLink(const LinkIndex linkIndex, const size_t nrOfContacts); - - /** - * Get the number of links. - */ - size_t getNrOfLinks() const; - - /** - * Get a specific ContactWrench - * - * @param[in] linkIndex the index of the link for which the contact is retrieved - * @param[in] contactIndex a index (between 0 and getNrOfContactsForLink(link)-1 ) identifing the specific contact. - */ - ContactWrench& contactWrench(const LinkIndex linkIndex, const size_t contactIndex); - - const ContactWrench& contactWrench(const LinkIndex linkIndex, const size_t contactIndex) const; - - /** - * - * \brief Compute the net wrenches vector from the contacts wrenches vector. - * - * This is just a loop that sums all the contact wrenches for every link - * and store the results (expressed in the link frame) in the netWrenches vector. - * - * @return true if all went well, false otherwise. - */ - bool computeNetWrenches(LinkNetExternalWrenches & netWrenches) const; - - - /** - * Get a human readable description of the LinkUnknownWrenchContacts (for debug) - */ - std::string toString(const Model& model) const; - }; - - -} - - -#endif /* IDYNTREE_CONTACT_WRENCH_H */ +#endif diff --git a/src/model/include/iDynTree/Model/DenavitHartenberg.h b/src/model/include/iDynTree/Model/DenavitHartenberg.h index 675982d93c8..fc1ebdbee00 100644 --- a/src/model/include/iDynTree/Model/DenavitHartenberg.h +++ b/src/model/include/iDynTree/Model/DenavitHartenberg.h @@ -1,185 +1,13 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef IDYNTREE_DENAVIT_HARTENBERG_H -#define IDYNTREE_DENAVIT_HARTENBERG_H +#ifndef IDYNTREE_MODEL_DH_H +#define IDYNTREE_MODEL_DH_H -#include -#include - -#include - -#include - -namespace iDynTree -{ - -/** - * Structure representing - * the four DH parameters of - * a link in a Chain. - */ -struct DHLink -{ - double A; - double D; - double Alpha; - double Offset; - double Min; - double Max; -}; - -/** - * Simple representation of a chain described with - * Denavit-Hartenberg Parameters. - * Directly inspiered by the iCub::iKin::iKinChain class. - */ -class DHChain -{ -private: - Transform H0; - std::vector dhParams; - Transform HN; - std::vector dofNames; - -public: - void setNrOfDOFs(size_t nDofs); - size_t getNrOfDOFs() const; - - void setH0(const iDynTree::Transform & _H0); - const iDynTree::Transform & getH0() const; - - void setHN(const Transform & _HN); - const Transform & getHN() const; - - /** - * Return a reference to the i-th link of the chain. - */ - DHLink & operator() (const size_t i); - - /** - * Return a reference to the i-th link of the chain (const version). - */ - const DHLink & operator() (const size_t i) const; - - /** - * Get the name of the i-th DOF. - */ - std::string getDOFName(size_t dofIdx) const; - - /** - * Get the name of the i-th DOF. - */ - void setDOFName(size_t dofIdx, const std::string &dofName); - - /** - * Create an iDynTree::Model from this DHChain. - * \see CreateModelFromDHChain . - */ - bool toModel(Model & outputModel) const; - - /** - * Create a DHChain from a Model. - * \see ExtractDHChainFromModel . - */ - bool fromModel(const iDynTree::Model & model, - std::string baseFrame, - std::string eeFrame); -}; - -/* - * DH_Craig1989 : constructs a transformationmatrix - * T_link(i-1)_link(i) with the Denavit-Hartenberg convention as - * described in the Craigs book: Craig, J. J.,Introduction to - * Robotics: Mechanics and Control, Addison-Wesley, - * isbn:0-201-10326-5, 1986. - * - * Note that the frame is a redundant way to express the information - * in the DH-convention. - * \verbatim - * Parameters in full : a(i-1),alpha(i-1),d(i),theta(i) - * - * axis i-1 is connected by link i-1 to axis i numbering axis 1 - * to axis n link 0 (immobile base) to link n - * - * link length a(i-1) length of the mutual perpendicular line - * (normal) between the 2 axes. This normal runs from (i-1) to - * (i) axis. - * - * link twist alpha(i-1): construct plane perpendicular to the - * normal project axis(i-1) and axis(i) into plane angle from - * (i-1) to (i) measured in the direction of the normal - * - * link offset d(i) signed distance between normal (i-1) to (i) - * and normal (i) to (i+1) along axis i joint angle theta(i) - * signed angle between normal (i-1) to (i) and normal (i) to - * (i+1) along axis i - * - * First and last joints : a(0)= a(n) = 0 - * alpha(0) = alpha(n) = 0 - * - * PRISMATIC : theta(1) = 0 d(1) arbitrarily - * - * REVOLUTE : theta(1) arbitrarily d(1) = 0 - * - * Not unique : if intersecting joint axis 2 choices for normal - * Frame assignment of the DH convention : Z(i-1) follows axis - * (i-1) X(i-1) is the normal between axis(i-1) and axis(i) - * Y(i-1) follows out of Z(i-1) and X(i-1) - * - * a(i-1) = distance from Z(i-1) to Z(i) along X(i-1) - * alpha(i-1) = angle between Z(i-1) to Z(i) along X(i-1) - * d(i) = distance from X(i-1) to X(i) along Z(i) - * theta(i) = angle between X(i-1) to X(i) along X(i) - * \endverbatim - * - * Function compatible with KDL's Frame::DH_Craig1989 method. - */ -Transform TransformFromDHCraig1989(double a,double alpha,double d,double theta); - -/** - * DH : constructs a transformationmatrix T_link(i-1)_link(i) with - * the Denavit-Hartenberg convention as described in the original - * publictation: Denavit, J. and Hartenberg, R. S., A kinematic - * notation for lower-pair mechanisms based on matrices, ASME - * Journal of Applied Mechanics, 23:215-221, 1955. - * - * Function compatible on KDL's Frame::DH method. - */ -Transform TransformFromDH(double a,double alpha,double d,double theta); - -/** - * Extract a Denavit Hartenberg Chain from a iDynTree::Model. - * In particular you can specify the base frame and the end effector frame - * of the chain. The chain is then extracted using the algorithm found - * in: - * Chapter 3, Spong, Mark W., Seth Hutchinson, and M. Vidyasagar. "Robot modeling and control". 2006. - * (section 3.2.3 of http://www.cs.duke.edu/brd/Teaching/Bio/asmb/current/Papers/chap3-forward-kinematics.pdf) - * - * \note The DH representation supports only revolute and translational - * 1-DOF joints. In this implementation only revolute joints are supported. - */ -bool ExtractDHChainFromModel(const iDynTree::Model & model, - const std::string baseFrame, - const std::string eeFrame, - DHChain & outputChain, - double tolerance = 1e-5); - -/** - * Create an iDynTree::Model from a DHChain. - * - * \note The names of the links will be link0, link1, ... linkN, - * furthermore there are two additional frame for the base and endEffector frames, - * named baseFrame and distalFrame . - * \note The inertia of the links will be set to 1 kg in the origin of the link. - * - * @return true if all went ok, false otherwise. - */ -bool CreateModelFromDHChain(const DHChain & inputChain, - Model & outputModel); - - -} +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif +#include #endif diff --git a/src/model/include/iDynTree/Model/Dynamics.h b/src/model/include/iDynTree/Model/Dynamics.h index 13fd1856674..9cc40db543a 100644 --- a/src/model/include/iDynTree/Model/Dynamics.h +++ b/src/model/include/iDynTree/Model/Dynamics.h @@ -1,193 +1,14 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef IDYNTREE_INVERSE_DYNAMICS_H -#define IDYNTREE_INVERSE_DYNAMICS_H - -#include - -#include -#include -#include - -namespace iDynTree -{ - class Model; - class Traversal; - class FreeFloatingPos; - class FreeFloatingVel; - class FreeFloatingAcc; - class FreeFloatingGeneralizedTorques; - class FreeFloatingMassMatrix; - class JointDOFsDoubleArray; - class DOFSpatialForceArray; - class DOFSpatialMotionArray; - class SpatialMomentum; - - /** - * \ingroup iDynTreeModel - * - * Compute the total linear and angular momentum of a robot, expressed in the world frame. - * - * @param[in] model the used model, - * @param[in] linkPositions linkPositions(l) contains the world_H_link transform. - * @param[in] linkVels linkVels(l) contains the link l velocity expressed in l frame. - * @param[out] totalMomentum total momentum, expressed in world frame. - * @return true if all went well, false otherwise. - */ - bool ComputeLinearAndAngularMomentum(const Model& model, - const LinkPositions& linkPositions, - const LinkVelArray& linkVels, - SpatialMomentum& totalMomentum); - - /** - * Compute the total momentum derivatitive bias, i.e. the part of the total momentum derivative that does not depend on robot acceleration. - * - * The linear and angular momentum derivative depends on the robot position, velocity and acceleration. - * This function computes the part that do not depend on the robot accelearation. - * - * This function returns the bias of the derivative of the ComputeLinearAndAngularMomentum function. - */ - bool ComputeLinearAndAngularMomentumDerivativeBias(const Model & model, - const LinkPositions& linkPositions, - const LinkVelArray & linkVel, - const LinkAccArray & linkBiasAcc, - Wrench& totalMomentumBias); - - /** - * \ingroup iDynTreeModel - * - * @brief Compute the inverse dynamics, i.e. the generalized torques corresponding to a given set of robot accelerations and external force/torques. - * - * @param[in] model The model used for the computation. - * @param[in] traversal The traversal used for the computation, it defines the used base link. - * @param[in] jointPos The (internal) joint position of the model. - * @param[in] linksVel Vector of left-trivialized velocities for each link of the model (for each link \f$L\f$, the corresponding velocity is \f${}^L \mathrm{v}_{A, L}\f$). - * @param[in] linksProperAcc Vector of left-trivialized proper acceleration for each link of the model - * (for each link \f$L\f$, the corresponding proper acceleration is \f${}^L \dot{\mathrm{v}}_{A, L} - \begin{bmatrix} {}^L R_A {}^A g \\ 0_{3\times1} \end{bmatrix} \f$), where \f$ {}^A g \in \mathbb{R}^3 \f$ is the gravity acceleration expressed in an inertial frame \f$A\f$ . See iDynTree::LinkNetExternalWrenches . - * @param[in] linkExtForces Vector of external 6D force/torques applied to the links. For each link \f$L\f$, the corresponding external force is \f${}_L \mathrm{f}^x_L\f$, i.e. the force that the enviroment applies on the on the link \f$L\f$, expressed in the link frame \f$L\f$. - * @param[out] linkIntWrenches Vector of internal joint force/torques. See iDynTree::LinkInternalWrenches . - * @param[out] baseForceAndJointTorques Generalized torques output. The base element is the residual force on the base (that is equal to zero if the robot acceleration and the external forces provided in LinkNetExternalWrenches were consistent), while the joint part is composed by the joint torques. - */ - bool RNEADynamicPhase(const iDynTree::Model & model, - const iDynTree::Traversal & traversal, - const iDynTree::JointPosDoubleArray & jointPos, - const iDynTree::LinkVelArray & linksVel, - const iDynTree::LinkAccArray & linksProperAcc, - const iDynTree::LinkNetExternalWrenches & linkExtForces, - iDynTree::LinkInternalWrenches & linkIntWrenches, - iDynTree::FreeFloatingGeneralizedTorques & baseForceAndJointTorques); - - /** - * Compute the floating base mass matrix, using the - * composite rigid body algorithm. - * - */ - bool CompositeRigidBodyAlgorithm(const Model& model, - const Traversal& traversal, - const JointPosDoubleArray& jointPos, - LinkCompositeRigidBodyInertias& linkCRBs, - FreeFloatingMassMatrix& massMatrix); - - - /** - * Structure of buffers required by ArticulatedBodyAlgorithm. - * - * As the ArticulatedBodyAlgorithm function needs some internal buffers - * to run, but we don't want to put memory allocation inside the ArticulatedBodyAlgorithm - * function, we put all the internal buffers in this structure. - * - * A convenient resize(Model) function is provided to automatically resize - * the buffers given a Model. - */ - struct ArticulatedBodyAlgorithmInternalBuffers - { - ArticulatedBodyAlgorithmInternalBuffers() {}; - - /** - * Call resize(model); - */ - ArticulatedBodyAlgorithmInternalBuffers(const Model & model); - - /** - * Resize all the buffers to the right size given the model, - * and reset all the buffers to 0. - */ - void resize(const Model& model); - - /** - * Check if the dimension of the buffer is consistent - * with a model (it should be after a call to resize(model) ). - */ - bool isConsistent(const Model& model); - - DOFSpatialMotionArray S; - DOFSpatialForceArray U; - JointDOFsDoubleArray D; - JointDOFsDoubleArray u; - LinkVelArray linksVel; - LinkAccArray linksBiasAcceleration; - LinkAccArray linksAccelerations; - LinkArticulatedBodyInertias linkABIs; - LinkWrenches linksBiasWrench; - - // Debug quantity - //LinkWrenches pa; - }; - - /** - * \ingroup iDynTreeModel - * - * Compute the floating base acceleration of an unconstrianed - * robot, using as input the external forces and the joint torques. - * We follow the algorithm described in Featherstone 2008, modified - * for the floating base case and for handling fixed joints. - * - */ - bool ArticulatedBodyAlgorithm(const Model& model, - const Traversal& traversal, - const FreeFloatingPos& robotPos, - const FreeFloatingVel& robotVel, - const LinkNetExternalWrenches & linkExtWrenches, - const JointDOFsDoubleArray & jointTorques, - ArticulatedBodyAlgorithmInternalBuffers & buffers, - FreeFloatingAcc & robotAcc); - - /** - * \ingroup iDynTreeModel - * - * - * @brief Compute the inverse dynamics of the model as linear function of the inertial parameters. - * - * This function computes the matrix that multiplied by the vector of inertial parameters of a model (see iDynTree::Model::getInertialParameters) - * returns the inverse dynamics generalized torques. In particular it is consistent with the result of the iDynTree::RNEADynamicPhase function, i.e. - * the first six rows of the regressor correspond to the sum of all external force/torques acting on the robot, expressed in the origin - * and with the orientation of the specified referenceFrame, as defined by the referenceFrame_H_link argument. - * - * - * @note The regressor only computes the inverse dynamics generalized torques assuming that the external forces are equal to zero, - * as the contribution of the external forces to the inverse dynamics is indipendent from inertial parameters. - * - * @param[in] model The model used for the computation. - * @param[in] traversal The traversal used for the computation, it defines the used base link. - * @param[in] referenceFrame_H_link Position of each link w.r.t. to given frame D (tipically an inertial frame A, the base frame B or the mixed frame B[A]). For each link \f$L\f$, the corresponding transform is \f${}^D H_L\f$. - * @param[in] linksVel Vector of left-trivialized velocities for each link of the model (for each link \f$L\f$, the corresponding velocity is \f${}^L \mathrm{v}_{A, L}\f$). - * @param[in] linksProperAcc Vector of left-trivialized proper acceleration for each link of the model - * (for each link \f$L\f$, the corresponding proper acceleration is \f${}^L \dot{\mathrm{v}}_{A, L} - \begin{bmatrix} {}^L R_A {}^A g \\ 0_{3\times1} \end{bmatrix} \f$), where \f$ {}^A g \in \mathbb{R}^3 \f$ is the gravity acceleration expressed in an inertial frame \f$A\f$ . - * @param[out] baseForceAndJointTorquesRegressor The (6+model.getNrOfDOFs() X 10*model.getNrOfLinks()) inverse dynamics regressor. - * - */ - bool InverseDynamicsInertialParametersRegressor(const iDynTree::Model & model, - const iDynTree::Traversal & traversal, - const iDynTree::LinkPositions& referenceFrame_H_link, - const iDynTree::LinkVelArray & linksVel, - const iDynTree::LinkAccArray & linksAcc, - iDynTree::MatrixDynSize & baseForceAndJointTorquesRegressor); - +#ifndef IDYNTREE_MODEL_DYNAMICS_H +#define IDYNTREE_MODEL_DYNAMICS_H +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif -} +#include #endif - diff --git a/src/model/include/iDynTree/Model/DynamicsLinearization.h b/src/model/include/iDynTree/Model/DynamicsLinearization.h index 9c5e386bd53..2760c44ecf7 100644 --- a/src/model/include/iDynTree/Model/DynamicsLinearization.h +++ b/src/model/include/iDynTree/Model/DynamicsLinearization.h @@ -1,102 +1,13 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef IDYNTREE_DYNAMICS_LINEARIZATION_H -#define IDYNTREE_DYNAMICS_LINEARIZATION_H +#ifndef IDYNTREE_MODEL_DYN_LINEARIZATION_H +#define IDYNTREE_MODEL_DYN_LINEARIZATION_H -#include -#include +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif -#include -#include +#include -namespace iDynTree -{ - /** - * Structure containing the internal buffers used - * by the ForwardDynamicsLinearization function. - */ - struct ForwardDynamicsLinearizationInternalBuffers - { - ForwardDynamicsLinearizationInternalBuffers() {}; - - /** - * Call resize(model); - */ - ForwardDynamicsLinearizationInternalBuffers(const Model & model); - - /** - * Resize all the buffers to the right size given the model, - * and reset all the buffers to 0. - */ - void resize(const Model& model); - - /** - * Buffers used for computing the ABA algorithm. - */ - ArticulatedBodyAlgorithmInternalBuffers aba; - - /** - * Buffers used for computing the derivative of ABA with respect - * to the joint DOFs position. - */ - std::vector dPos; - - /** - * Buffers used to compute the derivative with respect to the base velocity. - */ - LinkPositions linkPos; - std::vector dVb_linkBiasWrench; - std::vector dVb_u; - std::vector dVb_linkBiasAcceleration; - std::vector dVb_linksAccelerations; - - /** - * Buffer to store the derivative of - * the product - * \f[ - * V_l \bar\times^* M_l V_l - * \f] - * - * with respect to V_l - */ - std::vector dVl_linkLocalBiasWrench; - - - /** - * Buffers used for computing the derivative of ABA with respect - * to the joint DOFs velocities. - */ - std::vector dVel; - }; - - class FreeFloatingStateLinearization : public MatrixDynSize - { - public: - FreeFloatingStateLinearization(); - - FreeFloatingStateLinearization(const Model & model); - - void resize(const Model & model); - }; - - /* - * Compute the left-trivialized linearization, - * as described in [fill with left-trivialized repot when available]. - * - * HIGHLY EXPERIMENTAL FUNCTION, DO NOT USE - */ - bool ForwardDynamicsLinearization(const Model& model, - const Traversal& traversal, - const FreeFloatingPos& robotPos, - const FreeFloatingVel& robotVel, - const LinkNetExternalWrenches & linkExtWrenches, - const JointDOFsDoubleArray & jointTorques, - ForwardDynamicsLinearizationInternalBuffers & bufs, - FreeFloatingAcc & robotAcc, - FreeFloatingStateLinearization & A); - -} - - -#endif \ No newline at end of file +#endif diff --git a/src/model/include/iDynTree/Model/DynamicsLinearizationHelpers.h b/src/model/include/iDynTree/Model/DynamicsLinearizationHelpers.h index f0d94967fcd..a31d9004cdc 100644 --- a/src/model/include/iDynTree/Model/DynamicsLinearizationHelpers.h +++ b/src/model/include/iDynTree/Model/DynamicsLinearizationHelpers.h @@ -1,82 +1,13 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef IDYNTREE_DYNAMICS_LINEARIZATION_HELPERS_H -#define IDYNTREE_DYNAMICS_LINEARIZATION_HELPERS_H +#ifndef IDYNTREE_MODEL_DYN_LINEARIZATION_HELPERS_H +#define IDYNTREE_MODEL_DYN_LINEARIZATION_HELPERS_H -#include +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif -#include +#include -namespace iDynTree -{ - /** - * Class representing the derivative - * of a spatial motion vector with respect to - * another spatial motion vector. - */ - class SpatialMotionWrtMotionDerivative: public Matrix6x6 - { - public: - /** - * Equivalent to: - * - * ``` - * SpatialMotionWrtMotionDerivative ret; - * toEigen(ret) = toEigen(ret)*toEigen(transform.asAdjointMatrix()) - * return ret; - * ``` - */ - SpatialMotionWrtMotionDerivative operator*(const Transform & a_X_b); - - }; - - /** - * Equivalent to: - * - * ``` - * SpatialForceWrtMotionDerivative ret; - * toEigen(ret) = toEigen(transform.asAdjointMatrixWrench())*toEigen(ret); - * return ret; - * ``` - */ - SpatialMotionWrtMotionDerivative operator*(const Transform & a_X_b, const SpatialMotionWrtMotionDerivative & op2); - - - /** - * Class representing the derivative - * of a spatial force vector with respect to - * a spatial motion vector. - */ - class SpatialForceWrtMotionDerivative: public Matrix6x6 - { - public: - /** - * Equivalent to: - * - * ``` - * SpatialForceWrtMotionDerivative ret; - * toEigen(ret) = toEigen(ret)*toEigen(transform.asAdjointMatrix()) - * return ret; - * ``` - */ - SpatialForceWrtMotionDerivative operator*(const Transform & a_X_b); - - }; - - - /** - * Equivalent to: - * - * ``` - * SpatialForceWrtMotionDerivative ret; - * toEigen(ret) = toEigen(transform.asAdjointMatrixWrench())*toEigen(ret); - * return ret; - * ``` - */ - SpatialForceWrtMotionDerivative operator*(const Transform & a_X_b, const SpatialForceWrtMotionDerivative & op2); - -} - - -#endif /* IDYNTREE_DYNAMICS_LINEARIZATION_HELPERS_H */ \ No newline at end of file +#endif diff --git a/src/model/include/iDynTree/Model/DynamicsUtils.h b/src/model/include/iDynTree/Model/DynamicsUtils.h index 87eb093b86f..e04deeeaedb 100644 --- a/src/model/include/iDynTree/Model/DynamicsUtils.h +++ b/src/model/include/iDynTree/Model/DynamicsUtils.h @@ -1,25 +1,13 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef IDYNTREE_DYNAMICS_UTILS_H -#define IDYNTREE_DYNAMICS_UTILS_H +#ifndef IDYNTREE_MODEL_DYNAMICS_UTILS_H +#define IDYNTREE_MODEL_DYNAMICS_UTILS_H -#include -#include -#include +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif -namespace iDynTree -{ - /** - * Given a rigid body inertia \f$M\f$ and spatial motion vector \f$V\f$, - * the bias wrench \f$B\f$ of rigid body is defined as: - * \f[ - * B = \times - * \f] - * - */ - biasWrenchVelocityDerivative(SpatialInertia M, SpatialMotionVector V); -} +#include - -#endif /* IDYNTREE_DYNAMICS_UTILS_H */ \ No newline at end of file +#endif diff --git a/src/model/include/iDynTree/Model/FixedJoint.h b/src/model/include/iDynTree/Model/FixedJoint.h index bf48f4fdbc7..afa8867dc2b 100644 --- a/src/model/include/iDynTree/Model/FixedJoint.h +++ b/src/model/include/iDynTree/Model/FixedJoint.h @@ -1,189 +1,14 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef IDYNTREE_FIXED_JOINT_H -#define IDYNTREE_FIXED_JOINT_H +#ifndef IDYNTREE_MODEL_FIXED_JOINT_H +#define IDYNTREE_MODEL_FIXED_JOINT_H -#include +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif -#include -#include +#include -namespace iDynTree -{ - /** - * Class representing a fixed joint, i.e. a joint that rigidly attaches two links. - * - * \ingroup iDynTreeModel - */ - class FixedJoint : public IJoint - { - private: - JointIndex m_index; - std::size_t m_posCoordsOffset; - std::size_t m_DOFsOffset; - LinkIndex link1; - LinkIndex link2; - Transform link1_X_link2; - Transform link2_X_link1; - public: - - /** - * Default constructor. - * The joint is initialized with an Identity rest transform. - * You can call setRestTransform to set the rest transform at a - * second stage. - */ - explicit FixedJoint(); - - /** - * Constructor - */ - FixedJoint(const LinkIndex link1, const LinkIndex link2, - const Transform& link1_X_link2); - - /** - * Constructor in which the LinkIndex to which the joint is attached are not specified. - * This constructor is tipically used together with the Model::addJoint or - * Model::addJointAndLink methods, in which the links to which the joint is attached are - * specified by the other arguments of the method. - */ - FixedJoint(const Transform& link1_X_link2); - - /** - * Copy constructor - */ - FixedJoint(const FixedJoint & other); - - /** - * Destructor - */ - virtual ~FixedJoint(); - - // Documentation inherited - virtual IJoint * clone() const; - - // Documentation inherited - virtual unsigned int getNrOfPosCoords() const; - - // Documentation inherited - virtual unsigned int getNrOfDOFs() const; - - // Documentation inherited - virtual void setAttachedLinks(const LinkIndex link1, const LinkIndex link2); - - // Documentation inherited - virtual void setRestTransform(const Transform& link1_X_link2); - - // Documentation inherited - virtual LinkIndex getFirstAttachedLink() const; - - // Documentation inherited - virtual LinkIndex getSecondAttachedLink() const; - - // Documentation inherited - virtual Transform getRestTransform(const LinkIndex child, - const LinkIndex parent) const; - - - // Documentation inherited - virtual const Transform & getTransform(const VectorDynSize & jntPos, - const LinkIndex child, - const LinkIndex parent) const; - - // Documentation inherited - TransformDerivative getTransformDerivative(const VectorDynSize & jntPos, - const LinkIndex child, - const LinkIndex parent, - const int posCoord_i) const; - - // Documentation inherited - virtual SpatialMotionVector getMotionSubspaceVector(int dof_i, - const LinkIndex child, - const LinkIndex parent) const; - - // Documentation inherited - virtual void computeChildPosVelAcc(const VectorDynSize & jntPos, - const VectorDynSize & jntVel, - const VectorDynSize & jntAcc, - LinkPositions & linkPositions, - LinkVelArray & linkVels, - LinkAccArray & linkAccs, - const LinkIndex child, - const LinkIndex parent) const; - - // Documentation inherited - virtual void computeChildVelAcc(const VectorDynSize & jntPos, - const VectorDynSize & jntVel, - const VectorDynSize & jntAcc, - LinkVelArray & linkVels, - LinkAccArray & linkAccs, - const LinkIndex child, const LinkIndex parent) const; - - // Documentation inherited - virtual void computeChildVel(const VectorDynSize & jntPos, - const VectorDynSize & jntVel, - LinkVelArray & linkVels, - const LinkIndex child, - const LinkIndex parent) const; - - // Documentation inherited - virtual void computeChildAcc(const VectorDynSize & jntPos, - const VectorDynSize & jntVel, - const LinkVelArray & linkVels, - const VectorDynSize & jntAcc, - LinkAccArray & linkAccs, - const LinkIndex child, - const LinkIndex parent) const; - - // Documentation inherited - virtual void computeChildBiasAcc(const VectorDynSize & jntPos, - const VectorDynSize & jntVel, - const LinkVelArray & linkVels, - LinkAccArray & linkBiasAccs, - const LinkIndex child, - const LinkIndex parent) const; - - // Documentation inherited - virtual void computeJointTorque(const VectorDynSize & jntPos, const Wrench & internalWrench, - const LinkIndex linkThatAppliesWrench, const LinkIndex linkOnWhichWrenchIsApplied, - VectorDynSize & jntTorques) const; - - // Documentation inherited - virtual void setIndex(JointIndex & _index); - - // Documentation inherited - virtual JointIndex getIndex() const; - - // Documentation inherited - virtual void setPosCoordsOffset(const size_t _index); - - // Documentation inherited - virtual size_t getPosCoordsOffset() const; - - // Documentation inherited - virtual void setDOFsOffset(const size_t _index); - - // Documentation inherited - virtual size_t getDOFsOffset() const; - - // LIMITS METHODS - virtual bool hasPosLimits() const; - virtual bool enablePosLimits(const bool enable); - virtual bool getPosLimits(const size_t _index, double & min, double & max) const; - virtual double getMinPosLimit(const size_t _index) const; - virtual double getMaxPosLimit(const size_t _index) const; - virtual bool setPosLimits(const size_t _index, double & min, double & max); - - // DYNAMICS METHODS - virtual JointDynamicsType getJointDynamicsType() const; - virtual bool setJointDynamicsType(const JointDynamicsType enable); - virtual double getDamping(const size_t _index) const; - virtual double getStaticFriction(const size_t _index) const; - virtual bool setDamping(const size_t _index, double& damping); - virtual bool setStaticFriction(const size_t _index, double& staticFriction); - }; -} - -#endif /* IDYNTREE_FIXED_JOINT_H */ +#endif diff --git a/src/model/include/iDynTree/Model/ForwardKinematics.h b/src/model/include/iDynTree/Model/ForwardKinematics.h index 4cea4feca91..35cc4f6265d 100644 --- a/src/model/include/iDynTree/Model/ForwardKinematics.h +++ b/src/model/include/iDynTree/Model/ForwardKinematics.h @@ -1,154 +1,14 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef IDYNTREE_FORWARD_KINEMATICS_H -#define IDYNTREE_FORWARD_KINEMATICS_H +#ifndef IDYNTREE_MODEL_FWD_KINEMATICS_H +#define IDYNTREE_MODEL_FWD_KINEMATICS_H -#include - -namespace iDynTree -{ - class Model; - class Traversal; - class Transform; - class FreeFloatingPos; - class FreeFloatingVel; - class FreeFloatingAcc; - class LinkPositions; - class LinkVelArray; - class LinkAccArray; - class JointPosDoubleArray; - class SpatialAcc; - class VectorDynSize; - - /** - * \ingroup iDynTreeModel - * - * Given a robot floating base configuration (i.e. world_H_base and the joint positions) - * compute for each link the world_H_link transform. - * - * @param[in] model the used model, - * @param[in] traversal the used traversal, - * @param[in] worldHbase the world_H_base transform, - * @param[in] jointPositions the vector of (internal) joint positions, - * @param[out] linkPositions linkPositions(l) contains the world_H_link transform. - * @return true if all went well, false otherwise. - */ - bool ForwardPositionKinematics(const Model& model, - const Traversal& traversal, - const Transform& worldHbase, - const VectorDynSize& jointPositions, - LinkPositions& linkPositions); - - /** - * Variant of ForwardPositionKinematics that takes in input a FreeFloatingPos - * object instead of a separate couple of (worldHbase,jointPositions) - * - */ - bool ForwardPositionKinematics(const Model & model, - const Traversal & traversal, - const FreeFloatingPos & jointPos, - LinkPositions & linkPos); - - - - /** - * Function that compute the links velocities and accelerations - * given the free floating robot velocities and accelerations. - * - * This function implements what is usually known as the - * "forward pass" of the Recursive Newton Euler algorithm. - */ - bool ForwardVelAccKinematics(const iDynTree::Model & model, - const iDynTree::Traversal & traversal, - const iDynTree::FreeFloatingPos & robotPos, - const iDynTree::FreeFloatingVel & robotVel, - const iDynTree::FreeFloatingAcc & robotAcc, - iDynTree::LinkVelArray & linkVel, - iDynTree::LinkAccArray & linkAcc); - - /** - * Function that compute the links position, velocities and accelerations - * given the free floating robot position, velocities and accelerations. - * - * - */ - bool ForwardPosVelAccKinematics(const iDynTree::Model & model, - const iDynTree::Traversal & traversal, - const iDynTree::FreeFloatingPos & robotPos, - const iDynTree::FreeFloatingVel & robotVel, - const iDynTree::FreeFloatingAcc & robotAcc, - iDynTree::LinkPositions & linkPos, - iDynTree::LinkVelArray & linkVel, - iDynTree::LinkAccArray & linkAcc); - - /** - * Function that compute the links position and velocities and accelerations - * given the free floating robot position and velocities. - * - * - */ - bool ForwardPosVelKinematics(const iDynTree::Model & model, - const iDynTree::Traversal & traversal, - const iDynTree::FreeFloatingPos & robotPos, - const iDynTree::FreeFloatingVel & robotVel, - iDynTree::LinkPositions & linkPos, - iDynTree::LinkVelArray & linkVel); - - /** - * Function that computes the links accelerations - * given the free floating robot velocities and accelerations. - * - * This function implements what is usually known as the acceleration part - * "forward pass" of the Recursive Newton Euler algorithm. - */ - bool ForwardAccKinematics(const Model & model, - const Traversal & traversal, - const FreeFloatingPos & robotPos, - const FreeFloatingVel & robotVel, - const FreeFloatingAcc & robotAcc, - const LinkVelArray & linkVel, - LinkAccArray & linkAcc); - - /** - * Function that computes the links bias accelerations given the free floating robot velocities. - * - * @note This function can also consider a non-zero base acceleration, because when computing - * the bias acc for the MIXED representation, a zero mixed base acceleration is a equivalent - * to a non-zero BODY_FIXED base acceleration. - * - * @param[in] model the used model, - * @param[in] traversal the used traversal, - * @param[in] worldHbase the world_H_base transform, - * @param[in] robotPos the position of the robot, i.e. \f$ ({}^A H_B, s)\f$, - * @param[in] robotVel the velocity of the robot, with the base velocity expressed in BODY_FIXED - * representation i.e. \f$ \nu = \begin{bmatrix} {}^B \mathrm{v}_{A,B} \newline \dot{s} \end{bmatrix} \f$, - * @param[in] baseBiasAcc base bias acceleration with BODY_FIXED rapresentation, useful when the bias acceleration - * is considering the MIXED base acceleration to be zero, - * @param[in] linkVel the velocity of each link of the robot, with velocity expressed with BODY_FIXED - * representation i.e. for each link \f$ \f$ we have \f$ {}^L \mathrm{v}_{A,L} \f$, - * @param[out] linkBiasAcc the bias acceleration of each link of the robot, with the acceleration expressed with BODY_FIXED - * representation. - * - */ - bool ForwardBiasAccKinematics(const Model & model, - const Traversal & traversal, - const FreeFloatingPos & robotPos, - const FreeFloatingVel & robotVel, - const SpatialAcc & baseBiasAcc, - const LinkVelArray & linkVel, - LinkAccArray & linkBiasAcc); +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif - /** - * Legacy function, will be deprecated, use the variant with an explicit baseBiasAcc value. - */ - bool ForwardBiasAccKinematics(const Model & model, - const Traversal & traversal, - const FreeFloatingPos & robotPos, - const FreeFloatingVel & robotVel, - const LinkVelArray & linkVel, - LinkAccArray & linkBiasAcc); +#include -} #endif diff --git a/src/model/include/iDynTree/Model/FreeFloatingMatrices.h b/src/model/include/iDynTree/Model/FreeFloatingMatrices.h index 3f5c3bd3465..061933bc27c 100644 --- a/src/model/include/iDynTree/Model/FreeFloatingMatrices.h +++ b/src/model/include/iDynTree/Model/FreeFloatingMatrices.h @@ -1,98 +1,14 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef IDYNTREE_FREE_FLOATING_MATRICES_H -#define IDYNTREE_FREE_FLOATING_MATRICES_H - -#include - -namespace iDynTree -{ - -class Model; - -/** - * @brief Possible frame velocity representation convention. - * - * Given a link \f$L\f$ and an absolute frame \f$A\f$, the - * the possible frame velocity representation are the following: - * * `INERTIAL_FIXED_REPRESENTATION` : Velocity representation is \f${}^{A} \mathrm{v}_{A,B}\f$, - * * `BODY_FIXED_REPRESENTATION` : Velocity representation is \f${}^{B} \mathrm{v}_{A,B}\f$, - * * `MIXED_REPRESENTATION` : Velocity representation is \f${}^{B[A]} \mathrm{v}_{A,B}\f$. - * - * See iDynTree::KinDynComputations documentation for more details. - */ -enum FrameVelocityRepresentation -{ - INERTIAL_FIXED_REPRESENTATION, - BODY_FIXED_REPRESENTATION, - MIXED_REPRESENTATION -}; - -/** - * Jacobian of the 6D frame velocity. - */ -class FrameFreeFloatingJacobian : public MatrixDynSize -{ -public: - FrameFreeFloatingJacobian(size_t nrOfDofs=0); - FrameFreeFloatingJacobian(const iDynTree::Model & model); - - void resize(const iDynTree::Model & model); - bool isConsistent(const iDynTree::Model & model) const; - - virtual ~FrameFreeFloatingJacobian(); -}; - -/** - * Jacobian of the 6D momentum. - */ -class MomentumFreeFloatingJacobian : public MatrixDynSize -{ -public: - MomentumFreeFloatingJacobian(size_t nrOfDofs=0); - MomentumFreeFloatingJacobian(const iDynTree::Model & model); - - void resize(const iDynTree::Model & model); - bool isConsistent(const iDynTree::Model & model) const; - - virtual ~MomentumFreeFloatingJacobian(); -}; - - -/** - * Class representing the mass matrix of a Free Floating robot. - * - * - */ -class FreeFloatingMassMatrix : public MatrixDynSize -{ -public: - FreeFloatingMassMatrix(size_t nrOfDofs=0); - - /** - * Constructor from a model, to get the appropriate size of the - * mass matrix. - */ - FreeFloatingMassMatrix(const iDynTree::Model& model); - - /** - * Resize the class to match the dimension of the dofs contained in a Model. - * - * @param model the model from which to get the number and dimension of the joints. - * - * @warning This method wipes the content of the mass matrix. - * - */ - void resize(const iDynTree::Model& model); +#ifndef IDYNTREE_MODEL_FREE_FLOATING_MATRICES_H +#define IDYNTREE_MODEL_FREE_FLOATING_MATRICES_H +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif - /** - * Destructor - */ - virtual ~FreeFloatingMassMatrix(); -}; +#include -} #endif diff --git a/src/model/include/iDynTree/Model/FreeFloatingState.h b/src/model/include/iDynTree/Model/FreeFloatingState.h index 6480695d205..dcfd80fccd0 100644 --- a/src/model/include/iDynTree/Model/FreeFloatingState.h +++ b/src/model/include/iDynTree/Model/FreeFloatingState.h @@ -1,246 +1,14 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef IDYNTREE_FREE_FLOATING_STATE_H -#define IDYNTREE_FREE_FLOATING_STATE_H +#ifndef IDYNTREE_MODEL_FREE_FLOATING_STATE_H +#define IDYNTREE_MODEL_FREE_FLOATING_STATE_H -#include -#include -#include - -#include -#include -#include - -#include - -namespace iDynTree -{ - class Model; - - /** - * Class representing the position of a Free Floating robot. - * - * The position of a free floating robot is represented by the - * transform of a base frame with respect to an inertial frame, - * and a vector of joint positions. - * - */ - class FreeFloatingPos - { - private: - Transform m_worldBasePos; - JointPosDoubleArray m_jointPos; - - public: - // Documentation inherited - FreeFloatingPos(); - FreeFloatingPos(const iDynTree::Model& model); - - /** - * Resize the class to match the dimension of the joints contained in a Model. - * - * @param model the model from which to get the number and dimension of the joints. - * - * @warning This method wipes the joint positions if number and dimension of the joints - * of the model passed are different from the one already stored. - * - */ - void resize(const iDynTree::Model& model); - - /** - * Get the base position. - */ - Transform & worldBasePos(); - - /** - * Get the vector of joint positions. - */ - JointPosDoubleArray & jointPos(); - - /** - * Get the base position (const version). - */ - const Transform & worldBasePos() const; - - /** - * Get the vector of joint positions (const version). - */ - const JointPosDoubleArray & jointPos() const; - - /** - * Get the dimension of the joint positions vector. - */ - unsigned int getNrOfPosCoords() const; - - /** - * Destructor - */ - virtual ~FreeFloatingPos(); - }; - - - class FreeFloatingGeneralizedTorques - { - private: - Wrench m_baseWrench; - JointDOFsDoubleArray m_jointTorques; - - public: - // Documentation inherited - FreeFloatingGeneralizedTorques(); - FreeFloatingGeneralizedTorques(const iDynTree::Model& model); - - /** - * Resize the class to match the dimension of the joints contained in a Model. - * - * @param model the model from which to get the number and dimension of the joints. - * - * @warning This method wipes the joint torques if number and dimension of the joints - * of the model passed are different from the one already stored. - * - */ - void resize(const iDynTree::Model& model); - - /** - * Get the base wrench. - */ - Wrench & baseWrench(); - - /** - * Get the joint torques vector. - */ - JointDOFsDoubleArray & jointTorques(); - - /** - * Get the base wrench (const version). - */ - const Wrench & baseWrench() const; - - /** - * Get the joint torques vector (const version). - */ - const JointDOFsDoubleArray & jointTorques() const; - - unsigned int getNrOfDOFs() const; - - /** - * Destructor - */ - virtual ~FreeFloatingGeneralizedTorques(); - }; - - - class FreeFloatingVel - { - private: - Twist m_baseVel; - JointDOFsDoubleArray m_jointVel; - - public: - // Documentation inherited - FreeFloatingVel(); - FreeFloatingVel(const iDynTree::Model& model); - - /** - * Resize the class to match the number of internal DOFs contained in a Model. - * - * @param model the model from which to get the number and dimension of the joints. - * - * @warning This method wipes the joint velocities if number and dimension of the joints - * of the model passed are different from the one already stored. - * - */ - void resize(const iDynTree::Model& model); - - /** - * Get the base acceleration. - */ - Twist & baseVel(); - - /** - * Get the vector of joint accelerations. - */ - JointDOFsDoubleArray & jointVel(); - - /** - * Get the base acceleration (const version). - */ - const Twist & baseVel() const; - - /** - * Get the vector of joint accelerations (const version). - */ - const JointDOFsDoubleArray & jointVel() const; - - /** - * Get the dimension of the joint accelerations vector. - */ - unsigned int getNrOfDOFs() const; - - /** - * Destructor - */ - virtual ~FreeFloatingVel(); - }; - - /** - * Class representing the accelerations of a Free Floating robot. - * - */ - class FreeFloatingAcc - { - private: - SpatialAcc m_baseAcc; - JointDOFsDoubleArray m_jointAcc; - - public: - // Documentation inherited - FreeFloatingAcc(); - FreeFloatingAcc(const iDynTree::Model& model); - - /** - * Resize the class to match the number of internal DOFs contained in a Model. - * - * @param model the model from which to get the number and dimension of the joints. - * - * @warning This method wipes the joint positions if number and dimension of the joints - * of the model passed are different from the one already stored. - * - */ - void resize(const iDynTree::Model& model); - - /** - * Get the base acceleration. - */ - SpatialAcc & baseAcc(); - - /** - * Get the vector of joint accelerations. - */ - JointDOFsDoubleArray & jointAcc(); - - /** - * Get the base acceleration (const version). - */ - const SpatialAcc & baseAcc() const; - - /** - * Get the vector of joint accelerations (const version). - */ - const JointDOFsDoubleArray & jointAcc() const; - - /** - * Get the dimension of the joint accelerations vector. - */ - unsigned int getNrOfDOFs() const; +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif - /** - * Destructor - */ - virtual ~FreeFloatingAcc(); - }; +#include -} #endif diff --git a/src/model/include/iDynTree/Model/IJoint.h b/src/model/include/iDynTree/Model/IJoint.h index 6960e667cdc..1c405b68ada 100644 --- a/src/model/include/iDynTree/Model/IJoint.h +++ b/src/model/include/iDynTree/Model/IJoint.h @@ -1,463 +1,13 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef IDYNTREE_I_JOINT_H -#define IDYNTREE_I_JOINT_H +#ifndef IDYNTREE_MODEL_IJOINT_H +#define IDYNTREE_MODEL_IJOINT_H -#include +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif -namespace iDynTree -{ - class LinkPositions; - class LinkVelArray; - class LinkAccArray; - class Transform; - class TransformDerivative; - class Wrench; - class Twist; - class VectorDynSize; - class SpatialMotionVector; +#include - enum JointDynamicsType - { - /** - * NoDynamics: No joint dynamics is assumed for the joint. - * This joint dynamics type does not consider any parameter. - */ - NoJointDynamics = 0, - - /** - * URDFJointDynamics: Dynamics described by the URDF 1.0 specification. - * - * This joint dynamics type consider the following parameters: - * * `Damping` - * * `StaticFriction` - */ - URDFJointDynamics = 1 - }; - - /** - * Interface (i.e. abstract class) exposed by classes that implement a Joint. - * A Joint is the basic representation of the motion allowed between two links. - * - * This interface is mean to be used by kinematics and dynamics algorithm to - * query informations related to a joint and the relations (relative position, - * relative twist, relative acceleration) that it imposes to the connected links. - * - * The design of this class is heavily inspired by the Simbody implementation of joints, - * as described in this article: - * - * Seth, Ajay, et al. "Minimal formulation of joint motion for biomechanisms." - * Nonlinear dynamics 62.1-2 (2010): 291-303. - * - * Other sources of inspiration are RBDL, DART and Featherstone book. - * - * With respect to all this implementation we model the joints as undirected quantities, - * i.e. as object in which information can be queryied in symmetric way with respect to the - * attached links. This mean there is no parent and child link, but the joint is attached - * to two link, and the interface is agnostic with respect to which link the code considers - * as "parent" or "child". - * - * \ingroup iDynTreeModel - */ - class IJoint - { - public: - /** - * Denstructor - * - */ - virtual ~IJoint() = 0; - - /** - * Clone the joint object. - */ - virtual IJoint * clone() const = 0; - - /** - * Get the number of coordinates used to represent - * the position of the joint. - * - * For joints whose configuration is in R^n, - * the number of position coordinates should - * match the number of degrees of freedom of the joint. - * - * @return the number of position coordinates. - */ - virtual unsigned int getNrOfPosCoords() const = 0; - - /** - * Get the number of degrees of freedom of the joint. - * - * This should be a number between 0 (fixed joint) and 6 (free joint). - * - * @return the number of degrees of freedom of the joint. - */ - virtual unsigned int getNrOfDOFs() const = 0; - - - /** - * @name Methods to set joint - * Methods to set joint informations (used when building a model) - */ - //@{ - - /** - * Set the two links at which the joint is attached. - * @param link1 is the first link - * @param link2 is the second link - */ - virtual void setAttachedLinks(const LinkIndex link1, const LinkIndex link2) = 0; - - /** - * Set the transform between the link2 frame and link1 frame at joint position 0 - * (or at the identity configuration element for complex joints). - * - * The link1_T_link2 is transform that transforms a quantity - * expressed in link2 frame in a quantity expressed in the link1 - * frame, when the joint is in the 0 position : - * p_link1 = link1_T_link2*p_link2 . - */ - virtual void setRestTransform(const Transform& link1_X_link2) = 0; - - //@} - - /** - * Get the first link attached to the joint. - */ - virtual LinkIndex getFirstAttachedLink() const = 0; - - /** - * Get the second link attached to the joint. - */ - virtual LinkIndex getSecondAttachedLink() const = 0; - - - /** - * Get the transform between the link parent and the link child at joint position 0 - * (or at the identity configuration element for complex joints). - * Such that: - * p_child = child_H_parent*p_parent - * where p_child is a quantity expressed in the child frame, - * and p_parent is a quantity expressed in the child frame. - */ - virtual Transform getRestTransform(const LinkIndex child, - const LinkIndex parent) const = 0; - - /** - * Get the transform between the parent and the child, such that: - * p_child = child_H_parent*p_parent, - * where p_child is a quantity expressed in the child frame, - * and p_parent is a quantity expressed in the parent frame. - */ - virtual const Transform & getTransform(const VectorDynSize & jntPos, - const LinkIndex child, - const LinkIndex parent) const = 0; - - /** - * Get the derivative of the transform with - * respect to a position coordinate. - * - * In particular, if the selected position coordinate is \f$q\f$, return the derivative: - * \f[ - * \frac{\partial {}^\texttt{child} H_\texttt{parent} }{\partial q} - * \f] - * - * If posCoord_i is not >= 0 and < getNrOfPosCoords(), the returned value is undefined. - * - */ - virtual TransformDerivative getTransformDerivative(const VectorDynSize & jntPos, - const LinkIndex child, - const LinkIndex parent, - const int posCoord_i) const = 0; - - /** - * Get the motion subspace vector corresponding to the i-th - * dof of the joint, i.e. the i-th column of the motion subspace matrix. - * The motion subspace matrix is the matrix that - * maps the joint velocity to the relative twist between the two - * links. - * - * In particular the motion subspace vector of the i-th dof is the S - * vector such that - * v_child = S_{child,parent}*dq_i + child_X_parent*v_parent - * if the velocities associated to all other DOFs of the joint - * are considered zero, where v_child and v_parent are the left-trivialized - * (body) velocities of the link child and parent. - * - * See - * "Modelling, Estimation and Identification of Humanoid Robots Dynamics" - * Silvio Traversaro - Section 3.2 - * https://traversaro.github.io/preprints/traversaro-phd-thesis.pdf - * for more details. - * - * @return the motion subspace vector. - * - * If dof_i is not >= 0 and < getNrOfDOFs(), the returned value is undefined. - * - * \note The motion subspace matrix is also known in literature as hinge matrix, - * hinge map matrix, joint map matrix or joint motion map matrix. - */ - virtual SpatialMotionVector getMotionSubspaceVector(int dof_i, - const LinkIndex child, - const LinkIndex parent) const = 0; - - /** - * Compute the position, velocity and acceleration of link child, - * given the position, velocty and acceleration of link parent and - * the joint position, velocity and acceleration. - * - * The position, velocity and acceleration of link child - * are directly saved in the linkPositions, linkVels and linkAccs arguments. - * - */ - virtual void computeChildPosVelAcc(const VectorDynSize & jntPos, - const VectorDynSize & jntVel, - const VectorDynSize & jntAcc, - LinkPositions & linkPositions, - LinkVelArray & linkVels, - LinkAccArray & linkAccs, - const LinkIndex child, const LinkIndex parent) const = 0; - - /** - * Compute the velocity and acceleration of child, - * given the velocity and acceleration of parent and - * the joint position, velocity and acceleration. - * - */ - virtual void computeChildVelAcc(const VectorDynSize & jntPos, - const VectorDynSize & jntVel, - const VectorDynSize & jntAcc, - LinkVelArray & linkVels, - LinkAccArray & linkAccs, - const LinkIndex child, const LinkIndex parent) const = 0; - - /** - * Compute the velocity of child, - * given the velocity of parent and - * the joint position, velocity. - */ - virtual void computeChildVel(const VectorDynSize & jntPos, - const VectorDynSize & jntVel, - LinkVelArray & linkVels, - const LinkIndex child, - const LinkIndex parent) const = 0; - - /** - * Compute the (body-fixed) acceleration of a child link - * given the (body-fixed) acceleration of the parent - */ - virtual void computeChildAcc(const VectorDynSize & jntPos, - const VectorDynSize & jntVel, - const LinkVelArray & linkVels, - const VectorDynSize & jntAcc, - LinkAccArray & linkAccs, - const LinkIndex child, - const LinkIndex parent) const = 0; - - /** - * Compute the (body-fixed) bias acceleration of a child link - * given the (body-fixed) bias acceleration of the parent - */ - virtual void computeChildBiasAcc(const VectorDynSize & jntPos, - const VectorDynSize & jntVel, - const LinkVelArray & linkVels, - LinkAccArray & linkBiasAccs, - const LinkIndex child, - const LinkIndex parent) const = 0; - - /** - * Compute the internal torque of joint, given the internal wrench that the linkThatAppliesWrench applies - * on the linkOnWhichWrenchIsApplied, expressed in the link frame of the linkOnWhichWrenchIsApplied. - * - * @param[in] jntPos vector of joint positions. - * @param[in] internalWrench internal wrench that the linkThatAppliesWrench applies - * on the linkOnWhichWrenchIsApplied, expressed in the link - * frame of the linkOnWhichWrenchIsApplied - * @param[in] linkThatAppliesWrench link index of the link that applies the considered internal wrench. - * @param[in] linkOnWhichWrenchIsApplied link index of the link on which the considered internal wrench is applied. - * @param[out] jntTorques vector of joint torques. - */ - virtual void computeJointTorque(const VectorDynSize & jntPos, - const Wrench & internalWrench, - const LinkIndex linkThatAppliesWrench, - const LinkIndex linkOnWhichWrenchIsApplied, - VectorDynSize & jntTorques) const = 0; - - /** - * Set the index of the joint in the Model Joint serialization. - * - */ - virtual void setIndex(JointIndex & _index) = 0; - - /** - * Get the index of the joint in the model Joint serialization. - */ - virtual JointIndex getIndex() const = 0; - - - /** - * Set the offset of the position coordinates of this - * joint in the position coordiantes serialization of the model. - */ - virtual void setPosCoordsOffset(const size_t _index) = 0; - - /** - * Get the offset of the position coordinates of - * this joint in the position coordiantes serialization of the model. - */ - virtual size_t getPosCoordsOffset() const = 0; - - /** - * Set the offset of the coordinates of this - * joint in the velocity/acceleration coordiantes serialization of the model. - */ - virtual void setDOFsOffset(const size_t _index) = 0; - - /** - * Get the offset of the position coordinates of - * joint in the velocity/acceleration coordiantes serialization of the model. - */ - virtual size_t getDOFsOffset() const = 0; - - /** - * @name Limit handling methods. - * Methods for handling physical limits of joints. - * - * The model used for limits is rather simple: a joint can have limits (being bounded) - * or not. - * - * In the current version the limits are supported only for simple - * joints in which the velocity is the derivative of the position coordinate, - * and then getNrOfPosCoords() is equal to getNrOfDOFs() . - * The limits for such joints are specified by two constant vectors of dimension getNrOfDOFs(), - * the vector of minimum positions and the vector of maximum positions. - */ - ///@{ - /** - * Method to check if the joint has limits. - * - * @return true if the joints has limits - */ - virtual bool hasPosLimits() const = 0; - - /** - * Method to set if the joint has limits. - * - * @return true if everything went correctly, false otherwise - * (for example if the joint does not support joint position limits) - */ - virtual bool enablePosLimits(const bool enable) = 0; - - /** - * Get min and max position limits of the joint, for the _index dof. - * @param[in] _index index of the dof for which the limit are obtained. - * @return true if everything is correct, false otherwise. - */ - virtual bool getPosLimits(const size_t _index, double & min, double & max) const = 0; - - /** - * Get the min position limit of the joint, bindings-friendly version. - */ - virtual double getMinPosLimit(const size_t _index) const = 0; - - /** - * Get the max position limit of the joint, bindings-friendly version. - */ - virtual double getMaxPosLimit(const size_t _index) const = 0; - - /** - * Set the position limits for a dof the joint. - * - * @note This just sets the internal position limits of the joint. - * To set them as enabled, you need to call the enablePosLimits(true) method. - */ - virtual bool setPosLimits(const size_t _index, double & min, double & max) = 0; - - /** - * @name Joint dynamics methods. - * - * Methods for handling representation of joint dynamics. - * The precise definition of "joint dynamics" is not precisely, as depending on the - * specific application the kind of joint dynamics model can be different, and in some - * case it may be even just instantaneous models (for example, when only the damping is considered). - * - * For the type of joint dynamics supported, see the iDynTree::JointDynamicsType enum documentation. - * - * The joint dynamics model are used in the following contexts: - * * In methods to serialize and deserialize URDF files - * - * The joint dynamics are **not used at all** in classes to compute kinematics and dynamics quantities, - * such as iDynTree::KinDynComputations . - */ - ///@{ - - /** - * Method to get the specific joint dynamics type used for the joint. - * \note: It is assume that all the degrees of freedom of a joint share the same joint dynamics type. - * - * @return the specific joint dynamics type used for the joint. - */ - virtual JointDynamicsType getJointDynamicsType() const = 0; - - /** - * Method to get the specific joint dynamics type used for the joint. - * \note: It is assume that all the degrees of freedom of a joint share the same joint dynamics type. - * - * @return true if everything went correctly, false otherwise - */ - virtual bool setJointDynamicsType(const JointDynamicsType enable) = 0; - - /** - * Set damping parameter of the joint, for the _index dof. - * The damping coefficient is expressed in N∙s/m for a prismatic joint, N∙m∙s/rad for a revolute joint. - * - * This parameter is considered in the following joint dynamics types: - * * `URDFJointDynamics` - * - * @param[in] _index index of the dof for which the dynamic parameters are obtained. - * @return true if everything is correct, false otherwise. - */ - virtual bool setDamping(const size_t _index, double& damping) = 0; - - /** - * Set static friction parameter of the joint, for the _index dof. - * The static friction coefficient is expressed in N for a prismatic joint, N∙m for a revolute joint. - * - * This parameter is considered in the following joint dynamics types: - * * `URDFJointDynamics` - * - * @param[in] _index index of the dof for which the dynamic parameters are obtained. - * @return true if everything is correct, false otherwise. - */ - virtual bool setStaticFriction(const size_t _index, double& staticFriction) = 0; - - /** - * Get the damping coefficient of the joint. - * The unit is N∙s/m for a prismatic joint, N∙m∙s/rad for a revolute joint. - * - * This parameter is considered in the following joint dynamics types: - * * `URDFJointDynamics` - */ - virtual double getDamping(const size_t _index) const = 0; - - /** - * Get the static friction coefficient of the joint. - * The unit is N for a prismatic joint, N∙m for a revolute joint. - * - * This parameter is considered in the following joint dynamics types: - * * `URDFJointDynamics` - */ - virtual double getStaticFriction(const size_t _index) const = 0; - - - ///@} - }; - - typedef IJoint * IJointPtr; - typedef const IJoint * IJointConstPtr; - - -} - -#endif /* IDYNTREE_I_JOINT_H */ +#endif diff --git a/src/model/include/iDynTree/Model/Indices.h b/src/model/include/iDynTree/Model/Indices.h index d4bfb9d9c4f..78c1d2d86f6 100644 --- a/src/model/include/iDynTree/Model/Indices.h +++ b/src/model/include/iDynTree/Model/Indices.h @@ -1,40 +1,13 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef IDYNTREE_INDICES_H -#define IDYNTREE_INDICES_H +#ifndef IDYNTREE_MODEL_INDICES_H +#define IDYNTREE_MODEL_INDICES_H -#include -#include - -// Workaround for SWIG problems with GenerateExportHeader-generated code -#if defined(SWIG) -#define IDYNTREE_MODEL_EXPORT -#else -#include "ModelExport.h" +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. #endif -namespace iDynTree -{ - typedef std::ptrdiff_t LinkIndex; - IDYNTREE_MODEL_EXPORT extern LinkIndex LINK_INVALID_INDEX; - IDYNTREE_MODEL_EXPORT extern std::string LINK_INVALID_NAME; - - typedef std::ptrdiff_t JointIndex; - IDYNTREE_MODEL_EXPORT extern std::ptrdiff_t JOINT_INVALID_INDEX; - IDYNTREE_MODEL_EXPORT extern std::string JOINT_INVALID_NAME; - - typedef std::ptrdiff_t DOFIndex; - IDYNTREE_MODEL_EXPORT extern std::ptrdiff_t DOF_INVALID_INDEX; - IDYNTREE_MODEL_EXPORT extern std::string DOF_INVALID_NAME; - - typedef std::ptrdiff_t FrameIndex; - IDYNTREE_MODEL_EXPORT extern std::ptrdiff_t FRAME_INVALID_INDEX; - IDYNTREE_MODEL_EXPORT extern std::string FRAME_INVALID_NAME; +#include - typedef std::ptrdiff_t TraversalIndex; - IDYNTREE_MODEL_EXPORT extern TraversalIndex TRAVERSAL_INVALID_INDEX; - -} - -#endif /* IDYNTREE_INDICES_H */ +#endif diff --git a/src/model/include/iDynTree/Model/Jacobians.h b/src/model/include/iDynTree/Model/Jacobians.h index 39a4754f076..a335abbb383 100644 --- a/src/model/include/iDynTree/Model/Jacobians.h +++ b/src/model/include/iDynTree/Model/Jacobians.h @@ -1,53 +1,14 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef IDYNTREE_JACOBIANS_H -#define IDYNTREE_JACOBIANS_H +#ifndef IDYNTREE_MODEL_JACOBIANS_H +#define IDYNTREE_MODEL_JACOBIANS_H -#include - -namespace iDynTree -{ - class Model; - class Traversal; - class Transform; - class FreeFloatingPos; - class FreeFloatingVel; - class FreeFloatingAcc; - class LinkPositions; - class LinkVelArray; - class LinkAccArray; - class JointPosDoubleArray; - class MatrixDynSize; - - template - class MatrixView; - - /** - * \ingroup iDynTreeModel - * - * Compute a free floating jacobian - * - * @param[in] model the used model, - * @param[in] traversal the used traversal, - * @param[in] jointPositions the vector of (internal) joint positions, - * @param[in] linkPositions linkPositions(l) contains the world_H_link transform. - * @param[in] linkIndex the index of the link of which we compute the jacobian. - * @param[in] jacobFrame_X_world TODO - * @param[in] baseFrame_X_jacobBaseFrame TODO - * @param[out] jacobian the computed Jacobian - * @return true if all went well, false otherwise. - */ - bool FreeFloatingJacobianUsingLinkPos(const Model& model, - const Traversal& traversal, - const JointPosDoubleArray& jointPositions, - const LinkPositions& linkPositions, - const LinkIndex linkIndex, - const Transform & jacobFrame_X_world, - const Transform & baseFrame_X_jacobBaseFrame, - const MatrixView& jacobian); +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif +#include -} #endif diff --git a/src/model/include/iDynTree/Model/JointState.h b/src/model/include/iDynTree/Model/JointState.h index 889b893559c..1ed0c5b5b4a 100644 --- a/src/model/include/iDynTree/Model/JointState.h +++ b/src/model/include/iDynTree/Model/JointState.h @@ -1,108 +1,14 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef IDYNTREE_JOINT_STATE_H -#define IDYNTREE_JOINT_STATE_H +#ifndef IDYNTREE_MODEL_JOINT_STATE_H +#define IDYNTREE_MODEL_JOINT_STATE_H -#include -#include -#include - -#include - -namespace iDynTree -{ - class Model; - class SpatialMotionVector; - - /** - * Class for storing a vector of scalar values, - * one for each internal position coordinate in a model. - */ - class JointPosDoubleArray: public VectorDynSize - { - public: - JointPosDoubleArray(std::size_t nrOfDOFs = 0); - JointPosDoubleArray(const iDynTree::Model & model); - - void resize(std::size_t nrOfDOFs); - void resize(const iDynTree::Model & model); - - bool isConsistent(const iDynTree::Model & model) const; - - ~JointPosDoubleArray(); - }; - - /** - * Class for storing a vector of scalar values, - * one for each internal coordinate in a model. - */ - class JointDOFsDoubleArray: public VectorDynSize - { - public: - JointDOFsDoubleArray(std::size_t nrOfDOFs = 0); - JointDOFsDoubleArray(const iDynTree::Model & model); - - void resize(std::size_t nrOfDOFs); - void resize(const iDynTree::Model & model); - - bool isConsistent(const iDynTree::Model & model) const; - - ~JointDOFsDoubleArray(); - }; - - /** - * Class for storing a vector of spatial force vectors, - * one for each dof in a model. - */ - - /** - * Class for storing a vector of spatial force vectors, - * one for each (internal) dof in a model. - */ - class DOFSpatialForceArray - { - private: - std::vector m_dofSpatialForce; - - public: - DOFSpatialForceArray(std::size_t nrOfDOFs = 0); - DOFSpatialForceArray(const iDynTree::Model & model); - - void resize(const std::size_t nrOfDOFs); - void resize(const iDynTree::Model & model); - - bool isConsistent(const iDynTree::Model & model) const; - - iDynTree::SpatialForceVector & operator()(const size_t dof); - const iDynTree::SpatialForceVector & operator()(const size_t dof) const; - - ~DOFSpatialForceArray(); - }; - - /** - * Class for storing a vector of spatial motion vectors, - * one for each (internal) dof in a model. - */ - class DOFSpatialMotionArray - { - private: - std::vector m_dofSpatialMotion; - - public: - DOFSpatialMotionArray(std::size_t nrOfDOFs = 0); - DOFSpatialMotionArray(const iDynTree::Model & model); - - void resize(const std::size_t nrOfDOFs); - void resize(const iDynTree::Model & model); - - bool isConsistent(const iDynTree::Model & model) const; +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif - iDynTree::SpatialMotionVector & operator()(const size_t dof); - const iDynTree::SpatialMotionVector & operator()(const size_t dof) const; +#include - ~DOFSpatialMotionArray(); - }; -} #endif diff --git a/src/model/include/iDynTree/Model/Link.h b/src/model/include/iDynTree/Model/Link.h index ac93149a17f..ab5e94b1b9e 100644 --- a/src/model/include/iDynTree/Model/Link.h +++ b/src/model/include/iDynTree/Model/Link.h @@ -1,79 +1,14 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef IDYNTREE_LINK_H -#define IDYNTREE_LINK_H +#ifndef IDYNTREE_MODEL_LINK_H +#define IDYNTREE_MODEL_LINK_H -#include +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif -#include +#include -namespace iDynTree -{ - - /** - * Class that represents a link. - * - * - * \ingroup iDynTreeModel - */ - class Link - { - private: - LinkIndex m_index; - SpatialInertia m_inertia; - - public: - /** - * Costructor - */ - Link(); - - /** - * Low-level accessor to the link inertia. - */ - SpatialInertia & inertia(); - - - /** - * Low-level const accessor to the link inertia. - */ - const SpatialInertia & inertia() const; - - /** - * Set the spatial inertia of the link, - * expressed in the link reference frame - * (i.e. with respect to the link reference - * frame origin and with the orientation - * of the link reference frame). - */ - void setInertia(SpatialInertia & _inertia); - - /** - * Get the spatial inertia of the link, - * expressed in the link reference frame - * (i.e. with respect to the link reference - * frame origin and with the orientation - * of the link reference frame). - * - * @return a reference to the inertia of the link. - */ - const SpatialInertia & getInertia() const; - - /** - * Set the index of the link. - */ - void setIndex(LinkIndex & _index); - - /** - * Get the index of the link. - */ - LinkIndex getIndex() const; - }; - - typedef Link * LinkPtr; - typedef const Link * LinkConstPtr; -} - -#endif /* IDYNTREE_LINK_H */ +#endif diff --git a/src/model/include/iDynTree/Model/LinkState.h b/src/model/include/iDynTree/Model/LinkState.h index 5a358dd223b..847cd7d8729 100644 --- a/src/model/include/iDynTree/Model/LinkState.h +++ b/src/model/include/iDynTree/Model/LinkState.h @@ -1,234 +1,14 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef IDYNTREE_LINK_STATE_H -#define IDYNTREE_LINK_STATE_H +#ifndef IDYNTREE_MODEL_LINK_STATE_H +#define IDYNTREE_MODEL_LINK_STATE_H -#include -#include -#include -#include -#include +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif -#include +#include -#include -namespace iDynTree -{ - class Model; - - class LinkPositions - { - private: - std::vector m_linkPos; - - public: - LinkPositions(std::size_t nrOfLinks = 0); - LinkPositions(const iDynTree::Model & model); - - void resize(std::size_t nrOfLinks); - void resize(const iDynTree::Model & model); - - bool isConsistent(const Model& model) const; - - size_t getNrOfLinks() const; - - iDynTree::Transform & operator()(const LinkIndex link); - const iDynTree::Transform & operator()(const LinkIndex link) const; - - std::string toString(const Model & model) const; - - ~LinkPositions(); - }; - - /** - * Vector of wrenches connected in some way to the link of a model. - * - * It is used to model both the total external wrench - * acting on a link (LinkExternalWrenches), or the internal wrenches - * that a link excerts on his parent (given a Traversal) - * computed as a by product by the dynamic loop of the RNEA ( RNEADynamicPhase ). - * - * In both cases the Wrench corresponding to the link with LinkIndex i - * is always expressed with the orientation of the link frame and with - * respect to the link frame origin. - */ - class LinkWrenches - { - private: - std::vector m_linkWrenches; - - public: - /** - * Create a LinkWrenches vector, with the size given - * by nrOfLinks . - * - * @param[in] nrOfLinks the size of the vector. - */ - LinkWrenches(std::size_t nrOfLinks = 0); - LinkWrenches(const iDynTree::Model & model); - - /** - * Resize the vector to have size nrOfLinks. - * - * @param[in] nrOfLinks new size for the vector - */ - void resize(std::size_t nrOfLinks); - void resize(const iDynTree::Model & model); - - bool isConsistent(const Model& model) const; - size_t getNrOfLinks() const; - - iDynTree::Wrench & operator()(const LinkIndex link); - const iDynTree::Wrench & operator()(const LinkIndex link) const; - - std::string toString(const Model & model) const; - - /** - * Set all the elements to zero. - */ - void zero(); - - ~LinkWrenches(); - }; - - /** - * Vector of the sum of all the external wrenches excerted on each link. - * - * The wrench returned by operator(i) is the sum of all external wrenches - * (thus excluding the wrench applied on the link by other links in the model) - * that the environment applies on the link $i$, expressed ( - * both orientation and point) with respect to the reference frame of link i. - */ - typedef LinkWrenches LinkNetExternalWrenches; - - /** - * Vector of the wrenches acting that a link excert on his parent, - * given a Traversal. - * - * Given a Traversal with base link b, the wrench returned by operator(i) is the wrench - * the parent of link i excerts on link i, expressed (both orientation - * and point) with respect to the reference frame of link i. - */ - typedef LinkWrenches LinkInternalWrenches; - - - /** - * Vector of the sum of all the wrenches (both internal and external, excluding gravity) acting on - * link i, expressed (both orientation and point) with respect to the reference frame of link i. - * - * This is tipically computed as I*a+v*(I*v) , where a is the proper acceleration. - */ - typedef LinkWrenches LinkNetTotalWrenchesWithoutGravity; - - /** - * Class for storing a vector of SpatialInertia objects , one for each link in a model. - */ - class LinkInertias - { - private: - std::vector m_linkInertials; - - public: - LinkInertias(std::size_t nrOfLinks = 0); - LinkInertias(const iDynTree::Model & model); - - void resize(std::size_t nrOfLinks); - void resize(const iDynTree::Model & model); - - bool isConsistent(const Model& model) const; - - iDynTree::SpatialInertia & operator()(const LinkIndex link); - const iDynTree::SpatialInertia & operator()(const LinkIndex link) const; - - ~LinkInertias(); - }; - - typedef LinkInertias LinkCompositeRigidBodyInertias; - - /** - * Class for storing a vector of ArticulatedBodyInertias objects , one for each link in a model. - */ - class LinkArticulatedBodyInertias - { - private: - std::vector m_linkABIs; - - public: - LinkArticulatedBodyInertias(std::size_t nrOfLinks = 0); - LinkArticulatedBodyInertias(const iDynTree::Model & model); - - void resize(std::size_t nrOfLinks); - void resize(const iDynTree::Model & model); - - bool isConsistent(const Model& model) const; - - iDynTree::ArticulatedBodyInertia & operator()(const LinkIndex link); - const iDynTree::ArticulatedBodyInertia & operator()(const LinkIndex link) const; - - ~LinkArticulatedBodyInertias(); - }; - - /** - * Class for storing a vector of twists, one for each link in a model. - */ - class LinkVelArray - { - private: - std::vector m_linkTwist; - - public: - LinkVelArray(std::size_t nrOfLinks = 0); - LinkVelArray(const iDynTree::Model & model); - - void resize(std::size_t nrOfLinks); - void resize(const iDynTree::Model & model); - - bool isConsistent(const Model& model) const; - - size_t getNrOfLinks() const; - - iDynTree::Twist & operator()(const LinkIndex link); - const iDynTree::Twist & operator()(const LinkIndex link) const; - - std::string toString(const Model & model) const; - - ~LinkVelArray(); - }; - - /** - * Class for storing a vector of spatial accelerations, - * one for each link in a model. - */ - class LinkAccArray - { - private: - std::vector m_linkAcc; - - public: - LinkAccArray(std::size_t nrOfLinks = 0); - LinkAccArray(const iDynTree::Model & model); - - void resize(std::size_t nrOfLinks); - void resize(const iDynTree::Model & model); - - bool isConsistent(const Model& model) const; - - iDynTree::SpatialAcc & operator()(const LinkIndex link); - const iDynTree::SpatialAcc & operator()(const LinkIndex link) const; - - std::size_t getNrOfLinks() const; - - std::string toString(const Model & model) const; - - ~LinkAccArray(); - }; - - /** - * Typedef used when the vector is meant to be a vector of link proper accelerations. - */ - typedef LinkAccArray LinkProperAccArray; -} - -#endif /* IDYNTREE_LINK_STATE_H */ +#endif diff --git a/src/model/include/iDynTree/Model/LinkTraversalsCache.h b/src/model/include/iDynTree/Model/LinkTraversalsCache.h index 9297a663ea3..66bc31e4b19 100644 --- a/src/model/include/iDynTree/Model/LinkTraversalsCache.h +++ b/src/model/include/iDynTree/Model/LinkTraversalsCache.h @@ -1,74 +1,14 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef IDYNTREE_MODEL_LINKTRAVERSALSCACHE_H -#define IDYNTREE_MODEL_LINKTRAVERSALSCACHE_H +#ifndef IDYNTREE_MODEL_LINK_TRAVERSALS_CACHE_H +#define IDYNTREE_MODEL_LINK_TRAVERSALS_CACHE_H -#include -#include +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif -namespace iDynTree { - class Traversal; - class Model; - - class LinkTraversalsCache; -} +#include -/** - * Link traversal cache, store a traversal for each link in the model. - * - * Class that stores a traversal for each link in the model. - * It actually computes the traversal on the first time that a given traversal - * is requested - */ -class iDynTree::LinkTraversalsCache -{ -private: - std::vector m_linkTraversals; - void deleteTraversals(); -public: - /** - * Default constructur - */ - LinkTraversalsCache(); - - /** - * Default destructor - */ - ~LinkTraversalsCache(); - - - /** - * Resize the cache to contains the specified number of Traversals - * - * @param nrOfLinks number of traversal - */ - void resize(unsigned int nrOfLinks); - - - /** - * Resize the cache to contains the number of Traversals for the specified model - * - * @param model the model - */ - void resize(const iDynTree::Model& model); - - - /** - * return the traversal for the specified link in the specified model - * - * If the traversal has been cached this function returns the cached traversal - * otherwise it creates the new traversal, caches it and returns it, thus - * performin a memory allocation. - * - * @param model the model - * @param linkIdx the index to be considered as base - * @return the traversal for the specified model link - */ - Traversal& getTraversalWithLinkAsBase(const iDynTree::Model & model, - const iDynTree::LinkIndex linkIdx); -}; - - -#endif /* end of include guard: IDYNTREE_MODEL_LINKTRAVERSALSCACHE_H */ +#endif diff --git a/src/model/include/iDynTree/Model/Model.h b/src/model/include/iDynTree/Model/Model.h index ae1f2135e14..07782c9311a 100644 --- a/src/model/include/iDynTree/Model/Model.h +++ b/src/model/include/iDynTree/Model/Model.h @@ -1,578 +1,13 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef IDYNTREE_MODEL_H -#define IDYNTREE_MODEL_H +#ifndef IDYNTREE_MODEL_MODEL_H +#define IDYNTREE_MODEL_MODEL_H -#include -#include -#include +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif -#include -#include +#include -#include -#include -#include - -namespace iDynTree -{ - class Traversal; - - struct Neighbor - { - LinkIndex neighborLink; - JointIndex neighborJoint; - }; - - /** - * Class that represents a generic multibody model. - * - * A model is composed by rigid bodies (i.e. links) connected - * by joints. Each joint can have from 0 to 6 degrees of freedom. - * - * Each link has a "link frame" rigidly attached to it. - * Additionally, other rigidly attachable frames can be defined for each link. - * - * The model contains also a serialization for the different elements, - * i.e. a function between: - * * joint names and the integers 0..getNrOfJoints()-1 - * * dof names and the integers 0..getNrOfDOFs()-1 - * * link names and the integers 0..getNrOfLinks()-1 - * * frame names and the integers 0..getNrOfFrames()-1 - * - * For simplicity, these mappings are build when building the model. - * In particular the link and joint indices are assigned when the links - * and joint are added to the model using the `addLink` and `addJoint` - * methods. - * - * The DOF indices are also assigned when the joint is added to the model - * with the addJoint method. For example if a model is composed only of - * 0 or 1 DOF joints and the 1 DOFs joints are added before the 0 DOFs then - * the joint index and dof index for 1 DOF joints will be coincident (this is - * how the URDF parser is actually implemented). For this reason the current - * implementation does not have a concept of DOF explicit identifier, - * i.e. a getDOFName(DOFIndex dofIndex) method does not exist. - * - * The frame indices between 0 and getNrOfLinks()-1 are always assigned to the - * "main" link frame of the link with the same index. The frame indices - * between getNrOfLinks() and getNrOfFrames()-1 are assigned when the additional - * frame is added to the model with the addAdditionalFrameToLink call. All the additional - * frame indices are incremented by 1 whenever a new link is added, to ensure that - * its "link frame" has a frame index in the 0...getNrOfLinks()-1 range. - * - * - * - * \ingroup iDynTreeModel - */ - class Model - { - private: - /** Vector of links. For each link its index indicates its location in this vector */ - std::vector links; - - /** Vector of joints. For each joint its index indicates its location in this vector */ - std::vector joints; - - /** Vector of additional frames. - * The element additionalFrames[frameOffset] will be the link_H_frame transform of the frame with - * FrameIndex getNrOfLinks() + frameOffset . - */ - std::vector additionalFrames; - - /** - * Vector of link indices corresponding to an additional frame. - * The element additionalFrameNames[frameOffset] will be the link_H_frame transform of the frame with - * FrameIndex getNrOfLinks() + frameOffset . - */ - std::vector additionalFramesLinks; - - /** Vector of link names, matches the index of each link to its name. */ - std::vector linkNames; - - /** Vector of joint names, matches the index of each joint to its name. */ - std::vector jointNames; - - /** - * Vector of additional frame names. - * The element frameNames[frameOffset] will be the name of the frame with - * FrameIndex getNrOfLinks() + frameOffset . - */ - std::vector frameNames; - - /** Adjacency lists: match each link index to a list of its neighbors, - and the joint connecting to them. */ - std::vector< std::vector > neighbors; - - /** Vector containing the package directories associated to the model. */ - std::vector packageDirs; - - /** - * Most data structures are not undirected, so we store the original - * root of the tree, to provide a default root for Traversal generation. - */ - LinkIndex defaultBaseLink; - - /** - * Cache number of position coordinaties of the model. - * If all joints are 0 or 1 dofs, this is equal to nrOfDOFs. - */ - unsigned int nrOfPosCoords; - - /** - * Cached number of (internal) DOFs of the model. - * This is just the sum of all the getNrOfDOFs of the joint in the model. - */ - unsigned int nrOfDOFs; - - /** - * Solid shapes used for visualization. - */ - ModelSolidShapes m_visualSolidShapes; - - /** - * Solid shapes used for collision checking. - */ - ModelSolidShapes m_collisionSolidShapes; - - /** - * Copy the structure of the model from another instance of a model. - */ - void copy(const Model & model); - - /** - * Destroy the object, properly deallocating memory. - */ - void destroy(); - - public: - - /** - * Costructor - */ - Model(); - - /** - * Copy costructor - */ - Model(const Model & other); - - /** - * Copy operator - */ - Model& operator=(const Model &other); - - /** - * Copy the model. - * - * Get a copy of the model. - */ - Model copy() const; - - /** - * Destructor - * - */ - virtual ~Model(); - - /** - * Get the number of links in the model. - */ - size_t getNrOfLinks() const; - - /** - * @return a vector containing all the directories of the meshes - */ - const std::vector& getPackageDirs() const; - - void setPackageDirs(const std::vector& packageDirs); - - /** - * Get the name of a link given its index, or - * an LINK_INVALID_NAME string if linkIndex < 0 or >= getNrOfLinks() - */ - std::string getLinkName(const LinkIndex linkIndex) const; - - LinkIndex getLinkIndex(const std::string & linkName) const; - - /** - * \brief Check if a given LinkIndex is valid. - * - * A link index is valid if is different from - * LINK_INVALID_INDEX and 0 =< index < getNrOfLinks()-1 - * - * @return true if the index is valid, false otherwise. - */ - bool isValidLinkIndex(const LinkIndex index) const; - - LinkPtr getLink(const LinkIndex linkIndex); - LinkConstPtr getLink(const LinkIndex linkIndex) const; - - LinkIndex addLink(const std::string & name, const Link & link); - - /** - * Get number of joints in the model. - */ - size_t getNrOfJoints() const; - - /** - * Get the name of a joint given its index, or - * an JOINT_INVALID_NAME if linkIndex < 0 or >= getNrOfLinks() - */ - std::string getJointName(const JointIndex index) const; - - /** - * Get the total mass of the robot - */ - double getTotalMass() const; - - /** - * Get the index of a joint, given a jointName. - * If the jointName is not found in the model, - * return JOINT_INVALID_INDEX . - * - */ - JointIndex getJointIndex(const std::string & jointName) const; - - IJointPtr getJoint(const JointIndex index); - - IJointConstPtr getJoint(const JointIndex index) const; - - /** - * \brief Check if a given JointIndex is valid. - * - * A joint index is valid if is different from - * JOINT_INVALID_INDEX and 0 =< index < getNrOfJoints()-1 - * - * @return true if the index is valid, false otherwise. - */ - bool isValidJointIndex(const JointIndex index) const; - - /** - * Check if a name is already used for a link in the model. - * - * @return true if a name is used by a link in a model, false otherwise. - */ - bool isLinkNameUsed(const std::string linkName) const; - - /** - * Check if a name is already used for a joint in the model. - * - * @return true if a name is used by a joint in a model, false otherwise. - */ - bool isJointNameUsed(const std::string jointName) const; - - /** - * Check if a name is already used for a frame in the model. - * - * \note this function will check the name of the links and the names of the additional frames. - * @return true if a name is used by a frame in a model, false otherwise. - */ - bool isFrameNameUsed(const std::string frameName) const; - - /** - * - * Add a joint to the model. The two links to which the joint is connected - * are specified in the joint itself, throught the appropriate methods. - * @return the JointIndex of the added joint, or JOINT_INVALID_INDEX if - * there was an error in adding the joint. - */ - JointIndex addJoint(const std::string & jointName, IJointConstPtr joint); - - /** - * Add a joint to the model, and specify the two links that are connected - * by the specified joints. The setAttachedLinks of the passed joint - * is called appropriately, to ensure consistency in the model. - * @return the JointIndex of the added joint, or JOINT_INVALID_INDEX if - * there was an error in adding the joint. - */ - JointIndex addJoint(const std::string & link1, const std::string & link2, - const std::string & jointName, IJointConstPtr joint); - - /** - * Add a joint to the model, and add also a link. - * The added joints connects an existing link of the model to the newly - * added link. - * The setAttachedLinks of the passed joint - * is called appropriately, to ensure consistency in the model. - * @return the JointIndex of the added joint, or JOINT_INVALID_INDEX if - * there was an error in adding the joint. - */ - JointIndex addJointAndLink(const std::string & existingLink, - const std::string & jointName, IJointConstPtr joint, - const std::string & newLinkName, Link & newLink); - - /** - * \brief Displace a link by inserting a new link and a new joint. - * Displace a link by replacing it with a new link in the existing joint and insert new joint between the new link and the displaced link. - * Inputs: - * @param[in] existingJoint is where the new link will be inserted. - * @param[in] unmovableLink is the link that was previously connected to the displaced link by the existingJoint. - * @param[in] _unmovable_X_newLink is the transformation matrix from the unmovableLink to the new Link. - * @param[in] jointName is the name of the new joint. - * @param[in] joint is the new joint connecting the new link and the link that was displaced. - * @param[in] newLinkName is the name of the new link. - * @param[in] newLink is the new link that will be attached to the unmovable link using the existingJoint. - * The setAttachedLinks of the new joint - * and the existing joint are edited appropriately, to ensure consistency in the model. - * @return the JointIndex of the added joint, or JOINT_INVALID_INDEX if - * there was an error in adding the joint. - */ - JointIndex insertLinkToExistingJointAndAddJointForDisplacedLink(const std::string & existingJoint, - const std::string & unmovableLink, - const Transform& _unmovableLink_X_newLink, - const std::string & jointName, IJointConstPtr joint, - const std::string & newLinkName, Link & newLink); - - /** - * Get the dimension of the vector used to parametrize the positions of the joints of the robot. - * This number can be obtained by summing the getNrOfPosCoords of all the joints of the model. - * - * \warning This is *not* including the 6 degrees of freedom of the base. - */ - size_t getNrOfPosCoords() const; - - /** - * Get the number of degrees of freedom of the joint of the robot. - * This number can be obtained by summing the getNrOfDOFs of all the joints of the model. - * - * \warning This is *not* including the 6 degrees of freedom of the base. - */ - size_t getNrOfDOFs() const; - - /** - * Get the number of frames in the model. - * - * \note this will return the sum of the number of link - * (as each link has an attached frame) and the total number - * of additional frames. - * - * @return the number of frames in the model. - */ - size_t getNrOfFrames() const; - - /** - * Add an additional frame to a link. - * - * \note This function has signature different from - * addJoint/addLink because the FrameIndex of the - * additional frame are invalidated at each call to - * the addLink. So we don't return the FrameIndex in this - * function, as the model construction should be completed - * before that FrameIndex are stored. - * - * @param[in] linkName the link to which attach the additional frame. - * @param[in] frameName the name of the frame added to the model. - * @param[in] link_H_frame the pose of added frame with respect to the link main frame, - * expressed with a transform with: - * refFrame: the main link frame - * frame: the added frame - * @return true if all went well, false if an error occured. - * - */ - bool addAdditionalFrameToLink(const std::string & linkName, - const std::string & frameName, - iDynTree::Transform link_H_frame); - - /** - * Get the name of a frame given its index. - * - * @param[in] frameIndex the index of the frame whose name is requested. - * @return the name of a frame given its index, or - * a FRAME_INVALID_NAME string if frameIndex < 0 or >= getNrOfFrames() - */ - std::string getFrameName(const FrameIndex frameIndex) const; - - /** - * Get the index of a frame given its name. - * - * @param[in] frameName the name of the frame for which the index is requested. - * @return the index of the frame, of FRAME_INVALID_INDEX if frameName is not - * not a frame name. - */ - FrameIndex getFrameIndex(const std::string & frameName) const; - - /** - * \brief Check if a given FrameIndex is valid. - * - * A frame index is valid if is different from - * FRAME_INVALID_INDEX and 0 =< index < getNrOfFrames()-1 - * - * @return true if the index is valid, false otherwise. - */ - bool isValidFrameIndex(const FrameIndex index) const; - - /** - * Get the tranform of the frame with respect to the main - * frame of the link, returned as link_H_frame tranform - * with refFrame : the link main frame and frame : the - * frame with index frameIndex . - * - * @param[in] frameIndex the index of the frame for which transform is requested. - * @return the link_H_frame transform, or an identity tranform - * if frameIndex < 0 or frameIndex >= getNrOfFrames . - */ - Transform getFrameTransform(const FrameIndex frameIndex) const; - - /** - * Get the link at which the frame with index frameIndex - * is attached. - * - */ - LinkIndex getFrameLink(const FrameIndex frameIndex) const; - - /** - * Get the additional frames of a specified link. - * - * @note The vector of returned frame index is ordered according to the frame index. - * @warning This method searches linearly over all the frames. - * - * @param[in] link a LinkIndex of the specified link, - * @param[out] frames a vector of FrameIndex of the frame indeces attached to the specified link index, - * @return true if the specified link is a valid link, false otherwise. - */ - bool getLinkAdditionalFrames(const LinkIndex lnkIndex, std::vector& frameIndeces) const; - - /** - * Get the nr of neighbors of a given link. - */ - unsigned int getNrOfNeighbors(const LinkIndex link) const; - - /** - * Get the neighbor of a link. neighborIndex should be - * >= 0 and <= getNrOfNeighbors(link) - */ - Neighbor getNeighbor(const LinkIndex link, unsigned int neighborIndex) const; - - /** - * Set the default base link, used for generation of the default traversal. - */ - bool setDefaultBaseLink(const LinkIndex linkIndex); - - /** - * Get the default base link, used for generation of the default traversal. - * - * If no link are present in model, returns LINK_INVALID_INDEX. - * If setDefaultBaseLink was never called but at least a link has been added - * to the model, returns 0 (i.e. the index of the first link added to the model. - * - * @return index of the default base link, if valid - */ - LinkIndex getDefaultBaseLink() const; - - /** - * Compute a Traversal of all the links in the Model, doing a Depth First Search starting - * at the default base. - * - * \warning The traversal computed with this function contains pointers to the joints - * and links present in this model. Whenever these pointers are invalidated, - * for example because a link or a joint is added or the model is copied, - * the traversal need to be recomputed. - * - * \warning this function works only on Models without cycles. - * @param[out] traversal traversal of all links in the model - * @return true if all went well, false otherwise. - */ - bool computeFullTreeTraversal(Traversal & traversal) const; - - /** - * Compute a Traversal of all the links in the Model, doing a Depth First Search starting - * at the given traversalBase. - * - * \warning The traversal computed with this function contains pointers to the joints - * and links present in this model. Whenever these pointers are invalidated, - * for example because a link or a joint is added or the model is copied, - * the traversal need to be recomputed. - *. - * \warning this function works only on Models without cycles. - * @param[out] traversal traversal of all links in the model - * @param[in] traversalBase base (root) link in the traversal - * @return true if all went well, false otherwise - */ - bool computeFullTreeTraversal(Traversal & traversal, const LinkIndex traversalBase) const; - - /** - * Get the inertial parameters of the links of the model in vector forms. - * - * This methods gets the inertial parameters (mass, center - * of mass, 3D inertia matrix) of the links of the robot. - * - * The output vector of inertial parameters must have 10*getNrOfLinks() elements, - * each 10 elements subvector corresponds to the inertial parameters of one link, - * following the serialization induced by the link indices - * (link 0 corresponds to elements 0-9, link 1 to 10-19, etc). - * - * The mapping between the SpatialInertia class and the Vector10 elements is the one - * defined in SpatialInertia::asVector() method. - * - * @param[out] modelInertialParams vector of inertial parameters - * @return true if all went well, false otherwise. - * - */ - bool getInertialParameters(VectorDynSize & modelInertialParams) const; - - /** - * Update the inertial parameters of the links of the model. - * - * This methods modifies the inertial parameters (mass, center - * of mass, 3D inertia matrix) of the links of the robot. - * - * The input vector of inertial parameters must have 10*getNrOfLinks() elements, - * each 10 elements subvector corresponds to the inertial parameters of one link, - * following the serialization induced by the link indices - * (link 0 corresponds to elements 0-9, link 1 to 10-19, etc). - * - * The mapping between the SpatialInertia class and the Vector10 elements is the one - * defined in SpatialInertia::asVector() method. - * - * @note For efficency reason, inertial parameters are not checked for full physical - * consistency before being update. - * - * @param[in] modelInertialParams vector of inertial parameters - * @return true if all went well, false otherwise. - * - */ - bool updateInertialParameters(const VectorDynSize & modelInertialParams); - - /** - * Get the ModelSolidShapes meant for visualization. - * - * @return a reference to ModelSolidShapes meant for visualization. - */ - ModelSolidShapes& visualSolidShapes(); - - /** - * Get the ModelSolidShapes meant for visualization (const version) - * - * @return a reference to ModelSolidShapes meant for visualization. - */ - const ModelSolidShapes& visualSolidShapes() const; - - /** - * Get the ModelSolidShapes meant for collision checking. - * - * @return a reference to ModelSolidShapes meant for visualization. - */ - ModelSolidShapes& collisionSolidShapes(); - - - /** - * Get the ModelSolidShapes meant for collision checking (const version) - * - * @return a reference to ModelSolidShapes meant for visualization. - */ - const ModelSolidShapes& collisionSolidShapes() const; - - /** - * \brief Get a printable representation of the Model. - * - * Useful for debugging. - */ - std::string toString() const; - - }; - - -} - -#endif /* IDYNTREE_LINK_H */ +#endif diff --git a/src/model/include/iDynTree/Model/ModelTestUtils.h b/src/model/include/iDynTree/Model/ModelTestUtils.h index fb2ea778686..46f6a191107 100644 --- a/src/model/include/iDynTree/Model/ModelTestUtils.h +++ b/src/model/include/iDynTree/Model/ModelTestUtils.h @@ -1,231 +1,13 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef IDYNTREE_MODEL_TEST_UTILS_H -#define IDYNTREE_MODEL_TEST_UTILS_H +#ifndef IDYNTREE_MODEL_MODEL_TEST_UTILS_H +#define IDYNTREE_MODEL_MODEL_TEST_UTILS_H +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif -#include -#include -#include -#include -#include -#include +#include -#include - -#include -#include "IJoint.h" - -namespace iDynTree -{ - -inline Link getRandomLink() -{ - double cxx = getRandomDouble(0,3); - double cyy = getRandomDouble(0,4); - double czz = getRandomDouble(0,6); - double rotInertiaData[3*3] = {czz+cyy,0.0,0.0, - 0.0,cxx+czz,0.0, - 0.0,0.0,cxx+cyy}; - - Rotation rot = Rotation::RPY(getRandomDouble(),getRandomDouble(-1,1),getRandomDouble()); - - SpatialInertia inertiaLink(getRandomDouble(0,4), - Position(getRandomDouble(-2,2),getRandomDouble(-2,2),getRandomDouble(-2,2)), - rot*RotationalInertiaRaw(rotInertiaData,3,3)); - - Link link; - - link.setInertia(inertiaLink); - - return link; -} - -/** - * Add a random link with random model. - */ -inline void addRandomLinkToModel(Model & model, std::string parentLink, std::string newLinkName, bool onlyRevoluteJoints=false) -{ - // Add Link - LinkIndex newLinkIndex = model.addLink(newLinkName,getRandomLink()); - - // Now add joint - LinkIndex parentLinkIndex = model.getLinkIndex(parentLink); - - int nrOfJointTypes = 3; - - int jointType = rand() % nrOfJointTypes; - - if (onlyRevoluteJoints) - { - jointType = 1; - } - - if( jointType == 0 ) - { - FixedJoint fixJoint(parentLinkIndex,newLinkIndex,getRandomTransform()); - model.addJoint(newLinkName+"joint",&fixJoint); - } - else if( jointType == 1 ) - { - RevoluteJoint revJoint; - revJoint.setAttachedLinks(parentLinkIndex,newLinkIndex); - revJoint.setRestTransform(getRandomTransform()); - revJoint.setAxis(getRandomAxis(),newLinkIndex); - model.addJoint(newLinkName+"joint",&revJoint); - } - else if( jointType == 2 ) - { - PrismaticJoint prismJoint; - prismJoint.setAttachedLinks(parentLinkIndex,newLinkIndex); - prismJoint.setRestTransform(getRandomTransform()); - prismJoint.setAxis(getRandomAxis(),newLinkIndex); - model.addJoint(newLinkName+"joint",&prismJoint); - } - else - { - assert(false); - } -} - -/** - * Add a random additional frame to a model model. - */ -inline void addRandomAdditionalFrameToModel(Model & model, std::string parentLink, std::string newFrameName) -{ - model.addAdditionalFrameToLink(parentLink,newFrameName,getRandomTransform()); -} - -inline LinkIndex getRandomLinkIndexOfModel(const Model & model) -{ - int nrOfLinks = model.getNrOfLinks(); - - return rand() % nrOfLinks; -} - -inline std::string getRandomLinkOfModel(const Model & model) -{ - LinkIndex randomLink = getRandomLinkIndexOfModel(model); - - return model.getLinkName(randomLink); -} - -inline std::string int2string(int i) -{ - std::stringstream ss; - - ss << i; - - return ss.str(); -} - -inline Model getRandomModel(unsigned int nrOfJoints, size_t nrOfAdditionalFrames = 10) -{ - Model model; - - model.addLink("baseLink",getRandomLink()); - - for(unsigned int i=0; i < nrOfJoints; i++) - { - std::string parentLink = getRandomLinkOfModel(model); - std::string linkName = "link" + int2string(i); - addRandomLinkToModel(model,parentLink,linkName); - } - - for(unsigned int i=0; i < nrOfAdditionalFrames; i++) - { - std::string parentLink = getRandomLinkOfModel(model); - std::string frameName = "additionalFrame" + int2string(i); - addRandomAdditionalFrameToModel(model,parentLink,frameName); - } - - return model; -} - -inline Model getRandomChain(unsigned int nrOfJoints, size_t nrOfAdditionalFrames = 10, bool onlyRevoluteJoints=false) -{ - Model model; - - model.addLink("baseLink",getRandomLink()); - - std::string linkName = "baseLink"; - for(unsigned int i=0; i < nrOfJoints; i++) - { - std::string parentLink = linkName; - linkName = "link" + int2string(i); - addRandomLinkToModel(model,parentLink,linkName,onlyRevoluteJoints); - } - - for(unsigned int i=0; i < nrOfAdditionalFrames; i++) - { - std::string parentLink = getRandomLinkOfModel(model); - std::string frameName = "additionalFrame" + int2string(i); - addRandomAdditionalFrameToModel(model,parentLink,frameName); - } - - return model; -} - -/** - * Get random joint position consistently with the limits of the model. - * If the input vector has the wrong size, it will be resized. - */ -inline void getRandomJointPositions(VectorDynSize& vec, const Model& model) -{ - vec.resize(model.getNrOfPosCoords()); - for(JointIndex jntIdx=0; jntIdx < model.getNrOfJoints(); jntIdx++) - { - IJointConstPtr jntPtr = model.getJoint(jntIdx); - if( jntPtr->hasPosLimits() ) - { - for(int i=0; i < jntPtr->getNrOfPosCoords(); i++) - { - double max = jntPtr->getMaxPosLimit(i); - double min = jntPtr->getMinPosLimit(i); - vec(jntPtr->getDOFsOffset()+i) = getRandomDouble(min,max); - } - } - else - { - for(int i=0; i < jntPtr->getNrOfPosCoords(); i++) - { - vec(jntPtr->getDOFsOffset()+i) = getRandomDouble(); - } - } - } - - return; -} - -/** - * Get random robot positions, velocities and accelerations - * and external wrenches to be given as an input to InverseDynamics. - */ -inline bool getRandomInverseDynamicsInputs(FreeFloatingPos& pos, - FreeFloatingVel& vel, - FreeFloatingAcc& acc, - LinkNetExternalWrenches& extWrenches) -{ - pos.worldBasePos() = getRandomTransform(); - vel.baseVel() = getRandomTwist(); - acc.baseAcc() = getRandomTwist(); - - - for(unsigned int jnt=0; jnt < pos.getNrOfPosCoords(); jnt++) - { - pos.jointPos()(jnt) = getRandomDouble(); - } - - for(unsigned int jnt=0; jnt < vel.getNrOfDOFs(); jnt++) - { - vel.jointVel()(jnt) = getRandomDouble(); - acc.jointAcc()(jnt) = getRandomDouble(); - } - - return true; -} - -} - -#endif /* IDYNTREE_MODEL_TEST_UTILS_H */ +#endif diff --git a/src/model/include/iDynTree/Model/ModelTransformers.h b/src/model/include/iDynTree/Model/ModelTransformers.h index 87dc9ffab2f..f96f6cc73ba 100644 --- a/src/model/include/iDynTree/Model/ModelTransformers.h +++ b/src/model/include/iDynTree/Model/ModelTransformers.h @@ -1,100 +1,13 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -/** - * \file ModelTransformers.h - * \brief Collection of function to modify model in various ways - * - * - * In this file a series of functions for transforming Model - * objects are provided -*/ +#ifndef IDYNTREE_MODEL_MODEL_TRANSFOMERS_H +#define IDYNTREE_MODEL_MODEL_TRANSFOMERS_H +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif -#ifndef IDYNTREE_MODEL_TRANSFORMERS_H -#define IDYNTREE_MODEL_TRANSFORMERS_H - -namespace iDynTree -{ -class Model; -class SensorsList; - -/** - * \function Remove all fake links in the model, transforming them in frames. - * - * Given a Model in input, this function copies all its links - * and joints to the model in output, except for links that recognized - * as "fake links". - * - * The condition for a link to be classified as "fake link" are: - * * The link has a zero mass. - * * The link is a leaf, i.e. it is connected to only one neighbor. - * * The link is connected to its only neighbor with a fixed joint. - * - * Once a "fake link" has been identified to respect this two conditions, - * it and the joint that it connects it to its only neighbor is not copied - * to the output model, but a frame with the same name of the "fake link" - * and with the same transform is added to the model. - * - * \note The definition of "fake link" used in this function excludes - * the case in which two fake links are attached to one another. - * - */ -bool removeFakeLinks(const Model& modelWithFakeLinks, - Model& modelWithoutFakeLinks); - -/** - * This function takes in input a iDynTree::Model and - * an ordered list of joints and returns a model with - * just the joint specified in the list, with that exact order. - * - * All other joints are be removed by lumping (i.e. fusing together) - * the inertia of the links that are connected by that joint, assuming the joint - * to be in "rest" position (i.e. zero for revolute or prismatic joints). The links eliminated - * with this process are be added back to the reduced model as "frames", - * and are copied in the same way all the additional frames of the lumped links. - * - */ -bool createReducedModel(const Model& fullModel, - const std::vector& jointsInReducedModel, - Model& reducedModel); - -/** - * @brief Given a specified base, return a model with a "normalized" joint numbering for that base. - * - * This function takes in input a iDynTree::Model and a name of a link in that model. - * It returns a model identical to the one in input, but with the joint serialization - * of the non-fixed joint modified in such a way that a non-fixed joint has an index higher than - * all the non-fixed joints on the path between it and the base. After all the non-fixed joints, - * the fixed joints are also added with the same criteria, but applied to fixed joints. - * - * @note This method make sure that the non-fixed joint in the model have a "regular numbering" - * as described in Featherstone "Rigid Body Dynamics Algorithm", section 4.1.2 . Note that - * this numbering is not required by any algorithm in iDynTree, but it may be useful for - * example to ensure that for a chain model the joint numbering is the one induced by the - * kinematic structure. - * - * @return true if all went well, false if there was an error in conversion. - */ -bool createModelWithNormalizedJointNumbering(const Model& model, - const std::string& baseForNormalizedJointNumbering, - Model& reducedModel); - - -/** - * Extract sub model from sub model traversal. - * - * This function creates a new iDynTree::Model containing links and joints composing the subModelTraversal. - * The function takes in input a iDynTree::Model and a iDynTree::Traversal. The new model will contain joints - * and links composing the subModelTraversal, with the same order. - * The FT sensor frames are added as additional frames. - * - * @return true if all went well, false if there was an error in creating the sub model. - */ -bool extractSubModel(const iDynTree::Model& fullModel, const iDynTree::Traversal& subModelTraversal, - iDynTree::Model& outputSubModel); - -} - +#include #endif diff --git a/src/model/include/iDynTree/Model/MovableJointImpl.h b/src/model/include/iDynTree/Model/MovableJointImpl.h index 7538dc8cc33..3c843eb9392 100644 --- a/src/model/include/iDynTree/Model/MovableJointImpl.h +++ b/src/model/include/iDynTree/Model/MovableJointImpl.h @@ -1,165 +1,13 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef IDYNTREE_MOVABLE_JOINT_IMPL_H -#define IDYNTREE_MOVABLE_JOINT_IMPL_H +#ifndef IDYNTREE_MODEL_MOVABLE_JOINT_IMPL_H +#define IDYNTREE_MODEL_MOVABLE_JOINT_IMPL_H +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif +#include -#include - - -namespace iDynTree -{ - /** - * Base template for implementation of non-fixed joints. - * A specific joint can be derived from an instantiation of this template. - * - * For more information on the assumption of the joint model, check the IJoint interface. - * - * \ingroup iDynTreeModel - */ - template - class MovableJointImpl: public IJoint - { - protected: - JointIndex m_index; - size_t m_posCoordsOffset; - size_t m_DOFsOffset; - - public: - /** - * Denstructor - * - */ - virtual ~MovableJointImpl() = 0; - - // Documentation inherited - virtual unsigned int getNrOfPosCoords() const; - - // Documentation inherited - virtual unsigned int getNrOfDOFs() const; - - /** - * Get the motion subspace matrix, i.e. the matrix that - * maps the joint velocity to the relative twist between the two - * links. - * In particular the motion subspace matrix is the S matrix such that - * v_linkA = S_{linkA,linkB}*dq + linkA_X_linkB*v_linkB - * where dq is the joint velocity. - * - * @return the motion subspace matrix - * - * \note The motion subspace matrix is also known in literature as hinge matrix, - * hinge map matrix, joint map matrix or joint motion map matrix. - */ - //virtual MatrixFixSize<6, nrOfDOFs> getMotionSubspace(const JointPos & state, const int linkA, const int linkB) const = 0; - - /** - * Get the motion subspace matrix derivatives, i.e. the matrix that - * maps the joint velocities to the relative spatial acceleration between the two - * links. - * In particular the motion subspace matrix derivative is the dS matrix such that - * a_linkA = S_{linkA,linkB}*ddq + dS_{linkA,linkB}*dq + linkA_X_linkB*a_linkB - * where ddq is the joint acceleration and dq is the joint velocity. - * - * @return the motion subspace matrix - * - * \note The motion subspace matrix is also known in literature as hinge matrix, - * hinge map matrix, joint map matrix or joint motion map matrix. - */ - //virtual MatrixFixSize<6, nrOfDOFs> getMotionSubspaceDerivative(const JointPosVel & state, const int linkA, const int linkB) const = 0; - - /** - * Get the kinematic coupling matrix, i.e. the matrix that maps - * the joint velocities to the derivative of the joint coordinates. - * - * \note For the joint whose configuration is in R^n, the kinematic coupling matrix - * is equal to the identity. - - * @return the nrOfPosCoords times nrOfDOFs kinematic coupling matrix - */ - //virtual MatrixFixSize getKinematicCouplingMatrix(const JointPos & state) = 0; - - // Documentation inherited - virtual void setIndex(JointIndex & _index); - - // Documentation inherited - virtual JointIndex getIndex() const; - - // Documentation inherited - virtual void setPosCoordsOffset(const size_t _offset); - - // Documentation inherited - virtual size_t getPosCoordsOffset() const; - - // Documentation inherited - virtual void setDOFsOffset(const size_t _offset); - - // Documentation inherited - virtual size_t getDOFsOffset() const; - }; - - template - unsigned int MovableJointImpl::getNrOfPosCoords() const - { - return nrOfPosCoords; - } - - template - unsigned int MovableJointImpl::getNrOfDOFs() const - { - return nrOfDOFs; - } - - template - void MovableJointImpl::setIndex(JointIndex & _index) - { - this->m_index = _index; - } - - template - JointIndex MovableJointImpl::getIndex() const - { - return this->m_index; - } - - template - void MovableJointImpl::setPosCoordsOffset(const size_t _offset) - { - this->m_posCoordsOffset = _offset; - } - - template - size_t MovableJointImpl::getPosCoordsOffset() const - { - return this->m_posCoordsOffset; - } - - template - void MovableJointImpl::setDOFsOffset(const size_t _offset) - { - this->m_DOFsOffset = _offset; - } - - template - size_t MovableJointImpl::getDOFsOffset() const - { - return this->m_DOFsOffset; - } - - - template - MovableJointImpl::~MovableJointImpl() - { - } - - typedef MovableJointImpl<1,1> MovableJointImpl1; - typedef MovableJointImpl<2,2> MovableJointImpl2; - typedef MovableJointImpl<3,3> MovableJointImpl3; - typedef MovableJointImpl<4,4> MovableJointImpl4; - typedef MovableJointImpl<5,5> MovableJointImpl5; - typedef MovableJointImpl<6,6> MovableJointImpl6; -} - -#endif /* IDYNTREE_JOINT_IMPL_H */ +#endif diff --git a/src/model/include/iDynTree/Model/PrismaticJoint.h b/src/model/include/iDynTree/Model/PrismaticJoint.h index fa8d940d686..1bb1f59a8e5 100644 --- a/src/model/include/iDynTree/Model/PrismaticJoint.h +++ b/src/model/include/iDynTree/Model/PrismaticJoint.h @@ -1,217 +1,13 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef IDYNTREE_PRISMATIC_JOINT_H -#define IDYNTREE_PRISMATIC_JOINT_H +#ifndef IDYNTREE_MODEL_PRISMATIC_JOINT_H +#define IDYNTREE_MODEL_PRISMATIC_JOINT_H -#include -#include - -#include -#include -#include - -namespace iDynTree -{ - /** - * Class representing a prismatic joint, i.e. a joint that - * constraint two links to translate only along an axis. - * - * \ingroup iDynTreeModel - */ - class PrismaticJoint : public MovableJointImpl1 - { - private: - // Structure attributes - LinkIndex link1; - LinkIndex link2; - Transform link1_X_link2_at_rest; - Axis translation_axis_wrt_link1; - - // Limits - void disablePosLimits(); - bool m_hasPosLimits; - double m_minPos; - double m_maxPos; - - // Dynamic parameters - void resetJointDynamics(); - JointDynamicsType m_joint_dynamics_type; - double m_damping; - double m_static_friction; - - // Cache attributes - mutable double q_previous; - mutable Transform link1_X_link2; - mutable Transform link2_X_link1; - mutable SpatialMotionVector S_link1_link2; - mutable SpatialMotionVector S_link2_link1; - - void updateBuffers(const double new_q) const; - void resetBuffers(const double new_q) const; - void resetAxisBuffers() const; - - public: - /** - * Constructor - */ - PrismaticJoint(); - - IDYNTREE_DEPRECATED_WITH_MSG("Please use the setter methods to specify the parameters of the joint") - PrismaticJoint(const LinkIndex link1, const LinkIndex link2, - const Transform& link1_X_link2, const Axis& _translation_axis_wrt_link1); - - /** - * Copy constructor - */ - PrismaticJoint(const PrismaticJoint& other); - - /** - * Destructor - */ - virtual ~PrismaticJoint(); - - // Documentation inherited - virtual IJoint * clone() const; - - // Documentation inherited - virtual void setAttachedLinks(const LinkIndex link1, const LinkIndex link2); - - // Documentation inherited - virtual void setRestTransform(const Transform& link1_X_link2); - - /** - * Set the prismatic axis of the joint, expressed in specified link frame, that is considered the "child" - * frame regarding the sign of the axis. - * - * See getAxis method for more information. - * - * @warning This method should be called after a valid restTransform between link1 and link2 has been - * set by calling the setRestTransform method. - */ - virtual void setAxis(const Axis& prismaticAxis, - const LinkIndex child, - const LinkIndex parent=LINK_INVALID_INDEX); - - // Set the prismatic axis expressed in link1 - IDYNTREE_DEPRECATED_WITH_MSG("Please use the setAxis method in which the link considered \"child\" is explicitly specified") - virtual void setAxis(const Axis& prismaticAxis_wrt_link1); - - // Documentation inherited - virtual LinkIndex getFirstAttachedLink() const; - - // Documentation inherited - virtual LinkIndex getSecondAttachedLink() const; - - /** - * Get the revolute axis of the robot, expressed in linkA frame. - * - * @param child the link frame (one of the two at which the link is attached) - * in which the returned axis is expressed. Furthermore, the - * axis direction depends on the assumption that this frame is - * considered the "child" in the relationship. - * - * See - * - * Seth, A., Sherman, M., Eastman, P., & Delp, S. (2010). - * Minimal formulation of joint motion for biomechanisms. - * Nonlinear Dynamics, 62(1), 291-303. - * https://nmbl.stanford.edu/publications/pdf/Seth2010.pdf - * Section 2.4 - * - * and - * - * "Modelling, Estimation and Identification of Humanoid Robots Dynamics" - * Traversaro - Section 3.2 - * https://traversaro.github.io/preprints/traversaro-phd-thesis.pdf - * - * for more details. - */ - virtual Axis getAxis(const LinkIndex child, - const LinkIndex parent=LINK_INVALID_INDEX) const; - - // Documentation inherited - virtual Transform getRestTransform(const LinkIndex child, - const LinkIndex parent) const; - - - // Documentation inherited - virtual const Transform & getTransform(const VectorDynSize & jntPos, - const LinkIndex child, - const LinkIndex parent) const; - - // Documentation inherited - TransformDerivative getTransformDerivative(const VectorDynSize & jntPos, - const LinkIndex child, - const LinkIndex parent, - const int posCoord_i) const; - - // Documentation inherited - virtual SpatialMotionVector getMotionSubspaceVector(int dof_i, - const LinkIndex child, - const LinkIndex parent=LINK_INVALID_INDEX) const; - - // Documentation inherited - virtual void computeChildPosVelAcc(const VectorDynSize & jntPos, - const VectorDynSize & jntVel, - const VectorDynSize & jntAcc, - LinkPositions & linkPositions, - LinkVelArray & linkVels, - LinkAccArray & linkAccs, - const LinkIndex child, const LinkIndex parent) const; - - // Documentation inherited - virtual void computeChildVel(const VectorDynSize & jntPos, - const VectorDynSize & jntVel, - LinkVelArray & linkVels, - const LinkIndex child, const LinkIndex parent) const; - - // Documentation inherited - virtual void computeChildVelAcc(const VectorDynSize & jntPos, - const VectorDynSize & jntVel, - const VectorDynSize & jntAcc, - LinkVelArray & linkVels, - LinkAccArray & linkAccs, - const LinkIndex child, const LinkIndex parent) const; - - // Documentation inherited - virtual void computeChildAcc(const VectorDynSize & jntPos, - const VectorDynSize & jntVel, - const LinkVelArray & linkVels, - const VectorDynSize & jntAcc, - LinkAccArray & linkAccs, - const LinkIndex child, - const LinkIndex parent) const; - - // Documentation inherited - virtual void computeChildBiasAcc(const VectorDynSize & jntPos, - const VectorDynSize & jntVel, - const LinkVelArray & linkVels, - LinkAccArray & linkBiasAccs, - const LinkIndex child, - const LinkIndex parent) const; - - // Documentation inherited - virtual void computeJointTorque(const VectorDynSize & jntPos, const Wrench & internalWrench, - const LinkIndex linkThatAppliesWrench, const LinkIndex linkOnWhichWrenchIsApplied, - VectorDynSize & jntTorques) const; - - // LIMITS METHODS - virtual bool hasPosLimits() const; - virtual bool enablePosLimits(const bool enable); - virtual bool getPosLimits(const size_t _index, double & min, double & max) const; - virtual double getMinPosLimit(const size_t _index) const; - virtual double getMaxPosLimit(const size_t _index) const; - virtual bool setPosLimits(const size_t _index, double & min, double & max); +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif - // DYNAMICS METHODS - virtual JointDynamicsType getJointDynamicsType() const; - virtual bool setJointDynamicsType(const JointDynamicsType enable); - virtual double getDamping(const size_t _index) const; - virtual double getStaticFriction(const size_t _index) const; - virtual bool setDamping(const size_t _index, double& damping); - virtual bool setStaticFriction(const size_t _index, double& staticFriction); - }; -} +#include #endif diff --git a/src/model/include/iDynTree/Model/RevoluteJoint.h b/src/model/include/iDynTree/Model/RevoluteJoint.h index bfb768ff94a..012ee1ff4ab 100644 --- a/src/model/include/iDynTree/Model/RevoluteJoint.h +++ b/src/model/include/iDynTree/Model/RevoluteJoint.h @@ -1,225 +1,13 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef IDYNTREE_REVOLUTE_JOINT_H -#define IDYNTREE_REVOLUTE_JOINT_H +#ifndef IDYNTREE_MODEL_REVOLUTE_JOINT_H +#define IDYNTREE_MODEL_REVOLUTE_JOINT_H -#include -#include - -#include -#include -#include - -namespace iDynTree -{ - /** - * Class representing a revolute joint, i.e. a joint that - * constraint two links to move only around a common axis. - * - * \ingroup iDynTreeModel - */ - class RevoluteJoint : public MovableJointImpl1 - { - private: - // Structure attributes - LinkIndex link1; - LinkIndex link2; - Transform link1_X_link2_at_rest; - Axis rotation_axis_wrt_link1; - - // Limits - void disablePosLimits(); - bool m_hasPosLimits; - double m_minPos; - double m_maxPos; - - // Dynamic parameters - void resetJointDynamics(); - JointDynamicsType m_joint_dynamics_type; - double m_damping; - double m_static_friction; - - // Cache attributes - mutable double q_previous; - mutable Transform link1_X_link2; - mutable Transform link2_X_link1; - mutable SpatialMotionVector S_link1_link2; - mutable SpatialMotionVector S_link2_link1; - - void updateBuffers(const double new_q) const; - void resetBuffers(const double new_q) const; - void resetAxisBuffers() const; - - public: - /** - * Constructor - */ - RevoluteJoint(); - - IDYNTREE_DEPRECATED_WITH_MSG("Please use the setter methods to specify the parameters of the joint") - RevoluteJoint(const LinkIndex link1, const LinkIndex link2, - const Transform& link1_X_link2, const Axis& _rotation_axis_wrt_link1); - - /** - * Constructor in which the LinkIndex to which the joint is attached are not specified. - * This constructor is tipically used together with the Model::addJoint or - * Model::addJointAndLink methods, in which the links to which the joint is attached are - * specified by the other arguments of the method. - */ - RevoluteJoint(const Transform& link1_X_link2, const Axis& _rotation_axis_wrt_link1); - - /** - * Copy constructor - */ - RevoluteJoint(const RevoluteJoint& other); - - /** - * Destructor - */ - virtual ~RevoluteJoint(); - - // Documentation inherited - virtual IJoint * clone() const; - - // Documentation inherited - virtual void setAttachedLinks(const LinkIndex link1, const LinkIndex link2); - - // Documentation inherited - virtual void setRestTransform(const Transform& link1_X_link2); - - /** - * Set the revolute axis of the joint, expressed in specified link frame, that is considered the "child" - * frame regarding the sign of the axis. - * - * See getAxis method for more information. - * - * @warning This method should be called after a valid restTransform between link1 and link2 has been - * set by calling the setRestTransform method. - */ - virtual void setAxis(const Axis& revoluteAxis, - const LinkIndex child, - const LinkIndex parent=LINK_INVALID_INDEX); - - // Set the revolute axis expressed in link1 - IDYNTREE_DEPRECATED_WITH_MSG("Please use the setAxis method in which the link considered \"child\" is explicitly specified") - virtual void setAxis(const Axis& revoluteAxis_wrt_link1); - - // Documentation inherited - virtual LinkIndex getFirstAttachedLink() const; - - // Documentation inherited - virtual LinkIndex getSecondAttachedLink() const; - - /** - * Get the revolute axis of the robot, expressed in child frame. - * - * @param child the link frame (one of the two at which the link is attached) - * in which the returned axis is expressed. Furthermore, the - * axis direction depends on the assumption that this frame is - * considered the "child" in the relationship. - * - * See - * - * Seth, A., Sherman, M., Eastman, P., & Delp, S. (2010). - * Minimal formulation of joint motion for biomechanisms. - * Nonlinear Dynamics, 62(1), 291-303. - * https://nmbl.stanford.edu/publications/pdf/Seth2010.pdf - * Section 2.4 - * - * and - * - * "Modelling, Estimation and Identification of Humanoid Robots Dynamics" - * Traversaro - Section 3.2 - * https://traversaro.github.io/preprints/traversaro-phd-thesis.pdf - * - * for more details. - */ - virtual Axis getAxis(const LinkIndex child, - const LinkIndex parent=LINK_INVALID_INDEX) const; - - // Documentation inherited - virtual Transform getRestTransform(const LinkIndex child, - const LinkIndex parent) const; - - - // Documentation inherited - virtual const Transform & getTransform(const VectorDynSize & jntPos, - const LinkIndex child, - const LinkIndex parent) const; - - // Documentation inherited - TransformDerivative getTransformDerivative(const VectorDynSize & jntPos, - const LinkIndex child, - const LinkIndex parent, - const int posCoord_i) const; - - // Documentation inherited - virtual SpatialMotionVector getMotionSubspaceVector(int dof_i, - const LinkIndex child, - const LinkIndex parent=LINK_INVALID_INDEX) const; - - // Documentation inherited - virtual void computeChildPosVelAcc(const VectorDynSize & jntPos, - const VectorDynSize & jntVel, - const VectorDynSize & jntAcc, - LinkPositions & linkPositions, - LinkVelArray & linkVels, - LinkAccArray & linkAccs, - const LinkIndex child, const LinkIndex parent) const; - - // Documentation inherited - virtual void computeChildVel(const VectorDynSize & jntPos, - const VectorDynSize & jntVel, - LinkVelArray & linkVels, - const LinkIndex child, const LinkIndex parent) const; - - // Documentation inherited - virtual void computeChildVelAcc(const VectorDynSize & jntPos, - const VectorDynSize & jntVel, - const VectorDynSize & jntAcc, - LinkVelArray & linkVels, - LinkAccArray & linkAccs, - const LinkIndex child, const LinkIndex parent) const; - - // Documentation inherited - virtual void computeChildAcc(const VectorDynSize & jntPos, - const VectorDynSize & jntVel, - const LinkVelArray & linkVels, - const VectorDynSize & jntAcc, - LinkAccArray & linkAccs, - const LinkIndex child, - const LinkIndex parent) const; - - // Documentation inherited - virtual void computeChildBiasAcc(const VectorDynSize & jntPos, - const VectorDynSize & jntVel, - const LinkVelArray & linkVels, - LinkAccArray & linkBiasAccs, - const LinkIndex child, - const LinkIndex parent) const; - - // Documentation inherited - virtual void computeJointTorque(const VectorDynSize & jntPos, const Wrench & internalWrench, - const LinkIndex linkThatAppliesWrench, const LinkIndex linkOnWhichWrenchIsApplied, - VectorDynSize & jntTorques) const; - - // LIMITS METHODS - virtual bool hasPosLimits() const; - virtual bool enablePosLimits(const bool enable); - virtual bool getPosLimits(const size_t _index, double & min, double & max) const; - virtual double getMinPosLimit(const size_t _index) const; - virtual double getMaxPosLimit(const size_t _index) const; - virtual bool setPosLimits(const size_t _index, double & min, double & max); +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif - // DYNAMICS METHODS - virtual JointDynamicsType getJointDynamicsType() const; - virtual bool setJointDynamicsType(const JointDynamicsType enable); - virtual double getDamping(const size_t _index) const; - virtual double getStaticFriction(const size_t _index) const; - virtual bool setDamping(const size_t _index, double& damping); - virtual bool setStaticFriction(const size_t _index, double& staticFriction); - }; -} +#include #endif diff --git a/src/model/include/iDynTree/Model/SolidShapes.h b/src/model/include/iDynTree/Model/SolidShapes.h index 82e6963f6ee..3c16144bdec 100644 --- a/src/model/include/iDynTree/Model/SolidShapes.h +++ b/src/model/include/iDynTree/Model/SolidShapes.h @@ -1,306 +1,13 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef IDYNTREE_SOLID_SHAPES_H -#define IDYNTREE_SOLID_SHAPES_H +#ifndef IDYNTREE_MODEL_SOLID_SHAPES_H +#define IDYNTREE_MODEL_SOLID_SHAPES_H -#include -#include -#include -#include -#include - -namespace iDynTree -{ - class Material { - public: - explicit Material(); - explicit Material(const std::string& name); - - std::string name() const; - - bool hasColor() const; - Vector4 color() const; - void setColor(const Vector4& color); - - bool hasTexture() const; - std::string texture() const; - void setTexture(const std::string& texture); - - private: - Vector4 m_color; - bool m_isColorSet; - std::string m_texture; - std::string m_name; - }; - - class Sphere; - class Box; - class Cylinder; - class ExternalMesh; - class Model; - - class SolidShape - { - public: - explicit SolidShape(); - - virtual ~SolidShape()=0; - virtual SolidShape* clone()=0; - - /** - * Returns the name of the shape. - */ - const std::string& getName() const; - - /** - * Sets the specified name. - */ - void setName(const std::string& name); - - /** - * Returns if the name is valid. - */ - bool isNameValid() const; - - /** - * Returns the homogeneus transformation of the geometry w.r.t. the attached link. - */ - const Transform& getLink_H_geometry() const; - - /** - * Sets the homogeneus transformation of the geometry w.r.t. the attached link. - */ - void setLink_H_geometry(const Transform& newTransform); - - /** - * Returns if the material is valid, i.e. you can call getMaterial(). - */ - bool isMaterialSet() const; - - /** - * Returns the current material. - */ - const Material& getMaterial() const; - - /** - * Sets the material. isMaterialSet will return true after this call. - */ - void setMaterial(const Material& material); - - bool isSphere() const; - bool isBox() const; - bool isCylinder() const; - bool isExternalMesh() const; - - // Utility methods to traverse the SolidShape class hierachy. - Sphere* asSphere(); - Box *asBox(); - Cylinder* asCylinder(); - ExternalMesh* asExternalMesh(); - - const Sphere* asSphere() const; - const Box* asBox() const; - const Cylinder* asCylinder() const; - const ExternalMesh* asExternalMesh() const; - private: - std::string name; - /** - * True if the name is valid, false otherwise. - */ - bool nameIsValid; - - Transform link_H_geometry; - - /** - * Material of the geometry, encoded as a rgba vector. - */ - Vector4 material; - bool m_isMaterialSet; - Material m_material; - }; - - class Sphere: public SolidShape - { - public: - virtual ~Sphere(); - - virtual SolidShape* clone(); - - /** - * Returns the current radius. - */ - double getRadius() const; - - /** - * Sets the new radius. - */ - void setRadius(double radius); - - private: - double radius; - }; - - /** - * @brief Box, i.e. 3D rectangular parallelepiped. - * - * The box is centered in the mesh frame, its sides - * are aligned with the axis of the mesh frame, and - * the side lenghts in the x, y and z direction are given - * by the attributes x, y and z. - */ - class Box: public SolidShape - { - public: - virtual ~Box(); - virtual SolidShape* clone(); - - /** - * Returns the current x side length. - */ - double getX() const; - - /** - * Sets the x side length. - */ - void setX(double x); - - /** - * Returns the current y side length. - */ - double getY() const; - - /** - * Sets the y side length. - */ - void setY(double y); - - /** - * Returns the current z side length. - */ - double getZ() const; - - /** - * Sets the z side length. - */ - void setZ(double z); - - private: - double x; - double y; - double z; - }; - - class Cylinder: public SolidShape - { - public: - virtual ~Cylinder(); - virtual SolidShape* clone(); - - /** - * Returns the current cylinder length. - */ - double getLength() const; - - /** - * Sets the cylinder length. - */ - void setLength(double length); - - /** - * Returns the current cylinder radius. - */ - double getRadius() const; - - /** - * Sets the cylinder radius. - */ - void setRadius(double radius); - - private: - double length; - double radius; - }; - - class ExternalMesh: public SolidShape - { - public: - virtual ~ExternalMesh(); - virtual SolidShape* clone(); - - /** - * Returns the current filename. - */ - const std::string& getFilename() const; - - /** - * Returns the current package directories. - */ - const std::vector& getPackageDirs() const; - - /** - * Returns the filename substituting the prefix "package://" with the corresponding absolute path. - * The absolute path is determined by searching for the file using the paths specified in the - * packageDirs vector or if empty in the "GAZEBO_MODEL_PATH", "ROS_PACKAGE_PATH" and - * "AMENT_PREFIX_PATH" environmental variables. - */ - std::string getFileLocationOnLocalFileSystem() const; - - /** - * Sets the filename. - */ - void setFilename(const std::string& filename); - - /** - * Sets the the package directories. - * @note if not set the absolute path is determined by searching for the file using the - * paths specified in the "GAZEBO_MODEL_PATH", "ROS_PACKAGE_PATH" and "AMENT_PREFIX_PATH" - * environmental variables. - */ - void setPackageDirs(const std::vector& packageDirs); - - /** - * Returns the current scale. - */ - const iDynTree::Vector3& getScale() const; - - /** - * Sets the scale. - */ - void setScale(const iDynTree::Vector3& scale); - - private: - std::string filename; - std::vector packageDirs; - iDynTree::Vector3 scale; - }; - - class ModelSolidShapes - { - private: - ModelSolidShapes& copy(const ModelSolidShapes& other); - /** - * Storage ot ModelSolidShapes. - */ - std::vector< std::vector > linkSolidShapes; - - public: - ModelSolidShapes(); - ~ModelSolidShapes(); - - ModelSolidShapes(const ModelSolidShapes& other); - ModelSolidShapes& operator=(const ModelSolidShapes& other); - - void clear(); - void resize(size_t nrOfLinks); - void resize(const Model& model); - bool isConsistent(const Model & model) const; - - std::vector >& getLinkSolidShapes(); - - const std::vector >& getLinkSolidShapes() const; - }; +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif -} +#include #endif diff --git a/src/model/include/iDynTree/Model/SubModel.h b/src/model/include/iDynTree/Model/SubModel.h index 8f1f559af4d..002f5780fdf 100644 --- a/src/model/include/iDynTree/Model/SubModel.h +++ b/src/model/include/iDynTree/Model/SubModel.h @@ -1,149 +1,13 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef IDYNTREE_SUB_MODEL_H -#define IDYNTREE_SUB_MODEL_H +#ifndef IDYNTREE_MODEL_SUBMODEL_H +#define IDYNTREE_MODEL_SUBMODEL_H -#include - -#include -#include -#include - -namespace iDynTree -{ - class Model; - class Traversal; - class JointPosDoubleArray; - class LinkPositions; - class IRawVector; - - /** - * Class representing the decomposition in one model in several submodels. - * - * This class is used in algorithms, such as estimation of external wrenches, - * where a complete model is decomposed in several submodels. - * - * For each submodel a iDynTree::Traversal is provided, so that algorithms - * that are explicitly designed to run on both FullModel Traversal and SubModel - * traversal can be executed on the SubModel. - * - */ - class SubModelDecomposition - { - private: - /** - * Vector of size nrOfSubModels traversal, one for each submodel. - */ - std::vector subModelTraversals; - - /** - * Vector mapping link index to sub model index. - */ - std::vector link2subModelIndex; - - /** - * Copy constructor is forbidden - */ - SubModelDecomposition(const SubModelDecomposition & other); - - /** - * Copy operator is forbidden - */ - SubModelDecomposition& operator=(const SubModelDecomposition &other); - - public: - /** - * Constructor - */ - SubModelDecomposition(); - - /** - * Destructor - */ - ~SubModelDecomposition(); - - /** - * Create a submodel decomposition - * of a given model with a given full tree traversal, - * by separating the model across the given joints. - * - * @param[in] model the model to split - * @param[in] traversal the full model traversal to split - * @param[in] splitJoints the model will be split along - * the joints whose names are in this vector. - * @return true if all went fine, false otherwise - */ - bool splitModelAlongJoints(const Model & model, - const Traversal & traversal, - const std::vector& splitJoints); - - /** - * Set the number of the submodels in the decomposition. - */ - void setNrOfSubModels(const size_t nrOfSubModels); - - /** - * Get the number of submodels in the decomposition. - */ - size_t getNrOfSubModels() const; - - /** - * Get the total numer of links in the submodel decomposition. - */ - size_t getNrOfLinks() const; - - /** - * Get the traversal of a given submodel - */ - Traversal & getTraversal(const size_t subModelIndex); - - /** - * Get the traversal of a given submodel (const version) - */ - const Traversal & getTraversal(const size_t subModelIndex) const; - - /** - * Get the subModel to which a given links belongs. - */ - size_t getSubModelOfLink(const LinkIndex & link) const; - - /** - * Get the subModel to which a given frame belongs. - */ - size_t getSubModelOfFrame(const Model & model, const FrameIndex& frame) const; - - }; - - /** - * Helper loop to compute the position of each link - * wrt to the frame of the subModel base. - * - * @param[in] fullModel full model - * @param[in] traversal traversal on which to run the loop - * @param[in] jointPos joint positions for the full model - * @param[out] traversalBase_H_link traversalBase_H_link[i] will store the traversalBase_H_i transform - */ - void computeTransformToTraversalBase(const Model& fullModel, - const Traversal& traversal, - const JointPosDoubleArray& jointPos, - LinkPositions& traversalBase_H_link); - - /** - * Run the computeTransformToTraversalBase for all the - * traversal in the subModelDecomposition, and store the - * results in the linkPos array. - * - * @param[in] fullModel full model - * @param[in] subModelDecomposition model decomposition on which the loop will run - * @param[in] jointPos joint positions for the full model - * @param[out] subModelBase_H_link subModelBase_H_link[i] will store the subModelBase_H_i transform - */ - void computeTransformToSubModelBase(const Model& fullModel, - const SubModelDecomposition& subModelDecomposition, - const JointPosDoubleArray& jointPos, - LinkPositions& subModelBase_H_link); +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif -} +#include #endif diff --git a/src/model/include/iDynTree/Model/Traversal.h b/src/model/include/iDynTree/Model/Traversal.h index ecc6760f3f8..3c975755c33 100644 --- a/src/model/include/iDynTree/Model/Traversal.h +++ b/src/model/include/iDynTree/Model/Traversal.h @@ -1,216 +1,13 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef IDYNTREE_TRAVERSAL_H -#define IDYNTREE_TRAVERSAL_H +#ifndef IDYNTREE_MODEL_TRAVERSAL_H +#define IDYNTREE_MODEL_TRAVERSAL_H -#include +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif -#include +#include -namespace iDynTree -{ - class IJoint; - class Link; - class Model; - - /** - * Class that represents a traversal of a set of links of a Model. - * The traversal is represented by an ordered vector of links. - * For a given model, the traversal is always built in such a way - * that there exist a spanning tree that has as a root the first link - * (with traversal index 0) of the traversal, and such that every link comes - * after its spanning tree parent. - * - * For every link in the traversal are provided: - * * a pointer to the link itself - * * a pointer to the parent link in the spanning tree - * * a pointer to the joint connecting the link to the parent - * - * For the first link of the traversal (i.e. base of the traversal) - * there is not spanning tree parent, so the point to the parent link/joint are NULL. - * - * - * \ingroup iDynTreeModel - */ - class Traversal - { - private: - std::vector links; - std::vector parents; - std::vector toParentJoints; - std::vector linkIndexToTraversalIndex; - - /** - * Copy constructor is forbidden - */ - Traversal(const Traversal & other); - - /** - * Copy operator is forbidden - */ - Traversal& operator=(const Traversal &other); - - public: - Traversal(); - - ~Traversal(); - - /** - * Get the number of visited links. - * - * @return the number of links in the Traversal - */ - unsigned int getNrOfVisitedLinks() const; - - /** - * Get the traversalIndex-th link of the traversal. - * - * @return a pointer to the traversalIndex-th link of the traversal. - */ - const Link * getLink(const TraversalIndex traversalIndex) const; - - /** - * Get the base link of the traversal. - * - * @note this is equivalent to getLink(0), as the base link - * is by definition the first link of the traversal. - * @return a pointer to the base link of the traversal. - */ - const Link * getBaseLink() const; - - /** - * Get the parent link of the traversalIndex-th link of the traversal. - * - * @return a pointer to the parent link of the traversalIndex-th link of the traversal. - */ - const Link * getParentLink(const TraversalIndex traversalIndex) const; - - /** - * Get the joint connecting the traversalIndex-th link of the traversal - * to its parent. - * - * @return a pointer to the joint connecting the link traversalIndex-th link of the traversal. - */ - const IJoint * getParentJoint(const TraversalIndex traversalIndex) const; - - /** - * Get the parent link of the link with index linkIndex of the traversal. - * - * @return a pointer to the parent link of the traversalIndex-th link of the traversal. - */ - const Link * getParentLinkFromLinkIndex(const LinkIndex linkIndex) const; - - /** - * Get the joint connecting the link with index linkIndex - * to its parent. - * - * @return a pointer to the joint connecting the link traversalIndex-th link of the traversal. - */ - const IJoint * getParentJointFromLinkIndex(const LinkIndex linkIndex) const; - - /** - * Get the traversal index of the specified link - * - * @return the traversalIndex of the specified link, or TRAVERSAL_INDEX_INVALID if the link does not belong to the traversal. - */ - TraversalIndex getTraversalIndexFromLinkIndex(const LinkIndex linkIndex) const; - - - - /** - * Reset the Traversal. - * - * After a call to reset, the Traversal will contain - * no visited links, so it needs to be approprately populated - * with the addTraversalBase and addTraversalElement methods. - * - * @param[in] nrOfLinksInModel total number of links in the model, - * not the number of visited links in the Traversal. - * @return true if all went well, false otherwise - */ - bool reset(const unsigned int nrOfLinksInModel); - - /** - * Reset the Traversal. - * - * After a call to reset, all the pointers in the Traversal are set - * to 0, and the Traversal should be approprialy populated with the - * setters before use. - * - * @param[in] model Model on which this traversal will be used, - * used to initialize the internal data structure - * using the getNrOfLinks() method. - * - * @return true if all went well, false otherwise - */ - bool reset(const Model & model); - - /** - * Add a base to traversal. - * - * Equivalent to addTraversalElement(link,NULL,NULL), but - * will print an error and return false if it is called with a - * traversal that already has a base, i.e. getNrOfVisitedLinks() > 0 - * - * @param[in] link a pointer to the base link - * @return true if all went well, false otherwise - */ - bool addTraversalBase(const Link * link); - - /** - * Add an element to the traversal. - * - * After a call to this method, the getNrOfVisitedLinks() - * will be increased of 1 unit. - * - * @param[in] link a pointer to the link to add to the traversal - * @param[in] jointToParent a pointer to the joint that connects - * the added link to its parent in this traversal - * @param[in] parentLink a pointer to the parent on this link in this traversal. - * It should be a link already contained in this traversal. - * @return true if all went well, false otherwise. - */ - bool addTraversalElement(const Link * link, - const IJoint * jointToParent, - const Link * parentLink); - - /** - * \brief Check if a link is the parent of another link for this traversal. - * - * - * @param[in] parentCandidate the link candidate to be the parent of childCandidate. - * @param[in] childCandidate the link candidate to be a child of parentCandidate. - * @return true if parentCandidate is actually the parent of childCandidate, false otherwise. - */ - bool isParentOf(const LinkIndex parentCandidate, const LinkIndex childCandidate) const; - - /** - * \brief Get the child link (according to the traversal) of a Joint. - * - * @param[in] m_model the considered model. - * @param[in] jntIdx the index of the joint of which we want to get the child link. - * @return the index of the child link if all went well, LINK_INVALID_INDEX otherwise . - */ - LinkIndex getChildLinkIndexFromJointIndex(const Model & model, const JointIndex jntIdx) const; - - /** - * \brief Get the parent link (according to the traversal) of a Joint. - * - * @param[in] m_model the considered model. - * @param[in] jntIdx the index of the joint of which we want to get the parent link. - * @return the index of the parent link if all went well, LINK_INVALID_INDEX otherwise . - */ - LinkIndex getParentLinkIndexFromJointIndex(const Model & model, const JointIndex jntIdx) const; - - /** - * Return a human-readable representation of the traversal. - */ - std::string toString(const Model & model) const; - - }; - - -} - -#endif /* IDYNTREE_TRAVERSAL_H */ +#endif diff --git a/src/sensors/include/iDynTree/Sensors/ModelSensorsTransformers.h b/src/model/include/iDynTree/ModelSensorsTransformers.h similarity index 100% rename from src/sensors/include/iDynTree/Sensors/ModelSensorsTransformers.h rename to src/model/include/iDynTree/ModelSensorsTransformers.h diff --git a/src/model/include/iDynTree/ModelTestUtils.h b/src/model/include/iDynTree/ModelTestUtils.h new file mode 100644 index 00000000000..9af0de0740f --- /dev/null +++ b/src/model/include/iDynTree/ModelTestUtils.h @@ -0,0 +1,231 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_MODEL_TEST_UTILS_H +#define IDYNTREE_MODEL_TEST_UTILS_H + + +#include +#include +#include +#include +#include +#include + +#include + +#include +#include "IJoint.h" + +namespace iDynTree +{ + +inline Link getRandomLink() +{ + double cxx = getRandomDouble(0,3); + double cyy = getRandomDouble(0,4); + double czz = getRandomDouble(0,6); + double rotInertiaData[3*3] = {czz+cyy,0.0,0.0, + 0.0,cxx+czz,0.0, + 0.0,0.0,cxx+cyy}; + + Rotation rot = Rotation::RPY(getRandomDouble(),getRandomDouble(-1,1),getRandomDouble()); + + SpatialInertia inertiaLink(getRandomDouble(0,4), + Position(getRandomDouble(-2,2),getRandomDouble(-2,2),getRandomDouble(-2,2)), + rot*RotationalInertiaRaw(rotInertiaData,3,3)); + + Link link; + + link.setInertia(inertiaLink); + + return link; +} + +/** + * Add a random link with random model. + */ +inline void addRandomLinkToModel(Model & model, std::string parentLink, std::string newLinkName, bool onlyRevoluteJoints=false) +{ + // Add Link + LinkIndex newLinkIndex = model.addLink(newLinkName,getRandomLink()); + + // Now add joint + LinkIndex parentLinkIndex = model.getLinkIndex(parentLink); + + int nrOfJointTypes = 3; + + int jointType = rand() % nrOfJointTypes; + + if (onlyRevoluteJoints) + { + jointType = 1; + } + + if( jointType == 0 ) + { + FixedJoint fixJoint(parentLinkIndex,newLinkIndex,getRandomTransform()); + model.addJoint(newLinkName+"joint",&fixJoint); + } + else if( jointType == 1 ) + { + RevoluteJoint revJoint; + revJoint.setAttachedLinks(parentLinkIndex,newLinkIndex); + revJoint.setRestTransform(getRandomTransform()); + revJoint.setAxis(getRandomAxis(),newLinkIndex); + model.addJoint(newLinkName+"joint",&revJoint); + } + else if( jointType == 2 ) + { + PrismaticJoint prismJoint; + prismJoint.setAttachedLinks(parentLinkIndex,newLinkIndex); + prismJoint.setRestTransform(getRandomTransform()); + prismJoint.setAxis(getRandomAxis(),newLinkIndex); + model.addJoint(newLinkName+"joint",&prismJoint); + } + else + { + assert(false); + } +} + +/** + * Add a random additional frame to a model model. + */ +inline void addRandomAdditionalFrameToModel(Model & model, std::string parentLink, std::string newFrameName) +{ + model.addAdditionalFrameToLink(parentLink,newFrameName,getRandomTransform()); +} + +inline LinkIndex getRandomLinkIndexOfModel(const Model & model) +{ + int nrOfLinks = model.getNrOfLinks(); + + return rand() % nrOfLinks; +} + +inline std::string getRandomLinkOfModel(const Model & model) +{ + LinkIndex randomLink = getRandomLinkIndexOfModel(model); + + return model.getLinkName(randomLink); +} + +inline std::string int2string(int i) +{ + std::stringstream ss; + + ss << i; + + return ss.str(); +} + +inline Model getRandomModel(unsigned int nrOfJoints, size_t nrOfAdditionalFrames = 10) +{ + Model model; + + model.addLink("baseLink",getRandomLink()); + + for(unsigned int i=0; i < nrOfJoints; i++) + { + std::string parentLink = getRandomLinkOfModel(model); + std::string linkName = "link" + int2string(i); + addRandomLinkToModel(model,parentLink,linkName); + } + + for(unsigned int i=0; i < nrOfAdditionalFrames; i++) + { + std::string parentLink = getRandomLinkOfModel(model); + std::string frameName = "additionalFrame" + int2string(i); + addRandomAdditionalFrameToModel(model,parentLink,frameName); + } + + return model; +} + +inline Model getRandomChain(unsigned int nrOfJoints, size_t nrOfAdditionalFrames = 10, bool onlyRevoluteJoints=false) +{ + Model model; + + model.addLink("baseLink",getRandomLink()); + + std::string linkName = "baseLink"; + for(unsigned int i=0; i < nrOfJoints; i++) + { + std::string parentLink = linkName; + linkName = "link" + int2string(i); + addRandomLinkToModel(model,parentLink,linkName,onlyRevoluteJoints); + } + + for(unsigned int i=0; i < nrOfAdditionalFrames; i++) + { + std::string parentLink = getRandomLinkOfModel(model); + std::string frameName = "additionalFrame" + int2string(i); + addRandomAdditionalFrameToModel(model,parentLink,frameName); + } + + return model; +} + +/** + * Get random joint position consistently with the limits of the model. + * If the input vector has the wrong size, it will be resized. + */ +inline void getRandomJointPositions(VectorDynSize& vec, const Model& model) +{ + vec.resize(model.getNrOfPosCoords()); + for(JointIndex jntIdx=0; jntIdx < model.getNrOfJoints(); jntIdx++) + { + IJointConstPtr jntPtr = model.getJoint(jntIdx); + if( jntPtr->hasPosLimits() ) + { + for(int i=0; i < jntPtr->getNrOfPosCoords(); i++) + { + double max = jntPtr->getMaxPosLimit(i); + double min = jntPtr->getMinPosLimit(i); + vec(jntPtr->getDOFsOffset()+i) = getRandomDouble(min,max); + } + } + else + { + for(int i=0; i < jntPtr->getNrOfPosCoords(); i++) + { + vec(jntPtr->getDOFsOffset()+i) = getRandomDouble(); + } + } + } + + return; +} + +/** + * Get random robot positions, velocities and accelerations + * and external wrenches to be given as an input to InverseDynamics. + */ +inline bool getRandomInverseDynamicsInputs(FreeFloatingPos& pos, + FreeFloatingVel& vel, + FreeFloatingAcc& acc, + LinkNetExternalWrenches& extWrenches) +{ + pos.worldBasePos() = getRandomTransform(); + vel.baseVel() = getRandomTwist(); + acc.baseAcc() = getRandomTwist(); + + + for(unsigned int jnt=0; jnt < pos.getNrOfPosCoords(); jnt++) + { + pos.jointPos()(jnt) = getRandomDouble(); + } + + for(unsigned int jnt=0; jnt < vel.getNrOfDOFs(); jnt++) + { + vel.jointVel()(jnt) = getRandomDouble(); + acc.jointAcc()(jnt) = getRandomDouble(); + } + + return true; +} + +} + +#endif /* IDYNTREE_MODEL_TEST_UTILS_H */ diff --git a/src/model/include/iDynTree/ModelTransformers.h b/src/model/include/iDynTree/ModelTransformers.h new file mode 100644 index 00000000000..87dc9ffab2f --- /dev/null +++ b/src/model/include/iDynTree/ModelTransformers.h @@ -0,0 +1,100 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +/** + * \file ModelTransformers.h + * \brief Collection of function to modify model in various ways + * + * + * In this file a series of functions for transforming Model + * objects are provided +*/ + + +#ifndef IDYNTREE_MODEL_TRANSFORMERS_H +#define IDYNTREE_MODEL_TRANSFORMERS_H + +namespace iDynTree +{ +class Model; +class SensorsList; + +/** + * \function Remove all fake links in the model, transforming them in frames. + * + * Given a Model in input, this function copies all its links + * and joints to the model in output, except for links that recognized + * as "fake links". + * + * The condition for a link to be classified as "fake link" are: + * * The link has a zero mass. + * * The link is a leaf, i.e. it is connected to only one neighbor. + * * The link is connected to its only neighbor with a fixed joint. + * + * Once a "fake link" has been identified to respect this two conditions, + * it and the joint that it connects it to its only neighbor is not copied + * to the output model, but a frame with the same name of the "fake link" + * and with the same transform is added to the model. + * + * \note The definition of "fake link" used in this function excludes + * the case in which two fake links are attached to one another. + * + */ +bool removeFakeLinks(const Model& modelWithFakeLinks, + Model& modelWithoutFakeLinks); + +/** + * This function takes in input a iDynTree::Model and + * an ordered list of joints and returns a model with + * just the joint specified in the list, with that exact order. + * + * All other joints are be removed by lumping (i.e. fusing together) + * the inertia of the links that are connected by that joint, assuming the joint + * to be in "rest" position (i.e. zero for revolute or prismatic joints). The links eliminated + * with this process are be added back to the reduced model as "frames", + * and are copied in the same way all the additional frames of the lumped links. + * + */ +bool createReducedModel(const Model& fullModel, + const std::vector& jointsInReducedModel, + Model& reducedModel); + +/** + * @brief Given a specified base, return a model with a "normalized" joint numbering for that base. + * + * This function takes in input a iDynTree::Model and a name of a link in that model. + * It returns a model identical to the one in input, but with the joint serialization + * of the non-fixed joint modified in such a way that a non-fixed joint has an index higher than + * all the non-fixed joints on the path between it and the base. After all the non-fixed joints, + * the fixed joints are also added with the same criteria, but applied to fixed joints. + * + * @note This method make sure that the non-fixed joint in the model have a "regular numbering" + * as described in Featherstone "Rigid Body Dynamics Algorithm", section 4.1.2 . Note that + * this numbering is not required by any algorithm in iDynTree, but it may be useful for + * example to ensure that for a chain model the joint numbering is the one induced by the + * kinematic structure. + * + * @return true if all went well, false if there was an error in conversion. + */ +bool createModelWithNormalizedJointNumbering(const Model& model, + const std::string& baseForNormalizedJointNumbering, + Model& reducedModel); + + +/** + * Extract sub model from sub model traversal. + * + * This function creates a new iDynTree::Model containing links and joints composing the subModelTraversal. + * The function takes in input a iDynTree::Model and a iDynTree::Traversal. The new model will contain joints + * and links composing the subModelTraversal, with the same order. + * The FT sensor frames are added as additional frames. + * + * @return true if all went well, false if there was an error in creating the sub model. + */ +bool extractSubModel(const iDynTree::Model& fullModel, const iDynTree::Traversal& subModelTraversal, + iDynTree::Model& outputSubModel); + +} + + +#endif diff --git a/src/model/include/iDynTree/MovableJointImpl.h b/src/model/include/iDynTree/MovableJointImpl.h new file mode 100644 index 00000000000..4b2b7126453 --- /dev/null +++ b/src/model/include/iDynTree/MovableJointImpl.h @@ -0,0 +1,165 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_MOVABLE_JOINT_IMPL_H +#define IDYNTREE_MOVABLE_JOINT_IMPL_H + + + +#include + + +namespace iDynTree +{ + /** + * Base template for implementation of non-fixed joints. + * A specific joint can be derived from an instantiation of this template. + * + * For more information on the assumption of the joint model, check the IJoint interface. + * + * \ingroup iDynTreeModel + */ + template + class MovableJointImpl: public IJoint + { + protected: + JointIndex m_index; + size_t m_posCoordsOffset; + size_t m_DOFsOffset; + + public: + /** + * Denstructor + * + */ + virtual ~MovableJointImpl() = 0; + + // Documentation inherited + virtual unsigned int getNrOfPosCoords() const; + + // Documentation inherited + virtual unsigned int getNrOfDOFs() const; + + /** + * Get the motion subspace matrix, i.e. the matrix that + * maps the joint velocity to the relative twist between the two + * links. + * In particular the motion subspace matrix is the S matrix such that + * v_linkA = S_{linkA,linkB}*dq + linkA_X_linkB*v_linkB + * where dq is the joint velocity. + * + * @return the motion subspace matrix + * + * \note The motion subspace matrix is also known in literature as hinge matrix, + * hinge map matrix, joint map matrix or joint motion map matrix. + */ + //virtual MatrixFixSize<6, nrOfDOFs> getMotionSubspace(const JointPos & state, const int linkA, const int linkB) const = 0; + + /** + * Get the motion subspace matrix derivatives, i.e. the matrix that + * maps the joint velocities to the relative spatial acceleration between the two + * links. + * In particular the motion subspace matrix derivative is the dS matrix such that + * a_linkA = S_{linkA,linkB}*ddq + dS_{linkA,linkB}*dq + linkA_X_linkB*a_linkB + * where ddq is the joint acceleration and dq is the joint velocity. + * + * @return the motion subspace matrix + * + * \note The motion subspace matrix is also known in literature as hinge matrix, + * hinge map matrix, joint map matrix or joint motion map matrix. + */ + //virtual MatrixFixSize<6, nrOfDOFs> getMotionSubspaceDerivative(const JointPosVel & state, const int linkA, const int linkB) const = 0; + + /** + * Get the kinematic coupling matrix, i.e. the matrix that maps + * the joint velocities to the derivative of the joint coordinates. + * + * \note For the joint whose configuration is in R^n, the kinematic coupling matrix + * is equal to the identity. + + * @return the nrOfPosCoords times nrOfDOFs kinematic coupling matrix + */ + //virtual MatrixFixSize getKinematicCouplingMatrix(const JointPos & state) = 0; + + // Documentation inherited + virtual void setIndex(JointIndex & _index); + + // Documentation inherited + virtual JointIndex getIndex() const; + + // Documentation inherited + virtual void setPosCoordsOffset(const size_t _offset); + + // Documentation inherited + virtual size_t getPosCoordsOffset() const; + + // Documentation inherited + virtual void setDOFsOffset(const size_t _offset); + + // Documentation inherited + virtual size_t getDOFsOffset() const; + }; + + template + unsigned int MovableJointImpl::getNrOfPosCoords() const + { + return nrOfPosCoords; + } + + template + unsigned int MovableJointImpl::getNrOfDOFs() const + { + return nrOfDOFs; + } + + template + void MovableJointImpl::setIndex(JointIndex & _index) + { + this->m_index = _index; + } + + template + JointIndex MovableJointImpl::getIndex() const + { + return this->m_index; + } + + template + void MovableJointImpl::setPosCoordsOffset(const size_t _offset) + { + this->m_posCoordsOffset = _offset; + } + + template + size_t MovableJointImpl::getPosCoordsOffset() const + { + return this->m_posCoordsOffset; + } + + template + void MovableJointImpl::setDOFsOffset(const size_t _offset) + { + this->m_DOFsOffset = _offset; + } + + template + size_t MovableJointImpl::getDOFsOffset() const + { + return this->m_DOFsOffset; + } + + + template + MovableJointImpl::~MovableJointImpl() + { + } + + typedef MovableJointImpl<1,1> MovableJointImpl1; + typedef MovableJointImpl<2,2> MovableJointImpl2; + typedef MovableJointImpl<3,3> MovableJointImpl3; + typedef MovableJointImpl<4,4> MovableJointImpl4; + typedef MovableJointImpl<5,5> MovableJointImpl5; + typedef MovableJointImpl<6,6> MovableJointImpl6; +} + +#endif /* IDYNTREE_JOINT_IMPL_H */ diff --git a/src/sensors/include/iDynTree/Sensors/PredictSensorsMeasurements.h b/src/model/include/iDynTree/PredictSensorsMeasurements.h similarity index 98% rename from src/sensors/include/iDynTree/Sensors/PredictSensorsMeasurements.h rename to src/model/include/iDynTree/PredictSensorsMeasurements.h index a10e964f02b..76bb4037a8f 100644 --- a/src/sensors/include/iDynTree/Sensors/PredictSensorsMeasurements.h +++ b/src/model/include/iDynTree/PredictSensorsMeasurements.h @@ -4,9 +4,9 @@ #ifndef PREDICTSENSORSMEASUREMENTS_HPP #define PREDICTSENSORSMEASUREMENTS_HPP -#include +#include -#include +#include namespace iDynTree { diff --git a/src/model/include/iDynTree/PrismaticJoint.h b/src/model/include/iDynTree/PrismaticJoint.h new file mode 100644 index 00000000000..8630d469023 --- /dev/null +++ b/src/model/include/iDynTree/PrismaticJoint.h @@ -0,0 +1,217 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_PRISMATIC_JOINT_H +#define IDYNTREE_PRISMATIC_JOINT_H + +#include +#include + +#include +#include +#include + +namespace iDynTree +{ + /** + * Class representing a prismatic joint, i.e. a joint that + * constraint two links to translate only along an axis. + * + * \ingroup iDynTreeModel + */ + class PrismaticJoint : public MovableJointImpl1 + { + private: + // Structure attributes + LinkIndex link1; + LinkIndex link2; + Transform link1_X_link2_at_rest; + Axis translation_axis_wrt_link1; + + // Limits + void disablePosLimits(); + bool m_hasPosLimits; + double m_minPos; + double m_maxPos; + + // Dynamic parameters + void resetJointDynamics(); + JointDynamicsType m_joint_dynamics_type; + double m_damping; + double m_static_friction; + + // Cache attributes + mutable double q_previous; + mutable Transform link1_X_link2; + mutable Transform link2_X_link1; + mutable SpatialMotionVector S_link1_link2; + mutable SpatialMotionVector S_link2_link1; + + void updateBuffers(const double new_q) const; + void resetBuffers(const double new_q) const; + void resetAxisBuffers() const; + + public: + /** + * Constructor + */ + PrismaticJoint(); + + IDYNTREE_DEPRECATED_WITH_MSG("Please use the setter methods to specify the parameters of the joint") + PrismaticJoint(const LinkIndex link1, const LinkIndex link2, + const Transform& link1_X_link2, const Axis& _translation_axis_wrt_link1); + + /** + * Copy constructor + */ + PrismaticJoint(const PrismaticJoint& other); + + /** + * Destructor + */ + virtual ~PrismaticJoint(); + + // Documentation inherited + virtual IJoint * clone() const; + + // Documentation inherited + virtual void setAttachedLinks(const LinkIndex link1, const LinkIndex link2); + + // Documentation inherited + virtual void setRestTransform(const Transform& link1_X_link2); + + /** + * Set the prismatic axis of the joint, expressed in specified link frame, that is considered the "child" + * frame regarding the sign of the axis. + * + * See getAxis method for more information. + * + * @warning This method should be called after a valid restTransform between link1 and link2 has been + * set by calling the setRestTransform method. + */ + virtual void setAxis(const Axis& prismaticAxis, + const LinkIndex child, + const LinkIndex parent=LINK_INVALID_INDEX); + + // Set the prismatic axis expressed in link1 + IDYNTREE_DEPRECATED_WITH_MSG("Please use the setAxis method in which the link considered \"child\" is explicitly specified") + virtual void setAxis(const Axis& prismaticAxis_wrt_link1); + + // Documentation inherited + virtual LinkIndex getFirstAttachedLink() const; + + // Documentation inherited + virtual LinkIndex getSecondAttachedLink() const; + + /** + * Get the revolute axis of the robot, expressed in linkA frame. + * + * @param child the link frame (one of the two at which the link is attached) + * in which the returned axis is expressed. Furthermore, the + * axis direction depends on the assumption that this frame is + * considered the "child" in the relationship. + * + * See + * + * Seth, A., Sherman, M., Eastman, P., & Delp, S. (2010). + * Minimal formulation of joint motion for biomechanisms. + * Nonlinear Dynamics, 62(1), 291-303. + * https://nmbl.stanford.edu/publications/pdf/Seth2010.pdf + * Section 2.4 + * + * and + * + * "Modelling, Estimation and Identification of Humanoid Robots Dynamics" + * Traversaro - Section 3.2 + * https://traversaro.github.io/preprints/traversaro-phd-thesis.pdf + * + * for more details. + */ + virtual Axis getAxis(const LinkIndex child, + const LinkIndex parent=LINK_INVALID_INDEX) const; + + // Documentation inherited + virtual Transform getRestTransform(const LinkIndex child, + const LinkIndex parent) const; + + + // Documentation inherited + virtual const Transform & getTransform(const VectorDynSize & jntPos, + const LinkIndex child, + const LinkIndex parent) const; + + // Documentation inherited + TransformDerivative getTransformDerivative(const VectorDynSize & jntPos, + const LinkIndex child, + const LinkIndex parent, + const int posCoord_i) const; + + // Documentation inherited + virtual SpatialMotionVector getMotionSubspaceVector(int dof_i, + const LinkIndex child, + const LinkIndex parent=LINK_INVALID_INDEX) const; + + // Documentation inherited + virtual void computeChildPosVelAcc(const VectorDynSize & jntPos, + const VectorDynSize & jntVel, + const VectorDynSize & jntAcc, + LinkPositions & linkPositions, + LinkVelArray & linkVels, + LinkAccArray & linkAccs, + const LinkIndex child, const LinkIndex parent) const; + + // Documentation inherited + virtual void computeChildVel(const VectorDynSize & jntPos, + const VectorDynSize & jntVel, + LinkVelArray & linkVels, + const LinkIndex child, const LinkIndex parent) const; + + // Documentation inherited + virtual void computeChildVelAcc(const VectorDynSize & jntPos, + const VectorDynSize & jntVel, + const VectorDynSize & jntAcc, + LinkVelArray & linkVels, + LinkAccArray & linkAccs, + const LinkIndex child, const LinkIndex parent) const; + + // Documentation inherited + virtual void computeChildAcc(const VectorDynSize & jntPos, + const VectorDynSize & jntVel, + const LinkVelArray & linkVels, + const VectorDynSize & jntAcc, + LinkAccArray & linkAccs, + const LinkIndex child, + const LinkIndex parent) const; + + // Documentation inherited + virtual void computeChildBiasAcc(const VectorDynSize & jntPos, + const VectorDynSize & jntVel, + const LinkVelArray & linkVels, + LinkAccArray & linkBiasAccs, + const LinkIndex child, + const LinkIndex parent) const; + + // Documentation inherited + virtual void computeJointTorque(const VectorDynSize & jntPos, const Wrench & internalWrench, + const LinkIndex linkThatAppliesWrench, const LinkIndex linkOnWhichWrenchIsApplied, + VectorDynSize & jntTorques) const; + + // LIMITS METHODS + virtual bool hasPosLimits() const; + virtual bool enablePosLimits(const bool enable); + virtual bool getPosLimits(const size_t _index, double & min, double & max) const; + virtual double getMinPosLimit(const size_t _index) const; + virtual double getMaxPosLimit(const size_t _index) const; + virtual bool setPosLimits(const size_t _index, double & min, double & max); + + // DYNAMICS METHODS + virtual JointDynamicsType getJointDynamicsType() const; + virtual bool setJointDynamicsType(const JointDynamicsType enable); + virtual double getDamping(const size_t _index) const; + virtual double getStaticFriction(const size_t _index) const; + virtual bool setDamping(const size_t _index, double& damping); + virtual bool setStaticFriction(const size_t _index, double& staticFriction); + }; +} + +#endif diff --git a/src/model/include/iDynTree/RevoluteJoint.h b/src/model/include/iDynTree/RevoluteJoint.h new file mode 100644 index 00000000000..3dec9e70724 --- /dev/null +++ b/src/model/include/iDynTree/RevoluteJoint.h @@ -0,0 +1,225 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_REVOLUTE_JOINT_H +#define IDYNTREE_REVOLUTE_JOINT_H + +#include +#include + +#include +#include +#include + +namespace iDynTree +{ + /** + * Class representing a revolute joint, i.e. a joint that + * constraint two links to move only around a common axis. + * + * \ingroup iDynTreeModel + */ + class RevoluteJoint : public MovableJointImpl1 + { + private: + // Structure attributes + LinkIndex link1; + LinkIndex link2; + Transform link1_X_link2_at_rest; + Axis rotation_axis_wrt_link1; + + // Limits + void disablePosLimits(); + bool m_hasPosLimits; + double m_minPos; + double m_maxPos; + + // Dynamic parameters + void resetJointDynamics(); + JointDynamicsType m_joint_dynamics_type; + double m_damping; + double m_static_friction; + + // Cache attributes + mutable double q_previous; + mutable Transform link1_X_link2; + mutable Transform link2_X_link1; + mutable SpatialMotionVector S_link1_link2; + mutable SpatialMotionVector S_link2_link1; + + void updateBuffers(const double new_q) const; + void resetBuffers(const double new_q) const; + void resetAxisBuffers() const; + + public: + /** + * Constructor + */ + RevoluteJoint(); + + IDYNTREE_DEPRECATED_WITH_MSG("Please use the setter methods to specify the parameters of the joint") + RevoluteJoint(const LinkIndex link1, const LinkIndex link2, + const Transform& link1_X_link2, const Axis& _rotation_axis_wrt_link1); + + /** + * Constructor in which the LinkIndex to which the joint is attached are not specified. + * This constructor is tipically used together with the Model::addJoint or + * Model::addJointAndLink methods, in which the links to which the joint is attached are + * specified by the other arguments of the method. + */ + RevoluteJoint(const Transform& link1_X_link2, const Axis& _rotation_axis_wrt_link1); + + /** + * Copy constructor + */ + RevoluteJoint(const RevoluteJoint& other); + + /** + * Destructor + */ + virtual ~RevoluteJoint(); + + // Documentation inherited + virtual IJoint * clone() const; + + // Documentation inherited + virtual void setAttachedLinks(const LinkIndex link1, const LinkIndex link2); + + // Documentation inherited + virtual void setRestTransform(const Transform& link1_X_link2); + + /** + * Set the revolute axis of the joint, expressed in specified link frame, that is considered the "child" + * frame regarding the sign of the axis. + * + * See getAxis method for more information. + * + * @warning This method should be called after a valid restTransform between link1 and link2 has been + * set by calling the setRestTransform method. + */ + virtual void setAxis(const Axis& revoluteAxis, + const LinkIndex child, + const LinkIndex parent=LINK_INVALID_INDEX); + + // Set the revolute axis expressed in link1 + IDYNTREE_DEPRECATED_WITH_MSG("Please use the setAxis method in which the link considered \"child\" is explicitly specified") + virtual void setAxis(const Axis& revoluteAxis_wrt_link1); + + // Documentation inherited + virtual LinkIndex getFirstAttachedLink() const; + + // Documentation inherited + virtual LinkIndex getSecondAttachedLink() const; + + /** + * Get the revolute axis of the robot, expressed in child frame. + * + * @param child the link frame (one of the two at which the link is attached) + * in which the returned axis is expressed. Furthermore, the + * axis direction depends on the assumption that this frame is + * considered the "child" in the relationship. + * + * See + * + * Seth, A., Sherman, M., Eastman, P., & Delp, S. (2010). + * Minimal formulation of joint motion for biomechanisms. + * Nonlinear Dynamics, 62(1), 291-303. + * https://nmbl.stanford.edu/publications/pdf/Seth2010.pdf + * Section 2.4 + * + * and + * + * "Modelling, Estimation and Identification of Humanoid Robots Dynamics" + * Traversaro - Section 3.2 + * https://traversaro.github.io/preprints/traversaro-phd-thesis.pdf + * + * for more details. + */ + virtual Axis getAxis(const LinkIndex child, + const LinkIndex parent=LINK_INVALID_INDEX) const; + + // Documentation inherited + virtual Transform getRestTransform(const LinkIndex child, + const LinkIndex parent) const; + + + // Documentation inherited + virtual const Transform & getTransform(const VectorDynSize & jntPos, + const LinkIndex child, + const LinkIndex parent) const; + + // Documentation inherited + TransformDerivative getTransformDerivative(const VectorDynSize & jntPos, + const LinkIndex child, + const LinkIndex parent, + const int posCoord_i) const; + + // Documentation inherited + virtual SpatialMotionVector getMotionSubspaceVector(int dof_i, + const LinkIndex child, + const LinkIndex parent=LINK_INVALID_INDEX) const; + + // Documentation inherited + virtual void computeChildPosVelAcc(const VectorDynSize & jntPos, + const VectorDynSize & jntVel, + const VectorDynSize & jntAcc, + LinkPositions & linkPositions, + LinkVelArray & linkVels, + LinkAccArray & linkAccs, + const LinkIndex child, const LinkIndex parent) const; + + // Documentation inherited + virtual void computeChildVel(const VectorDynSize & jntPos, + const VectorDynSize & jntVel, + LinkVelArray & linkVels, + const LinkIndex child, const LinkIndex parent) const; + + // Documentation inherited + virtual void computeChildVelAcc(const VectorDynSize & jntPos, + const VectorDynSize & jntVel, + const VectorDynSize & jntAcc, + LinkVelArray & linkVels, + LinkAccArray & linkAccs, + const LinkIndex child, const LinkIndex parent) const; + + // Documentation inherited + virtual void computeChildAcc(const VectorDynSize & jntPos, + const VectorDynSize & jntVel, + const LinkVelArray & linkVels, + const VectorDynSize & jntAcc, + LinkAccArray & linkAccs, + const LinkIndex child, + const LinkIndex parent) const; + + // Documentation inherited + virtual void computeChildBiasAcc(const VectorDynSize & jntPos, + const VectorDynSize & jntVel, + const LinkVelArray & linkVels, + LinkAccArray & linkBiasAccs, + const LinkIndex child, + const LinkIndex parent) const; + + // Documentation inherited + virtual void computeJointTorque(const VectorDynSize & jntPos, const Wrench & internalWrench, + const LinkIndex linkThatAppliesWrench, const LinkIndex linkOnWhichWrenchIsApplied, + VectorDynSize & jntTorques) const; + + // LIMITS METHODS + virtual bool hasPosLimits() const; + virtual bool enablePosLimits(const bool enable); + virtual bool getPosLimits(const size_t _index, double & min, double & max) const; + virtual double getMinPosLimit(const size_t _index) const; + virtual double getMaxPosLimit(const size_t _index) const; + virtual bool setPosLimits(const size_t _index, double & min, double & max); + + // DYNAMICS METHODS + virtual JointDynamicsType getJointDynamicsType() const; + virtual bool setJointDynamicsType(const JointDynamicsType enable); + virtual double getDamping(const size_t _index) const; + virtual double getStaticFriction(const size_t _index) const; + virtual bool setDamping(const size_t _index, double& damping); + virtual bool setStaticFriction(const size_t _index, double& staticFriction); + }; +} + +#endif diff --git a/src/sensors/include/iDynTree/Sensors/Sensors.h b/src/model/include/iDynTree/Sensors.h similarity index 99% rename from src/sensors/include/iDynTree/Sensors/Sensors.h rename to src/model/include/iDynTree/Sensors.h index 673dc37340d..f37b7650b2c 100644 --- a/src/sensors/include/iDynTree/Sensors/Sensors.h +++ b/src/model/include/iDynTree/Sensors.h @@ -4,7 +4,7 @@ #ifndef IDYNTREE_CORE_SENSORS_HPP #define IDYNTREE_CORE_SENSORS_HPP -#include +#include namespace iDynTree { @@ -17,10 +17,10 @@ namespace iDynTree { #include #include -#include +#include -#include -#include +#include +#include namespace iDynTree { diff --git a/src/model/include/iDynTree/Sensors/AccelerometerSensor.h b/src/model/include/iDynTree/Sensors/AccelerometerSensor.h new file mode 100644 index 00000000000..d36fc5f8667 --- /dev/null +++ b/src/model/include/iDynTree/Sensors/AccelerometerSensor.h @@ -0,0 +1,13 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_SENSORS_ACCELEROMETER_SENSOR_H +#define IDYNTREE_SENSORS_ACCELEROMETER_SENSOR_H + +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif + +#include + +#endif diff --git a/src/model/include/iDynTree/Sensors/AllSensorsTypes.h b/src/model/include/iDynTree/Sensors/AllSensorsTypes.h new file mode 100644 index 00000000000..82e053a3a71 --- /dev/null +++ b/src/model/include/iDynTree/Sensors/AllSensorsTypes.h @@ -0,0 +1,13 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_SENSORS_ALL_SENSORS_TYPES_H +#define IDYNTREE_SENSORS_ALL_SENSORS_TYPES_H + +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif + +#include + +#endif diff --git a/src/model/include/iDynTree/Sensors/GyroscopeSensor.h b/src/model/include/iDynTree/Sensors/GyroscopeSensor.h new file mode 100644 index 00000000000..73a2714b7b1 --- /dev/null +++ b/src/model/include/iDynTree/Sensors/GyroscopeSensor.h @@ -0,0 +1,13 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_SENSORS_GYROSCOPE_SENSOR_H +#define IDYNTREE_SENSORS_GYROSCOPE_SENSOR_H + +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif + +#include + +#endif diff --git a/src/model/include/iDynTree/Sensors/ModelSensorsTransformers.h b/src/model/include/iDynTree/Sensors/ModelSensorsTransformers.h new file mode 100644 index 00000000000..26cb92809c6 --- /dev/null +++ b/src/model/include/iDynTree/Sensors/ModelSensorsTransformers.h @@ -0,0 +1,13 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_SENSORS_MODEL_SENSORS_TRANSFORMERS_H +#define IDYNTREE_SENSORS_MODEL_SENSORS_TRANSFORMERS_H + +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif + +#include + +#endif diff --git a/src/model/include/iDynTree/Sensors/PredictSensorsMeasurements.h b/src/model/include/iDynTree/Sensors/PredictSensorsMeasurements.h new file mode 100644 index 00000000000..712393c242d --- /dev/null +++ b/src/model/include/iDynTree/Sensors/PredictSensorsMeasurements.h @@ -0,0 +1,13 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_SENSORS_PREDICT_SENSORS_MEASUREMENTS_H +#define IDYNTREE_SENSORS_PREDICT_SENSORS_MEASUREMENTS_H + +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif + +#include + +#endif diff --git a/src/model/include/iDynTree/Sensors/Sensors.h b/src/model/include/iDynTree/Sensors/Sensors.h new file mode 100644 index 00000000000..dff3dcd7100 --- /dev/null +++ b/src/model/include/iDynTree/Sensors/Sensors.h @@ -0,0 +1,13 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_SENSORS_SENSORS_H +#define IDYNTREE_SENSORS_SENSORS_H + +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif + +#include + +#endif diff --git a/src/model/include/iDynTree/Sensors/SixAxisForceTorqueSensor.h b/src/model/include/iDynTree/Sensors/SixAxisForceTorqueSensor.h new file mode 100644 index 00000000000..f3de33e008f --- /dev/null +++ b/src/model/include/iDynTree/Sensors/SixAxisForceTorqueSensor.h @@ -0,0 +1,13 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_SENSORS_SIX_AXIS_FT_H +#define IDYNTREE_SENSORS_SIX_AXIS_FT_H + +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif + +#include + +#endif diff --git a/src/model/include/iDynTree/Sensors/ThreeAxisAngularAccelerometerSensor.h b/src/model/include/iDynTree/Sensors/ThreeAxisAngularAccelerometerSensor.h new file mode 100644 index 00000000000..446ab61fea6 --- /dev/null +++ b/src/model/include/iDynTree/Sensors/ThreeAxisAngularAccelerometerSensor.h @@ -0,0 +1,13 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_SENSORS_3D_ANG_ACC_H +#define IDYNTREE_SENSORS_3D_ANG_ACC_H + +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif + +#include + +#endif diff --git a/src/model/include/iDynTree/Sensors/ThreeAxisForceTorqueContactSensor.h b/src/model/include/iDynTree/Sensors/ThreeAxisForceTorqueContactSensor.h new file mode 100644 index 00000000000..661e03d3a6d --- /dev/null +++ b/src/model/include/iDynTree/Sensors/ThreeAxisForceTorqueContactSensor.h @@ -0,0 +1,13 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_MODEL_3D_FT_SENSOR_H +#define IDYNTREE_MODEL_3D_FT_SENSOR_H + +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif + +#include + +#endif diff --git a/src/sensors/include/iDynTree/Sensors/SixAxisForceTorqueSensor.h b/src/model/include/iDynTree/SixAxisForceTorqueSensor.h similarity index 99% rename from src/sensors/include/iDynTree/Sensors/SixAxisForceTorqueSensor.h rename to src/model/include/iDynTree/SixAxisForceTorqueSensor.h index 9f5d1949d60..572117f4775 100644 --- a/src/sensors/include/iDynTree/Sensors/SixAxisForceTorqueSensor.h +++ b/src/model/include/iDynTree/SixAxisForceTorqueSensor.h @@ -14,9 +14,9 @@ namespace iDynTree } -#include +#include -#include +#include #include diff --git a/src/model/include/iDynTree/SolidShapes.h b/src/model/include/iDynTree/SolidShapes.h new file mode 100644 index 00000000000..6310b598470 --- /dev/null +++ b/src/model/include/iDynTree/SolidShapes.h @@ -0,0 +1,306 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_SOLID_SHAPES_H +#define IDYNTREE_SOLID_SHAPES_H + +#include +#include +#include +#include +#include + +namespace iDynTree +{ + class Material { + public: + explicit Material(); + explicit Material(const std::string& name); + + std::string name() const; + + bool hasColor() const; + Vector4 color() const; + void setColor(const Vector4& color); + + bool hasTexture() const; + std::string texture() const; + void setTexture(const std::string& texture); + + private: + Vector4 m_color; + bool m_isColorSet; + std::string m_texture; + std::string m_name; + }; + + class Sphere; + class Box; + class Cylinder; + class ExternalMesh; + class Model; + + class SolidShape + { + public: + explicit SolidShape(); + + virtual ~SolidShape()=0; + virtual SolidShape* clone()=0; + + /** + * Returns the name of the shape. + */ + const std::string& getName() const; + + /** + * Sets the specified name. + */ + void setName(const std::string& name); + + /** + * Returns if the name is valid. + */ + bool isNameValid() const; + + /** + * Returns the homogeneus transformation of the geometry w.r.t. the attached link. + */ + const Transform& getLink_H_geometry() const; + + /** + * Sets the homogeneus transformation of the geometry w.r.t. the attached link. + */ + void setLink_H_geometry(const Transform& newTransform); + + /** + * Returns if the material is valid, i.e. you can call getMaterial(). + */ + bool isMaterialSet() const; + + /** + * Returns the current material. + */ + const Material& getMaterial() const; + + /** + * Sets the material. isMaterialSet will return true after this call. + */ + void setMaterial(const Material& material); + + bool isSphere() const; + bool isBox() const; + bool isCylinder() const; + bool isExternalMesh() const; + + // Utility methods to traverse the SolidShape class hierachy. + Sphere* asSphere(); + Box *asBox(); + Cylinder* asCylinder(); + ExternalMesh* asExternalMesh(); + + const Sphere* asSphere() const; + const Box* asBox() const; + const Cylinder* asCylinder() const; + const ExternalMesh* asExternalMesh() const; + private: + std::string name; + /** + * True if the name is valid, false otherwise. + */ + bool nameIsValid; + + Transform link_H_geometry; + + /** + * Material of the geometry, encoded as a rgba vector. + */ + Vector4 material; + bool m_isMaterialSet; + Material m_material; + }; + + class Sphere: public SolidShape + { + public: + virtual ~Sphere(); + + virtual SolidShape* clone(); + + /** + * Returns the current radius. + */ + double getRadius() const; + + /** + * Sets the new radius. + */ + void setRadius(double radius); + + private: + double radius; + }; + + /** + * @brief Box, i.e. 3D rectangular parallelepiped. + * + * The box is centered in the mesh frame, its sides + * are aligned with the axis of the mesh frame, and + * the side lenghts in the x, y and z direction are given + * by the attributes x, y and z. + */ + class Box: public SolidShape + { + public: + virtual ~Box(); + virtual SolidShape* clone(); + + /** + * Returns the current x side length. + */ + double getX() const; + + /** + * Sets the x side length. + */ + void setX(double x); + + /** + * Returns the current y side length. + */ + double getY() const; + + /** + * Sets the y side length. + */ + void setY(double y); + + /** + * Returns the current z side length. + */ + double getZ() const; + + /** + * Sets the z side length. + */ + void setZ(double z); + + private: + double x; + double y; + double z; + }; + + class Cylinder: public SolidShape + { + public: + virtual ~Cylinder(); + virtual SolidShape* clone(); + + /** + * Returns the current cylinder length. + */ + double getLength() const; + + /** + * Sets the cylinder length. + */ + void setLength(double length); + + /** + * Returns the current cylinder radius. + */ + double getRadius() const; + + /** + * Sets the cylinder radius. + */ + void setRadius(double radius); + + private: + double length; + double radius; + }; + + class ExternalMesh: public SolidShape + { + public: + virtual ~ExternalMesh(); + virtual SolidShape* clone(); + + /** + * Returns the current filename. + */ + const std::string& getFilename() const; + + /** + * Returns the current package directories. + */ + const std::vector& getPackageDirs() const; + + /** + * Returns the filename substituting the prefix "package://" with the corresponding absolute path. + * The absolute path is determined by searching for the file using the paths specified in the + * packageDirs vector or if empty in the "GAZEBO_MODEL_PATH", "ROS_PACKAGE_PATH" and + * "AMENT_PREFIX_PATH" environmental variables. + */ + std::string getFileLocationOnLocalFileSystem() const; + + /** + * Sets the filename. + */ + void setFilename(const std::string& filename); + + /** + * Sets the the package directories. + * @note if not set the absolute path is determined by searching for the file using the + * paths specified in the "GAZEBO_MODEL_PATH", "ROS_PACKAGE_PATH" and "AMENT_PREFIX_PATH" + * environmental variables. + */ + void setPackageDirs(const std::vector& packageDirs); + + /** + * Returns the current scale. + */ + const iDynTree::Vector3& getScale() const; + + /** + * Sets the scale. + */ + void setScale(const iDynTree::Vector3& scale); + + private: + std::string filename; + std::vector packageDirs; + iDynTree::Vector3 scale; + }; + + class ModelSolidShapes + { + private: + ModelSolidShapes& copy(const ModelSolidShapes& other); + /** + * Storage ot ModelSolidShapes. + */ + std::vector< std::vector > linkSolidShapes; + + public: + ModelSolidShapes(); + ~ModelSolidShapes(); + + ModelSolidShapes(const ModelSolidShapes& other); + ModelSolidShapes& operator=(const ModelSolidShapes& other); + + void clear(); + void resize(size_t nrOfLinks); + void resize(const Model& model); + bool isConsistent(const Model & model) const; + + std::vector >& getLinkSolidShapes(); + + const std::vector >& getLinkSolidShapes() const; + }; + +} + +#endif diff --git a/src/model/include/iDynTree/SubModel.h b/src/model/include/iDynTree/SubModel.h new file mode 100644 index 00000000000..69834284878 --- /dev/null +++ b/src/model/include/iDynTree/SubModel.h @@ -0,0 +1,149 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_SUB_MODEL_H +#define IDYNTREE_SUB_MODEL_H + +#include + +#include +#include +#include + +namespace iDynTree +{ + class Model; + class Traversal; + class JointPosDoubleArray; + class LinkPositions; + class IRawVector; + + /** + * Class representing the decomposition in one model in several submodels. + * + * This class is used in algorithms, such as estimation of external wrenches, + * where a complete model is decomposed in several submodels. + * + * For each submodel a iDynTree::Traversal is provided, so that algorithms + * that are explicitly designed to run on both FullModel Traversal and SubModel + * traversal can be executed on the SubModel. + * + */ + class SubModelDecomposition + { + private: + /** + * Vector of size nrOfSubModels traversal, one for each submodel. + */ + std::vector subModelTraversals; + + /** + * Vector mapping link index to sub model index. + */ + std::vector link2subModelIndex; + + /** + * Copy constructor is forbidden + */ + SubModelDecomposition(const SubModelDecomposition & other); + + /** + * Copy operator is forbidden + */ + SubModelDecomposition& operator=(const SubModelDecomposition &other); + + public: + /** + * Constructor + */ + SubModelDecomposition(); + + /** + * Destructor + */ + ~SubModelDecomposition(); + + /** + * Create a submodel decomposition + * of a given model with a given full tree traversal, + * by separating the model across the given joints. + * + * @param[in] model the model to split + * @param[in] traversal the full model traversal to split + * @param[in] splitJoints the model will be split along + * the joints whose names are in this vector. + * @return true if all went fine, false otherwise + */ + bool splitModelAlongJoints(const Model & model, + const Traversal & traversal, + const std::vector& splitJoints); + + /** + * Set the number of the submodels in the decomposition. + */ + void setNrOfSubModels(const size_t nrOfSubModels); + + /** + * Get the number of submodels in the decomposition. + */ + size_t getNrOfSubModels() const; + + /** + * Get the total numer of links in the submodel decomposition. + */ + size_t getNrOfLinks() const; + + /** + * Get the traversal of a given submodel + */ + Traversal & getTraversal(const size_t subModelIndex); + + /** + * Get the traversal of a given submodel (const version) + */ + const Traversal & getTraversal(const size_t subModelIndex) const; + + /** + * Get the subModel to which a given links belongs. + */ + size_t getSubModelOfLink(const LinkIndex & link) const; + + /** + * Get the subModel to which a given frame belongs. + */ + size_t getSubModelOfFrame(const Model & model, const FrameIndex& frame) const; + + }; + + /** + * Helper loop to compute the position of each link + * wrt to the frame of the subModel base. + * + * @param[in] fullModel full model + * @param[in] traversal traversal on which to run the loop + * @param[in] jointPos joint positions for the full model + * @param[out] traversalBase_H_link traversalBase_H_link[i] will store the traversalBase_H_i transform + */ + void computeTransformToTraversalBase(const Model& fullModel, + const Traversal& traversal, + const JointPosDoubleArray& jointPos, + LinkPositions& traversalBase_H_link); + + /** + * Run the computeTransformToTraversalBase for all the + * traversal in the subModelDecomposition, and store the + * results in the linkPos array. + * + * @param[in] fullModel full model + * @param[in] subModelDecomposition model decomposition on which the loop will run + * @param[in] jointPos joint positions for the full model + * @param[out] subModelBase_H_link subModelBase_H_link[i] will store the subModelBase_H_i transform + */ + void computeTransformToSubModelBase(const Model& fullModel, + const SubModelDecomposition& subModelDecomposition, + const JointPosDoubleArray& jointPos, + LinkPositions& subModelBase_H_link); + +} + +#endif diff --git a/src/sensors/include/iDynTree/Sensors/ThreeAxisAngularAccelerometerSensor.h b/src/model/include/iDynTree/ThreeAxisAngularAccelerometerSensor.h similarity index 97% rename from src/sensors/include/iDynTree/Sensors/ThreeAxisAngularAccelerometerSensor.h rename to src/model/include/iDynTree/ThreeAxisAngularAccelerometerSensor.h index 03493715717..c56bcdda694 100644 --- a/src/sensors/include/iDynTree/Sensors/ThreeAxisAngularAccelerometerSensor.h +++ b/src/model/include/iDynTree/ThreeAxisAngularAccelerometerSensor.h @@ -5,7 +5,7 @@ #ifndef THREE_AXIS_ANGULAR_ACCELEROMETER_H #define THREE_AXIS_ANGULAR_ACCELEROMETER_H -#include +#include namespace iDynTree { @@ -15,7 +15,7 @@ namespace iDynTree class Twist; } -#include +#include namespace iDynTree { diff --git a/src/sensors/include/iDynTree/Sensors/ThreeAxisForceTorqueContactSensor.h b/src/model/include/iDynTree/ThreeAxisForceTorqueContactSensor.h similarity index 98% rename from src/sensors/include/iDynTree/Sensors/ThreeAxisForceTorqueContactSensor.h rename to src/model/include/iDynTree/ThreeAxisForceTorqueContactSensor.h index 94e7695e185..c38c7e2d106 100644 --- a/src/sensors/include/iDynTree/Sensors/ThreeAxisForceTorqueContactSensor.h +++ b/src/model/include/iDynTree/ThreeAxisForceTorqueContactSensor.h @@ -5,7 +5,7 @@ #ifndef THREE_AXIS_FORCE_TORQUE_CONTACT_SENSOR_H #define THREE_AXIS_FORCE_TORQUE_CONTACT_SENSOR_H -#include +#include namespace iDynTree { @@ -15,7 +15,7 @@ namespace iDynTree class Twist; } -#include +#include namespace iDynTree { diff --git a/src/model/include/iDynTree/Traversal.h b/src/model/include/iDynTree/Traversal.h new file mode 100644 index 00000000000..e9de75384f0 --- /dev/null +++ b/src/model/include/iDynTree/Traversal.h @@ -0,0 +1,216 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_TRAVERSAL_H +#define IDYNTREE_TRAVERSAL_H + +#include + +#include + +namespace iDynTree +{ + class IJoint; + class Link; + class Model; + + /** + * Class that represents a traversal of a set of links of a Model. + * The traversal is represented by an ordered vector of links. + * For a given model, the traversal is always built in such a way + * that there exist a spanning tree that has as a root the first link + * (with traversal index 0) of the traversal, and such that every link comes + * after its spanning tree parent. + * + * For every link in the traversal are provided: + * * a pointer to the link itself + * * a pointer to the parent link in the spanning tree + * * a pointer to the joint connecting the link to the parent + * + * For the first link of the traversal (i.e. base of the traversal) + * there is not spanning tree parent, so the point to the parent link/joint are NULL. + * + * + * \ingroup iDynTreeModel + */ + class Traversal + { + private: + std::vector links; + std::vector parents; + std::vector toParentJoints; + std::vector linkIndexToTraversalIndex; + + /** + * Copy constructor is forbidden + */ + Traversal(const Traversal & other); + + /** + * Copy operator is forbidden + */ + Traversal& operator=(const Traversal &other); + + public: + Traversal(); + + ~Traversal(); + + /** + * Get the number of visited links. + * + * @return the number of links in the Traversal + */ + unsigned int getNrOfVisitedLinks() const; + + /** + * Get the traversalIndex-th link of the traversal. + * + * @return a pointer to the traversalIndex-th link of the traversal. + */ + const Link * getLink(const TraversalIndex traversalIndex) const; + + /** + * Get the base link of the traversal. + * + * @note this is equivalent to getLink(0), as the base link + * is by definition the first link of the traversal. + * @return a pointer to the base link of the traversal. + */ + const Link * getBaseLink() const; + + /** + * Get the parent link of the traversalIndex-th link of the traversal. + * + * @return a pointer to the parent link of the traversalIndex-th link of the traversal. + */ + const Link * getParentLink(const TraversalIndex traversalIndex) const; + + /** + * Get the joint connecting the traversalIndex-th link of the traversal + * to its parent. + * + * @return a pointer to the joint connecting the link traversalIndex-th link of the traversal. + */ + const IJoint * getParentJoint(const TraversalIndex traversalIndex) const; + + /** + * Get the parent link of the link with index linkIndex of the traversal. + * + * @return a pointer to the parent link of the traversalIndex-th link of the traversal. + */ + const Link * getParentLinkFromLinkIndex(const LinkIndex linkIndex) const; + + /** + * Get the joint connecting the link with index linkIndex + * to its parent. + * + * @return a pointer to the joint connecting the link traversalIndex-th link of the traversal. + */ + const IJoint * getParentJointFromLinkIndex(const LinkIndex linkIndex) const; + + /** + * Get the traversal index of the specified link + * + * @return the traversalIndex of the specified link, or TRAVERSAL_INDEX_INVALID if the link does not belong to the traversal. + */ + TraversalIndex getTraversalIndexFromLinkIndex(const LinkIndex linkIndex) const; + + + + /** + * Reset the Traversal. + * + * After a call to reset, the Traversal will contain + * no visited links, so it needs to be approprately populated + * with the addTraversalBase and addTraversalElement methods. + * + * @param[in] nrOfLinksInModel total number of links in the model, + * not the number of visited links in the Traversal. + * @return true if all went well, false otherwise + */ + bool reset(const unsigned int nrOfLinksInModel); + + /** + * Reset the Traversal. + * + * After a call to reset, all the pointers in the Traversal are set + * to 0, and the Traversal should be approprialy populated with the + * setters before use. + * + * @param[in] model Model on which this traversal will be used, + * used to initialize the internal data structure + * using the getNrOfLinks() method. + * + * @return true if all went well, false otherwise + */ + bool reset(const Model & model); + + /** + * Add a base to traversal. + * + * Equivalent to addTraversalElement(link,NULL,NULL), but + * will print an error and return false if it is called with a + * traversal that already has a base, i.e. getNrOfVisitedLinks() > 0 + * + * @param[in] link a pointer to the base link + * @return true if all went well, false otherwise + */ + bool addTraversalBase(const Link * link); + + /** + * Add an element to the traversal. + * + * After a call to this method, the getNrOfVisitedLinks() + * will be increased of 1 unit. + * + * @param[in] link a pointer to the link to add to the traversal + * @param[in] jointToParent a pointer to the joint that connects + * the added link to its parent in this traversal + * @param[in] parentLink a pointer to the parent on this link in this traversal. + * It should be a link already contained in this traversal. + * @return true if all went well, false otherwise. + */ + bool addTraversalElement(const Link * link, + const IJoint * jointToParent, + const Link * parentLink); + + /** + * \brief Check if a link is the parent of another link for this traversal. + * + * + * @param[in] parentCandidate the link candidate to be the parent of childCandidate. + * @param[in] childCandidate the link candidate to be a child of parentCandidate. + * @return true if parentCandidate is actually the parent of childCandidate, false otherwise. + */ + bool isParentOf(const LinkIndex parentCandidate, const LinkIndex childCandidate) const; + + /** + * \brief Get the child link (according to the traversal) of a Joint. + * + * @param[in] m_model the considered model. + * @param[in] jntIdx the index of the joint of which we want to get the child link. + * @return the index of the child link if all went well, LINK_INVALID_INDEX otherwise . + */ + LinkIndex getChildLinkIndexFromJointIndex(const Model & model, const JointIndex jntIdx) const; + + /** + * \brief Get the parent link (according to the traversal) of a Joint. + * + * @param[in] m_model the considered model. + * @param[in] jntIdx the index of the joint of which we want to get the parent link. + * @return the index of the parent link if all went well, LINK_INVALID_INDEX otherwise . + */ + LinkIndex getParentLinkIndexFromJointIndex(const Model & model, const JointIndex jntIdx) const; + + /** + * Return a human-readable representation of the traversal. + */ + std::string toString(const Model & model) const; + + }; + + +} + +#endif /* IDYNTREE_TRAVERSAL_H */ diff --git a/src/sensors/src/AccelerometerSensor.cpp b/src/model/src/AccelerometerSensor.cpp similarity index 95% rename from src/sensors/src/AccelerometerSensor.cpp rename to src/model/src/AccelerometerSensor.cpp index af44bc6ac6d..afecaa7d34e 100644 --- a/src/sensors/src/AccelerometerSensor.cpp +++ b/src/model/src/AccelerometerSensor.cpp @@ -2,12 +2,12 @@ // SPDX-License-Identifier: BSD-3-Clause -#include "iDynTree/Sensors/AccelerometerSensor.h" +#include "iDynTree/AccelerometerSensor.h" -#include "iDynTree/Core/Transform.h" +#include "iDynTree/Transform.h" -#include "iDynTree/Core/SpatialAcc.h" -#include "iDynTree/Core/Twist.h" +#include "iDynTree/SpatialAcc.h" +#include "iDynTree/Twist.h" namespace iDynTree { diff --git a/src/model/src/ContactWrench.cpp b/src/model/src/ContactWrench.cpp index bee50a4589d..c8716cd782d 100644 --- a/src/model/src/ContactWrench.cpp +++ b/src/model/src/ContactWrench.cpp @@ -2,8 +2,8 @@ // SPDX-License-Identifier: BSD-3-Clause -#include -#include +#include +#include namespace iDynTree { diff --git a/src/model/src/DenavitHartenberg.cpp b/src/model/src/DenavitHartenberg.cpp index a3505949ff2..f64d562c2cc 100644 --- a/src/model/src/DenavitHartenberg.cpp +++ b/src/model/src/DenavitHartenberg.cpp @@ -1,15 +1,15 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include -#include -#include -#include -#include -#include -#include - -#include +#include +#include +#include +#include +#include +#include +#include + +#include namespace iDynTree diff --git a/src/model/src/Dynamics.cpp b/src/model/src/Dynamics.cpp index 29e0fabe52b..2c6f2af0c8e 100644 --- a/src/model/src/Dynamics.cpp +++ b/src/model/src/Dynamics.cpp @@ -3,20 +3,20 @@ -#include -#include +#include +#include -#include -#include -#include -#include +#include +#include +#include +#include -#include -#include -#include -#include +#include +#include +#include +#include -#include +#include #include diff --git a/src/model/src/DynamicsLinearization.cpp b/src/model/src/DynamicsLinearization.cpp index 8b00c1cdbeb..6c5097031e5 100644 --- a/src/model/src/DynamicsLinearization.cpp +++ b/src/model/src/DynamicsLinearization.cpp @@ -3,15 +3,15 @@ -#include +#include -#include -#include +#include +#include -#include +#include -#include -#include +#include +#include #include diff --git a/src/model/src/DynamicsLinearizationHelpers.cpp b/src/model/src/DynamicsLinearizationHelpers.cpp index 2069874ca54..6a38b7dfb1b 100644 --- a/src/model/src/DynamicsLinearizationHelpers.cpp +++ b/src/model/src/DynamicsLinearizationHelpers.cpp @@ -2,9 +2,9 @@ // SPDX-License-Identifier: BSD-3-Clause -#include +#include -#include +#include namespace iDynTree { diff --git a/src/model/src/FixedJoint.cpp b/src/model/src/FixedJoint.cpp index 4899bd41139..290d330c8eb 100644 --- a/src/model/src/FixedJoint.cpp +++ b/src/model/src/FixedJoint.cpp @@ -1,12 +1,12 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include -#include -#include +#include +#include +#include -#include -#include +#include +#include #include diff --git a/src/model/src/ForwardKinematics.cpp b/src/model/src/ForwardKinematics.cpp index 872ea2f5114..7cb98e517f2 100644 --- a/src/model/src/ForwardKinematics.cpp +++ b/src/model/src/ForwardKinematics.cpp @@ -2,13 +2,13 @@ // SPDX-License-Identifier: BSD-3-Clause -#include +#include -#include -#include +#include +#include -#include -#include +#include +#include namespace iDynTree { diff --git a/src/model/src/FreeFloatingMatrices.cpp b/src/model/src/FreeFloatingMatrices.cpp index 84e680ab059..5fe102f4f56 100644 --- a/src/model/src/FreeFloatingMatrices.cpp +++ b/src/model/src/FreeFloatingMatrices.cpp @@ -2,8 +2,8 @@ // SPDX-License-Identifier: BSD-3-Clause -#include -#include +#include +#include #include diff --git a/src/model/src/FreeFloatingState.cpp b/src/model/src/FreeFloatingState.cpp index 7e46cd9493e..d84d7967129 100644 --- a/src/model/src/FreeFloatingState.cpp +++ b/src/model/src/FreeFloatingState.cpp @@ -2,8 +2,8 @@ // SPDX-License-Identifier: BSD-3-Clause -#include -#include +#include +#include #include diff --git a/src/sensors/src/GyroscopeSensor.cpp b/src/model/src/GyroscopeSensor.cpp similarity index 95% rename from src/sensors/src/GyroscopeSensor.cpp rename to src/model/src/GyroscopeSensor.cpp index ea39c4936eb..bcc7caee327 100644 --- a/src/sensors/src/GyroscopeSensor.cpp +++ b/src/model/src/GyroscopeSensor.cpp @@ -1,10 +1,10 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include "iDynTree/Sensors/GyroscopeSensor.h" -#include "iDynTree/Core/Transform.h" -#include "iDynTree/Core/Wrench.h" -#include "iDynTree/Core/Twist.h" +#include "iDynTree/GyroscopeSensor.h" +#include "iDynTree/Transform.h" +#include "iDynTree/Wrench.h" +#include "iDynTree/Twist.h" namespace iDynTree { diff --git a/src/model/src/Indices.cpp b/src/model/src/Indices.cpp index d0a2ee28ab3..c78e498701f 100644 --- a/src/model/src/Indices.cpp +++ b/src/model/src/Indices.cpp @@ -1,7 +1,7 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include +#include namespace iDynTree { diff --git a/src/model/src/Jacobians.cpp b/src/model/src/Jacobians.cpp index 736b0cc6f6c..1327db4baf8 100644 --- a/src/model/src/Jacobians.cpp +++ b/src/model/src/Jacobians.cpp @@ -2,13 +2,13 @@ // SPDX-License-Identifier: BSD-3-Clause -#include +#include -#include +#include -#include -#include -#include +#include +#include +#include namespace iDynTree { diff --git a/src/model/src/JointState.cpp b/src/model/src/JointState.cpp index 867375ab5c7..727edf725e1 100644 --- a/src/model/src/JointState.cpp +++ b/src/model/src/JointState.cpp @@ -1,11 +1,11 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include +#include -#include +#include -#include +#include namespace iDynTree { diff --git a/src/model/src/Link.cpp b/src/model/src/Link.cpp index 3637bc01907..6a82d7fe753 100644 --- a/src/model/src/Link.cpp +++ b/src/model/src/Link.cpp @@ -1,7 +1,7 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include +#include namespace iDynTree { diff --git a/src/model/src/LinkState.cpp b/src/model/src/LinkState.cpp index 1634891cb5d..37399df9787 100644 --- a/src/model/src/LinkState.cpp +++ b/src/model/src/LinkState.cpp @@ -1,10 +1,10 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include -#include +#include +#include -#include +#include namespace iDynTree { diff --git a/src/model/src/LinkTraversalsCache.cpp b/src/model/src/LinkTraversalsCache.cpp index 73a4fc3a81d..0191cd9ff95 100644 --- a/src/model/src/LinkTraversalsCache.cpp +++ b/src/model/src/LinkTraversalsCache.cpp @@ -1,10 +1,10 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include "iDynTree/Model/LinkTraversalsCache.h" +#include "iDynTree/LinkTraversalsCache.h" -#include "iDynTree/Model/Traversal.h" -#include "iDynTree/Model/Model.h" +#include "iDynTree/Traversal.h" +#include "iDynTree/Model.h" #include diff --git a/src/model/src/Model.cpp b/src/model/src/Model.cpp index 227c17c38f3..225421c22ae 100644 --- a/src/model/src/Model.cpp +++ b/src/model/src/Model.cpp @@ -1,12 +1,12 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include -#include -#include +#include +#include +#include -#include -#include +#include +#include #include #include diff --git a/src/model/src/ModelInterfaceDestructors.cpp b/src/model/src/ModelInterfaceDestructors.cpp index 170672fb901..48e0fc5766d 100644 --- a/src/model/src/ModelInterfaceDestructors.cpp +++ b/src/model/src/ModelInterfaceDestructors.cpp @@ -11,7 +11,7 @@ * */ -#include +#include namespace iDynTree { diff --git a/src/sensors/src/ModelSensorsTransformers.cpp b/src/model/src/ModelSensorsTransformers.cpp similarity index 97% rename from src/sensors/src/ModelSensorsTransformers.cpp rename to src/model/src/ModelSensorsTransformers.cpp index 0f297af29c2..4b71dcf978d 100644 --- a/src/sensors/src/ModelSensorsTransformers.cpp +++ b/src/model/src/ModelSensorsTransformers.cpp @@ -1,13 +1,13 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include -#include +#include +#include -#include -#include +#include +#include -#include +#include #include diff --git a/src/model/src/ModelTransformers.cpp b/src/model/src/ModelTransformers.cpp index df8b4ce4eb7..84ec0c77ed9 100644 --- a/src/model/src/ModelTransformers.cpp +++ b/src/model/src/ModelTransformers.cpp @@ -1,14 +1,14 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include -#include -#include -#include - -#include -#include -#include +#include +#include +#include +#include + +#include +#include +#include #include #include diff --git a/src/sensors/src/PredictSensorsMeasurements.cpp b/src/model/src/PredictSensorsMeasurements.cpp similarity index 89% rename from src/sensors/src/PredictSensorsMeasurements.cpp rename to src/model/src/PredictSensorsMeasurements.cpp index 1d549e600dd..512277b53f8 100644 --- a/src/sensors/src/PredictSensorsMeasurements.cpp +++ b/src/model/src/PredictSensorsMeasurements.cpp @@ -1,22 +1,22 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -#include -#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include namespace iDynTree { diff --git a/src/model/src/PrismaticJoint.cpp b/src/model/src/PrismaticJoint.cpp index 45ddb72a112..2e15ec2a9d2 100644 --- a/src/model/src/PrismaticJoint.cpp +++ b/src/model/src/PrismaticJoint.cpp @@ -1,14 +1,14 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include - -#include -#include -#include -#include -#include -#include +#include + +#include +#include +#include +#include +#include +#include #include diff --git a/src/model/src/RevoluteJoint.cpp b/src/model/src/RevoluteJoint.cpp index 6573b2606f3..71bf77b23e7 100644 --- a/src/model/src/RevoluteJoint.cpp +++ b/src/model/src/RevoluteJoint.cpp @@ -1,14 +1,14 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include - -#include -#include -#include -#include -#include -#include +#include + +#include +#include +#include +#include +#include +#include #include diff --git a/src/sensors/src/Sensors.cpp b/src/model/src/Sensors.cpp similarity index 99% rename from src/sensors/src/Sensors.cpp rename to src/model/src/Sensors.cpp index f863a9c11a8..c1bf395ca48 100644 --- a/src/sensors/src/Sensors.cpp +++ b/src/model/src/Sensors.cpp @@ -6,14 +6,14 @@ #include #include -#include -#include +#include +#include -#include -#include -#include +#include +#include +#include -#include +#include #include #include diff --git a/src/sensors/src/SixAxisForceTorqueSensor.cpp b/src/model/src/SixAxisForceTorqueSensor.cpp similarity index 98% rename from src/sensors/src/SixAxisForceTorqueSensor.cpp rename to src/model/src/SixAxisForceTorqueSensor.cpp index b05a578dde3..8f35a43a00b 100644 --- a/src/sensors/src/SixAxisForceTorqueSensor.cpp +++ b/src/model/src/SixAxisForceTorqueSensor.cpp @@ -1,14 +1,14 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include "iDynTree/Core/Wrench.h" -#include "iDynTree/Sensors/SixAxisForceTorqueSensor.h" +#include "iDynTree/Wrench.h" +#include "iDynTree/SixAxisForceTorqueSensor.h" -#include "iDynTree/Core/Transform.h" +#include "iDynTree/Transform.h" -#include -#include +#include +#include -#include +#include #include diff --git a/src/model/src/SolidShapes.cpp b/src/model/src/SolidShapes.cpp index 6de3d609e70..c5dc80d1097 100644 --- a/src/model/src/SolidShapes.cpp +++ b/src/model/src/SolidShapes.cpp @@ -1,8 +1,8 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include -#include +#include +#include #include #include diff --git a/src/model/src/SubModel.cpp b/src/model/src/SubModel.cpp index a698cc0c3c2..780fc54cdd4 100644 --- a/src/model/src/SubModel.cpp +++ b/src/model/src/SubModel.cpp @@ -1,13 +1,13 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include +#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include #include diff --git a/src/sensors/src/ThreeAxisAngularAccelerometerSensor.cpp b/src/model/src/ThreeAxisAngularAccelerometerSensor.cpp similarity index 94% rename from src/sensors/src/ThreeAxisAngularAccelerometerSensor.cpp rename to src/model/src/ThreeAxisAngularAccelerometerSensor.cpp index 84390bf640d..34eeb3420b0 100644 --- a/src/sensors/src/ThreeAxisAngularAccelerometerSensor.cpp +++ b/src/model/src/ThreeAxisAngularAccelerometerSensor.cpp @@ -2,14 +2,14 @@ // SPDX-License-Identifier: BSD-3-Clause -#include "iDynTree/Sensors/ThreeAxisAngularAccelerometerSensor.h" +#include "iDynTree/ThreeAxisAngularAccelerometerSensor.h" -#include "iDynTree/Core/Transform.h" +#include "iDynTree/Transform.h" -#include "iDynTree/Core/SpatialAcc.h" -#include "iDynTree/Core/Twist.h" +#include "iDynTree/SpatialAcc.h" +#include "iDynTree/Twist.h" -#include +#include namespace iDynTree { diff --git a/src/sensors/src/ThreeAxisForceTorqueContactSensor.cpp b/src/model/src/ThreeAxisForceTorqueContactSensor.cpp similarity index 96% rename from src/sensors/src/ThreeAxisForceTorqueContactSensor.cpp rename to src/model/src/ThreeAxisForceTorqueContactSensor.cpp index 08e43460ded..92b0281c416 100644 --- a/src/sensors/src/ThreeAxisForceTorqueContactSensor.cpp +++ b/src/model/src/ThreeAxisForceTorqueContactSensor.cpp @@ -1,14 +1,14 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include "iDynTree/Sensors/ThreeAxisForceTorqueContactSensor.h" +#include "iDynTree/ThreeAxisForceTorqueContactSensor.h" -#include "iDynTree/Core/Transform.h" +#include "iDynTree/Transform.h" -#include "iDynTree/Core/SpatialAcc.h" -#include "iDynTree/Core/Twist.h" +#include "iDynTree/SpatialAcc.h" +#include "iDynTree/Twist.h" -#include +#include namespace iDynTree { diff --git a/src/model/src/Traversal.cpp b/src/model/src/Traversal.cpp index a6dd1707510..6a29c33efdf 100644 --- a/src/model/src/Traversal.cpp +++ b/src/model/src/Traversal.cpp @@ -2,8 +2,8 @@ // SPDX-License-Identifier: BSD-3-Clause -#include -#include +#include +#include #include #include diff --git a/src/model/tests/CMakeLists.txt b/src/model/tests/CMakeLists.txt index ea0dcd81c7d..568b40e0f34 100644 --- a/src/model/tests/CMakeLists.txt +++ b/src/model/tests/CMakeLists.txt @@ -19,3 +19,5 @@ endmacro() add_unit_test(Joint) add_unit_test(Link) add_unit_test(Model) +add_unit_test(SensorsList) +add_unit_test(ThreeAxisForceTorqueContactSensor) diff --git a/src/model/tests/ForwardKinematicsUnitTest.cpp b/src/model/tests/ForwardKinematicsUnitTest.cpp deleted file mode 100644 index 2a6c82ffa4a..00000000000 --- a/src/model/tests/ForwardKinematicsUnitTest.cpp +++ /dev/null @@ -1,19 +0,0 @@ -// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) -// SPDX-License-Identifier: BSD-3-Clause - - -#include -#include - -#include - -#include -#include - -using namespace iDynTree; - -int main() -{ - - return EXIT_SUCCESS; -} diff --git a/src/model/tests/JointUnitTest.cpp b/src/model/tests/JointUnitTest.cpp index 3e37c201f7d..00f5b7c1b51 100644 --- a/src/model/tests/JointUnitTest.cpp +++ b/src/model/tests/JointUnitTest.cpp @@ -1,17 +1,17 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include +#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include -#include -#include +#include +#include #include #include diff --git a/src/model/tests/LinkUnitTest.cpp b/src/model/tests/LinkUnitTest.cpp index de07e68a2eb..fbf84d636c3 100644 --- a/src/model/tests/LinkUnitTest.cpp +++ b/src/model/tests/LinkUnitTest.cpp @@ -1,10 +1,10 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include +#include -#include -#include +#include +#include #include #include diff --git a/src/model/tests/ModelUnitTest.cpp b/src/model/tests/ModelUnitTest.cpp index f808000bf52..a6a1283614a 100644 --- a/src/model/tests/ModelUnitTest.cpp +++ b/src/model/tests/ModelUnitTest.cpp @@ -1,14 +1,14 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include -#include +#include #include #include @@ -16,11 +16,11 @@ #include // For modelTransformsers testing -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include using namespace iDynTree; diff --git a/src/sensors/tests/SensorsListUnitTest.cpp b/src/model/tests/SensorsListUnitTest.cpp similarity index 96% rename from src/sensors/tests/SensorsListUnitTest.cpp rename to src/model/tests/SensorsListUnitTest.cpp index c3489658c83..52d5f1c1624 100644 --- a/src/sensors/tests/SensorsListUnitTest.cpp +++ b/src/model/tests/SensorsListUnitTest.cpp @@ -1,11 +1,11 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include void checkList() { diff --git a/src/sensors/tests/ThreeAxisForceTorqueContactSensorUnitTest.cpp b/src/model/tests/ThreeAxisForceTorqueContactSensorUnitTest.cpp similarity index 95% rename from src/sensors/tests/ThreeAxisForceTorqueContactSensorUnitTest.cpp rename to src/model/tests/ThreeAxisForceTorqueContactSensorUnitTest.cpp index b97b5e1ce47..5fb06e9738c 100644 --- a/src/sensors/tests/ThreeAxisForceTorqueContactSensorUnitTest.cpp +++ b/src/model/tests/ThreeAxisForceTorqueContactSensorUnitTest.cpp @@ -1,8 +1,8 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include -#include +#include +#include using namespace iDynTree; diff --git a/src/model_io/codecs/CMakeLists.txt b/src/model_io/codecs/CMakeLists.txt index 779756a451b..3ddd4ecddb8 100644 --- a/src/model_io/codecs/CMakeLists.txt +++ b/src/model_io/codecs/CMakeLists.txt @@ -1,10 +1,10 @@ # SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) # SPDX-License-Identifier: BSD-3-Clause -set(IDYNTREE_MODELIO_HEADERS include/iDynTree/ModelIO/URDFDofsImport.h - include/iDynTree/ModelIO/ModelLoader.h - include/iDynTree/ModelIO/ModelExporter.h - include/iDynTree/ModelIO/ModelCalibrationHelper.h) +set(IDYNTREE_MODELIO_HEADERS include/iDynTree/URDFDofsImport.h + include/iDynTree/ModelLoader.h + include/iDynTree/ModelExporter.h + include/iDynTree/ModelCalibrationHelper.h) set(IDYNTREE_MODELIO_PRIVATE_HEADERS include/private/URDFDocument.h include/private/InertialElement.h @@ -51,7 +51,7 @@ target_include_directories(${libraryname} PUBLIC "$" "$") -target_link_libraries(${libraryname} PUBLIC idyntree-core idyntree-model idyntree-sensors idyntree-modelio-xml +target_link_libraries(${libraryname} PUBLIC idyntree-core idyntree-model idyntree-modelio-xml PRIVATE Eigen3::Eigen LibXml2::LibXml2) # See https://stackoverflow.com/questions/38832528/transitive-target-include-directories-on-object-libraries # Can be removed with CMake 3.12 @@ -67,10 +67,15 @@ install(TARGETS ${libraryname} RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT bin LIBRARY DESTINATION "${CMAKE_INSTALL_LIBDIR}" COMPONENT shlib ARCHIVE DESTINATION "${CMAKE_INSTALL_LIBDIR}" COMPONENT lib - PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/iDynTree/ModelIO) + PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/iDynTree) set_property(GLOBAL APPEND PROPERTY ${VARS_PREFIX}_TARGETS ${libraryname}) if(IDYNTREE_COMPILE_TESTS) add_subdirectory(tests) endif(IDYNTREE_COMPILE_TESTS) + + +# Install deprecated headers +install(DIRECTORY include/iDynTree/ModelIO + DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/iDynTree) \ No newline at end of file diff --git a/src/model_io/codecs/include/iDynTree/ModelCalibrationHelper.h b/src/model_io/codecs/include/iDynTree/ModelCalibrationHelper.h new file mode 100644 index 00000000000..bbb9438f80a --- /dev/null +++ b/src/model_io/codecs/include/iDynTree/ModelCalibrationHelper.h @@ -0,0 +1,113 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_MODEL_CALIBRATION_HELPER_H +#define IDYNTREE_MODEL_CALIBRATION_HELPER_H + +#include +#include +#include + +#include +#include +#include + +namespace iDynTree +{ + + +/** + * \ingroup iDynTreeModelIO + * + * Helper class to load a model, modify its parameters based on calibration, + * and save it again to file. + * + */ +class ModelCalibrationHelper +{ +private: + class ModelCalibrationHelperPimpl; + std::unique_ptr m_pimpl; + +public: + + /** + * @name Constructor/Destructor + */ + //@{ + + /** + * Constructor + * + */ + ModelCalibrationHelper(); + + ~ModelCalibrationHelper(); + + //@} + + /** + * Load the model of the robot from a string. + * + * @param modelString string containg the model of the robot. + * @param filetype type of the file to load, currently supporting only urdf type. + * + */ + bool loadModelFromString(const std::string & modelString, const std::string & filetype="urdf"); + + /** + * Load the model of the robot from an external file. + * + * @param filename path to the file to load + * @param filetype type of the file to load, currently supporting only urdf type. + * + */ + bool loadModelFromFile(const std::string & filename, const std::string & filetype="urdf"); + + /** + * Update the inertial parameters of the loaded model. + * + * @note the inertial params vector follow the structure of the Model::getInertialParameters method. + */ + bool updateModelInertialParametersToString(std::string & modelString, + const iDynTree::VectorDynSize& inertialParams, + const std::string filetype="urdf", + const iDynTree::ModelExporterOptions options=iDynTree::ModelExporterOptions()); + + /** + * Update the inertial parameters of the loaded model. + * + * @note the inertial params vector follows the structure of the Model::getInertialParameters method. + */ + bool updateModelInertialParametersToFile(const std::string & filename, + const iDynTree::VectorDynSize& inertialParams, + const std::string filetype="urdf", + const iDynTree::ModelExporterOptions options=iDynTree::ModelExporterOptions()); + + /** + * Get the loaded model. + * + * @note This always return the model loaded via loadModel methods, and is not affected by the updateModel methods. + */ + const Model & model(); + + /** + * Get the loaded sensors. + * + * @note This always return the model loaded via loadModel method, and is not affected by the updateModel methods. + */ + const SensorsList & sensors(); + + /** + * Return true if the model have been correctly true. + * + * @note This always return the validity of the model loaded via loadModel method, and is not affected by the updateModel methods. + * @return True if the model was loaded correctly. + */ + bool isValid(); + //@} +}; + +} + +#endif diff --git a/src/model_io/codecs/include/iDynTree/ModelExporter.h b/src/model_io/codecs/include/iDynTree/ModelExporter.h new file mode 100644 index 00000000000..fcbe1c9e681 --- /dev/null +++ b/src/model_io/codecs/include/iDynTree/ModelExporter.h @@ -0,0 +1,198 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_MODEL_EXPORTER_H +#define IDYNTREE_MODEL_EXPORTER_H + +#include +#include + +#include +#include +#include + +namespace iDynTree +{ + +/** + * \ingroup iDynTreeModelIO + * + * Options for the iDynTree exporter. + */ +struct ModelExporterOptions +{ + /** + * Specify the base link of the exported model. + * + * Differently from the iDynTree::Model class, some file formats (such as the URDF) represent the multibody structure + * using a directed tree representation, for which it is necessary to explicitly specify a base link. + * + * If this string is empty (default value), the default base link contained in the model will be used. + * + * Default value: "". + * Supported formats: urdf. + */ + std::string baseLink; + + /** + * Select if the first additional frame of the base link is exported as fake base link. + * + * The URDF exporter by default exports the first additional frame of the base link as + * a parent "fake" link to the actual base link, as a workaround for https://github.com/ros/kdl_parser/issues/27). + * By setting this option to false, is possible to disable this behaviour, for more info see iDynTree::ModelExporter docs. + * This option is ignored in non-URDF exporter. + * + * Default value: true. + * Supported formats: urdf. + */ + bool exportFirstBaseLinkAdditionalFrameAsFakeURDFBase; + + /** + * Specify the robot name. + * + * Default: "iDynTreeURDFModelExportModelName". + * Supported formats: urdf. + */ + std::string robotExportedName; + /** + * Specify the xml blobs to be exported at the end of the urdf as child of the robot tag. + * + * Default: {}. + * Supported formats: urdf. + */ + std::vector xmlBlobs; + + /** + * Constructor. + */ + ModelExporterOptions(); + +}; + + +/** + * \ingroup iDynTreeModelIO + * + * Helper class to export a model to the supported textual formats. + * + * Currently the only format supported for export is the URDF format, + * as it is described in http://wiki.ros.org/urdf/XML . + * + * Only iDynTree::Model classes that represent multibody system with no loops + * can be exported. + * + * Furthermore, currently the model exporter only exports a subset of the features + * supported in iDynTree::Model. In particular, the following features are not exported: + * + * * Sensors + * * Joint limits, damping and static friction. + * + * # Format documentation + * + * The following format are supported by the exporter. + * + * | Format | Extendend Name | Website | String for filetype argument | + * |:-----------------------------:|:-------:|:-------:|:--------:| + * | URDF | Unified Robot Description Format | http://wiki.ros.org/urdf | `urdf` | + * + * ## URDF + * + * As the URDF format does not distinguish between frames and links (see https://discourse.ros.org/t/urdf-ng-link-and-frame-concepts/56 + * for an extensive discussion on this) the URDF model exporter converts iDynTree's *additional frames* to mass-less and shape-less + * *fake* URDF links that are connected as child links via `fixed` joints to the corresponding **real** URDF links. + * + * Furthermore, it is widespread use in URDF models to never use a real link (with mass) as the root link of a model, mainly + * due to workaround a bug in official %KDL parser used in ROS (see https://github.com/ros/kdl_parser/issues/27 for more info). For this reason, + * if the selected base_link has at least one additional frame, by default the first additional frame of the base link is added as a **parent** fake URDF link, + * instead as a **child** fake URDF link as done with the rest of %iDynTree's additional frames. If no additional frame is available for the base link, + * the base link of the URDF will have a mass, and will generate a warning then used with the ROS's [`kdl_parser`](https://github.com/ros/kdl_parser) . + * This behaviour can be disabled by setting to false the `exportFirstBaseLinkAdditionalFrameAsFakeURDFBase` attribute of ModelExporterOptions. + * + */ +class ModelExporter +{ +private: + + class Pimpl; + std::unique_ptr m_pimpl; + +public: + + /** + * @name Constructor/Destructor + */ + //@{ + + /** + * Constructor + */ + ModelExporter(); + + ~ModelExporter(); + + //@} + + /** + * @name Model exporting and definition methods + * This methods are used to export the structure of your model. + */ + //@{ + const ModelExporterOptions& exportingOptions() const; + + void setExportingOptions(const ModelExporterOptions& options); + + /** + * Specifies the model of the robot to export. + * + * @param[in] model The used model. + * @param[in] sensors The used sensors. + * @param[in] options The used options. + * @return true if all went well, false otherwise. + */ + bool init(const Model& model, + const SensorsList& sensors=SensorsList(), + const ModelExporterOptions options=ModelExporterOptions()); + + /** + * Get the loaded model that will be exported. + * + */ + const Model & model(); + + /** + * Get the loaded sensors that will be exported. + */ + const SensorsList & sensors(); + + /** + * Return true if the model have been correctly loaded, and can be exported. + * + * @return True if the model was loaded correctly. + */ + bool isValid(); + + /** + * Export the model of the robot to a string. + * + * @param modelString string containg the model of the robot. + * @param filetype type of the file to load, currently supporting only urdf type. + * + */ + bool exportModelToString(std::string & modelString, const std::string filetype="urdf"); + + /** + * Export the model of the robot to an external file. + * + * @param filename path to the file to export. + * It can be either a relative filename with respect to the current working directory, + * or an absolute filename. + * @param filetype type of the file to load, currently supporting only urdf type. + * + */ + bool exportModelToFile(const std::string & filename, const std::string filetype="urdf"); + //@} +}; + +} + +#endif diff --git a/src/model_io/codecs/include/iDynTree/ModelIO/ModelCalibrationHelper.h b/src/model_io/codecs/include/iDynTree/ModelIO/ModelCalibrationHelper.h index acaa0f0b0c0..060cca62961 100644 --- a/src/model_io/codecs/include/iDynTree/ModelIO/ModelCalibrationHelper.h +++ b/src/model_io/codecs/include/iDynTree/ModelIO/ModelCalibrationHelper.h @@ -1,113 +1,13 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef IDYNTREE_MODEL_CALIBRATION_HELPER_H -#define IDYNTREE_MODEL_CALIBRATION_HELPER_H +#ifndef IDYNTREE_DEPRECATED_MODEL_CALIBRATION_HELPER_H +#define IDYNTREE_DEPRECATED_MODEL_CALIBRATION_HELPER_H -#include -#include -#include - -#include -#include -#include - -namespace iDynTree -{ - - -/** - * \ingroup iDynTreeModelIO - * - * Helper class to load a model, modify its parameters based on calibration, - * and save it again to file. - * - */ -class ModelCalibrationHelper -{ -private: - class ModelCalibrationHelperPimpl; - std::unique_ptr m_pimpl; - -public: - - /** - * @name Constructor/Destructor - */ - //@{ - - /** - * Constructor - * - */ - ModelCalibrationHelper(); - - ~ModelCalibrationHelper(); - - //@} - - /** - * Load the model of the robot from a string. - * - * @param modelString string containg the model of the robot. - * @param filetype type of the file to load, currently supporting only urdf type. - * - */ - bool loadModelFromString(const std::string & modelString, const std::string & filetype="urdf"); - - /** - * Load the model of the robot from an external file. - * - * @param filename path to the file to load - * @param filetype type of the file to load, currently supporting only urdf type. - * - */ - bool loadModelFromFile(const std::string & filename, const std::string & filetype="urdf"); - - /** - * Update the inertial parameters of the loaded model. - * - * @note the inertial params vector follow the structure of the Model::getInertialParameters method. - */ - bool updateModelInertialParametersToString(std::string & modelString, - const iDynTree::VectorDynSize& inertialParams, - const std::string filetype="urdf", - const iDynTree::ModelExporterOptions options=iDynTree::ModelExporterOptions()); - - /** - * Update the inertial parameters of the loaded model. - * - * @note the inertial params vector follows the structure of the Model::getInertialParameters method. - */ - bool updateModelInertialParametersToFile(const std::string & filename, - const iDynTree::VectorDynSize& inertialParams, - const std::string filetype="urdf", - const iDynTree::ModelExporterOptions options=iDynTree::ModelExporterOptions()); - - /** - * Get the loaded model. - * - * @note This always return the model loaded via loadModel methods, and is not affected by the updateModel methods. - */ - const Model & model(); - - /** - * Get the loaded sensors. - * - * @note This always return the model loaded via loadModel method, and is not affected by the updateModel methods. - */ - const SensorsList & sensors(); - - /** - * Return true if the model have been correctly true. - * - * @note This always return the validity of the model loaded via loadModel method, and is not affected by the updateModel methods. - * @return True if the model was loaded correctly. - */ - bool isValid(); - //@} -}; +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif -} +#include #endif diff --git a/src/model_io/codecs/include/iDynTree/ModelIO/ModelExporter.h b/src/model_io/codecs/include/iDynTree/ModelIO/ModelExporter.h index 323214041cd..7b7e8cefe66 100644 --- a/src/model_io/codecs/include/iDynTree/ModelIO/ModelExporter.h +++ b/src/model_io/codecs/include/iDynTree/ModelIO/ModelExporter.h @@ -1,198 +1,13 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef IDYNTREE_MODEL_EXPORTER_H -#define IDYNTREE_MODEL_EXPORTER_H +#ifndef IDYNTREE_DEPRECATED_MODEL_EXPORTER_H +#define IDYNTREE_DEPRECATED_MODEL_EXPORTER_H -#include -#include - -#include -#include -#include - -namespace iDynTree -{ - -/** - * \ingroup iDynTreeModelIO - * - * Options for the iDynTree exporter. - */ -struct ModelExporterOptions -{ - /** - * Specify the base link of the exported model. - * - * Differently from the iDynTree::Model class, some file formats (such as the URDF) represent the multibody structure - * using a directed tree representation, for which it is necessary to explicitly specify a base link. - * - * If this string is empty (default value), the default base link contained in the model will be used. - * - * Default value: "". - * Supported formats: urdf. - */ - std::string baseLink; - - /** - * Select if the first additional frame of the base link is exported as fake base link. - * - * The URDF exporter by default exports the first additional frame of the base link as - * a parent "fake" link to the actual base link, as a workaround for https://github.com/ros/kdl_parser/issues/27). - * By setting this option to false, is possible to disable this behaviour, for more info see iDynTree::ModelExporter docs. - * This option is ignored in non-URDF exporter. - * - * Default value: true. - * Supported formats: urdf. - */ - bool exportFirstBaseLinkAdditionalFrameAsFakeURDFBase; - - /** - * Specify the robot name. - * - * Default: "iDynTreeURDFModelExportModelName". - * Supported formats: urdf. - */ - std::string robotExportedName; - /** - * Specify the xml blobs to be exported at the end of the urdf as child of the robot tag. - * - * Default: {}. - * Supported formats: urdf. - */ - std::vector xmlBlobs; - - /** - * Constructor. - */ - ModelExporterOptions(); - -}; - - -/** - * \ingroup iDynTreeModelIO - * - * Helper class to export a model to the supported textual formats. - * - * Currently the only format supported for export is the URDF format, - * as it is described in http://wiki.ros.org/urdf/XML . - * - * Only iDynTree::Model classes that represent multibody system with no loops - * can be exported. - * - * Furthermore, currently the model exporter only exports a subset of the features - * supported in iDynTree::Model. In particular, the following features are not exported: - * - * * Sensors - * * Joint limits, damping and static friction. - * - * # Format documentation - * - * The following format are supported by the exporter. - * - * | Format | Extendend Name | Website | String for filetype argument | - * |:-----------------------------:|:-------:|:-------:|:--------:| - * | URDF | Unified Robot Description Format | http://wiki.ros.org/urdf | `urdf` | - * - * ## URDF - * - * As the URDF format does not distinguish between frames and links (see https://discourse.ros.org/t/urdf-ng-link-and-frame-concepts/56 - * for an extensive discussion on this) the URDF model exporter converts iDynTree's *additional frames* to mass-less and shape-less - * *fake* URDF links that are connected as child links via `fixed` joints to the corresponding **real** URDF links. - * - * Furthermore, it is widespread use in URDF models to never use a real link (with mass) as the root link of a model, mainly - * due to workaround a bug in official %KDL parser used in ROS (see https://github.com/ros/kdl_parser/issues/27 for more info). For this reason, - * if the selected base_link has at least one additional frame, by default the first additional frame of the base link is added as a **parent** fake URDF link, - * instead as a **child** fake URDF link as done with the rest of %iDynTree's additional frames. If no additional frame is available for the base link, - * the base link of the URDF will have a mass, and will generate a warning then used with the ROS's [`kdl_parser`](https://github.com/ros/kdl_parser) . - * This behaviour can be disabled by setting to false the `exportFirstBaseLinkAdditionalFrameAsFakeURDFBase` attribute of ModelExporterOptions. - * - */ -class ModelExporter -{ -private: - - class Pimpl; - std::unique_ptr m_pimpl; - -public: - - /** - * @name Constructor/Destructor - */ - //@{ - - /** - * Constructor - */ - ModelExporter(); - - ~ModelExporter(); - - //@} - - /** - * @name Model exporting and definition methods - * This methods are used to export the structure of your model. - */ - //@{ - const ModelExporterOptions& exportingOptions() const; - - void setExportingOptions(const ModelExporterOptions& options); - - /** - * Specifies the model of the robot to export. - * - * @param[in] model The used model. - * @param[in] sensors The used sensors. - * @param[in] options The used options. - * @return true if all went well, false otherwise. - */ - bool init(const Model& model, - const SensorsList& sensors=SensorsList(), - const ModelExporterOptions options=ModelExporterOptions()); - - /** - * Get the loaded model that will be exported. - * - */ - const Model & model(); - - /** - * Get the loaded sensors that will be exported. - */ - const SensorsList & sensors(); - - /** - * Return true if the model have been correctly loaded, and can be exported. - * - * @return True if the model was loaded correctly. - */ - bool isValid(); - - /** - * Export the model of the robot to a string. - * - * @param modelString string containg the model of the robot. - * @param filetype type of the file to load, currently supporting only urdf type. - * - */ - bool exportModelToString(std::string & modelString, const std::string filetype="urdf"); - - /** - * Export the model of the robot to an external file. - * - * @param filename path to the file to export. - * It can be either a relative filename with respect to the current working directory, - * or an absolute filename. - * @param filetype type of the file to load, currently supporting only urdf type. - * - */ - bool exportModelToFile(const std::string & filename, const std::string filetype="urdf"); - //@} -}; +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif -} +#include #endif diff --git a/src/model_io/codecs/include/iDynTree/ModelIO/ModelLoader.h b/src/model_io/codecs/include/iDynTree/ModelIO/ModelLoader.h index be28f18af0e..0557fe139bc 100644 --- a/src/model_io/codecs/include/iDynTree/ModelIO/ModelLoader.h +++ b/src/model_io/codecs/include/iDynTree/ModelIO/ModelLoader.h @@ -1,226 +1,13 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef IDYNTREE_MODEL_LOADER_H -#define IDYNTREE_MODEL_LOADER_H +#ifndef IDYNTREE_DEPRECATED_MODEL_LOADER_H +#define IDYNTREE_DEPRECATED_MODEL_LOADER_H -#include -#include - -#include -#include -#include - -namespace iDynTree -{ - -/** - * \ingroup iDynTreeModelIO - * - * Options for the iDynTree parser. - */ -struct ModelParserOptions -{ - // TODO: migrate to set/get - // ???: what about adding a search dir instead of a filename? -public: - - /** - * If true, add to the model the sensor frames - * as additional frames with the same name of the sensor. - * If there is already a link or additional frame with the same - * name of the sensor, a warning is printed and no frame is added. - */ - bool addSensorFramesAsAdditionalFrames; - - /** - * Original filename of the URDF sensor parsed. - * - * This attribute is the original filename of the URDF sensor parsed. - * It is useful when loading a model from a string, if that URDF string - * has tags that point to external meshes. To find the location - * of this external meshes, we need also the original filename of the URDF file. - */ - std::string originalFilename; - - /** Default options - * - * - addSensorFramesAsAdditionalFrames = True - * - originalFilename = empty string - */ - ModelParserOptions(); - -}; - - -/** - * \ingroup iDynTreeModelIO - * - * Helper class to load a model from a generic format. - * - * Unless the methods for loading a model with an explicit serialization are used, - * the default joint serialization of the model loaded will be a "normalized" joint serialization - * based on the default base link, see the iDynTree::createModelWithNormalizedJointNumbering function - * for more details. - */ -class ModelLoader -{ -private: - - class ModelLoaderPimpl; - std::unique_ptr m_pimpl; - -public: - - /** - * @name Constructor/Destructor - */ - //@{ - - /** - * Constructor - * - */ - ModelLoader(); - - ~ModelLoader(); - - //@} - - /** - * @name Model loading and definition methods - * This methods are used to load the structure of your model. - */ - //@{ - const ModelParserOptions& parsingOptions() const; - - void setParsingOptions(const ModelParserOptions& options); - - /** - * Load the model of the robot from a string. - * - * @param modelString string containg the model of the robot. - * @param filetype type of the file to load, currently supporting only urdf type - * @param packageDirs a vector containing the different directories where to search for model meshes - * @note In case no package is specified ModelLoader will look for the meshes in `GAZEBO_MODEL_PATH`, - * `ROS_PACKAGE_PATH` and `AMENT_PREFIX_PATH` - * @note If a given model searches for the meshes in `package://StrangeModel/Nested/mesh.stl`, - * and the actual mesh is in `/usr/local/share/StrangeModel/Nested/mesh.stl`, `packageDirs` - * should contain `/usr/local/share`. - */ - bool loadModelFromString(const std::string & modelString, - const std::string & filetype="urdf", - const std::vector& packageDirs = {}); - - /** - * Load the model of the robot from an external file. - * - * @param filename path to the file to load - * @param filetype type of the file to load, currently supporting only urdf type. - * @param packageDirs a vector containing the different directories where to search for model meshes - * @note In case no package is specified ModelLoader will look for the meshes in `GAZEBO_MODEL_PATH`, - * `ROS_PACKAGE_PATH` and `AMENT_PREFIX_PATH` - * @note If a given model searches for the meshes in `package://StrangeModel/Nested/mesh.stl`, - * and the actual mesh is in `/usr/local/share/StrangeModel/Nested/mesh.stl`, `packageDirs` - * should contain `/usr/local/share`. - */ - bool loadModelFromFile(const std::string & filename, - const std::string & filetype="urdf", - const std::vector& packageDirs = {}); - - /** - * Load reduced model from another model, specifyng only the desired joints in the model. - * - * All other joints will be considered to be fixed to their default position, - * and their child links will be lumped together. - * - * @note the order of the degreese of freedom of the newly loaded model - * will be the one specified by the input joints serialization, i.e. consideredJoints - * - * @param[in] filename path to the file to load. - * @param[in] consideredJoints list of joints to consider in the model. - * @param[in] filetype (optional) explicit definition of the type of the loaded file. Only "urdf" is supported at the moment. - * @return true if all went well (files were correctly loaded and consistent), false otherwise. - * - * \note Until https://github.com/robotology/idyntree/issues/132 is fixed, this method does not - * accounts for sensors. - * - */ - bool loadReducedModelFromFullModel(const Model& fullModel, - const std::vector & consideredJoints, - const std::string filetype=""); - - /** - * Load reduced model from string, specifyng only the desired joints in the model. - * - * All other joints will be considered to be fixed to their default position, - * and their child links will be lumped together. - * - * @param[in] modelString string containg the model of the robot. - * @param[in] consideredJoints list of joints to consider in the model. - * @param[in] filetype (optional) explicit definiton of the filetype to load. - * Only "urdf" is supported at the moment. - * @param[in] packageDirs (optional) a vector containing the different directories where to - * search for model meshes - * @return true if all went well (files were correctly loaded and consistent), false otherwise. - * - * @note In case no package is specified ModelLoader will look for the meshes in `GAZEBO_MODEL_PATH`, - * `ROS_PACKAGE_PATH` and `AMENT_PREFIX_PATH` - * @note If a given model searches for the meshes in `package://StrangeModel/Nested/mesh.stl`, - * and the actual mesh is in `/usr/local/share/StrangeModel/Nested/mesh.stl`, `packageDirs` - * should contain `/usr/local/share`. - * - */ - bool loadReducedModelFromString(const std::string modelString, - const std::vector & consideredJoints, - const std::string filetype="", - const std::vector& packageDirs = {}); - - /** - * Load reduced model from file, specifyng only the desired joints in the model. - * - * All other joints will be considered to be fixed to their default position, - * and their child links will be lumped together. - * - * @param[in] filename path to the file to load. - * @param[in] consideredJoints list of joints to consider in the model. - * @param[in] filetype (optional) explicit definiton of the filetype to load. - * Only "urdf" is supported at the moment. - * @param[in] packageDirs (optional) a vector containing the different directories where to - * search for model meshes - * @return true if all went well (files were correctly loaded and consistent), false otherwise. - * - * @note In case no package is specified ModelLoader will look for the meshes in `GAZEBO_MODEL_PATH`, - * `ROS_PACKAGE_PATH` and `AMENT_PREFIX_PATH` - * @note If a given model searches for the meshes in `package://StrangeModel/Nested/mesh.stl`, - * and the actual mesh is in `/usr/local/share/StrangeModel/Nested/mesh.stl`, `packageDirs` - * should contain `/usr/local/share`. - */ - bool loadReducedModelFromFile(const std::string filename, - const std::vector & consideredJoints, - const std::string filetype="", - const std::vector& packageDirs = {}); - - /** - * Get the loaded model. - * - */ - const Model & model(); - - /** - * Get the loaded sensors. - */ - const SensorsList & sensors(); - - /** - * Return true if the model have been correctly true. - * - * @return True if the model was loaded correctly. - */ - bool isValid(); - //@} -}; +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif -} +#include #endif diff --git a/src/model_io/codecs/include/iDynTree/ModelIO/URDFDofsImport.h b/src/model_io/codecs/include/iDynTree/ModelIO/URDFDofsImport.h index 8547d84d6a3..3325c1a9ecd 100644 --- a/src/model_io/codecs/include/iDynTree/ModelIO/URDFDofsImport.h +++ b/src/model_io/codecs/include/iDynTree/ModelIO/URDFDofsImport.h @@ -1,37 +1,13 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef IDYNTREE_URDF_DOFS_IMPORT_H -#define IDYNTREE_URDF_DOFS_IMPORT_H +#ifndef IDYNTREE_DEPRECATED_URDF_DOFS_IMPORT_H +#define IDYNTREE_DEPRECATED_URDF_DOFS_IMPORT_H -#include -#include - -namespace iDynTree - -{ - -/** - * \ingroup iDynTreeModelIO - * - * Load a list of dofs names from a URDF file. - * - * @return true if all went ok, false otherwise. - * - */ -bool dofsListFromURDF(const std::string & urdf_filename, - std::vector& dofs); - -/** - * \ingroup iDynTreeModelIO - * - * Load a list of dofs object from a URDF string. - * - * @return true if all went ok, false otherwise. - */ -bool dofsListFromURDFString(const std::string & urdf_string, - std::vector& dofs); +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif -} +#include #endif diff --git a/src/model_io/codecs/include/iDynTree/ModelLoader.h b/src/model_io/codecs/include/iDynTree/ModelLoader.h new file mode 100644 index 00000000000..c197bfc244e --- /dev/null +++ b/src/model_io/codecs/include/iDynTree/ModelLoader.h @@ -0,0 +1,226 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_MODEL_LOADER_H +#define IDYNTREE_MODEL_LOADER_H + +#include +#include + +#include +#include +#include + +namespace iDynTree +{ + +/** + * \ingroup iDynTreeModelIO + * + * Options for the iDynTree parser. + */ +struct ModelParserOptions +{ + // TODO: migrate to set/get + // ???: what about adding a search dir instead of a filename? +public: + + /** + * If true, add to the model the sensor frames + * as additional frames with the same name of the sensor. + * If there is already a link or additional frame with the same + * name of the sensor, a warning is printed and no frame is added. + */ + bool addSensorFramesAsAdditionalFrames; + + /** + * Original filename of the URDF sensor parsed. + * + * This attribute is the original filename of the URDF sensor parsed. + * It is useful when loading a model from a string, if that URDF string + * has tags that point to external meshes. To find the location + * of this external meshes, we need also the original filename of the URDF file. + */ + std::string originalFilename; + + /** Default options + * + * - addSensorFramesAsAdditionalFrames = True + * - originalFilename = empty string + */ + ModelParserOptions(); + +}; + + +/** + * \ingroup iDynTreeModelIO + * + * Helper class to load a model from a generic format. + * + * Unless the methods for loading a model with an explicit serialization are used, + * the default joint serialization of the model loaded will be a "normalized" joint serialization + * based on the default base link, see the iDynTree::createModelWithNormalizedJointNumbering function + * for more details. + */ +class ModelLoader +{ +private: + + class ModelLoaderPimpl; + std::unique_ptr m_pimpl; + +public: + + /** + * @name Constructor/Destructor + */ + //@{ + + /** + * Constructor + * + */ + ModelLoader(); + + ~ModelLoader(); + + //@} + + /** + * @name Model loading and definition methods + * This methods are used to load the structure of your model. + */ + //@{ + const ModelParserOptions& parsingOptions() const; + + void setParsingOptions(const ModelParserOptions& options); + + /** + * Load the model of the robot from a string. + * + * @param modelString string containg the model of the robot. + * @param filetype type of the file to load, currently supporting only urdf type + * @param packageDirs a vector containing the different directories where to search for model meshes + * @note In case no package is specified ModelLoader will look for the meshes in `GAZEBO_MODEL_PATH`, + * `ROS_PACKAGE_PATH` and `AMENT_PREFIX_PATH` + * @note If a given model searches for the meshes in `package://StrangeModel/Nested/mesh.stl`, + * and the actual mesh is in `/usr/local/share/StrangeModel/Nested/mesh.stl`, `packageDirs` + * should contain `/usr/local/share`. + */ + bool loadModelFromString(const std::string & modelString, + const std::string & filetype="urdf", + const std::vector& packageDirs = {}); + + /** + * Load the model of the robot from an external file. + * + * @param filename path to the file to load + * @param filetype type of the file to load, currently supporting only urdf type. + * @param packageDirs a vector containing the different directories where to search for model meshes + * @note In case no package is specified ModelLoader will look for the meshes in `GAZEBO_MODEL_PATH`, + * `ROS_PACKAGE_PATH` and `AMENT_PREFIX_PATH` + * @note If a given model searches for the meshes in `package://StrangeModel/Nested/mesh.stl`, + * and the actual mesh is in `/usr/local/share/StrangeModel/Nested/mesh.stl`, `packageDirs` + * should contain `/usr/local/share`. + */ + bool loadModelFromFile(const std::string & filename, + const std::string & filetype="urdf", + const std::vector& packageDirs = {}); + + /** + * Load reduced model from another model, specifyng only the desired joints in the model. + * + * All other joints will be considered to be fixed to their default position, + * and their child links will be lumped together. + * + * @note the order of the degreese of freedom of the newly loaded model + * will be the one specified by the input joints serialization, i.e. consideredJoints + * + * @param[in] filename path to the file to load. + * @param[in] consideredJoints list of joints to consider in the model. + * @param[in] filetype (optional) explicit definition of the type of the loaded file. Only "urdf" is supported at the moment. + * @return true if all went well (files were correctly loaded and consistent), false otherwise. + * + * \note Until https://github.com/robotology/idyntree/issues/132 is fixed, this method does not + * accounts for sensors. + * + */ + bool loadReducedModelFromFullModel(const Model& fullModel, + const std::vector & consideredJoints, + const std::string filetype=""); + + /** + * Load reduced model from string, specifyng only the desired joints in the model. + * + * All other joints will be considered to be fixed to their default position, + * and their child links will be lumped together. + * + * @param[in] modelString string containg the model of the robot. + * @param[in] consideredJoints list of joints to consider in the model. + * @param[in] filetype (optional) explicit definiton of the filetype to load. + * Only "urdf" is supported at the moment. + * @param[in] packageDirs (optional) a vector containing the different directories where to + * search for model meshes + * @return true if all went well (files were correctly loaded and consistent), false otherwise. + * + * @note In case no package is specified ModelLoader will look for the meshes in `GAZEBO_MODEL_PATH`, + * `ROS_PACKAGE_PATH` and `AMENT_PREFIX_PATH` + * @note If a given model searches for the meshes in `package://StrangeModel/Nested/mesh.stl`, + * and the actual mesh is in `/usr/local/share/StrangeModel/Nested/mesh.stl`, `packageDirs` + * should contain `/usr/local/share`. + * + */ + bool loadReducedModelFromString(const std::string modelString, + const std::vector & consideredJoints, + const std::string filetype="", + const std::vector& packageDirs = {}); + + /** + * Load reduced model from file, specifyng only the desired joints in the model. + * + * All other joints will be considered to be fixed to their default position, + * and their child links will be lumped together. + * + * @param[in] filename path to the file to load. + * @param[in] consideredJoints list of joints to consider in the model. + * @param[in] filetype (optional) explicit definiton of the filetype to load. + * Only "urdf" is supported at the moment. + * @param[in] packageDirs (optional) a vector containing the different directories where to + * search for model meshes + * @return true if all went well (files were correctly loaded and consistent), false otherwise. + * + * @note In case no package is specified ModelLoader will look for the meshes in `GAZEBO_MODEL_PATH`, + * `ROS_PACKAGE_PATH` and `AMENT_PREFIX_PATH` + * @note If a given model searches for the meshes in `package://StrangeModel/Nested/mesh.stl`, + * and the actual mesh is in `/usr/local/share/StrangeModel/Nested/mesh.stl`, `packageDirs` + * should contain `/usr/local/share`. + */ + bool loadReducedModelFromFile(const std::string filename, + const std::vector & consideredJoints, + const std::string filetype="", + const std::vector& packageDirs = {}); + + /** + * Get the loaded model. + * + */ + const Model & model(); + + /** + * Get the loaded sensors. + */ + const SensorsList & sensors(); + + /** + * Return true if the model have been correctly true. + * + * @return True if the model was loaded correctly. + */ + bool isValid(); + //@} +}; + +} + +#endif diff --git a/src/model_io/codecs/include/iDynTree/URDFDofsImport.h b/src/model_io/codecs/include/iDynTree/URDFDofsImport.h new file mode 100644 index 00000000000..8547d84d6a3 --- /dev/null +++ b/src/model_io/codecs/include/iDynTree/URDFDofsImport.h @@ -0,0 +1,37 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_URDF_DOFS_IMPORT_H +#define IDYNTREE_URDF_DOFS_IMPORT_H + +#include +#include + +namespace iDynTree + +{ + +/** + * \ingroup iDynTreeModelIO + * + * Load a list of dofs names from a URDF file. + * + * @return true if all went ok, false otherwise. + * + */ +bool dofsListFromURDF(const std::string & urdf_filename, + std::vector& dofs); + +/** + * \ingroup iDynTreeModelIO + * + * Load a list of dofs object from a URDF string. + * + * @return true if all went ok, false otherwise. + */ +bool dofsListFromURDFString(const std::string & urdf_string, + std::vector& dofs); + +} + +#endif diff --git a/src/model_io/codecs/include/private/InertialElement.h b/src/model_io/codecs/include/private/InertialElement.h index bfae2313c2f..6ebce4dcb79 100644 --- a/src/model_io/codecs/include/private/InertialElement.h +++ b/src/model_io/codecs/include/private/InertialElement.h @@ -6,8 +6,8 @@ #include -#include -#include +#include +#include namespace iDynTree { class InertialElement; diff --git a/src/model_io/codecs/include/private/JointElement.h b/src/model_io/codecs/include/private/JointElement.h index 89fd02f5541..b5195021fdf 100644 --- a/src/model_io/codecs/include/private/JointElement.h +++ b/src/model_io/codecs/include/private/JointElement.h @@ -6,10 +6,10 @@ #include -#include +#include -#include -#include +#include +#include #include #include diff --git a/src/model_io/codecs/include/private/LinkElement.h b/src/model_io/codecs/include/private/LinkElement.h index fcf4a14e114..9542dfca1c4 100644 --- a/src/model_io/codecs/include/private/LinkElement.h +++ b/src/model_io/codecs/include/private/LinkElement.h @@ -8,7 +8,7 @@ #include "VisualElement.h" -#include +#include namespace iDynTree { class LinkElement; diff --git a/src/model_io/codecs/include/private/MaterialElement.h b/src/model_io/codecs/include/private/MaterialElement.h index 3b3e0ffa8ae..3f35c44d2b4 100644 --- a/src/model_io/codecs/include/private/MaterialElement.h +++ b/src/model_io/codecs/include/private/MaterialElement.h @@ -6,7 +6,7 @@ #include -#include +#include #include #include diff --git a/src/model_io/codecs/include/private/SensorElement.h b/src/model_io/codecs/include/private/SensorElement.h index 3b93349cd93..84f449bbec5 100644 --- a/src/model_io/codecs/include/private/SensorElement.h +++ b/src/model_io/codecs/include/private/SensorElement.h @@ -6,8 +6,8 @@ #include -#include -#include +#include +#include #include #include diff --git a/src/model_io/codecs/include/private/URDFDocument.h b/src/model_io/codecs/include/private/URDFDocument.h index 669ae14769f..ab4ce845724 100644 --- a/src/model_io/codecs/include/private/URDFDocument.h +++ b/src/model_io/codecs/include/private/URDFDocument.h @@ -10,10 +10,10 @@ #include "MaterialElement.h" #include "SensorElement.h" #include "VisualElement.h" -#include "iDynTree/ModelIO/ModelLoader.h" +#include "iDynTree/ModelLoader.h" -#include -#include +#include +#include #include #include diff --git a/src/model_io/codecs/include/private/URDFModelExport.h b/src/model_io/codecs/include/private/URDFModelExport.h index a53958609f0..fb0d89ab1b2 100644 --- a/src/model_io/codecs/include/private/URDFModelExport.h +++ b/src/model_io/codecs/include/private/URDFModelExport.h @@ -6,7 +6,7 @@ #include -#include +#include namespace iDynTree diff --git a/src/model_io/codecs/include/private/URDFParsingUtils.h b/src/model_io/codecs/include/private/URDFParsingUtils.h index 46a9ec337f6..831d9e0bfcf 100644 --- a/src/model_io/codecs/include/private/URDFParsingUtils.h +++ b/src/model_io/codecs/include/private/URDFParsingUtils.h @@ -10,8 +10,8 @@ #include #include -#include -#include +#include +#include #include diff --git a/src/model_io/codecs/include/private/VisualElement.h b/src/model_io/codecs/include/private/VisualElement.h index c7a807139f5..84439033767 100644 --- a/src/model_io/codecs/include/private/VisualElement.h +++ b/src/model_io/codecs/include/private/VisualElement.h @@ -8,8 +8,8 @@ #include "MaterialElement.h" -#include -#include +#include +#include #include #include diff --git a/src/model_io/codecs/src/ForceTorqueSensorElement.cpp b/src/model_io/codecs/src/ForceTorqueSensorElement.cpp index 4f92e09f8c9..99aaa21d2fb 100644 --- a/src/model_io/codecs/src/ForceTorqueSensorElement.cpp +++ b/src/model_io/codecs/src/ForceTorqueSensorElement.cpp @@ -3,8 +3,8 @@ #include "ForceTorqueSensorElement.h" -#include -#include +#include +#include namespace iDynTree { diff --git a/src/model_io/codecs/src/GeometryElement.cpp b/src/model_io/codecs/src/GeometryElement.cpp index c895df4c46f..123b61d5062 100644 --- a/src/model_io/codecs/src/GeometryElement.cpp +++ b/src/model_io/codecs/src/GeometryElement.cpp @@ -6,8 +6,8 @@ #include "URDFParsingUtils.h" #include -#include -#include +#include +#include namespace iDynTree{ diff --git a/src/model_io/codecs/src/JointElement.cpp b/src/model_io/codecs/src/JointElement.cpp index d7fd8056dfa..29f6d456e57 100644 --- a/src/model_io/codecs/src/JointElement.cpp +++ b/src/model_io/codecs/src/JointElement.cpp @@ -9,9 +9,9 @@ #include #include -#include -#include -#include +#include +#include +#include #include diff --git a/src/model_io/codecs/src/LinkElement.cpp b/src/model_io/codecs/src/LinkElement.cpp index e3bde83bd83..7a729efe29c 100644 --- a/src/model_io/codecs/src/LinkElement.cpp +++ b/src/model_io/codecs/src/LinkElement.cpp @@ -8,7 +8,7 @@ #include -#include +#include #include #include diff --git a/src/model_io/codecs/src/MaterialElement.cpp b/src/model_io/codecs/src/MaterialElement.cpp index ad6e8e863b6..56fc5bb95d9 100644 --- a/src/model_io/codecs/src/MaterialElement.cpp +++ b/src/model_io/codecs/src/MaterialElement.cpp @@ -7,7 +7,7 @@ #include -#include +#include namespace iDynTree { diff --git a/src/model_io/codecs/src/ModelCalibrationHelper.cpp b/src/model_io/codecs/src/ModelCalibrationHelper.cpp index 70340c5d15f..8b46a0332f1 100644 --- a/src/model_io/codecs/src/ModelCalibrationHelper.cpp +++ b/src/model_io/codecs/src/ModelCalibrationHelper.cpp @@ -1,9 +1,9 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include -#include -#include +#include +#include +#include #include #include diff --git a/src/model_io/codecs/src/ModelExporter.cpp b/src/model_io/codecs/src/ModelExporter.cpp index b30c2108c2c..ce13ab85693 100644 --- a/src/model_io/codecs/src/ModelExporter.cpp +++ b/src/model_io/codecs/src/ModelExporter.cpp @@ -1,7 +1,7 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include "iDynTree/ModelIO/ModelExporter.h" +#include "iDynTree/ModelExporter.h" #include "URDFModelExport.h" diff --git a/src/model_io/codecs/src/ModelLoader.cpp b/src/model_io/codecs/src/ModelLoader.cpp index 24de5b7faa7..7a03d2ce6b7 100644 --- a/src/model_io/codecs/src/ModelLoader.cpp +++ b/src/model_io/codecs/src/ModelLoader.cpp @@ -1,12 +1,12 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include "iDynTree/ModelIO/ModelLoader.h" +#include "iDynTree/ModelLoader.h" #include "URDFDocument.h" #include -#include +#include #include diff --git a/src/model_io/codecs/src/OriginElement.cpp b/src/model_io/codecs/src/OriginElement.cpp index d0fb80ad0bb..1031b6c049d 100644 --- a/src/model_io/codecs/src/OriginElement.cpp +++ b/src/model_io/codecs/src/OriginElement.cpp @@ -7,7 +7,7 @@ #include -#include +#include namespace iDynTree { OriginElement::OriginElement( diff --git a/src/model_io/codecs/src/RobotElement.cpp b/src/model_io/codecs/src/RobotElement.cpp index a3774f42122..4fabf5a59b7 100644 --- a/src/model_io/codecs/src/RobotElement.cpp +++ b/src/model_io/codecs/src/RobotElement.cpp @@ -8,8 +8,8 @@ #include "SensorElement.h" #include "MaterialElement.h" -#include -#include +#include +#include #include diff --git a/src/model_io/codecs/src/SensorElement.cpp b/src/model_io/codecs/src/SensorElement.cpp index 7036ca65911..803c6f04259 100644 --- a/src/model_io/codecs/src/SensorElement.cpp +++ b/src/model_io/codecs/src/SensorElement.cpp @@ -7,10 +7,10 @@ #include "ForceTorqueSensorElement.h" #include -#include -#include -#include -#include +#include +#include +#include +#include namespace iDynTree { diff --git a/src/model_io/codecs/src/URDFDocument.cpp b/src/model_io/codecs/src/URDFDocument.cpp index d5052e7b7f6..355a7820bce 100644 --- a/src/model_io/codecs/src/URDFDocument.cpp +++ b/src/model_io/codecs/src/URDFDocument.cpp @@ -4,10 +4,10 @@ #include "URDFDocument.h" #include "RobotElement.h" -#include -#include -#include -#include +#include +#include +#include +#include #include #include diff --git a/src/model_io/codecs/src/URDFDofsImport.cpp b/src/model_io/codecs/src/URDFDofsImport.cpp index 310ed915457..a930998eb36 100644 --- a/src/model_io/codecs/src/URDFDofsImport.cpp +++ b/src/model_io/codecs/src/URDFDofsImport.cpp @@ -1,11 +1,11 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include "iDynTree/ModelIO/URDFDofsImport.h" +#include "iDynTree/URDFDofsImport.h" -#include "iDynTree/ModelIO/ModelLoader.h" +#include "iDynTree/ModelLoader.h" -#include +#include #include #include diff --git a/src/model_io/codecs/src/URDFModelExport.cpp b/src/model_io/codecs/src/URDFModelExport.cpp index c4c64fb5e8c..c372933ea37 100644 --- a/src/model_io/codecs/src/URDFModelExport.cpp +++ b/src/model_io/codecs/src/URDFModelExport.cpp @@ -4,16 +4,16 @@ #include "URDFModelExport.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include "URDFParsingUtils.h" diff --git a/src/model_io/codecs/src/VisualElement.cpp b/src/model_io/codecs/src/VisualElement.cpp index 7f1de91990d..8a58e2ab5dc 100644 --- a/src/model_io/codecs/src/VisualElement.cpp +++ b/src/model_io/codecs/src/VisualElement.cpp @@ -8,7 +8,7 @@ #include "OriginElement.h" #include -#include +#include namespace iDynTree { diff --git a/src/model_io/codecs/tests/ModelCalibrationHelperUnitTest.cpp b/src/model_io/codecs/tests/ModelCalibrationHelperUnitTest.cpp index e04bc67dab0..ec682c88625 100644 --- a/src/model_io/codecs/tests/ModelCalibrationHelperUnitTest.cpp +++ b/src/model_io/codecs/tests/ModelCalibrationHelperUnitTest.cpp @@ -3,11 +3,11 @@ #include "testModels.h" -#include +#include -#include -#include -#include +#include +#include +#include #include #include diff --git a/src/model_io/codecs/tests/ModelExporterUnitTest.cpp b/src/model_io/codecs/tests/ModelExporterUnitTest.cpp index eefe65d3bef..e461f7b5a3f 100644 --- a/src/model_io/codecs/tests/ModelExporterUnitTest.cpp +++ b/src/model_io/codecs/tests/ModelExporterUnitTest.cpp @@ -3,13 +3,13 @@ #include "testModels.h" -#include +#include -#include -#include -#include -#include +#include +#include +#include +#include #include diff --git a/src/model_io/codecs/tests/PredictSensorsMeasurementUnitTest.cpp b/src/model_io/codecs/tests/PredictSensorsMeasurementUnitTest.cpp index 49ef6c75d6d..d4492e1cc22 100644 --- a/src/model_io/codecs/tests/PredictSensorsMeasurementUnitTest.cpp +++ b/src/model_io/codecs/tests/PredictSensorsMeasurementUnitTest.cpp @@ -3,17 +3,17 @@ #include -# include +# include #include "testModels.h" -#include -#include -#include -#include -#include -#include -#include - -#include +#include +#include +#include +#include +#include +#include +#include + +#include const double acclTestVal = 1.5; const double gyroTestVal = 1.5; #include diff --git a/src/model_io/codecs/tests/URDFGenericSensorImportUnitTest.cpp b/src/model_io/codecs/tests/URDFGenericSensorImportUnitTest.cpp index c2b7dc0b0a3..b625cdc86b9 100644 --- a/src/model_io/codecs/tests/URDFGenericSensorImportUnitTest.cpp +++ b/src/model_io/codecs/tests/URDFGenericSensorImportUnitTest.cpp @@ -1,10 +1,10 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include -#include +#include +#include #include "testModels.h" -#include +#include #include #include diff --git a/src/model_io/codecs/tests/URDFModelImportUnitTest.cpp b/src/model_io/codecs/tests/URDFModelImportUnitTest.cpp index 50f8ea50209..565c0bfe3cd 100644 --- a/src/model_io/codecs/tests/URDFModelImportUnitTest.cpp +++ b/src/model_io/codecs/tests/URDFModelImportUnitTest.cpp @@ -3,11 +3,11 @@ #include "testModels.h" -#include +#include -#include -#include -#include +#include +#include +#include #include #include diff --git a/src/model_io/codecs/tests/icubSensorURDFUnitTest.cpp b/src/model_io/codecs/tests/icubSensorURDFUnitTest.cpp index 95a0c3d6f9d..04a8bb3122d 100644 --- a/src/model_io/codecs/tests/icubSensorURDFUnitTest.cpp +++ b/src/model_io/codecs/tests/icubSensorURDFUnitTest.cpp @@ -1,14 +1,14 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -# include +# include #include "testModels.h" -#include -#include -#include +#include +#include +#include -#include -#include -#include +#include +#include +#include #include #include diff --git a/src/model_io/xml/src/XMLParser.cpp b/src/model_io/xml/src/XMLParser.cpp index f59e53bd3d9..424c397b4dc 100644 --- a/src/model_io/xml/src/XMLParser.cpp +++ b/src/model_io/xml/src/XMLParser.cpp @@ -7,7 +7,7 @@ #include "XMLAttribute.h" #include "XMLDocument.h" -#include +#include #include #include diff --git a/src/model_io/xml/tests/XMLParserUnitTest.cpp b/src/model_io/xml/tests/XMLParserUnitTest.cpp index 148b3a4caf3..1435e84f5a0 100644 --- a/src/model_io/xml/tests/XMLParserUnitTest.cpp +++ b/src/model_io/xml/tests/XMLParserUnitTest.cpp @@ -1,4 +1,4 @@ -#include +#include #include #include diff --git a/src/optimalcontrol/include/iDynTree/Constraint.h b/src/optimalcontrol/include/iDynTree/Constraint.h index 18feb3963aa..92f6053b239 100644 --- a/src/optimalcontrol/include/iDynTree/Constraint.h +++ b/src/optimalcontrol/include/iDynTree/Constraint.h @@ -13,7 +13,7 @@ #include #include -#include +#include #include namespace iDynTree { diff --git a/src/optimalcontrol/include/iDynTree/DynamicalSystem.h b/src/optimalcontrol/include/iDynTree/DynamicalSystem.h index f369b729f96..3a97c35ff04 100644 --- a/src/optimalcontrol/include/iDynTree/DynamicalSystem.h +++ b/src/optimalcontrol/include/iDynTree/DynamicalSystem.h @@ -11,7 +11,7 @@ #ifndef IDYNTREE_OPTIMALCONTROL_DYNAMICALSYSTEM_H #define IDYNTREE_OPTIMALCONTROL_DYNAMICALSYSTEM_H -#include +#include #include namespace iDynTree { diff --git a/src/optimalcontrol/include/iDynTree/Integrator.h b/src/optimalcontrol/include/iDynTree/Integrator.h index 9bd9697a2b3..70a610f11e8 100644 --- a/src/optimalcontrol/include/iDynTree/Integrator.h +++ b/src/optimalcontrol/include/iDynTree/Integrator.h @@ -11,8 +11,8 @@ #ifndef IDYNTREE_OPTIMALCONTROL_INTEGRATOR_H #define IDYNTREE_OPTIMALCONTROL_INTEGRATOR_H -#include -#include +#include +#include #include #include #include diff --git a/src/optimalcontrol/include/iDynTree/Integrators/FixedStepIntegrator.h b/src/optimalcontrol/include/iDynTree/Integrators/FixedStepIntegrator.h index e4d2a945690..5204c0de31d 100644 --- a/src/optimalcontrol/include/iDynTree/Integrators/FixedStepIntegrator.h +++ b/src/optimalcontrol/include/iDynTree/Integrators/FixedStepIntegrator.h @@ -12,7 +12,7 @@ #define IDYNTREE_OPTIMALCONTROL_FIXEDSTEPINTEGRATOR_H #include -#include +#include namespace iDynTree { namespace optimalcontrol { diff --git a/src/optimalcontrol/include/iDynTree/Integrators/RK4.h b/src/optimalcontrol/include/iDynTree/Integrators/RK4.h index 1e10aceca7d..4b086cac1fe 100644 --- a/src/optimalcontrol/include/iDynTree/Integrators/RK4.h +++ b/src/optimalcontrol/include/iDynTree/Integrators/RK4.h @@ -14,7 +14,7 @@ #include #include #include -#include +#include namespace iDynTree { namespace optimalcontrol { diff --git a/src/optimalcontrol/include/iDynTree/L2NormCost.h b/src/optimalcontrol/include/iDynTree/L2NormCost.h index 411840b00e9..784a2bf1b48 100644 --- a/src/optimalcontrol/include/iDynTree/L2NormCost.h +++ b/src/optimalcontrol/include/iDynTree/L2NormCost.h @@ -14,7 +14,7 @@ #include #include -#include +#include #include #include diff --git a/src/optimalcontrol/include/iDynTree/LinearConstraint.h b/src/optimalcontrol/include/iDynTree/LinearConstraint.h index 974d440a917..26b4fd274c3 100644 --- a/src/optimalcontrol/include/iDynTree/LinearConstraint.h +++ b/src/optimalcontrol/include/iDynTree/LinearConstraint.h @@ -12,7 +12,7 @@ #define IDYNTREE_OPTIMALCONTROL_LINEARCONSTRAINT_H #include -#include +#include #include #include #include diff --git a/src/optimalcontrol/include/iDynTree/OptimalControlProblem.h b/src/optimalcontrol/include/iDynTree/OptimalControlProblem.h index 4263b688c3c..4c1a6b8c434 100644 --- a/src/optimalcontrol/include/iDynTree/OptimalControlProblem.h +++ b/src/optimalcontrol/include/iDynTree/OptimalControlProblem.h @@ -15,7 +15,7 @@ #include #include -#include +#include #include #include diff --git a/src/optimalcontrol/include/iDynTree/QuadraticCost.h b/src/optimalcontrol/include/iDynTree/QuadraticCost.h index 7182b7f28dd..12257743e99 100644 --- a/src/optimalcontrol/include/iDynTree/QuadraticCost.h +++ b/src/optimalcontrol/include/iDynTree/QuadraticCost.h @@ -13,8 +13,8 @@ #include #include -#include -#include +#include +#include #include namespace iDynTree { diff --git a/src/optimalcontrol/include/iDynTree/SparsityStructure.h b/src/optimalcontrol/include/iDynTree/SparsityStructure.h index 41447e7d4fc..31c1ebf25b8 100644 --- a/src/optimalcontrol/include/iDynTree/SparsityStructure.h +++ b/src/optimalcontrol/include/iDynTree/SparsityStructure.h @@ -11,7 +11,7 @@ #ifndef IDYNTREE_OPTIMALCONTROL_SPARSITYSTRUCTURE_H #define IDYNTREE_OPTIMALCONTROL_SPARSITYSTRUCTURE_H -#include +#include #include #include #include diff --git a/src/optimalcontrol/include/iDynTree/TimeVaryingObject.h b/src/optimalcontrol/include/iDynTree/TimeVaryingObject.h index 9e661225224..79bbd0ea284 100644 --- a/src/optimalcontrol/include/iDynTree/TimeVaryingObject.h +++ b/src/optimalcontrol/include/iDynTree/TimeVaryingObject.h @@ -12,9 +12,9 @@ #ifndef IDYNTREE_OPTIMALCONTROL_TIMEVARYINGOBJECT_H #define IDYNTREE_OPTIMALCONTROL_TIMEVARYINGOBJECT_H -#include -#include -#include +#include +#include +#include namespace iDynTree { diff --git a/src/optimalcontrol/src/AlglibInterface.cpp b/src/optimalcontrol/src/AlglibInterface.cpp index 87b0ccb6a31..82723b83720 100644 --- a/src/optimalcontrol/src/AlglibInterface.cpp +++ b/src/optimalcontrol/src/AlglibInterface.cpp @@ -11,11 +11,11 @@ #include #include -#include +#include -#include -#include -#include +#include +#include +#include #include #include diff --git a/src/optimalcontrol/src/AlglibInterfaceNotImplemented.cpp b/src/optimalcontrol/src/AlglibInterfaceNotImplemented.cpp index 4c253820268..5419b3b0e7e 100644 --- a/src/optimalcontrol/src/AlglibInterfaceNotImplemented.cpp +++ b/src/optimalcontrol/src/AlglibInterfaceNotImplemented.cpp @@ -9,7 +9,7 @@ */ #include -#include +#include using namespace iDynTree::optimization; using namespace iDynTree; diff --git a/src/optimalcontrol/src/Constraint.cpp b/src/optimalcontrol/src/Constraint.cpp index 2d8e88bc410..1ed22af80a1 100644 --- a/src/optimalcontrol/src/Constraint.cpp +++ b/src/optimalcontrol/src/Constraint.cpp @@ -9,8 +9,8 @@ */ #include -#include -#include +#include +#include #include diff --git a/src/optimalcontrol/src/ConstraintsGroup.cpp b/src/optimalcontrol/src/ConstraintsGroup.cpp index 0e429d04b84..77c805c54fe 100644 --- a/src/optimalcontrol/src/ConstraintsGroup.cpp +++ b/src/optimalcontrol/src/ConstraintsGroup.cpp @@ -8,14 +8,14 @@ * - ADRL Control Toolbox (https://adrlab.bitbucket.io/ct/ct_doc/doc/html/index.html) */ -#include -#include -#include +#include +#include +#include #include #include #include #include -#include +#include #include #include #include diff --git a/src/optimalcontrol/src/ControlledDynamicalSystem.cpp b/src/optimalcontrol/src/ControlledDynamicalSystem.cpp index df3dcc6f66f..5b7545889a3 100644 --- a/src/optimalcontrol/src/ControlledDynamicalSystem.cpp +++ b/src/optimalcontrol/src/ControlledDynamicalSystem.cpp @@ -9,7 +9,7 @@ */ #include -#include +#include #include namespace iDynTree { diff --git a/src/optimalcontrol/src/Cost.cpp b/src/optimalcontrol/src/Cost.cpp index 0e5609dd8c9..fb5528ca614 100644 --- a/src/optimalcontrol/src/Cost.cpp +++ b/src/optimalcontrol/src/Cost.cpp @@ -9,7 +9,7 @@ */ #include -#include +#include #include namespace iDynTree { diff --git a/src/optimalcontrol/src/DynamicalSystem.cpp b/src/optimalcontrol/src/DynamicalSystem.cpp index dcfbfa04e67..c300fedf713 100644 --- a/src/optimalcontrol/src/DynamicalSystem.cpp +++ b/src/optimalcontrol/src/DynamicalSystem.cpp @@ -10,7 +10,7 @@ #include #include -#include +#include #include diff --git a/src/optimalcontrol/src/FixedStepIntegrator.cpp b/src/optimalcontrol/src/FixedStepIntegrator.cpp index a53fe53b390..2b5f8b1ce64 100644 --- a/src/optimalcontrol/src/FixedStepIntegrator.cpp +++ b/src/optimalcontrol/src/FixedStepIntegrator.cpp @@ -10,8 +10,8 @@ #include #include -#include -#include +#include +#include #include #include #include diff --git a/src/optimalcontrol/src/ForwardEuler.cpp b/src/optimalcontrol/src/ForwardEuler.cpp index 57f7b37d973..5cceafcfc97 100644 --- a/src/optimalcontrol/src/ForwardEuler.cpp +++ b/src/optimalcontrol/src/ForwardEuler.cpp @@ -10,11 +10,11 @@ #include #include -#include +#include -#include +#include #include -#include +#include namespace iDynTree { namespace optimalcontrol { diff --git a/src/optimalcontrol/src/ImplicitTrapezoidal.cpp b/src/optimalcontrol/src/ImplicitTrapezoidal.cpp index 8efff08431f..917e39d7800 100644 --- a/src/optimalcontrol/src/ImplicitTrapezoidal.cpp +++ b/src/optimalcontrol/src/ImplicitTrapezoidal.cpp @@ -10,9 +10,9 @@ #include #include -#include +#include #include -#include +#include #include #include #include diff --git a/src/optimalcontrol/src/Integrator.cpp b/src/optimalcontrol/src/Integrator.cpp index 463e80214f6..30051797932 100644 --- a/src/optimalcontrol/src/Integrator.cpp +++ b/src/optimalcontrol/src/Integrator.cpp @@ -10,8 +10,8 @@ #include #include -#include -#include +#include +#include #include #include diff --git a/src/optimalcontrol/src/IpoptInterface.cpp b/src/optimalcontrol/src/IpoptInterface.cpp index 6a211429de9..9ea4fdcd682 100644 --- a/src/optimalcontrol/src/IpoptInterface.cpp +++ b/src/optimalcontrol/src/IpoptInterface.cpp @@ -28,12 +28,12 @@ #include #include -#include +#include -#include -#include +#include +#include #include -#include +#include #include #include diff --git a/src/optimalcontrol/src/IpoptInterfaceNotImplemented.cpp b/src/optimalcontrol/src/IpoptInterfaceNotImplemented.cpp index c58272aac6d..8c2bc927e86 100644 --- a/src/optimalcontrol/src/IpoptInterfaceNotImplemented.cpp +++ b/src/optimalcontrol/src/IpoptInterfaceNotImplemented.cpp @@ -9,7 +9,7 @@ */ #include -#include +#include using namespace iDynTree::optimization; using namespace iDynTree; diff --git a/src/optimalcontrol/src/L2NormCost.cpp b/src/optimalcontrol/src/L2NormCost.cpp index 2c862443815..885f2409170 100644 --- a/src/optimalcontrol/src/L2NormCost.cpp +++ b/src/optimalcontrol/src/L2NormCost.cpp @@ -9,13 +9,13 @@ */ #include -#include -#include -#include -#include +#include +#include +#include +#include #include #include -#include +#include namespace iDynTree { namespace optimalcontrol { diff --git a/src/optimalcontrol/src/LinearConstraint.cpp b/src/optimalcontrol/src/LinearConstraint.cpp index 80db49e079b..8798fe3bd7c 100644 --- a/src/optimalcontrol/src/LinearConstraint.cpp +++ b/src/optimalcontrol/src/LinearConstraint.cpp @@ -10,7 +10,7 @@ #include -#include +#include #include diff --git a/src/optimalcontrol/src/LinearCost.cpp b/src/optimalcontrol/src/LinearCost.cpp index ef03fed7a60..f875e681a5d 100644 --- a/src/optimalcontrol/src/LinearCost.cpp +++ b/src/optimalcontrol/src/LinearCost.cpp @@ -9,7 +9,7 @@ */ #include -#include +#include namespace iDynTree { namespace optimalcontrol { diff --git a/src/optimalcontrol/src/LinearSystem.cpp b/src/optimalcontrol/src/LinearSystem.cpp index 9127dbb5af6..543856a8613 100644 --- a/src/optimalcontrol/src/LinearSystem.cpp +++ b/src/optimalcontrol/src/LinearSystem.cpp @@ -10,10 +10,10 @@ #include -#include -#include -#include -#include +#include +#include +#include +#include #include #include diff --git a/src/optimalcontrol/src/MultipleShootingSolver.cpp b/src/optimalcontrol/src/MultipleShootingSolver.cpp index cdcf1d1b275..3fdd5f3d8a4 100644 --- a/src/optimalcontrol/src/MultipleShootingSolver.cpp +++ b/src/optimalcontrol/src/MultipleShootingSolver.cpp @@ -17,11 +17,11 @@ #include #include -#include -#include +#include +#include #include -#include +#include #include #include diff --git a/src/optimalcontrol/src/OptimalControlProblem.cpp b/src/optimalcontrol/src/OptimalControlProblem.cpp index 65fac8099db..208950d51cb 100644 --- a/src/optimalcontrol/src/OptimalControlProblem.cpp +++ b/src/optimalcontrol/src/OptimalControlProblem.cpp @@ -20,11 +20,11 @@ #include #include #include -#include -#include -#include +#include +#include +#include -#include +#include #include #include diff --git a/src/optimalcontrol/src/OptimalControlSolver.cpp b/src/optimalcontrol/src/OptimalControlSolver.cpp index dafaf60b3bb..b902d0740dc 100644 --- a/src/optimalcontrol/src/OptimalControlSolver.cpp +++ b/src/optimalcontrol/src/OptimalControlSolver.cpp @@ -12,7 +12,7 @@ #include -#include +#include #include diff --git a/src/optimalcontrol/src/OptimizationProblem.cpp b/src/optimalcontrol/src/OptimizationProblem.cpp index 8b257e90fdf..20097c45acb 100644 --- a/src/optimalcontrol/src/OptimizationProblem.cpp +++ b/src/optimalcontrol/src/OptimizationProblem.cpp @@ -9,7 +9,7 @@ */ #include -#include +#include namespace iDynTree { diff --git a/src/optimalcontrol/src/Optimizer.cpp b/src/optimalcontrol/src/Optimizer.cpp index f0dafab4938..0bc1ef74603 100644 --- a/src/optimalcontrol/src/Optimizer.cpp +++ b/src/optimalcontrol/src/Optimizer.cpp @@ -8,9 +8,9 @@ * - ADRL Control Toolbox (https://adrlab.bitbucket.io/ct/ct_doc/doc/html/index.html) */ -#include +#include #include -#include +#include namespace iDynTree { namespace optimization { diff --git a/src/optimalcontrol/src/OsqpInterface.cpp b/src/optimalcontrol/src/OsqpInterface.cpp index 9c54165d5e2..043bacd63f1 100644 --- a/src/optimalcontrol/src/OsqpInterface.cpp +++ b/src/optimalcontrol/src/OsqpInterface.cpp @@ -12,11 +12,11 @@ #include #include -#include +#include -#include -#include -#include +#include +#include +#include #include #include diff --git a/src/optimalcontrol/src/OsqpInterfaceNotImplemented.cpp b/src/optimalcontrol/src/OsqpInterfaceNotImplemented.cpp index 49dd8f49cc4..146107245eb 100644 --- a/src/optimalcontrol/src/OsqpInterfaceNotImplemented.cpp +++ b/src/optimalcontrol/src/OsqpInterfaceNotImplemented.cpp @@ -9,7 +9,7 @@ */ #include -#include +#include using namespace iDynTree::optimization; diff --git a/src/optimalcontrol/src/QuadraticCost.cpp b/src/optimalcontrol/src/QuadraticCost.cpp index 2962b551a34..230dfb7e303 100644 --- a/src/optimalcontrol/src/QuadraticCost.cpp +++ b/src/optimalcontrol/src/QuadraticCost.cpp @@ -9,9 +9,9 @@ */ #include -#include +#include -#include +#include #include namespace iDynTree { diff --git a/src/optimalcontrol/src/QuadraticLikeCost.cpp b/src/optimalcontrol/src/QuadraticLikeCost.cpp index 9f4880a65cb..adb04982160 100644 --- a/src/optimalcontrol/src/QuadraticLikeCost.cpp +++ b/src/optimalcontrol/src/QuadraticLikeCost.cpp @@ -9,10 +9,10 @@ */ #include -#include +#include #include -#include +#include #include namespace iDynTree { diff --git a/src/optimalcontrol/src/RK4.cpp b/src/optimalcontrol/src/RK4.cpp index 3147e825a83..a3b50d709fe 100644 --- a/src/optimalcontrol/src/RK4.cpp +++ b/src/optimalcontrol/src/RK4.cpp @@ -10,7 +10,7 @@ #include #include -#include +#include namespace iDynTree { namespace optimalcontrol { diff --git a/src/optimalcontrol/src/SparsityStructure.cpp b/src/optimalcontrol/src/SparsityStructure.cpp index 968421693cb..fbcede5ac11 100644 --- a/src/optimalcontrol/src/SparsityStructure.cpp +++ b/src/optimalcontrol/src/SparsityStructure.cpp @@ -9,7 +9,7 @@ */ #include -#include +#include #include void iDynTree::optimalcontrol::SparsityStructure::addNonZero(size_t row, size_t col) diff --git a/src/optimalcontrol/src/TimeRange.cpp b/src/optimalcontrol/src/TimeRange.cpp index 3216162a828..0363cd0a008 100644 --- a/src/optimalcontrol/src/TimeRange.cpp +++ b/src/optimalcontrol/src/TimeRange.cpp @@ -9,7 +9,7 @@ */ #include -#include +#include namespace iDynTree { namespace optimalcontrol { diff --git a/src/optimalcontrol/src/WorhpInterface.cpp b/src/optimalcontrol/src/WorhpInterface.cpp index a02853c3f8a..6ce202e50dc 100644 --- a/src/optimalcontrol/src/WorhpInterface.cpp +++ b/src/optimalcontrol/src/WorhpInterface.cpp @@ -9,9 +9,9 @@ */ #include -#include -#include -#include +#include +#include +#include #include #include #include diff --git a/src/optimalcontrol/src/WorhpInterfaceNotImplemented.cpp b/src/optimalcontrol/src/WorhpInterfaceNotImplemented.cpp index e83c7f98ce0..cf118d1a2c3 100644 --- a/src/optimalcontrol/src/WorhpInterfaceNotImplemented.cpp +++ b/src/optimalcontrol/src/WorhpInterfaceNotImplemented.cpp @@ -9,7 +9,7 @@ */ #include -#include +#include using namespace iDynTree::optimization; using namespace iDynTree; diff --git a/src/optimalcontrol/tests/AlglibInterfaceTest.cpp b/src/optimalcontrol/tests/AlglibInterfaceTest.cpp index 668aadb4cb6..10ee909ee33 100644 --- a/src/optimalcontrol/tests/AlglibInterfaceTest.cpp +++ b/src/optimalcontrol/tests/AlglibInterfaceTest.cpp @@ -9,10 +9,10 @@ */ #include #include -#include -#include -#include -#include +#include +#include +#include +#include #include #include diff --git a/src/optimalcontrol/tests/ConstraintsGroupTest.cpp b/src/optimalcontrol/tests/ConstraintsGroupTest.cpp index ffaecba2d17..33a4c5c3cbb 100644 --- a/src/optimalcontrol/tests/ConstraintsGroupTest.cpp +++ b/src/optimalcontrol/tests/ConstraintsGroupTest.cpp @@ -11,8 +11,8 @@ #include #include #include -#include -#include +#include +#include #include using namespace iDynTree; diff --git a/src/optimalcontrol/tests/IntegratorsTest.cpp b/src/optimalcontrol/tests/IntegratorsTest.cpp index 774ba7386f5..8bcc9b30db8 100644 --- a/src/optimalcontrol/tests/IntegratorsTest.cpp +++ b/src/optimalcontrol/tests/IntegratorsTest.cpp @@ -9,8 +9,8 @@ */ #include -#include -#include +#include +#include #include #include #include diff --git a/src/optimalcontrol/tests/IpoptInterfaceTest.cpp b/src/optimalcontrol/tests/IpoptInterfaceTest.cpp index ee26964feb7..271d6e92589 100644 --- a/src/optimalcontrol/tests/IpoptInterfaceTest.cpp +++ b/src/optimalcontrol/tests/IpoptInterfaceTest.cpp @@ -10,10 +10,10 @@ #include #include -#include -#include -#include -#include +#include +#include +#include +#include #include #include diff --git a/src/optimalcontrol/tests/L2NormTest.cpp b/src/optimalcontrol/tests/L2NormTest.cpp index a2992ab3e8e..e629776acc3 100644 --- a/src/optimalcontrol/tests/L2NormTest.cpp +++ b/src/optimalcontrol/tests/L2NormTest.cpp @@ -4,8 +4,8 @@ #include #include -#include -#include +#include +#include #include int main() { diff --git a/src/optimalcontrol/tests/LinearOCOsqpTest.cpp b/src/optimalcontrol/tests/LinearOCOsqpTest.cpp index d69b09f1f20..b0cbd0b730f 100644 --- a/src/optimalcontrol/tests/LinearOCOsqpTest.cpp +++ b/src/optimalcontrol/tests/LinearOCOsqpTest.cpp @@ -8,10 +8,10 @@ * - ADRL Control Toolbox (https://adrlab.bitbucket.io/ct/ct_doc/doc/html/index.html) */ -#include -#include -#include -#include +#include +#include +#include +#include #include #include #include diff --git a/src/optimalcontrol/tests/MultipleShootingTest.cpp b/src/optimalcontrol/tests/MultipleShootingTest.cpp index 93545b8227b..f560b1559f7 100644 --- a/src/optimalcontrol/tests/MultipleShootingTest.cpp +++ b/src/optimalcontrol/tests/MultipleShootingTest.cpp @@ -16,14 +16,14 @@ #include #include #include -#include -#include +#include +#include #include -#include -#include +#include +#include #include #include -#include +#include #include class TestSystem : public iDynTree::optimalcontrol::DynamicalSystem { diff --git a/src/optimalcontrol/tests/OCProblemTest.cpp b/src/optimalcontrol/tests/OCProblemTest.cpp index 65fdb05bbe4..e7f32f02e50 100644 --- a/src/optimalcontrol/tests/OCProblemTest.cpp +++ b/src/optimalcontrol/tests/OCProblemTest.cpp @@ -12,14 +12,14 @@ #include #include #include -#include -#include -#include -#include +#include +#include +#include +#include #include #include #include -#include +#include #include class TestSystem : public iDynTree::optimalcontrol::DynamicalSystem { diff --git a/src/optimalcontrol/tests/OptimalControlIpoptTest.cpp b/src/optimalcontrol/tests/OptimalControlIpoptTest.cpp index ca35db4bd4e..09907478682 100644 --- a/src/optimalcontrol/tests/OptimalControlIpoptTest.cpp +++ b/src/optimalcontrol/tests/OptimalControlIpoptTest.cpp @@ -8,10 +8,10 @@ * - ADRL Control Toolbox (https://adrlab.bitbucket.io/ct/ct_doc/doc/html/index.html) */ -#include -#include -#include -#include +#include +#include +#include +#include #include #include #include diff --git a/src/optimalcontrol/tests/OptimalControlTest.cpp b/src/optimalcontrol/tests/OptimalControlTest.cpp index be94c076ccc..b1404eaccb0 100644 --- a/src/optimalcontrol/tests/OptimalControlTest.cpp +++ b/src/optimalcontrol/tests/OptimalControlTest.cpp @@ -4,7 +4,7 @@ // TODO: change this to correct include #include -#include +#include int main() { diff --git a/src/optimalcontrol/tests/WorhpInterfaceTest.cpp b/src/optimalcontrol/tests/WorhpInterfaceTest.cpp index 7f4d138654e..d664cc48c15 100644 --- a/src/optimalcontrol/tests/WorhpInterfaceTest.cpp +++ b/src/optimalcontrol/tests/WorhpInterfaceTest.cpp @@ -10,10 +10,10 @@ #include #include -#include -#include -#include -#include +#include +#include +#include +#include #include #include diff --git a/src/sensors/CMakeLists.txt b/src/sensors/CMakeLists.txt deleted file mode 100644 index 3e19b456824..00000000000 --- a/src/sensors/CMakeLists.txt +++ /dev/null @@ -1,57 +0,0 @@ -# SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) -# SPDX-License-Identifier: BSD-3-Clause - - -set(IDYNTREE_SENSORS_HEADERS include/iDynTree/Sensors/AllSensorsTypes.h - include/iDynTree/Sensors/Sensors.h - include/iDynTree/Sensors/SixAxisForceTorqueSensor.h - include/iDynTree/Sensors/GyroscopeSensor.h - include/iDynTree/Sensors/AccelerometerSensor.h - include/iDynTree/Sensors/ThreeAxisAngularAccelerometerSensor.h - include/iDynTree/Sensors/ThreeAxisForceTorqueContactSensor.h - include/iDynTree/Sensors/PredictSensorsMeasurements.h - include/iDynTree/Sensors/ModelSensorsTransformers.h) - -set(IDYNTREE_SENSORS_SOURCES src/Sensors.cpp - src/SixAxisForceTorqueSensor.cpp - src/AccelerometerSensor.cpp - src/GyroscopeSensor.cpp - src/ThreeAxisAngularAccelerometerSensor.cpp - src/ThreeAxisForceTorqueContactSensor.cpp - src/PredictSensorsMeasurements.cpp - src/ModelSensorsTransformers.cpp) - - -SOURCE_GROUP("Source Files" FILES ${IDYNTREE_SENSORS_SOURCES}) -SOURCE_GROUP("Header Files" FILES ${IDYNTREE_SENSORS_HEADERS}) - -set(libraryname idyntree-sensors) - -add_library(${libraryname} ${IDYNTREE_SENSORS_SOURCES} ${IDYNTREE_SENSORS_HEADERS} ${IDYNTREE_SENSORS_PRIVATE_INCLUDES}) -add_library(iDynTree::${libraryname} ALIAS ${libraryname}) - -target_include_directories(${libraryname} PUBLIC "$" - "$") - -target_link_libraries(${libraryname} PUBLIC idyntree-core idyntree-model - PRIVATE Eigen3::Eigen) - - -target_compile_options(${libraryname} PRIVATE ${IDYNTREE_WARNING_FLAGS}) - -set_property(TARGET ${libraryname} PROPERTY PUBLIC_HEADER ${IDYNTREE_SENSORS_HEADERS}) - -install(TARGETS ${libraryname} - EXPORT iDynTree - COMPONENT runtime - RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" - LIBRARY DESTINATION "${CMAKE_INSTALL_LIBDIR}" - ARCHIVE DESTINATION "${CMAKE_INSTALL_LIBDIR}" - PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/iDynTree/Sensors - PRIVATE_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/iDynTree/Sensors/Impl) - -set_property(GLOBAL APPEND PROPERTY ${VARS_PREFIX}_TARGETS ${libraryname}) - -if(IDYNTREE_COMPILE_TESTS) - add_subdirectory(tests) -endif(IDYNTREE_COMPILE_TESTS) diff --git a/src/sensors/include/iDynTree/Sensors/AllSensorsTypes.h b/src/sensors/include/iDynTree/Sensors/AllSensorsTypes.h deleted file mode 100644 index 93c03723f30..00000000000 --- a/src/sensors/include/iDynTree/Sensors/AllSensorsTypes.h +++ /dev/null @@ -1,13 +0,0 @@ -// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) -// SPDX-License-Identifier: BSD-3-Clause - -#ifndef IDYNTREE_ALL_SENSORS_TYPE_H -#define IDYNTREE_ALL_SENSORS_TYPE_H - -#include -#include -#include -#include -#include - -#endif diff --git a/src/sensors/tests/CMakeLists.txt b/src/sensors/tests/CMakeLists.txt deleted file mode 100644 index 54c8bb435ee..00000000000 --- a/src/sensors/tests/CMakeLists.txt +++ /dev/null @@ -1,18 +0,0 @@ -macro(add_unit_test classname) - set(testsrc ${classname}UnitTest.cpp) - set(testbinary ${classname}UnitTest) - set(testname UnitTest${classname}) - add_executable(${testbinary} ${testsrc}) - target_link_libraries(${testbinary} PRIVATE idyntree-sensors idyntree-testmodels Eigen3::Eigen) - add_test(NAME ${testname} COMMAND ${testbinary}) - - if(IDYNTREE_RUN_VALGRIND_TESTS) - add_test(NAME memcheck_${testname} COMMAND ${MEMCHECK_COMMAND_COMPLETE} $) - - endif() -endmacro() - -add_unit_test(SensorsList) -add_unit_test(ThreeAxisForceTorqueContactSensor) -add_unit_test(ReducedModelWithFT) -target_link_libraries(ReducedModelWithFTUnitTest PRIVATE idyntree-high-level) diff --git a/src/solid-shapes/include/iDynTree/InertialParametersSolidShapesHelpers.h b/src/solid-shapes/include/iDynTree/InertialParametersSolidShapesHelpers.h index c61932ca84a..52aa0170018 100644 --- a/src/solid-shapes/include/iDynTree/InertialParametersSolidShapesHelpers.h +++ b/src/solid-shapes/include/iDynTree/InertialParametersSolidShapesHelpers.h @@ -4,8 +4,8 @@ #ifndef IDYNTREE_INERTIAL_PARAMETERS_HELPERS_H #define IDYNTREE_INERTIAL_PARAMETERS_HELPERS_H -#include -#include +#include +#include namespace iDynTree { diff --git a/src/solid-shapes/src/InertialParametersSolidShapesHelpers.cpp b/src/solid-shapes/src/InertialParametersSolidShapesHelpers.cpp index 49314c69d42..f00922117a0 100644 --- a/src/solid-shapes/src/InertialParametersSolidShapesHelpers.cpp +++ b/src/solid-shapes/src/InertialParametersSolidShapesHelpers.cpp @@ -3,13 +3,13 @@ #include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include -#include +#include #ifdef IDYNTREE_USES_ASSIMP #include diff --git a/src/solid-shapes/src/ModelTransformersSolidShapes.cpp b/src/solid-shapes/src/ModelTransformersSolidShapes.cpp index eab2ab1e4ef..b30d07471b4 100644 --- a/src/solid-shapes/src/ModelTransformersSolidShapes.cpp +++ b/src/solid-shapes/src/ModelTransformersSolidShapes.cpp @@ -4,7 +4,7 @@ #include -#include +#include #include #include diff --git a/src/solid-shapes/tests/InertialParametersSolidShapesHelpersIntegrationTest.cpp b/src/solid-shapes/tests/InertialParametersSolidShapesHelpersIntegrationTest.cpp index adabe64666a..4f7f7bd7b24 100644 --- a/src/solid-shapes/tests/InertialParametersSolidShapesHelpersIntegrationTest.cpp +++ b/src/solid-shapes/tests/InertialParametersSolidShapesHelpersIntegrationTest.cpp @@ -1,13 +1,13 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include +#include #include -#include -#include +#include +#include using namespace iDynTree; diff --git a/src/solid-shapes/tests/ModelTransformersSolidShapesIntegrationTest.cpp b/src/solid-shapes/tests/ModelTransformersSolidShapesIntegrationTest.cpp index 38c193357a1..b837084554d 100644 --- a/src/solid-shapes/tests/ModelTransformersSolidShapesIntegrationTest.cpp +++ b/src/solid-shapes/tests/ModelTransformersSolidShapesIntegrationTest.cpp @@ -2,13 +2,13 @@ // SPDX-License-Identifier: BSD-3-Clause -#include +#include #include -#include -#include +#include +#include using namespace iDynTree; diff --git a/src/tests/benchmark/DynamicsBenchmark.cpp b/src/tests/benchmark/DynamicsBenchmark.cpp index 267b15dc0c9..97329e56b02 100644 --- a/src/tests/benchmark/DynamicsBenchmark.cpp +++ b/src/tests/benchmark/DynamicsBenchmark.cpp @@ -10,26 +10,26 @@ #include #include -#include +#include // iDynTree includes -#include -#include -#include +#include +#include +#include -#include -#include +#include +#include -#include -#include -#include -#include +#include +#include +#include +#include -#include +#include -#include +#include -#include +#include using namespace iDynTree; diff --git a/src/tests/icub_consistency/iCubExternalWrenchesEstimationConsistencyTest.cpp b/src/tests/icub_consistency/iCubExternalWrenchesEstimationConsistencyTest.cpp index 4b1dde1a228..6b9859475d1 100644 --- a/src/tests/icub_consistency/iCubExternalWrenchesEstimationConsistencyTest.cpp +++ b/src/tests/icub_consistency/iCubExternalWrenchesEstimationConsistencyTest.cpp @@ -23,27 +23,27 @@ #include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include -#include - -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include +#include #include using namespace iCub::iDyn; diff --git a/src/tests/icub_consistency/iKinConsistencyTest.cpp b/src/tests/icub_consistency/iKinConsistencyTest.cpp index cfe9b48c91f..bd88d6bda3b 100644 --- a/src/tests/icub_consistency/iKinConsistencyTest.cpp +++ b/src/tests/icub_consistency/iKinConsistencyTest.cpp @@ -1,20 +1,20 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include +#include #include #include -#include -#include +#include +#include -#include +#include #include -#include +#include #include #include diff --git a/src/tests/integration/CMakeLists.txt b/src/tests/integration/CMakeLists.txt index 6f421651d91..ef65231d66c 100644 --- a/src/tests/integration/CMakeLists.txt +++ b/src/tests/integration/CMakeLists.txt @@ -8,7 +8,7 @@ macro(add_integration_test testname) set(testtarget IntegrationTest${testname}) add_executable(${testbinary} ${testsrc}) target_link_libraries(${testbinary} PRIVATE idyntree-core idyntree-model idyntree-modelio - idyntree-high-level idyntree-sensors idyntree-estimation + idyntree-high-level idyntree-estimation idyntree-solid-shapes idyntree-testmodels Eigen3::Eigen) # Setting explicitly the WORKING_DIRECTORY is necessary to make sure that meshes are correctly loaded, # as a workaround for https://github.com/robotology/idyntree/issues/291 @@ -25,7 +25,7 @@ macro(add_integration_test_no_valgrind testname) set(testtarget IntegrationTest${testname}) add_executable(${testbinary} ${testsrc}) target_link_libraries(${testbinary} PRIVATE idyntree-core idyntree-model idyntree-modelio - idyntree-high-level idyntree-sensors idyntree-estimation + idyntree-high-level idyntree-estimation idyntree-solid-shapes idyntree-testmodels Eigen3::Eigen) add_test(NAME ${testtarget} COMMAND ${testbinary}) endmacro() @@ -41,6 +41,8 @@ endmacro() add_integration_test(Dynamics) add_integration_test(DenavitHartenberg) +add_integration_test(ReducedModelWithFT) + # See issue https://github.com/robotology/idyntree/issues/367 add_integration_test_no_valgrind(iCubTorqueEstimation) diff --git a/src/tests/integration/DenavitHartenbergIntegrationTest.cpp b/src/tests/integration/DenavitHartenbergIntegrationTest.cpp index 820c1211831..82d00c44fbe 100644 --- a/src/tests/integration/DenavitHartenbergIntegrationTest.cpp +++ b/src/tests/integration/DenavitHartenbergIntegrationTest.cpp @@ -2,12 +2,12 @@ // SPDX-License-Identifier: BSD-3-Clause -#include -#include +#include +#include -#include -#include -#include +#include +#include +#include #include diff --git a/src/tests/integration/DynamicsIntegrationTest.cpp b/src/tests/integration/DynamicsIntegrationTest.cpp index 5f432da9d1f..e5dc1444c82 100644 --- a/src/tests/integration/DynamicsIntegrationTest.cpp +++ b/src/tests/integration/DynamicsIntegrationTest.cpp @@ -2,21 +2,21 @@ // SPDX-License-Identifier: BSD-3-Clause -#include -#include +#include +#include -#include -#include +#include +#include -#include +#include -#include -#include +#include +#include -#include -#include -#include -#include +#include +#include +#include +#include #include "testModels.h" diff --git a/src/tests/integration/DynamicsLinearizationIntegrationTest.cpp b/src/tests/integration/DynamicsLinearizationIntegrationTest.cpp index 2dda78331d5..00416e9aaec 100644 --- a/src/tests/integration/DynamicsLinearizationIntegrationTest.cpp +++ b/src/tests/integration/DynamicsLinearizationIntegrationTest.cpp @@ -2,24 +2,24 @@ // SPDX-License-Identifier: BSD-3-Clause -#include -#include +#include +#include -#include -#include +#include +#include -#include -#include -#include +#include +#include +#include -#include -#include -#include -#include +#include +#include +#include +#include -#include +#include -#include +#include #include "testModels.h" diff --git a/src/tests/integration/InertialParametersSolidShapesHelpersIntegrationTest.cpp b/src/tests/integration/InertialParametersSolidShapesHelpersIntegrationTest.cpp index f60b375143f..629a21a9bae 100644 --- a/src/tests/integration/InertialParametersSolidShapesHelpersIntegrationTest.cpp +++ b/src/tests/integration/InertialParametersSolidShapesHelpersIntegrationTest.cpp @@ -1,12 +1,12 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include +#include #include "testModels.h" #include -#include +#include using namespace iDynTree; diff --git a/src/sensors/tests/ReducedModelWithFTUnitTest.cpp b/src/tests/integration/ReducedModelWithFTIntegrationTest.cpp similarity index 96% rename from src/sensors/tests/ReducedModelWithFTUnitTest.cpp rename to src/tests/integration/ReducedModelWithFTIntegrationTest.cpp index 466ba63ba5f..1782acbfab2 100644 --- a/src/sensors/tests/ReducedModelWithFTUnitTest.cpp +++ b/src/tests/integration/ReducedModelWithFTIntegrationTest.cpp @@ -1,11 +1,11 @@ -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include #include "testModels.h" #include #include diff --git a/src/tests/integration/iCubTorqueEstimationIntegrationTest.cpp b/src/tests/integration/iCubTorqueEstimationIntegrationTest.cpp index 067abaca09a..fbad78a6a3f 100644 --- a/src/tests/integration/iCubTorqueEstimationIntegrationTest.cpp +++ b/src/tests/integration/iCubTorqueEstimationIntegrationTest.cpp @@ -6,20 +6,20 @@ #include #include -#include -#include -#include -#include +#include +#include +#include +#include -#include -#include +#include +#include -#include -#include +#include +#include -#include -#include -#include +#include +#include +#include using namespace iDynTree; diff --git a/src/tests/yarp_benchmark/PseudoInverseBenchmark.cpp b/src/tests/yarp_benchmark/PseudoInverseBenchmark.cpp index cb65c64f7e9..ccdcb74c837 100644 --- a/src/tests/yarp_benchmark/PseudoInverseBenchmark.cpp +++ b/src/tests/yarp_benchmark/PseudoInverseBenchmark.cpp @@ -3,12 +3,12 @@ #include "testModels.h" -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include #include #include diff --git a/src/tools/idyntree-model-info.cpp b/src/tools/idyntree-model-info.cpp index 870f954f64d..b1ceb64529b 100644 --- a/src/tools/idyntree-model-info.cpp +++ b/src/tools/idyntree-model-info.cpp @@ -1,8 +1,8 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include -#include +#include +#include #include #include "cmdline.h" diff --git a/src/tools/idyntree-model-simplify-shapes.cpp b/src/tools/idyntree-model-simplify-shapes.cpp index db526d598f1..526d604adb8 100644 --- a/src/tools/idyntree-model-simplify-shapes.cpp +++ b/src/tools/idyntree-model-simplify-shapes.cpp @@ -1,16 +1,16 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include +#include // To load models from file -#include +#include // To approximate shapes of a model #include // To write model to file -#include +#include #include "cmdline.h" diff --git a/src/tools/idyntree-model-view.cpp b/src/tools/idyntree-model-view.cpp index 897a37e02f1..ced17f0ddf5 100644 --- a/src/tools/idyntree-model-view.cpp +++ b/src/tools/idyntree-model-view.cpp @@ -1,8 +1,8 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include -#include +#include +#include #include diff --git a/src/visualization/include/iDynTree/MeshcatVisualizer.h b/src/visualization/include/iDynTree/MeshcatVisualizer.h index f88ebf4c1bf..7654a464de5 100644 --- a/src/visualization/include/iDynTree/MeshcatVisualizer.h +++ b/src/visualization/include/iDynTree/MeshcatVisualizer.h @@ -7,11 +7,11 @@ #include #include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include namespace iDynTree { diff --git a/src/visualization/include/iDynTree/Visualizer.h b/src/visualization/include/iDynTree/Visualizer.h index 0d20aecdd50..681cb85749f 100644 --- a/src/visualization/include/iDynTree/Visualizer.h +++ b/src/visualization/include/iDynTree/Visualizer.h @@ -7,11 +7,11 @@ #include #include -#include -#include +#include +#include -#include -#include +#include +#include namespace iDynTree { diff --git a/src/visualization/src/DummyImplementations.h b/src/visualization/src/DummyImplementations.h index 8b35d593e57..60edfd852a6 100644 --- a/src/visualization/src/DummyImplementations.h +++ b/src/visualization/src/DummyImplementations.h @@ -5,7 +5,7 @@ #define IDYNTREE_DUMMYIMPLEMENTATIONS_H #include -#include +#include namespace iDynTree { diff --git a/src/visualization/src/IrrlichtUtils.h b/src/visualization/src/IrrlichtUtils.h index 038416ae1a8..b503fcef55f 100644 --- a/src/visualization/src/IrrlichtUtils.h +++ b/src/visualization/src/IrrlichtUtils.h @@ -5,7 +5,7 @@ #define IDYNTREE_IRRLICHT_UTILS_H #include -#include +#include #include #include @@ -13,7 +13,7 @@ #include #include -#include +#include #include "FloorGridSceneNode.h" diff --git a/src/visualization/src/JetsVisualization.cpp b/src/visualization/src/JetsVisualization.cpp index 55240ede631..716968710f9 100644 --- a/src/visualization/src/JetsVisualization.cpp +++ b/src/visualization/src/JetsVisualization.cpp @@ -7,7 +7,7 @@ #include "IrrlichtUtils.h" -#include +#include #include diff --git a/src/visualization/src/MeshcatVisualizer.cpp b/src/visualization/src/MeshcatVisualizer.cpp index 02991011264..2096bd8da20 100644 --- a/src/visualization/src/MeshcatVisualizer.cpp +++ b/src/visualization/src/MeshcatVisualizer.cpp @@ -5,17 +5,17 @@ #include #include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include #include -#include -#include -#include -#include +#include +#include +#include +#include #include #include diff --git a/src/visualization/src/ModelVisualization.cpp b/src/visualization/src/ModelVisualization.cpp index dfb3957e91b..dff4c4f7c59 100644 --- a/src/visualization/src/ModelVisualization.cpp +++ b/src/visualization/src/ModelVisualization.cpp @@ -6,9 +6,9 @@ #include "IrrlichtUtils.h" #include "Label.h" -#include -#include -#include +#include +#include +#include namespace iDynTree { diff --git a/src/visualization/src/Visualizer.cpp b/src/visualization/src/Visualizer.cpp index 55dc32601c4..123ef002200 100644 --- a/src/visualization/src/Visualizer.cpp +++ b/src/visualization/src/Visualizer.cpp @@ -1,8 +1,8 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include -#include +#include +#include #include #ifdef IDYNTREE_USES_IRRLICHT diff --git a/src/visualization/tests/CMakeLists.txt b/src/visualization/tests/CMakeLists.txt index 1efe4eea22e..77e5de5c2b0 100644 --- a/src/visualization/tests/CMakeLists.txt +++ b/src/visualization/tests/CMakeLists.txt @@ -7,7 +7,7 @@ macro(add_unit_test classname) set(testbinary ${classname}UnitTest) set(testname UnitTest${classname}) add_executable(${testbinary} ${testsrc}) - target_link_libraries(${testbinary} PRIVATE idyntree-sensors idyntree-modelio idyntree-visualization idyntree-testmodels Eigen3::Eigen) + target_link_libraries(${testbinary} PRIVATE idyntree-modelio idyntree-visualization idyntree-testmodels Eigen3::Eigen) add_test(NAME ${testname} COMMAND ${testbinary}) if(IDYNTREE_RUN_VALGRIND_TESTS) diff --git a/src/visualization/tests/VisualizerUnitTest.cpp b/src/visualization/tests/VisualizerUnitTest.cpp index 8047fc165b6..890f751cad8 100644 --- a/src/visualization/tests/VisualizerUnitTest.cpp +++ b/src/visualization/tests/VisualizerUnitTest.cpp @@ -1,10 +1,10 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#include +#include #include -#include +#include #include "testModels.h" diff --git a/src/yarp/CMakeLists.txt b/src/yarp/CMakeLists.txt index 2e86df33cff..beb518412bb 100644 --- a/src/yarp/CMakeLists.txt +++ b/src/yarp/CMakeLists.txt @@ -1,11 +1,11 @@ # SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) # SPDX-License-Identifier: BSD-3-Clause -SET(iDynTree_YARP_header include/iDynTree/yarp/YARPConversions.h - include/iDynTree/yarp/YARPEigenConversions.h - include/iDynTree/yarp/YARPConfigurationsLoader.h - include/iDynTree/yarp/YARPConversionsImplementation.h - include/iDynTree/yarp/YARPConfigurationsLoaderImplementation.h) +SET(iDynTree_YARP_header include/iDynTree/YARPConversions.h + include/iDynTree/YARPEigenConversions.h + include/iDynTree/YARPConfigurationsLoader.h + include/iDynTree/YARPConversionsImplementation.h + include/iDynTree/YARPConfigurationsLoaderImplementation.h) SOURCE_GROUP("Header Files" FILES ${iDynTree_YARP_header}) @@ -24,6 +24,10 @@ install(TARGETS idyntree-yarp RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT bin LIBRARY DESTINATION "${CMAKE_INSTALL_LIBDIR}" COMPONENT shlib ARCHIVE DESTINATION "${CMAKE_INSTALL_LIBDIR}" COMPONENT lib - PUBLIC_HEADER DESTINATION "${CMAKE_INSTALL_INCLUDEDIR}/iDynTree/yarp" COMPONENT dev) + PUBLIC_HEADER DESTINATION "${CMAKE_INSTALL_INCLUDEDIR}/iDynTree" COMPONENT dev) set_property(GLOBAL APPEND PROPERTY ${VARS_PREFIX}_TARGETS idyntree-yarp) + +# Install deprecated headers +install(DIRECTORY include/iDynTree/yarp + DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/iDynTree) diff --git a/src/yarp/include/iDynTree/YARPConfigurationsLoader.h b/src/yarp/include/iDynTree/YARPConfigurationsLoader.h new file mode 100644 index 00000000000..46511e4b9eb --- /dev/null +++ b/src/yarp/include/iDynTree/YARPConfigurationsLoader.h @@ -0,0 +1,26 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_YARP_CONFIGURATIONS_LOADER_H +#define IDYNTREE_YARP_CONFIGURATIONS_LOADER_H + +#include +#include +#include + +namespace iDynTree +{ + /** + * Takes a rotation matrix from configuration file. + * Notice, the matrix is parsed row-wise. + * @param rf yarp::os::Searchable The input searchable + * @param key The name corresponding to the matrix to be read + * @param rotation The output rotation + * @return true if successfull + */ + bool parseRotationMatrix(const yarp::os::Searchable& rf, const std::string& key, iDynTree::Rotation& rotation); +} + +#include "YARPConfigurationsLoaderImplementation.h" + +#endif \ No newline at end of file diff --git a/src/yarp/include/iDynTree/YARPConfigurationsLoaderImplementation.h b/src/yarp/include/iDynTree/YARPConfigurationsLoaderImplementation.h new file mode 100644 index 00000000000..448a7627714 --- /dev/null +++ b/src/yarp/include/iDynTree/YARPConfigurationsLoaderImplementation.h @@ -0,0 +1,41 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_YARP_CONFIGURATIONS_LOADER_IMPLEMENTATION_H +#define IDYNTREE_YARP_CONFIGURATIONS_LOADER_IMPLEMENTATION_H + +#include + +inline bool iDynTree::parseRotationMatrix(const yarp::os::Searchable& rf, const std::string& key, iDynTree::Rotation& rotation) +{ + yarp::os::Value ini = rf.find(key); + if (ini.isNull() || !ini.isList()) + { + return false; + } + yarp::os::Bottle *outerList = ini.asList(); + if (!outerList || outerList->size() != 3) + { + return false; + } + for (int row = 0; row < outerList->size(); ++row) + { + yarp::os::Value& innerValue = outerList->get(row); + if (innerValue.isNull() || !innerValue.isList()) + { + return false; + } + yarp::os::Bottle *innerList = innerValue.asList(); + if (!innerList || innerList->size() != 3) + { + return false; + } + for (int column = 0; column < innerList->size(); ++column) + { + rotation.setVal(row, column, innerList->get(column).asFloat64()); + } + } + return true; +} + +#endif diff --git a/src/yarp/include/iDynTree/YARPConversions.h b/src/yarp/include/iDynTree/YARPConversions.h new file mode 100644 index 00000000000..30a0d105916 --- /dev/null +++ b/src/yarp/include/iDynTree/YARPConversions.h @@ -0,0 +1,217 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_YARP_CONVERSIONS_H +#define IDYNTREE_YARP_CONVERSIONS_H + +#include +#include + +#include + +namespace iDynTree +{ + +class Direction; +class VectorDynSize; +class Transform; + +/** + * Convert a yarp::sig::Vector to a iDynTree::Wrench + * @param yarpVector yarp::sig::Vector input + * @param iDynTreeWrench iDynTree::Wrench output + * @return true if conversion was successful, false otherwise + * \ingroup iDynTreeYARP + */ +bool toiDynTree(const yarp::sig::Vector & yarpVector, iDynTree::Wrench & iDynTreeWrench); + +/** + * Convert a iDynTree::Wrench to a yarp::sig::Vector + * @param iDynTreeWrench iDynTree::Wrench input + * @param yarpVector yarp::sig::Vector output + * @return true if conversion was successful, false otherwise + * \ingroup iDynTreeYARP + */ +bool toYarp(const iDynTree::Wrench & iDynTreeWrench, yarp::sig::Vector & yarpVector); + +/** + * Convert a yarp::sig::Vector to a iDynTree::Position + * @param yarpVector yarp::sig::Vector input + * @param iDynTreePosition iDynTree::Position output + * @return true if conversion was successful, false otherwise (if the input yarpVector has size different from 3) + * \ingroup iDynTreeYARP + */ +bool toiDynTree(const yarp::sig::Vector & yarpVector, iDynTree::Position & iDynTreePosition); + +/** + * Convert a yarp::sig::Vector to a iDynTree::Vector3 + * @param yarpVector yarp::sig::Vector input + * @param iDynTreePosition iDynTree::Vector3 output + * @return true if conversion was successful, false otherwise (if the input yarpVector has size different from 3) + * \ingroup iDynTreeYARP + */ +bool toiDynTree(const yarp::sig::Vector & yarpVector, iDynTree::Vector3 & iDynTreeVector3); + +/** + * Convert a iDynTree::Position to a yarp::sig::Vector of 3 elements. + * @param iDynTreePosition iDynTree::Position input + * @param yarpVector yarp::sig::Vector output + * @return true if conversion was sucessful, false otherwise + * \ingroup iDynTreeYARP + */ +bool toYarp(const iDynTree::Position & iDynTreePosition, yarp::sig::Vector & yarpVector); + +/** + * Convert a yarp::sig::Vector of 3 elements to a iDynTree::Direction + * @param yarpVector yarp::sig::Vector input + * @param iDynTreeDirection iDynTree::Direction output + * @return true if conversion was successful, false otherwise (if the input yarpVector has size different from 3) + * + * \note the direction vector will be normalized to have unit norm. + * + * \ingroup iDynTreeYARP + */ +bool toiDynTree(const yarp::sig::Vector & yarpVector, iDynTree::Direction & iDynTreeDirection); + +/** + * Convert a iDynTree::Direction to a yarp::sig::Vector of 3 elements. + * @param iDynTreeDirection iDynTree::Position input + * @param yarpVector yarp::sig::Vector output + * @return true if conversion was sucessful, false otherwise + * + * \ingroup iDynTreeYARP + */ +bool toYarp(const iDynTree::Vector3 & iDynTreeDirection, yarp::sig::Vector & yarpVector); + +/** + * Convert a 4x4 yarp::sig::Matrix representing an homegeneous matrix to a iDynTree::Transform + * @param yarpHomogeneousMatrix yarp::sig::Matrix 4x4 homegeneous matrix input + * @param iDynTreeTransform iDynTree::Transform output + * @return true if conversion was successful, false otherwise + * + * \ingroup iDynTreeYARP + */ +bool toiDynTree(const yarp::sig::Matrix & yarpHomogeneousMatrix, iDynTree::Transform & iDynTreeTransform); + +/** + * Convert a iDynTree::Transform to a 4x4 yarp::sig::Matrix representing an homegeneous matrix + * @param iDynTreeTransform iDynTree::Transform input + * @param yarpHomogeneousMatrix yarp::sig::Matrix 4x4 homegeneous matrix output + * @return true if conversion was successful, false otherwise + * + * \ingroup iDynTreeYARP + */ +bool toYarp(const iDynTree::Transform & iDynTreeTransform, yarp::sig::Matrix & yarpHomogeneousMatrix); + +/** + * Convert a yarp::sig::Vector to a iDynTree::VectorDynSize + * @param yarpVector yarp::sig::Vector input + * @param iDynTreeVector iDynTree::VectorDynSize output + * @return true if conversion was successful, false otherwise + * \note the output VectorDynSize will be resized if necessary. + * + * \ingroup iDynTreeYARP + */ +bool toiDynTree(const yarp::sig::Vector & yarpVector, iDynTree::VectorDynSize & iDynTreeVector); + +/** + * Convert a iDynTree::VectorFixSize to a yarp::sig::Vector + * @param iDynTreeVector iDynTree::VectorFixSize input + * @param yarpVector yarp::sig::Vector output + * \note the output yarp::sig::Vector will be resized if necessary. + * + * \ingroup iDynTreeYARP + */ +template +void toYarp(const VectorType& iDynTreeVector, yarp::sig::Vector& yarpVector) +{ + size_t vecSize = iDynTreeVector.size(); + if( yarpVector.size() != vecSize ) + { + yarpVector.resize(vecSize); + } + + memcpy(yarpVector.data(),iDynTreeVector.data(),vecSize*sizeof(double)); +} + +/** + * Convert a iDynTree::MatrixFixSize to a yarp::sig::Matrix + * @param iDynTreeMatrix iDynTree::MatrixFixSize input + * @param yarpMatrix yarp::sig::Matrix output + * \note the output yarp::sig::Matrix will be resized if necessary. + * + * \ingroup iDynTreeYARP + */ +template +void toYarp(const MatrixType& iDynTreeMatrix, yarp::sig::Matrix& yarpMatrix) +{ + size_t rows = iDynTreeMatrix.rows(); + size_t cols = iDynTreeMatrix.cols(); + + if( static_cast(yarpMatrix.rows()) != rows || + static_cast(yarpMatrix.cols()) != cols ) + { + yarpMatrix.resize(rows,cols); + } + + // Here we explot the fact that both YARP and iDynTree store + // Matrices in row major forma . + memcpy(yarpMatrix.data(),iDynTreeMatrix.data(),rows*cols*sizeof(double)); +} + +/** + * Convert a yarp::sig::Vector to a iDynTree::VectorFixSize + * @param yarpVector yarp::sig::Vector input + * @param iDynTreeVector iDynTree::VectorFixSize output + * @return true if conversion was successful, false otherwise + * (if the input yarpMatrix has size different from the output VectorFixSize) + * + * \ingroup iDynTreeYARP + */ +template +bool toiDynTree(const yarp::sig::Vector& yarpVector, VectorType& iDynTreeVector) +{ + size_t vecSize = iDynTreeVector.size(); + if( yarpVector.size() != vecSize ) + { + return false; + } + + memcpy(iDynTreeVector.data(),yarpVector.data(),vecSize*sizeof(double)); + return true; +} + +/** + * Convert a yarp::sig::Matrix to a iDynTree::MatrixFixSize + * @param yarpMatrix yarp::sig::Matrix input + * @param iDynTreeMatrix iDynTree::MatrixFixSize output + * @return true if conversion was successful, false otherwise + * (if the input yarpMatrix has size different from the MatrixFixSize) + * + * \ingroup iDynTreeYARP + */ +template +bool toiDynTree(const yarp::sig::Matrix& yarpMatrix, MatrixType& iDynTreeMatrix) +{ + size_t rows = iDynTreeMatrix.rows(); + size_t cols = iDynTreeMatrix.cols(); + + if( static_cast(yarpMatrix.rows()) != rows || + static_cast(yarpMatrix.cols()) != cols ) + { + return false; + } + + // Here we explot the fact that both YARP and iDynTree store + // Matrices in row major forma . + memcpy(iDynTreeMatrix.data(),yarpMatrix.data(),rows*cols*sizeof(double)); + + return true; +} + + +} + +#include "YARPConversionsImplementation.h" + +#endif diff --git a/src/yarp/include/iDynTree/YARPConversionsImplementation.h b/src/yarp/include/iDynTree/YARPConversionsImplementation.h new file mode 100644 index 00000000000..4b800a1e020 --- /dev/null +++ b/src/yarp/include/iDynTree/YARPConversionsImplementation.h @@ -0,0 +1,153 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_YARP_CONVERSIONS_IMPLEMENTATION_H +#define IDYNTREE_YARP_CONVERSIONS_IMPLEMENTATION_H + +#include +#include +#include + +#include +#include + +using namespace yarp::math; + +namespace iDynTree +{ + +inline bool toiDynTree(const yarp::sig::Vector & yarpVector, iDynTree::Wrench & iDynTreeWrench) +{ + if( yarpVector.size() != 6 ) + { + return false; + } + + memcpy(iDynTreeWrench.getLinearVec3().data(),yarpVector.data(),3*sizeof(double)); + memcpy(iDynTreeWrench.getAngularVec3().data(),yarpVector.data()+3,3*sizeof(double)); + return true; +} + + +inline bool toYarp(const iDynTree::Wrench & iDynTreeWrench,yarp::sig::Vector & yarpVector) +{ + if( yarpVector.size() != 6 ) + { + yarpVector.resize(6); + } + + memcpy(yarpVector.data(),iDynTreeWrench.getLinearVec3().data(),3*sizeof(double)); + memcpy(yarpVector.data()+3,iDynTreeWrench.getAngularVec3().data(),3*sizeof(double)); + return true; +} + +inline bool toiDynTree(const yarp::sig::Vector& yarpVector, iDynTree::Position& iDynTreePosition) +{ + if( yarpVector.size() != 3 ) + { + return false; + } + + memcpy(iDynTreePosition.data(),yarpVector.data(),3*sizeof(double)); + return true; +} + +inline bool toiDynTree(const yarp::sig::Vector& yarpVector, iDynTree::Vector3& iDynTreeVector3) +{ + if( yarpVector.size() != 3 ) + { + return false; + } + + memcpy(iDynTreeVector3.data(),yarpVector.data(),3*sizeof(double)); + return true; +} + +inline bool toYarp(const iDynTree::Position& iDynTreePosition, yarp::sig::Vector& yarpVector) +{ + if( yarpVector.size() != 3 ) + { + yarpVector.resize(3); + } + + memcpy(yarpVector.data(),iDynTreePosition.data(),3*sizeof(double)); + return true; +} + +inline bool toiDynTree(const yarp::sig::Vector& yarpVector, Direction& direction) +{ + if( yarpVector.size() != 3 ) + { + return false; + } + + memcpy(direction.data(),yarpVector.data(),3*sizeof(double)); + + // normalize + direction.Normalize(); + + return true; +} + +inline bool toYarp(const Vector3& iDynTreeVector3, yarp::sig::Vector& yarpVector) +{ + if( yarpVector.size() != 3 ) + { + yarpVector.resize(3); + } + + memcpy(yarpVector.data(),iDynTreeVector3.data(),3*sizeof(double)); + return true; +} + +inline bool toiDynTree(const yarp::sig::Vector& yarpVector, VectorDynSize& iDynTreeVector) +{ + iDynTreeVector.resize(yarpVector.size()); + memcpy(iDynTreeVector.data(),yarpVector.data(),yarpVector.size()*sizeof(double)); + return true; +} + +inline bool toiDynTree(const yarp::sig::Matrix& yarpHomogeneousMatrix, + iDynTree::Transform& iDynTreeTransform) +{ + if( yarpHomogeneousMatrix.rows() != 4 || + yarpHomogeneousMatrix.cols() != 4 ) + { + reportError("","toiDynTree","Input yarp homegeneous matrix is not 4x4"); + return false; + } + + Rotation rot; + for(int r=0; r<3; r++) + { + for( int c=0; c<3; c++) + { + rot(r,c) = yarpHomogeneousMatrix(r,c); + } + } + + Position pos; + for(int i=0; i<3; i++) + { + pos(i) = yarpHomogeneousMatrix(i,3); + } + + iDynTreeTransform.setPosition(pos); + iDynTreeTransform.setRotation(rot); + + return true; +} + +inline bool toYarp(const iDynTree::Transform& iDynTreeTransform, + yarp::sig::Matrix& yarpHomogeneousMatrix) +{ + iDynTree::Matrix4x4 homTrans = iDynTreeTransform.asHomogeneousTransform(); + + toYarp(homTrans,yarpHomogeneousMatrix); + + return true; +} + +} + +#endif diff --git a/src/yarp/include/iDynTree/YARPEigenConversions.h b/src/yarp/include/iDynTree/YARPEigenConversions.h new file mode 100644 index 00000000000..cec729a6cd7 --- /dev/null +++ b/src/yarp/include/iDynTree/YARPEigenConversions.h @@ -0,0 +1,57 @@ +// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +// SPDX-License-Identifier: BSD-3-Clause + +#ifndef IDYNTREE_YARP_EIGEN_CONVERSIONS_H +#define IDYNTREE_YARP_EIGEN_CONVERSIONS_H + +#include +#include + +#include + +namespace iDynTree +{ + +/** + * Convert a yarp::sig::Vector to a Eigen::Map object + * @param yarpVector yarp::sig::Vector input + * @return a Eigen::Map vector that points to the data contained in the yarp vector + */ +inline Eigen::Map toEigen(yarp::sig::Vector & yarpVector) +{ + return Eigen::Map(yarpVector.data(),yarpVector.size()); +} + +/** + * Convert a yarp::sig::Matrix to a Eigen::Map< Eigen::Matrix > object + * @param yarpVector yarp::sig::Matrix input + * @return a Eigen::Map vector that points to the data contained in the yarp matrix + */ +inline Eigen::Map< Eigen::Matrix > toEigen(yarp::sig::Matrix & yarpMatrix) +{ + return Eigen::Map< Eigen::Matrix >(yarpMatrix.data(),yarpMatrix.rows(),yarpMatrix.cols()); +} + +/** + * Convert a const yarp::sig::Vector to a Eigen::Map object + * @param yarpVector yarp::sig::Vector input + * @return a Eigen::Map vector that points to the data contained in the yarp vector + */ +inline Eigen::Map toEigen(const yarp::sig::Vector & yarpVector) +{ + return Eigen::Map(yarpVector.data(),yarpVector.size()); +} + +/** + * Convert a const yarp::sig::Matrix to a Eigen::Map< const Eigen::Matrix > object + * @param yarpVector yarp::sig::Matrix input + * @return a Eigen::Map vector that points to the data contained in the yarp matrix + */ +inline Eigen::Map > toEigen(const yarp::sig::Matrix & yarpMatrix) +{ + return Eigen::Map >(yarpMatrix.data(),yarpMatrix.rows(),yarpMatrix.cols()); +} + +} + +#endif diff --git a/src/yarp/include/iDynTree/yarp/YARPConfigurationsLoader.h b/src/yarp/include/iDynTree/yarp/YARPConfigurationsLoader.h index 3d24dabd07c..ca70efeca1c 100644 --- a/src/yarp/include/iDynTree/yarp/YARPConfigurationsLoader.h +++ b/src/yarp/include/iDynTree/yarp/YARPConfigurationsLoader.h @@ -1,26 +1,13 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef IDYNTREE_YARP_CONFIGURATIONS_LOADER_H -#define IDYNTREE_YARP_CONFIGURATIONS_LOADER_H +#ifndef IDYNTREE_YARP_YARP_CONFIGURATIONS_LOADER_H +#define IDYNTREE_YARP_YARP_CONFIGURATIONS_LOADER_H -#include -#include -#include +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif -namespace iDynTree -{ - /** - * Takes a rotation matrix from configuration file. - * Notice, the matrix is parsed row-wise. - * @param rf yarp::os::Searchable The input searchable - * @param key The name corresponding to the matrix to be read - * @param rotation The output rotation - * @return true if successfull - */ - bool parseRotationMatrix(const yarp::os::Searchable& rf, const std::string& key, iDynTree::Rotation& rotation); -} - -#include "YARPConfigurationsLoaderImplementation.h" +#include #endif \ No newline at end of file diff --git a/src/yarp/include/iDynTree/yarp/YARPConfigurationsLoaderImplementation.h b/src/yarp/include/iDynTree/yarp/YARPConfigurationsLoaderImplementation.h index 448a7627714..f17c47fb9b7 100644 --- a/src/yarp/include/iDynTree/yarp/YARPConfigurationsLoaderImplementation.h +++ b/src/yarp/include/iDynTree/yarp/YARPConfigurationsLoaderImplementation.h @@ -1,41 +1,13 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef IDYNTREE_YARP_CONFIGURATIONS_LOADER_IMPLEMENTATION_H -#define IDYNTREE_YARP_CONFIGURATIONS_LOADER_IMPLEMENTATION_H +#ifndef IDYNTREE_YARP_YARP_CONFIGURATIONS_LOADER_IMPLEMENTATION_H +#define IDYNTREE_YARP_YARP_CONFIGURATIONS_LOADER_IMPLEMENTATION_H -#include +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif -inline bool iDynTree::parseRotationMatrix(const yarp::os::Searchable& rf, const std::string& key, iDynTree::Rotation& rotation) -{ - yarp::os::Value ini = rf.find(key); - if (ini.isNull() || !ini.isList()) - { - return false; - } - yarp::os::Bottle *outerList = ini.asList(); - if (!outerList || outerList->size() != 3) - { - return false; - } - for (int row = 0; row < outerList->size(); ++row) - { - yarp::os::Value& innerValue = outerList->get(row); - if (innerValue.isNull() || !innerValue.isList()) - { - return false; - } - yarp::os::Bottle *innerList = innerValue.asList(); - if (!innerList || innerList->size() != 3) - { - return false; - } - for (int column = 0; column < innerList->size(); ++column) - { - rotation.setVal(row, column, innerList->get(column).asFloat64()); - } - } - return true; -} +#include #endif diff --git a/src/yarp/include/iDynTree/yarp/YARPConversions.h b/src/yarp/include/iDynTree/yarp/YARPConversions.h index 1256ce134d9..f900086c091 100644 --- a/src/yarp/include/iDynTree/yarp/YARPConversions.h +++ b/src/yarp/include/iDynTree/yarp/YARPConversions.h @@ -1,217 +1,12 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef IDYNTREE_YARP_CONVERSIONS_H -#define IDYNTREE_YARP_CONVERSIONS_H - -#include -#include - -#include - -namespace iDynTree -{ - -class Direction; -class VectorDynSize; -class Transform; - -/** - * Convert a yarp::sig::Vector to a iDynTree::Wrench - * @param yarpVector yarp::sig::Vector input - * @param iDynTreeWrench iDynTree::Wrench output - * @return true if conversion was successful, false otherwise - * \ingroup iDynTreeYARP - */ -bool toiDynTree(const yarp::sig::Vector & yarpVector, iDynTree::Wrench & iDynTreeWrench); - -/** - * Convert a iDynTree::Wrench to a yarp::sig::Vector - * @param iDynTreeWrench iDynTree::Wrench input - * @param yarpVector yarp::sig::Vector output - * @return true if conversion was successful, false otherwise - * \ingroup iDynTreeYARP - */ -bool toYarp(const iDynTree::Wrench & iDynTreeWrench, yarp::sig::Vector & yarpVector); - -/** - * Convert a yarp::sig::Vector to a iDynTree::Position - * @param yarpVector yarp::sig::Vector input - * @param iDynTreePosition iDynTree::Position output - * @return true if conversion was successful, false otherwise (if the input yarpVector has size different from 3) - * \ingroup iDynTreeYARP - */ -bool toiDynTree(const yarp::sig::Vector & yarpVector, iDynTree::Position & iDynTreePosition); - -/** - * Convert a yarp::sig::Vector to a iDynTree::Vector3 - * @param yarpVector yarp::sig::Vector input - * @param iDynTreePosition iDynTree::Vector3 output - * @return true if conversion was successful, false otherwise (if the input yarpVector has size different from 3) - * \ingroup iDynTreeYARP - */ -bool toiDynTree(const yarp::sig::Vector & yarpVector, iDynTree::Vector3 & iDynTreeVector3); - -/** - * Convert a iDynTree::Position to a yarp::sig::Vector of 3 elements. - * @param iDynTreePosition iDynTree::Position input - * @param yarpVector yarp::sig::Vector output - * @return true if conversion was sucessful, false otherwise - * \ingroup iDynTreeYARP - */ -bool toYarp(const iDynTree::Position & iDynTreePosition, yarp::sig::Vector & yarpVector); - -/** - * Convert a yarp::sig::Vector of 3 elements to a iDynTree::Direction - * @param yarpVector yarp::sig::Vector input - * @param iDynTreeDirection iDynTree::Direction output - * @return true if conversion was successful, false otherwise (if the input yarpVector has size different from 3) - * - * \note the direction vector will be normalized to have unit norm. - * - * \ingroup iDynTreeYARP - */ -bool toiDynTree(const yarp::sig::Vector & yarpVector, iDynTree::Direction & iDynTreeDirection); - -/** - * Convert a iDynTree::Direction to a yarp::sig::Vector of 3 elements. - * @param iDynTreeDirection iDynTree::Position input - * @param yarpVector yarp::sig::Vector output - * @return true if conversion was sucessful, false otherwise - * - * \ingroup iDynTreeYARP - */ -bool toYarp(const iDynTree::Vector3 & iDynTreeDirection, yarp::sig::Vector & yarpVector); - -/** - * Convert a 4x4 yarp::sig::Matrix representing an homegeneous matrix to a iDynTree::Transform - * @param yarpHomogeneousMatrix yarp::sig::Matrix 4x4 homegeneous matrix input - * @param iDynTreeTransform iDynTree::Transform output - * @return true if conversion was successful, false otherwise - * - * \ingroup iDynTreeYARP - */ -bool toiDynTree(const yarp::sig::Matrix & yarpHomogeneousMatrix, iDynTree::Transform & iDynTreeTransform); - -/** - * Convert a iDynTree::Transform to a 4x4 yarp::sig::Matrix representing an homegeneous matrix - * @param iDynTreeTransform iDynTree::Transform input - * @param yarpHomogeneousMatrix yarp::sig::Matrix 4x4 homegeneous matrix output - * @return true if conversion was successful, false otherwise - * - * \ingroup iDynTreeYARP - */ -bool toYarp(const iDynTree::Transform & iDynTreeTransform, yarp::sig::Matrix & yarpHomogeneousMatrix); - -/** - * Convert a yarp::sig::Vector to a iDynTree::VectorDynSize - * @param yarpVector yarp::sig::Vector input - * @param iDynTreeVector iDynTree::VectorDynSize output - * @return true if conversion was successful, false otherwise - * \note the output VectorDynSize will be resized if necessary. - * - * \ingroup iDynTreeYARP - */ -bool toiDynTree(const yarp::sig::Vector & yarpVector, iDynTree::VectorDynSize & iDynTreeVector); - -/** - * Convert a iDynTree::VectorFixSize to a yarp::sig::Vector - * @param iDynTreeVector iDynTree::VectorFixSize input - * @param yarpVector yarp::sig::Vector output - * \note the output yarp::sig::Vector will be resized if necessary. - * - * \ingroup iDynTreeYARP - */ -template -void toYarp(const VectorType& iDynTreeVector, yarp::sig::Vector& yarpVector) -{ - size_t vecSize = iDynTreeVector.size(); - if( yarpVector.size() != vecSize ) - { - yarpVector.resize(vecSize); - } - - memcpy(yarpVector.data(),iDynTreeVector.data(),vecSize*sizeof(double)); -} - -/** - * Convert a iDynTree::MatrixFixSize to a yarp::sig::Matrix - * @param iDynTreeMatrix iDynTree::MatrixFixSize input - * @param yarpMatrix yarp::sig::Matrix output - * \note the output yarp::sig::Matrix will be resized if necessary. - * - * \ingroup iDynTreeYARP - */ -template -void toYarp(const MatrixType& iDynTreeMatrix, yarp::sig::Matrix& yarpMatrix) -{ - size_t rows = iDynTreeMatrix.rows(); - size_t cols = iDynTreeMatrix.cols(); - - if( static_cast(yarpMatrix.rows()) != rows || - static_cast(yarpMatrix.cols()) != cols ) - { - yarpMatrix.resize(rows,cols); - } - - // Here we explot the fact that both YARP and iDynTree store - // Matrices in row major forma . - memcpy(yarpMatrix.data(),iDynTreeMatrix.data(),rows*cols*sizeof(double)); -} - -/** - * Convert a yarp::sig::Vector to a iDynTree::VectorFixSize - * @param yarpVector yarp::sig::Vector input - * @param iDynTreeVector iDynTree::VectorFixSize output - * @return true if conversion was successful, false otherwise - * (if the input yarpMatrix has size different from the output VectorFixSize) - * - * \ingroup iDynTreeYARP - */ -template -bool toiDynTree(const yarp::sig::Vector& yarpVector, VectorType& iDynTreeVector) -{ - size_t vecSize = iDynTreeVector.size(); - if( yarpVector.size() != vecSize ) - { - return false; - } - - memcpy(iDynTreeVector.data(),yarpVector.data(),vecSize*sizeof(double)); - return true; -} - -/** - * Convert a yarp::sig::Matrix to a iDynTree::MatrixFixSize - * @param yarpMatrix yarp::sig::Matrix input - * @param iDynTreeMatrix iDynTree::MatrixFixSize output - * @return true if conversion was successful, false otherwise - * (if the input yarpMatrix has size different from the MatrixFixSize) - * - * \ingroup iDynTreeYARP - */ -template -bool toiDynTree(const yarp::sig::Matrix& yarpMatrix, MatrixType& iDynTreeMatrix) -{ - size_t rows = iDynTreeMatrix.rows(); - size_t cols = iDynTreeMatrix.cols(); - - if( static_cast(yarpMatrix.rows()) != rows || - static_cast(yarpMatrix.cols()) != cols ) - { - return false; - } - - // Here we explot the fact that both YARP and iDynTree store - // Matrices in row major forma . - memcpy(iDynTreeMatrix.data(),yarpMatrix.data(),rows*cols*sizeof(double)); - - return true; -} - - -} +#ifndef IDYNTREE_YARP_YARP_CONVERSIONS_H +#define IDYNTREE_YARP_YARP_CONVERSIONS_H +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif -#include "YARPConversionsImplementation.h" +#include #endif diff --git a/src/yarp/include/iDynTree/yarp/YARPConversionsImplementation.h b/src/yarp/include/iDynTree/yarp/YARPConversionsImplementation.h index 918d47036b1..d248ea4689c 100644 --- a/src/yarp/include/iDynTree/yarp/YARPConversionsImplementation.h +++ b/src/yarp/include/iDynTree/yarp/YARPConversionsImplementation.h @@ -1,153 +1,13 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef IDYNTREE_YARP_CONVERSIONS_IMPLEMENTATION_H -#define IDYNTREE_YARP_CONVERSIONS_IMPLEMENTATION_H +#ifndef IDYNTREE_YARP_YARP_CONVERSIONS_IMPLEMENTATION_H +#define IDYNTREE_YARP_YARP_CONVERSIONS_IMPLEMENTATION_H -#include -#include -#include - -#include -#include - -using namespace yarp::math; - -namespace iDynTree -{ - -inline bool toiDynTree(const yarp::sig::Vector & yarpVector, iDynTree::Wrench & iDynTreeWrench) -{ - if( yarpVector.size() != 6 ) - { - return false; - } - - memcpy(iDynTreeWrench.getLinearVec3().data(),yarpVector.data(),3*sizeof(double)); - memcpy(iDynTreeWrench.getAngularVec3().data(),yarpVector.data()+3,3*sizeof(double)); - return true; -} - - -inline bool toYarp(const iDynTree::Wrench & iDynTreeWrench,yarp::sig::Vector & yarpVector) -{ - if( yarpVector.size() != 6 ) - { - yarpVector.resize(6); - } - - memcpy(yarpVector.data(),iDynTreeWrench.getLinearVec3().data(),3*sizeof(double)); - memcpy(yarpVector.data()+3,iDynTreeWrench.getAngularVec3().data(),3*sizeof(double)); - return true; -} - -inline bool toiDynTree(const yarp::sig::Vector& yarpVector, iDynTree::Position& iDynTreePosition) -{ - if( yarpVector.size() != 3 ) - { - return false; - } - - memcpy(iDynTreePosition.data(),yarpVector.data(),3*sizeof(double)); - return true; -} - -inline bool toiDynTree(const yarp::sig::Vector& yarpVector, iDynTree::Vector3& iDynTreeVector3) -{ - if( yarpVector.size() != 3 ) - { - return false; - } - - memcpy(iDynTreeVector3.data(),yarpVector.data(),3*sizeof(double)); - return true; -} - -inline bool toYarp(const iDynTree::Position& iDynTreePosition, yarp::sig::Vector& yarpVector) -{ - if( yarpVector.size() != 3 ) - { - yarpVector.resize(3); - } - - memcpy(yarpVector.data(),iDynTreePosition.data(),3*sizeof(double)); - return true; -} - -inline bool toiDynTree(const yarp::sig::Vector& yarpVector, Direction& direction) -{ - if( yarpVector.size() != 3 ) - { - return false; - } - - memcpy(direction.data(),yarpVector.data(),3*sizeof(double)); - - // normalize - direction.Normalize(); - - return true; -} - -inline bool toYarp(const Vector3& iDynTreeVector3, yarp::sig::Vector& yarpVector) -{ - if( yarpVector.size() != 3 ) - { - yarpVector.resize(3); - } - - memcpy(yarpVector.data(),iDynTreeVector3.data(),3*sizeof(double)); - return true; -} - -inline bool toiDynTree(const yarp::sig::Vector& yarpVector, VectorDynSize& iDynTreeVector) -{ - iDynTreeVector.resize(yarpVector.size()); - memcpy(iDynTreeVector.data(),yarpVector.data(),yarpVector.size()*sizeof(double)); - return true; -} - -inline bool toiDynTree(const yarp::sig::Matrix& yarpHomogeneousMatrix, - iDynTree::Transform& iDynTreeTransform) -{ - if( yarpHomogeneousMatrix.rows() != 4 || - yarpHomogeneousMatrix.cols() != 4 ) - { - reportError("","toiDynTree","Input yarp homegeneous matrix is not 4x4"); - return false; - } - - Rotation rot; - for(int r=0; r<3; r++) - { - for( int c=0; c<3; c++) - { - rot(r,c) = yarpHomogeneousMatrix(r,c); - } - } - - Position pos; - for(int i=0; i<3; i++) - { - pos(i) = yarpHomogeneousMatrix(i,3); - } - - iDynTreeTransform.setPosition(pos); - iDynTreeTransform.setRotation(rot); - - return true; -} - -inline bool toYarp(const iDynTree::Transform& iDynTreeTransform, - yarp::sig::Matrix& yarpHomogeneousMatrix) -{ - iDynTree::Matrix4x4 homTrans = iDynTreeTransform.asHomogeneousTransform(); - - toYarp(homTrans,yarpHomogeneousMatrix); - - return true; -} +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif -} +#include #endif diff --git a/src/yarp/include/iDynTree/yarp/YARPEigenConversions.h b/src/yarp/include/iDynTree/yarp/YARPEigenConversions.h index cec729a6cd7..8efa9306c09 100644 --- a/src/yarp/include/iDynTree/yarp/YARPEigenConversions.h +++ b/src/yarp/include/iDynTree/yarp/YARPEigenConversions.h @@ -1,57 +1,13 @@ // SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) // SPDX-License-Identifier: BSD-3-Clause -#ifndef IDYNTREE_YARP_EIGEN_CONVERSIONS_H -#define IDYNTREE_YARP_EIGEN_CONVERSIONS_H +#ifndef IDYNTREE_YARP_YARP_EIGEN_CONVERSIONS_H +#define IDYNTREE_YARP_YARP_EIGEN_CONVERSIONS_H -#include -#include - -#include - -namespace iDynTree -{ - -/** - * Convert a yarp::sig::Vector to a Eigen::Map object - * @param yarpVector yarp::sig::Vector input - * @return a Eigen::Map vector that points to the data contained in the yarp vector - */ -inline Eigen::Map toEigen(yarp::sig::Vector & yarpVector) -{ - return Eigen::Map(yarpVector.data(),yarpVector.size()); -} - -/** - * Convert a yarp::sig::Matrix to a Eigen::Map< Eigen::Matrix > object - * @param yarpVector yarp::sig::Matrix input - * @return a Eigen::Map vector that points to the data contained in the yarp matrix - */ -inline Eigen::Map< Eigen::Matrix > toEigen(yarp::sig::Matrix & yarpMatrix) -{ - return Eigen::Map< Eigen::Matrix >(yarpMatrix.data(),yarpMatrix.rows(),yarpMatrix.cols()); -} - -/** - * Convert a const yarp::sig::Vector to a Eigen::Map object - * @param yarpVector yarp::sig::Vector input - * @return a Eigen::Map vector that points to the data contained in the yarp vector - */ -inline Eigen::Map toEigen(const yarp::sig::Vector & yarpVector) -{ - return Eigen::Map(yarpVector.data(),yarpVector.size()); -} - -/** - * Convert a const yarp::sig::Matrix to a Eigen::Map< const Eigen::Matrix > object - * @param yarpVector yarp::sig::Matrix input - * @return a Eigen::Map vector that points to the data contained in the yarp matrix - */ -inline Eigen::Map > toEigen(const yarp::sig::Matrix & yarpMatrix) -{ - return Eigen::Map >(yarpMatrix.data(),yarpMatrix.rows(),yarpMatrix.cols()); -} +#ifdef __DEPRECATED + #warning is deprecated. Please use . To disable this warning use -Wno-deprecated. +#endif -} +#include #endif