Merge pull request #211 from dic-iit/fix/remove_9.81

Substitute 9.81 with BipedalLocomotion::Math::StandardAccelerationOfGravitation

CHANGELOG.md

@@ -12,6 +12,7 @@ All notable changes to this project are documented in this file.
 - Move all the Contacts related classes in Contacts component (https://github.com/dic-iit/bipedal-locomotion-framework/pull/204)
 - Move all the ContactDetectors related classes in Contacts component (https://github.com/dic-iit/bipedal-locomotion-framework/pull/209)
 - The DCMPlanner and TimeVaryingDCMPlanner initialize functions take as input an std::weak_ptr. (https://github.com/dic-iit/bipedal-locomotion-framework/pull/208)
+- Use `Math::StandardAccelerationOfGravitation` instead of hardcoding 9.81. (https://github.com/dic-iit/bipedal-locomotion-framework/pull/211)
 
 ### Fixed
 - Fix missing implementation of `YarpSensorBridge::getFailedSensorReads()`. (https://github.com/dic-iit/bipedal-locomotion-framework/pull/202)

src/System/CMakeLists.txt

@@ -13,7 +13,7 @@ if(FRAMEWORK_COMPILE_System)
     PUBLIC_HEADERS         ${H_PREFIX}/Advanceable.h ${H_PREFIX}/VariablesHandler.h ${H_PREFIX}/DynamicalSystem.h ${H_PREFIX}/FloatingBaseSystemDynamics.h ${H_PREFIX}/DynamicalSystem.tpp ${H_PREFIX}/ContactWrench.h ${H_PREFIX}/LinearTimeInvariantSystem.h ${H_PREFIX}/Integrator.h ${H_PREFIX}/Integrator.tpp ${H_PREFIX}/FixedStepIntegrator.h ${H_PREFIX}/FixedStepIntegrator.tpp ${H_PREFIX}/Integrator.tpp ${H_PREFIX}/ForwardEuler.h ${H_PREFIX}/ForwardEuler.tpp ${H_PREFIX}/FloatingBaseSystemKinematics.h
     SOURCES                src/FloatingBaseSystemDynamics.cpp src/VariablesHandler.cpp src/ContactWrench.cpp src/LinearTimeInvariantSystem.cpp src/FloatingBaseSystemKinematics.cpp
     PUBLIC_LINK_LIBRARIES  BipedalLocomotion::ParametersHandler BipedalLocomotion::ContactModels iDynTree::idyntree-high-level iDynTree::idyntree-model Eigen3::Eigen
-    PRIVATE_LINK_LIBRARIES  BipedalLocomotion::CommonConversions
+    PRIVATE_LINK_LIBRARIES  BipedalLocomotion::CommonConversions BipedalLocomotion::Math
     SUBDIRECTORIES         tests
     )
 

src/System/include/BipedalLocomotion/System/FloatingBaseSystemDynamics.h

@@ -86,8 +86,9 @@ class FloatingBaseDynamicalSystem
     /**
      * Constructor.
      * @note The constructor set the gravity acceleration vector to
-     * \f$\begin{bmatrix} 0 & 0 & -9.81\end{bmatrix}^\top\f$. Please call setGravityVector() if you
-     * want define your custom gravity vector.
+     * \f$\begin{bmatrix} 0 & 0 & -g_s \end{bmatrix}^\top\f$. Where \f$g_s\$ is equal to
+     * BipedalLocomotion::Math::StandardAccelerationOfGravitation. Please call setGravityVector() if
+     * you want define your custom gravity vector.
      */
     FloatingBaseDynamicalSystem();
 

src/System/src/FloatingBaseSystemDynamics.cpp

@@ -11,6 +11,7 @@
 
 #include <BipedalLocomotion/Conversions/CommonConversions.h>
 #include <BipedalLocomotion/System/FloatingBaseSystemDynamics.h>
+#include <BipedalLocomotion/Math/Constants.h>
 
 using namespace BipedalLocomotion;
 using namespace BipedalLocomotion::System;
@@ -44,7 +45,7 @@ FloatingBaseDynamicalSystem::FloatingBaseDynamicalSystem()
 {
     // set the gravity vector
     m_gravity.setZero();
-    m_gravity(2) = -9.81;
+    m_gravity(2) = -BipedalLocomotion::Math::StandardAccelerationOfGravitation;
 }
 
 void FloatingBaseDynamicalSystem::setGravityVector(const Eigen::Ref<const Eigen::Vector3d>& gravity)

utilities/mas-imu-test/CMakeLists.txt

@@ -28,7 +28,7 @@ if(FRAMEWORK_COMPILE_MasImuTest)
     NAME ${EXE_TARGET_NAME}
     SOURCES ${${EXE_TARGET_NAME}_SRC}
     HEADERS ${${EXE_TARGET_NAME}_HDR} ${${EXE_TARGET_NAME}_THRIFT_GEN_FILES}
-    LINK_LIBRARIES ${YARP_LIBRARIES} ${iDynTree_LIBRARIES} BipedalLocomotion::GenericContainer BipedalLocomotion::YarpUtilities BipedalLocomotion::ParametersHandlerYarpImplementation BipedalLocomotion::matioCppConversions
+    LINK_LIBRARIES ${YARP_LIBRARIES} ${iDynTree_LIBRARIES} BipedalLocomotion::GenericContainer BipedalLocomotion::YarpUtilities BipedalLocomotion::ParametersHandlerYarpImplementation BipedalLocomotion::matioCppConversions BipedalLocomotion::Math
     )
 
   install_ini_files(${CMAKE_CURRENT_SOURCE_DIR}/app)

utilities/mas-imu-test/scripts/plotResults.m

@@ -8,6 +8,8 @@
 
 fileName='model.urdf'; %% Name of the urdf file
 
+gravity = 9.80665
+
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 %% No need to edit from here on
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@@ -25,71 +27,71 @@
     opt = tests.(tests_fields{testIndex}).options;
     rpyImu = zeros(3, size(data, 1));
     rpyFK = zeros(3, size(data, 1));
-    
+
     %% Base initialization
     jointOrder = opt.ConsideredJoints;
     world_H_base = eye(4);
     world_H_base(1:3, 1:3) = settings.base_rotation;
-    
+
     %% Model loading and set robot state
     KinDynModel = iDynTreeWrappers.loadReducedModel(jointOrder,settings.base_link,modelPath,fileName,false);
     joints_positions = data(1).JointPositions_rad;
-    iDynTreeWrappers.setRobotState(KinDynModel,world_H_base,joints_positions,zeros(6,1),zeros(size(joints_positions)),[0,0,-9.81]);
-    
+    iDynTreeWrappers.setRobotState(KinDynModel,world_H_base,joints_positions,zeros(6,1),zeros(size(joints_positions)),[0,0,-gravity]);
+
     %% Visualization setup
     [visualizer,objects]=iDynTreeWrappers.prepareVisualization(KinDynModel,meshFilePrefix, 'transparency',1, 'name', 'Imu Test', 'reuseFigure', 'name');
     set(gcf, 'WindowState', 'maximized');
     xlim([-0.8, 0.8])
     ylim([-0.8, 0.8])
     zlim([-1, 0.8])
     title(opt.TestName);
-    
+
     %% Frame from forward kinematics and plotting (long and thin frame)
     frameTransform = iDynTreeWrappers.getWorldTransform(KinDynModel, opt.FrameName);
     frameTransform(1:3, 1:3) = data(1).RotationFromEncoders;
     rpyFK(:, 1) = wbc.rollPitchYawFromRotation(frameTransform(1:3, 1:3));
     forward_kin_frame = iDynTreeWrappers.plotFrame(frameTransform, 0.2, 10);
-    
+
     %% Frame from IMU (short and thick frame)
     rpyImu(:, 1) = data(1).RPYfromIMUinDegRemapped;
     frameTransform(1:3, 1:3) = data(1).RotationFromIMUInInertialYawFiltered;
     imu_frame = iDynTreeWrappers.plotFrame(frameTransform, 0.4, 5);
-    
+
     %% Plot of accelerometer
     or = frameTransform(1:3, 4);
-    acc = frameTransform * [data(1).Accelerometer'/9.81/2; 1]; 
+    acc = frameTransform * [data(1).Accelerometer'/gravity/2; 1];
     gravity = plot3([or(1) acc(1)], [or(2) acc(2)], [or(3) acc(3)], 'm', 'linewidth', 7);
-    
+
     %% Data loop
     for i = 2 : length(data)
         %% Update robot state
         joints_positions = data(i).JointPositions_rad;
-        iDynTreeWrappers.setRobotState(KinDynModel,world_H_base,joints_positions,zeros(6,1),zeros(size(joints_positions)),[0,0,-9.81]);
-        
+        iDynTreeWrappers.setRobotState(KinDynModel,world_H_base,joints_positions,zeros(6,1),zeros(size(joints_positions)),[0,0,-gravity]);
+
         %% Update frame from forward kinematics
         frameTransform = iDynTreeWrappers.getWorldTransform(KinDynModel, opt.FrameName);
         frameTransform(1:3, 1:3) = data(i).RotationFromEncoders;
         rpyFK(:, i) = wbc.rollPitchYawFromRotation(frameTransform(1:3, 1:3));
         iDynTreeWrappers.updateFrame(forward_kin_frame, frameTransform);
-        
+
         %% Update frame from IMU
         rpyImu(:, i) = data(i).RPYfromIMUinDegRemapped;
         frameTransform(1:3, 1:3) = data(i).RotationFromIMUInInertialYawFiltered;
         iDynTreeWrappers.updateFrame(imu_frame, frameTransform);
-        
+
         %% Update robot visualization
         iDynTreeWrappers.updateVisualization(KinDynModel,visualizer);
-        
+
         %% Update gravity visualization
         or = frameTransform(1:3, 4);
-        acc = frameTransform * [data(i).Accelerometer'/9.81/2; 1];
+        acc = frameTransform * [data(i).Accelerometer'/gravity/2; 1];
         set(gravity, 'XData', [or(1) acc(1)], 'YData', [or(2) acc(2)], 'ZData', [or(3) acc(3)]);
 
         %% Force rendering
         drawnow()
         pause(0.01)
     end
-    
+
     %% Plotting of the RPY from forward kinematics and from IMU
     figure
     plot(rpyFK')
@@ -103,5 +105,5 @@
     if (testIndex ~= numel(tests_fields))
         pause
     end
-    
-end
+
+end

utilities/mas-imu-test/src/MasImuTest.cpp

@@ -5,21 +5,25 @@
  * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version.
  */
 
+#include <BipedalLocomotion/Conversions/matioCppConversions.h>
 #include <BipedalLocomotion/MasImuTest.h>
+#include <BipedalLocomotion/Math/Constants.h>
 #include <BipedalLocomotion/YarpUtilities/Helper.h>
-#include <BipedalLocomotion/Conversions/matioCppConversions.h>
+
+#include <iDynTree/Core/EigenHelpers.h>
+#include <iDynTree/Core/SO3Utils.h>
+#include <iDynTree/Model/IJoint.h>
 #include <iDynTree/ModelIO/ModelLoader.h>
 #include <iDynTree/yarp/YARPConfigurationsLoader.h>
-#include <iDynTree/Model/IJoint.h>
-#include <iDynTree/Core/SO3Utils.h>
-#include <iDynTree/Core/EigenHelpers.h>
+
 #include <yarp/os/LogStream.h>
-#include <iostream>
+
+#include <algorithm>
 #include <cassert>
+#include <cctype>
 #include <cmath>
+#include <iostream>
 #include <sstream>
-#include <algorithm>
-#include <cctype>
 
 using namespace BipedalLocomotion;
 using namespace BipedalLocomotion::GenericContainer;
@@ -518,7 +522,7 @@ bool MasImuTest::MasImuData::updateRotationFromEncoders()
     iDynTree::Vector3 gravity;
     gravity(0) = 0.0;
     gravity(1) = 0.0;
-    gravity(2) = -9.81;
+    gravity(2) = -BipedalLocomotion::Math::StandardAccelerationOfGravitation;
 
     bool ok = m_kinDyn.setRobotState(m_commonDataPtr->baseTransform, m_positionFeedbackInRad, dummy, m_dummyVelocity, gravity);
 
@@ -1351,4 +1355,3 @@ void MasImuTest::quit()
     std::lock_guard<std::mutex> guard(m_mutex);
     stopModule();
 }
-