[devices] Add FloatingBaseEstimatorDevice

ami-iit · Nov 10, 2020 · c41ecec · c41ecec
1 parent be1e0a2
commit c41ecec
Show file tree

Hide file tree

Showing 17 changed files with 1,171 additions and 0 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -75,6 +75,7 @@ include(AddBipedalLocomotionLibrary)
 include(InstallIniFiles)
 
 add_subdirectory(src)
+add_subdirectory(devices)
 
 #Creates the interface target BipedalLocomotion::Framework that depends on all the targets.
 #By including the file "BipedalLocomotion/Framework.h" it is possible to include all the headers automatically.

diff --git a/devices/CMakeLists.txt b/devices/CMakeLists.txt
@@ -0,0 +1,6 @@
+# Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT). All rights reserved.
+# This software may be modified and distributed under the terms of the
+# GNU Lesser General Public License v2.1 or any later version.
+
+add_subdirectory(FloatingBaseEstimatorDevice)
+
diff --git a/devices/FloatingBaseEstimatorDevice/CMakeLists.txt b/devices/FloatingBaseEstimatorDevice/CMakeLists.txt
@@ -0,0 +1,47 @@
+# Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT). All rights reserved.
+# This software may be modified and distributed under the terms of the
+# GNU Lesser General Public License v2.1 or any later version.
+
+if(FRAMEWORK_COMPILE_YarpImplementation)
+  cmake_minimum_required(VERSION 3.5)
+  set(PLUGIN_NAME FloatingBaseEstimatorDevice)
+
+  set(${PLUGIN_NAME}_SRC src/FloatingBaseEstimatorDevice.cpp)
+  set(${PLUGIN_NAME}_HDR include/BipedalLocomotion/FloatingBaseEstimatorDevice.h)
+
+  yarp_prepare_plugin(${PLUGIN_NAME} CATEGORY device
+                                     TYPE BipedalLocomotion::FloatingBaseEstimatorDevice
+                                     INCLUDE ${${PLUGIN_NAME}_HDR}
+                                     DEFAULT ON)
+
+  yarp_add_plugin(${PLUGIN_NAME} ${${PLUGIN_NAME}_SRC} ${${PLUGIN_NAME}_HDR})
+
+  target_link_libraries(${PLUGIN_NAME} PUBLIC ${YARP_LIBRARIES}
+                                              ${iDynTree_LIBRARIES}
+                                              BipedalLocomotion::YarpUtilities
+                                              BipedalLocomotion::RobotInterfaceYarpImplementation
+                                              BipedalLocomotion::ParametersHandlerYarpImplementation
+                                              BipedalLocomotion::FloatingBaseEstimators)
+  target_compile_features(${PLUGIN_NAME} PUBLIC cxx_std_17)
+
+  # Specify include directories for both compilation and installation process.
+  # The $<INSTALL_PREFIX> generator expression is useful to ensure to create
+  # relocatable configuration files, see https://cmake.org/cmake/help/latest/manual/cmake-packages.7.html#creating-relocatable-packages
+  target_include_directories(${PLUGIN_NAME} PUBLIC "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+    "<INSTALLINTERFACE:<INSTALL_PREFIX>/${CMAKE_INSTALL_INCLUDEDIR}>")
+
+  # Specify installation targets, typology and destination folders.
+  yarp_install(TARGETS ${PLUGIN_NAME}
+               COMPONENT runtime
+               LIBRARY DESTINATION ${YARP_DYNAMIC_PLUGINS_INSTALL_DIR}/
+               ARCHIVE DESTINATION ${YARP_STATIC_PLUGINS_INSTALL_DIR}/)
+  yarp_install(FILES FloatingBaseEstimatorDevice.ini
+               COMPONENT runtime
+               DESTINATION ${YARP_PLUGIN_MANIFESTS_INSTALL_DIR}/)
+
+  add_subdirectory(app)
+
+  message(STATUS "Created device ${PLUGIN_NAME}.")
+
+endif()
+
diff --git a/devices/FloatingBaseEstimatorDevice/FloatingBaseEstimatorDevice.ini b/devices/FloatingBaseEstimatorDevice/FloatingBaseEstimatorDevice.ini
@@ -0,0 +1,4 @@
+[plugin FloatingBaseEstimatorDevice]
+type device
+name FloatingBaseEstimatorDevice
+library FloatingBaseEstimatorDevice
diff --git a/devices/FloatingBaseEstimatorDevice/app/CMakeLists.txt b/devices/FloatingBaseEstimatorDevice/app/CMakeLists.txt
@@ -0,0 +1,25 @@
+# Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved.
+# This software may be modified and distributed under the terms of the
+# GNU Lesser General Public License v2.1 or any later version.
+
+# Thanks to Stefano Dafarra <[email protected]> for this CMakeLists.txt
+
+# List the subdirectory (http://stackoverflow.com/questions/7787823/cmake-how-to-get-the-name-of-all-subdirectories-of-a-directory)
+macro(SUBDIRLIST result curdir)
+  file(GLOB children RELATIVE ${curdir} ${curdir}/*)
+  set(dirlist "")
+  foreach(child ${children})
+    if(IS_DIRECTORY ${curdir}/${child})
+      list(APPEND dirlist ${child})
+    endif()
+  endforeach()
+  set(${result} ${dirlist})
+endmacro()
+
+# Get list of models
+subdirlist(subdirs ${CMAKE_CURRENT_SOURCE_DIR}/robots/)
+# Install each model
+foreach (dir ${subdirs})
+  yarp_install(DIRECTORY robots/${dir} DESTINATION ${YARP_ROBOTS_INSTALL_DIR})
+endforeach ()
+
diff --git a/...loatingBaseEstimatorDevice/app/robots/iCubGazeboV2_5/estimators/invekf-base-estimator.xml b/...loatingBaseEstimatorDevice/app/robots/iCubGazeboV2_5/estimators/invekf-base-estimator.xml
@@ -0,0 +1,128 @@
+<!-- Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved.
+This software may be modified and distributed under the terms of the
+GNU Lesser General Public License v2.1 or any later version. -->
+
+<?xml version="1.0" encoding="UTF-8" ?>
+<device  xmlns:xi="http://www.w3.org/2001/XInclude" name="base-estimator" type="FloatingBaseEstimatorDevice">
+    <param name="robot">icubSim</param>
+    <param name="sampling_period_in_s">0.01</param>
+    <param name="port_prefix">/base-estimator</param>
+    <param name="model_file">model.urdf</param>
+    <param name="joint_list">("neck_pitch", "neck_roll", "neck_yaw", "torso_pitch", "torso_roll", "torso_yaw", "l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "l_elbow", "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", "r_elbow", "l_hip_pitch", "l_hip_roll", "l_hip_yaw", "l_knee", "l_ankle_pitch", "l_ankle_roll", "r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll")</param>
+    <param name="base_link_imu">root_link_imu_acc</param>
+    <param name="left_foot_wrench">left_foot_cartesian_wrench</param>
+    <param name="right_foot_wrench">right_foot_cartesian_wrench</param>
+    <param name="estimator_type">InvEKF</param>
+
+
+     <group name="RobotSensorBridge">
+        <param name="check_for_nan">false</param>
+        <param name="stream_joint_states">true</param>
+        <param name="stream_inertials">true</param>
+        <param name="stream_cartesian_wrenches">true</param>
+        <param name="stream_forcetorque_sensors">true</param>
+        <param name="stream_cameras">true</param>
+
+        <group name="RemoteControlBoardRemapper">
+            <param name="joints_list">("neck_pitch", "neck_roll", "neck_yaw", "torso_pitch", "torso_roll", "torso_yaw", "l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "l_elbow", "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", "r_elbow", "l_hip_pitch", "l_hip_roll", "l_hip_yaw", "l_knee", "l_ankle_pitch", "l_ankle_roll", "r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll")</param>
+        </group>
+
+        <group name="InertialSensors">
+            <param name="imu_list">("root_link_imu_acc")</param>
+        </group>
+
+        <group name="CartesianWrenches">
+            <param name="cartesian_wrenches_list">("right_foot_cartesian_wrench", "left_foot_cartesian_wrench")</param>
+        </group>
+    </group>
+
+    <group name="ContactSchmittTrigger">
+        <group name="left_foot">
+            <param name="schmitt_stable_contact_make_time">0.05</param>
+            <param name="schmitt_stable_contact_break_time">0.02</param>
+            <param name="schmitt_contact_make_force_threshold">170.0</param>
+            <param name="schmitt_contact_break_force_threshold">75.0</param>
+        </group>
+        <group name="right_foot">
+            <param name="schmitt_stable_contact_make_time">0.05</param>
+            <param name="schmitt_stable_contact_break_time">0.02</param>
+            <param name="schmitt_contact_make_force_threshold">170.0</param>
+            <param name="schmitt_contact_break_force_threshold">75.0</param>
+        </group>
+    </group>
+
+    <group name="ModelInfo">
+        <param name="base_link">root_link</param>
+        <param name="left_foot_contact_frame">l_sole</param>
+        <param name="right_foot_contact_frame">r_sole</param>
+        <param name="base_link_imu">root_link_imu_acc</param>
+    </group>
+
+    <group name="Options">
+        <param name="enable_imu_bias_estimation">false</param>
+        <param name="enable_static_imu_bias_initialization">false</param>
+        <param name="nr_samples_for_imu_bias_initialization">0</param>
+        <param name="enable_ekf_update">true</param>
+        <param name="acceleration_due_to_gravity">(0.0, 0.0, -9.80665)</param>
+    </group>
+
+    <group name="SensorsStdDev">
+        <!-- <param name="accelerometer_measurement_noise_std_dev">(0.0382, 0.01548, 0.0042)</param>
+        <param name="gyroscope_measurement_noise_std_dev">(0.0111, 0.0024, 0.0043)</param>  -->
+        <param name="accelerometer_measurement_noise_std_dev">(0.102, 0.148, 0.142)</param>
+        <param name="gyroscope_measurement_noise_std_dev">(0.05, 0.04, 0.06)</param>
+        <param name="contact_foot_linear_velocity_noise_std_dev">(9e-3, 9.5e-3, 7e-3)</param>
+        <param name="contact_foot_angular_velocity_noise_std_dev">(0.007, 0.0075, 0.004)</param>
+        <param name="swing_foot_linear_velocity_noise_std_dev">(0.05, 0.05, 0.05)</param>
+        <param name="swing_foot_angular_velocity_noise_std_dev">(0.015, 0.015, 0.015)</param>
+        <param name="forward_kinematic_measurement_noise_std_dev">(1e-3, 1e-3, 1e-3, 1e-6, 1e-6, 1e-6)</param>
+        <param name="encoders_measurement_noise_std_dev">(1e-4, 1e-4, 1e-4,
+                                                          1e-4, 1e-4, 1e-4,
+                                                          1e-4, 1e-4, 1e-4, 1e-4,
+                                                          1e-4, 1e-4, 1e-4, 1e-4,
+                                                          1e-4, 1e-4, 1e-4,
+                                                          1e-4, 1e-4, 1e-4,
+                                                          1e-4, 1e-4, 1e-4,
+                                                          1e-4, 1e-4, 1e-4)</param>
+        <param name="accelerometer_measurement_bias_noise_std_dev">(1e-4, 1e-4, 1e-4)</param>
+        <param name="gyroscope_measurement_bias_noise_std_dev">(1e-4, 1e-4, 1e-4)</param>
+    </group>
+
+    <group name="InitialStates">
+        <param name="imu_orientation_quaternion_wxyz">(0.3218, -0.6304, -0.6292, 0.3212)</param>
+        <param name="imu_position_xyz">(0.0296,-0.1439, 0.4915)</param>
+        <param name="imu_linear_velocity_xyz">(0.0, 0.0, 0.0)</param>
+        <param name="l_contact_frame_orientation_quaternion_wxyz">(1.0000, -0.0059, -0.0001, -0.0015)</param>
+        <param name="l_contact_frame_position_xyz">(0.0791, -0.0817, 0.0109)</param>
+        <param name="r_contact_frame_orientation_quaternion_wxyz">(1.0000, 0.0059, -0.0002, -0.0004)</param>
+        <param name="r_contact_frame_position_xyz">(0.0788, -0.2282, 0.0109)</param>
+        <param name="accelerometer_bias">(0.0, 0.0, 0.0)</param>
+        <param name="gyroscope_bias">(0.0, 0.0, 0.0)</param>
+    </group>
+
+    <group name="PriorsStdDev">
+        <param name="imu_orientation">(0.175, 0.175, 0.0175)</param>
+        <param name="imu_position">(1e-1, 1e-1, 1e-1)</param>
+        <param name="imu_linear_velocity">(0.075, 0.05, 0.05)</param>
+        <param name="l_contact_frame_orientation">(0.175, 0.175, 0.0175)</param>
+        <param name="l_contact_frame_position">(1e-1, 1e-1, 1e-1)</param>
+        <param name="r_contact_frame_orientation">(0.175, 0.175, 0.0175)</param>
+        <param name="r_contact_frame_position">(1e-1, 1e-1, 1e-1)</param>
+        <param name="accelerometer_bias">(1e-3, 1e-3, 1e-3)</param>
+        <param name="gyroscope_bias">(1e-3, 1e-3, 1e-3)</param>
+    </group>
+
+    <!-- ATTACH -->
+    <action phase="startup" level="15" type="attach">
+        <paramlist name="networks">
+            <elem name="all_joints">all_joints_mc</elem>
+            <elem name="root_link_imu_acc">root_link_imu_acc</elem>
+            <elem name="left_foot_cartesian_wrench">left_foot_cartesian_wrench</elem>
+            <elem name="right_foot_cartesian_wrench">right_foot_cartesian_wrench</elem>
+        </paramlist>
+    </action>
+
+    <action phase="shutdown" level="2" type="detach" />
+    <!-- FINISH ATTACH-->
+
+</device>
diff --git a/devices/FloatingBaseEstimatorDevice/app/robots/iCubGazeboV2_5/interface/all_joints_mc.xml b/devices/FloatingBaseEstimatorDevice/app/robots/iCubGazeboV2_5/interface/all_joints_mc.xml
@@ -0,0 +1,10 @@
+<!-- Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved.
+This software may be modified and distributed under the terms of the
+GNU Lesser General Public License v2.1 or any later version. -->
+<?xml version="1.0" encoding="UTF-8" ?>
+<device xmlns:xi="http://www.w3.org/2001/XInclude" name="all_joints_mc" type="remotecontrolboardremapper">
+    <param name="remoteControlBoards">("/icubSim/head", "/icubSim/torso", "/icubSim/left_arm", "/icubSim/right_arm", "/icubSim/left_leg", "/icubSim/right_leg")</param>
+    <param name="axesNames">("neck_pitch", "neck_roll", "neck_yaw", "torso_pitch", "torso_roll", "torso_yaw", "l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "l_elbow", "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", "r_elbow", "l_hip_pitch", "l_hip_roll", "l_hip_yaw", "l_knee", "l_ankle_pitch", "l_ankle_roll", "r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll")</param>
+    <param name="localPortPrefix">/baseestimation</param>
+</device>
+
diff --git a/devices/FloatingBaseEstimatorDevice/app/robots/iCubGazeboV2_5/interface/root_imu.xml b/devices/FloatingBaseEstimatorDevice/app/robots/iCubGazeboV2_5/interface/root_imu.xml
@@ -0,0 +1,9 @@
+<!-- Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved.
+This software may be modified and distributed under the terms of the
+GNU Lesser General Public License v2.1 or any later version. -->
+<?xml version="1.0" encoding="UTF-8" ?>
+<device xmlns:xi="http://www.w3.org/2001/XInclude" name="root_link_imu_acc" type="genericSensorClient">
+    <param name="remote">/icubSim/xsens_inertial</param>
+    <param name="local">/baseestimation/xsens_inertial</param>
+</device>
+
diff --git a/devices/FloatingBaseEstimatorDevice/app/robots/iCubGazeboV2_5/interface/wbd_left_foot.xml b/devices/FloatingBaseEstimatorDevice/app/robots/iCubGazeboV2_5/interface/wbd_left_foot.xml
@@ -0,0 +1,9 @@
+<!-- Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved.
+This software may be modified and distributed under the terms of the
+GNU Lesser General Public License v2.1 or any later version. -->
+<?xml version="1.0" encoding="UTF-8" ?>
+<device xmlns:xi="http://www.w3.org/2001/XInclude" name="left_foot_cartesian_wrench" type="genericSensorClient">
+    <param name="remote">/wholeBodyDynamics/left_foot/cartesianEndEffectorWrench:o</param>
+    <param name="local">/baseestimation/left_foot/cartesianEndEffectorWrench</param>
+</device>
+
diff --git a/devices/FloatingBaseEstimatorDevice/app/robots/iCubGazeboV2_5/interface/wbd_right_foot.xml b/devices/FloatingBaseEstimatorDevice/app/robots/iCubGazeboV2_5/interface/wbd_right_foot.xml
@@ -0,0 +1,9 @@
+<!-- Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved.
+This software may be modified and distributed under the terms of the
+GNU Lesser General Public License v2.1 or any later version. -->
+<?xml version="1.0" encoding="UTF-8" ?>
+<device xmlns:xi="http://www.w3.org/2001/XInclude" name="right_foot_cartesian_wrench" type="genericSensorClient">
+    <param name="remote">/wholeBodyDynamics/right_foot/cartesianEndEffectorWrench:o</param>
+    <param name="local">/baseestimation/right_foot/cartesianEndEffectorWrench</param>
+</device>
+
diff --git a/devices/FloatingBaseEstimatorDevice/app/robots/iCubGazeboV2_5/launch-base-estimator.xml b/devices/FloatingBaseEstimatorDevice/app/robots/iCubGazeboV2_5/launch-base-estimator.xml
@@ -0,0 +1,21 @@
+<!-- Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved.
+This software may be modified and distributed under the terms of the
+GNU Lesser General Public License v2.1 or any later version. -->
+
+<?xml version="1.0" encoding="UTF-8" ?>
+<!DOCTYPE robot PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
+
+<robot name="iCubGazeboV2_5" portprefix="icubSim" build="1" xmlns:xi="http://www.w3.org/2001/XInclude">
+    <devices>
+        <!-- interface -->
+        <xi:include href="interface/all_joints_mc.xml" />
+        <xi:include href="interface/root_imu.xml" />
+        <xi:include href="interface/wbd_left_foot.xml" />
+        <xi:include href="interface/wbd_right_foot.xml" />
+
+
+        <xi:include href="estimators/invekf-base-estimator.xml" />
+    </devices>
+
+</robot>
+
diff --git a/devices/FloatingBaseEstimatorDevice/include/BipedalLocomotion/FloatingBaseEstimatorDevice.h b/devices/FloatingBaseEstimatorDevice/include/BipedalLocomotion/FloatingBaseEstimatorDevice.h
@@ -0,0 +1,92 @@
+/**
+ * @file FloatingBaseEstimatorDevice.h
+ * @authors Prashanth Ramadoss
+ * @copyright 2020 Istituto Italiano di Tecnologia (IIT). This software may be modified and
+ * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version.
+ */
+
+#ifndef BIPEDAL_LOCOMOTION_FRAMEWORK_BASE_ESTIMATOR_DEVICE_H
+#define BIPEDAL_LOCOMOTION_FRAMEWORK_BASE_ESTIMATOR_DEVICE_H
+
+#include <BipedalLocomotion/ParametersHandler/YarpImplementation.h>
+#include <BipedalLocomotion/RobotInterface/YarpSensorBridge.h>
+#include <BipedalLocomotion/FloatingBaseEstimators/FloatingBaseEstimator.h>
+
+#include <iDynTree/Estimation/ContactStateMachine.h>
+#include <iDynTree/ModelIO/ModelLoader.h>
+
+#include <yarp/os/PeriodicThread.h>
+#include <yarp/dev/DeviceDriver.h>
+#include <yarp/dev/Wrapper.h>
+#include <yarp/os/ResourceFinder.h>
+
+#include <mutex>
+#include <memory>
+
+namespace BipedalLocomotion
+{
+    class FloatingBaseEstimatorDevice;
+}
+
+class BipedalLocomotion::FloatingBaseEstimatorDevice : public yarp::dev::DeviceDriver,
+                                                       public yarp::dev::IMultipleWrapper,
+                                                       public yarp::os::PeriodicThread
+{
+public:
+    explicit FloatingBaseEstimatorDevice(double period, yarp::os::ShouldUseSystemClock useSystemClock = yarp::os::ShouldUseSystemClock::No);
+    FloatingBaseEstimatorDevice();
+    ~FloatingBaseEstimatorDevice();
+
+    virtual bool open(yarp::os::Searchable& config) final;
+    virtual bool close() final;
+    virtual bool attachAll(const yarp::dev::PolyDriverList & poly) final;
+    virtual bool detachAll() final;
+    virtual void run() final;
+
+private:
+    bool setupRobotModel(yarp::os::Searchable& config);
+    bool setupRobotSensorBridge(yarp::os::Searchable& config);
+    bool setupFeetContactStateMachines(yarp::os::Searchable& config);
+    bool parseFootSchmittParams(yarp::os::Searchable& config, iDynTree::SchmittParams& params);
+    bool setupBaseEstimator(yarp::os::Searchable& config);
+    bool updateMeasurements();
+    bool updateInertialBuffers();
+    bool updateContactStates();
+    bool updateKinematics();
+
+    void publish();
+    void publishBaseLinkState(const BipedalLocomotion::Estimators::FloatingBaseEstimators::Output& out);
+    void publishInternalStateAndStdDev(const BipedalLocomotion::Estimators::FloatingBaseEstimators::Output& out);
+    void publishFootContactStatesAndNormalForces();
+    bool openCommunications();
+    void closeCommunications();
+    bool openBufferedSigPort(yarp::os::BufferedPort<yarp::sig::Vector>& port,
+                             const std::string& portPrefix,
+                             const std::string& address);
+    void closeBufferedSigPort(yarp::os::BufferedPort<yarp::sig::Vector>& port);
+
+    struct
+    {
+        yarp::os::BufferedPort<yarp::sig::Vector> floatingBaseStatePort;
+        yarp::os::BufferedPort<yarp::sig::Vector> internalStateAndStdDevPort;
+        yarp::os::BufferedPort<yarp::sig::Vector> contactStatePort;
+    } m_comms;
+
+    iDynTree::Model m_model;
+    std::unique_ptr<BipedalLocomotion::RobotInterface::YarpSensorBridge> m_robotSensorBridge;
+    std::unique_ptr<BipedalLocomotion::Estimators::FloatingBaseEstimator> m_estimator;
+    std::unique_ptr<iDynTree::ContactStateMachine> m_lFootCSM, m_rFootCSM;
+    bool m_currentlFootState{false}, m_currentrFootState{false};
+    double m_currentlContactNormal{0.0}, m_currentrContactNormal{0.0};
+    std::mutex m_deviceMutex;
+    std::string m_portPrefix{"/base-estimator"};
+    std::string m_robot{"icubSim"};
+    std::string m_estimatorType{"InvEKF"};
+    std::string m_baseLinkImuName{"root_link_imu_acc"};
+    std::string m_leftFootWrenchName{"left_foot_cartesian_wrench"};
+    std::string m_rightFootWrenchName{"right_foot_cartesian_wrench"};
+};
+
+
+
+#endif //BIPEDAL_LOCOMOTION_FRAMEWORK_BASE_ESTIMATOR_DEVICE_H