-
Notifications
You must be signed in to change notification settings - Fork 38
/
FloatingBaseEstimator.h
executable file
·448 lines (401 loc) · 25.5 KB
/
FloatingBaseEstimator.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
/**
* @file FloatingBaseEstimator.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_ESTIMATORS_FLOATING_BASE_ESTIMATOR_H
#define BIPEDAL_LOCOMOTION_ESTIMATORS_FLOATING_BASE_ESTIMATOR_H
#include <BipedalLocomotion/System/Source.h>
#include <BipedalLocomotion/ParametersHandler/IParametersHandler.h>
#include <BipedalLocomotion/FloatingBaseEstimators/FloatingBaseEstimatorParams.h>
#include <BipedalLocomotion/FloatingBaseEstimators/FloatingBaseEstimatorIO.h>
#include <iDynTree/KinDynComputations.h>
#include <iostream>
#include <memory>
namespace BipedalLocomotion
{
namespace Estimators
{
/**
* FloatingBaseEstimator class contains the bare-bones implementation for
* a LeggedOdometry based or Extended Kalman Filter based floating base estimation
* algorithms for bipedal robots.
* @note it is assumed that if an IMU (primary) is specified in the inherited implementation,
* then this IMU is rigidly attached to the specified base link in the implementation
*/
class FloatingBaseEstimator : public BipedalLocomotion::System::Source<FloatingBaseEstimators::Output>
{
public:
FloatingBaseEstimator();
virtual ~FloatingBaseEstimator() { };
/**
* iDynTree based model-specific computations class
* This is class is used in a required configuration step for the estimator
* All the model related kinematics and dynamics computations specific to
* the floating base estimator are contained within this class.
*/
class ModelComputations
{
public:
/**
* Set the shared kindyn object
* @param[in] kinDyn shared pointer of the common KinDynComputations resource
* @return True in case of success, false otherwise.
*
* @note Expects only a valid pointer to the object, need not be loaded with the robot model.
*/
bool setKinDynObject(std::shared_ptr<iDynTree::KinDynComputations> kinDyn);
/**
* Set base link name and name of the IMU rigidly attached to the IMU
* @param[in] baseLinkFrame name of the base link frame
* @param[in] imuFrame name of the IMU frame
* @return True in case of success, false otherwise.
*/
bool setBaseLinkAndIMU(const std::string& baseLinkFrame, const std::string& imuFrame);
/**
* Set the feet contact frames, expected to be in contact with the environment
* @param[in] lFootContactFrame left foot contact frame
* @param[in] rFootContactFrame right foot contact frame
* @return True in case of success, false otherwise.
*/
bool setFeetContactFrames(const std::string& lFootContactFrame, const std::string& rFootContactFrame);
/**
* Check if model is configured with the required information
* @return True in case of success, false otherwise.
*/
bool isModelInfoLoaded();
/**
* Get relative pose between IMU and the feet
* @param[in] encoders joint positions through encoder measurements
* @param[out] IMU_H_l_foot pose of the left foot contact frame with respect to the IMU frame
* @param[out] IMU_H_r_foot pose of the right foot contact frame with respect to the IMU frame
* @return True in case of success, false otherwise.
*/
bool getIMU_H_feet(Eigen::Ref<const Eigen::VectorXd> encoders,
manif::SE3d& IMU_H_l_foot,
manif::SE3d& IMU_H_r_foot);
/**
* Get relative pose between IMU and the feet
* @param[in] encoders joint positions through encoder measurements
* @param[out] IMU_H_l_foot pose of the left foot contact frame with respect to the IMU frame
* @param[out] IMU_H_r_foot pose of the right foot contact frame with respect to the IMU frame
* @param[out] J_IMULF Jacobian of left foot frame with respect to IMU frame in mixed-velocity trivialization
* @param[out] J_IMURF Jacobian of right foot frame with respect to IMU frame in mixed-velocity trivialization
* @return True in case of success, false otherwise.
*/
bool getIMU_H_feet(const Eigen::Ref<const Eigen::VectorXd> encoders,
manif::SE3d& IMU_H_l_foot,
manif::SE3d& IMU_H_r_foot,
Eigen::Ref<Eigen::MatrixXd> J_IMULF,
Eigen::Ref<Eigen::MatrixXd> J_IMURF);
/**
* Get the base link pose and velocity from the estimated IMU pose and velocity
* @note the input and output velocities are both specified in mixed-velocity representation
* @param[in] A_H_IMU pose of the IMU in the world
* @param[in] v_IMU mixed-trivialized velocity of the IMU in the world
* @param[out] A_H_B pose of the base link in the world
* @param[out] v_B mixed-trivialized velocity of the base link in the world
* @return True in case of success, false otherwise.
*/
bool getBaseStateFromIMUState(const manif::SE3d& A_H_IMU, Eigen::Ref<const Eigen::Matrix<double, 6, 1> > v_IMU,
manif::SE3d& A_H_B, Eigen::Ref<Eigen::Matrix<double, 6, 1> > v_B);
/**
* Getters
*/
const int& nrJoints() const { return m_nrJoints; }
const std::string& baseLink() const { return m_baseLink; }
const iDynTree::FrameIndex& baseLinkIdx() const { return m_baseLinkIdx; }
const iDynTree::FrameIndex& baseIMUIdx() const { return m_baseImuIdx; }
const std::string& baseLinkIMU() const { return m_baseImuFrame; }
const std::string& leftFootContactFrame() const { return m_lFootContactFrame; }
const std::string& rightFootContactFrame() const { return m_rFootContactFrame; }
const manif::SE3d& base_H_IMU() const { return m_base_H_imu; }
const bool& isKinDynValid() const { return m_validKinDyn; }
std::shared_ptr<iDynTree::KinDynComputations> kinDyn() const { return m_kindyn; }
private:
std::string m_baseLink{""}; /**< name of the floating base link*/
std::string m_baseImuFrame{""}; /**< name of the IMU frame rigidly attached to the floating base link*/
std::string m_lFootContactFrame{""}; /**< name of the left foot contact frame expected to be in contact with the environment*/
std::string m_rFootContactFrame{""}; /**< name of the right foot contact frame expected to be in contact with the environment*/
iDynTree::FrameIndex m_baseLinkIdx{iDynTree::FRAME_INVALID_INDEX}; /**< base link's frame index in the loaded model*/
iDynTree::FrameIndex m_baseImuIdx{iDynTree::FRAME_INVALID_INDEX}; /**< IMU index in the loaded model*/
iDynTree::FrameIndex m_lFootContactIdx{iDynTree::FRAME_INVALID_INDEX}; /**< Left foot contact frame index in the loaded model*/
iDynTree::FrameIndex m_rFootContactIdx{iDynTree::FRAME_INVALID_INDEX}; /**< Right foot contact freame index in the loaded model*/
std::shared_ptr<iDynTree::KinDynComputations> m_kindyn{nullptr}; /**< KinDynComputations object to do the model specific computations */
manif::SE3d m_base_H_imu; /**< Rigid body transform of IMU frame with respect to the base link frame */
int m_nrJoints{0}; /**< number of joints in the loaded reduced model */
bool m_validKinDyn{false};
};
/**
* Configure generic parameters and the model. The generic parameters include,
* - sampling_period_in_s [PARAMETER|REQUIRED|Sampling period of the estimator in seconds]
* - ModelInfo [GROUP|REQUIRED|URDF Model specific parameters used by the estimator]
* - base_link [PARAMETER|REQUIRED|base link frame from the URDF model| Exists in "ModelInfo" GROUP]
* - base_link_imu [PARAMETER|REQUIRED|IMU frame rigidly attached to thebase link from the URDF model| Exists in "ModelInfo" GROUP]
* - left_foot_contact_frame [PARAMETER|REQUIRED|left foot contact frame from the URDF model| Exists in "ModelInfo" GROUP]
* - right_foot_contact_frame [PARAMETER|REQUIRED|right foot contact frame from the URDF model| Exists in "ModelInfo" GROUP]
* @param[in] handler configure the generic parameters for the estimator
* @param[in] kindyn shared pointer of iDynTree kindyncomputations object (model will be loaded internally)
* @note any custom initialization of parameters or the algorithm implementation is not done here,
* it must be done in customInitialization() by the child class implementing the algorithm
* @return True in case of success, false otherwise.
*/
bool initialize(std::weak_ptr<BipedalLocomotion::ParametersHandler::IParametersHandler> handler,
std::shared_ptr<iDynTree::KinDynComputations> kindyn);
/**
* Configure generic parameters, calling this overloaded method assumes model information is not going to be used.
* @param[in] handler configure the generic parameters for the estimator
* @return True in case of success, false otherwise.
*/
bool initialize(std::weak_ptr<BipedalLocomotion::ParametersHandler::IParametersHandler> handler);
/**
* Set the polled IMU measurement
* @param[in] accMeas linear accelerometer measurement expressed in the local IMU frame (m/s^2)
* @param[in] gyroMeas gyroscope measurement expressed in the local IMU frame (rad/s)
* @return True in case of success, false otherwise.
*/
bool setIMUMeasurement(const Eigen::Vector3d& accMeas,
const Eigen::Vector3d& gyroMeas);
/**
* Set feet contact states
* @param[in] lfInContact left foot contact state [0, 1]
* @param[in] rfInContact right foot contact state [0, 1]
* @return True in case of success, false otherwise.
*/
bool setContacts(const bool& lfInContact, const bool& rfInContact);
/**
* Set contact status
*
* @param[in] name contact frame name
* @param[in] contactStatus flag to check active contact
* @param[in] switchTime time of switching contact
* @param[in] timeNow current measurement update time
*/
bool setContactStatus(const std::string& name,
const bool& contactStatus,
const double& switchTime,
double timeNow = 0.);
/**
* Set kinematic measurements
* @note it is assumed that the order of the joints loaded in the model and the order of the measurements in these vectors match
* @param[in] encoders joint positions measured through encoders
* @param[in] encoderSpeeds joint velocities measured through encoders
* @return True in case of success, false otherwise.
*/
bool setKinematics(const Eigen::VectorXd& encoders,
const Eigen::VectorXd& encoderSpeeds);
/**
* Set the relative pose of a landmark relative to the base link
*
* @param[in] landmarkID unique landmark ID
* @param[in] quat relative orientation of the landmark as a quaternion
* @param[in] pos relative position of the landmark
* @param[in] timeNow relative position of the landmark
* @return true in case of success, false otherwise
*/
bool setLandmarkRelativePose(const int& landmarkID,
const Eigen::Quaterniond& quat,
const Eigen::Vector3d& pos,
const double& timeNow);
/**
* Compute one step of the estimator
* @return True in case of success, false otherwise.
*/
virtual bool advance() final;
/**
* Reset the internal state of the estimator
* @param[in] newState Internal state of the estimator
* @return True in case of success, false otherwise.
*
* @note reset and advance estimator to get updated estimator output
*/
virtual bool resetEstimator(const FloatingBaseEstimators::InternalState& newState);
/**
* Reset the base pose estimate and consequently the internal state of the estimator
* @param[in] newBaseOrientation base link orientation as a Eigen quaternion
* @param[in] newBasePosition base link position
* @return True in case of success, false otherwise.
*
* * @note reset and advance estimator to get updated estimator output
*/
virtual bool resetEstimator(const Eigen::Quaterniond& newBaseOrientation,
const Eigen::Vector3d& newBasePosition);
/**
* Get estimator outputs
* @return A struct containing he estimated internal states of the estiamtor and the associated covariance matrix
*/
virtual const FloatingBaseEstimators::Output& getOutput() const final;
/**
* Determines the validity of the object retrieved with get()
* @return True in case of success, false otherwise.
*/
virtual bool isOutputValid() const final { return (m_estimatorState == State::Running); };
/**
* Get ModelComputations object by reference
* @return ModelComputations object providing information between considered model related quantities in the estimator
* like the base link, IMU, feet contact frames.
*/
ModelComputations& modelComputations();
protected:
/**
* These custom parameter specifications should be specified by the derived class.
* - If setupOptions() is called from within customInitialization(), ensure the group "Options" exist.
* Please check the documentation of setupOptions() for relevant parameters
* - If setupSensorDevs() is called from within customInitialization(), ensure the group "SensorsStdDev" exist.
* Please check the documentation of setupSensorDevs() for relevant parameters
* - If setupInitialStates() is called from within customInitialization(), ensure the group "InitialStates" exist.
* Please check the documentation of setupInitialStates() for relevant parameters
* - If setupPriorDevs() is called from within customInitialization(), ensure the group "PriorsStdDev" exist.
* Please check the documentation of setupPriorDevs() for relevant parameters
* @param[in] handler configure the custom parameters for the estimator
* @return bool
*/
virtual bool customInitialization(std::weak_ptr<BipedalLocomotion::ParametersHandler::IParametersHandler> handler) { return true; };
/**
* Propagate the states through the prediction model, if there exists any (eg. a strap-down IMU model)
* @param[in] meas measurements passed as exogeneous inputs to the prediction model
* @param[in] dt sampling period in seconds
* @param[out] state previous state estimate
* @param[out] P previous state covariance matrix
* @return True in case of success, false otherwise.
*/
virtual bool predictState(const FloatingBaseEstimators::Measurements& meas,
const double& dt) { return true; };
/**
* Update the predicted state estimates using kinematics and contact measurements
* @param[in] meas measurements to update the predicted states
* @param[in] dt sampling period in seconds
* @return True in case of success, false otherwise.
*/
virtual bool updateKinematics(FloatingBaseEstimators::Measurements& meas,
const double& dt) { return true; };
/**
* Update the predicted state estimates using relative pose measurements of static landmarks in the environment
* @param[in] meas measurements to update the predicted states
* @param[in] dt sampling period in seconds
* @return True in case of success, false otherwise.
*/
virtual bool updateLandmarkRelativePoses(FloatingBaseEstimators::Measurements& meas,
const double& dt) { return true; };
/**
* Wrapper method for getting base state from internal IMU state
* @param[in] state internal state of the estimator
* @param[in] meas previous measurement to the estimator
* @param[in] basePose base pose as an iDynTree Transform object
* @param[in] baseTwist mixe- trivialized base velocity as an iDynTree Twist object
*/
bool updateBaseStateFromIMUState(const FloatingBaseEstimators::InternalState& state,
const FloatingBaseEstimators::Measurements& meas,
manif::SE3d& basePose,
Eigen::Ref<Eigen::Matrix<double, 6, 1>> baseTwist);
/**
* Setup estimator options. The parameters in the Options group are,
* - enable_imu_bias_estimation [PARAMETER|-|Enable/disable IMU bias estimation]
* - enable_static_imu_bias_initialization [PARAMETER|-|Enable/disable IMU bias initialization assuming static configuration]
* - nr_samples_for_imu_bias_initialization [PARAMETER|REQUIRED, if staticImuBiasInitializationEnabled is set to true|Number of samples for static bias initialization]
* - enable_ekf_update [PARAMETER|-|Enable/disable update step of EKF (not recommended to set to false)]
* - acceleration_due_to_gravity [PARAMETER|-|Acceleration due to gravity, 3d vector]
* @note this is a recipe method that can be called by the derived class from within customInitialization()
* @param[in] handler parameter handler
* @return True in case of success, false otherwise.
*/
bool setupOptions(std::weak_ptr<BipedalLocomotion::ParametersHandler::IParametersHandler> handler);
/**
* Setup measurement noise standard deviations The parameters in the SensorsStdDev group are,
* - accelerometer_measurement_noise_std_dev [PARAMETER|REQUIRED|Accelerometer measurement white noise deviation]
* - gyroscope_measurement_noise_std_dev [PARAMETER|REQUIRED|Gyroscope measurement white noise deviation]
* - accelerometer_measurement_bias_noise_std_dev [PARAMETER|REQUIRED, if imuBiasEstimationEnabled is set to true|Accelerometer measurement bias noise deviation]
* - gyroscope_measurement_bias_noise_std_dev [PARAMETER|REQUIRED, if imuBiasEstimationEnabled is set to true|Gyroscope measurement bias noise deviation]
* - contact_foot_linear_velocity_noise_std_dev [PARAMETER|REQUIRED|Linear velocity white noise deviation when foot is in contact]
* - contact_foot_angular_velocity_noise_std_dev [PARAMETER|REQUIRED|Angular velocity white noise deviation when foot is in contact]
* - swing_foot_linear_velocity_noise_std_dev [PARAMETER|REQUIRED|Linear velocity white noise deviation when foot is swinging off contact]
* - swing_foot_angular_velocity_noise_std_dev [PARAMETER|REQUIRED|Angular velocity white noise deviation when foot is swinging off contact]
* - forward_kinematic_measurement_noise_std_dev [PARAMETER|REQUIRED|White noise deviation in relative poses computed through forward kinematics]
* - encoders_measurement_noise_std_dev [PARAMETER|REQUIRED|Encoder measurement white noise deviation]
* @note this is a recipe method that can be called by the derived class from within customInitialization()
* @note ensure to call setupOptions() before calling setupSensorDevs() to handle bias related parameters to be configured properly
* @param[in] handler parameter handler
* @return True in case of success, false otherwise.
*/
bool setupSensorDevs(std::weak_ptr<BipedalLocomotion::ParametersHandler::IParametersHandler> handler);
/**
* Setup initial states. The parameters in the InitialStates group are,
* - imu_orientation_quaternion_wxyz [PARAMETER|REQUIRED|Initial IMU orientation wrt inertial frame, as a quaternion]
* - imu_position_xyz [PARAMETER|REQUIRED|Initial IMU position wrt inertial frame]
* - imu_linear_velocity_xyz [PARAMETER|REQUIRED|Initial IMU linear velocity wrt inertial frame, in a mixed-velocity representation]
* - l_contact_frame_orientation_quaternion_wxyz [PARAMETER|REQUIRED|Initial left foot contact frame orientation wrt inertial frame, as a quaternion]
* - l_contact_frame_position_xyz [PARAMETER|REQUIRED|Initial left foot contact frame position wrt inertial frame]
* - r_contact_frame_orientation_quaternion_wxyz [PARAMETER|REQUIRED|Initial right foot contact frame orientation wrt inertial frame, as a quaternion]
* - r_contact_frame_position_xyz [PARAMETER|REQUIRED|Initial right foot contact frame position wrt inertial frame]
* - accelerometer_bias [PARAMETER|REQUIRED, if imuBiasEstimationEnabled is set to true|Initial accelerometer bias expressed in IMU frame]
* - gyroscope_bias [PARAMETER|REQUIRED, if imuBiasEstimationEnabled is set to true|Initial gyroscope bias expressed in IMU frame]
* @note this is a recipe method that can be called by the derived class from within customInitialization()
* @note ensure to call setupOptions() before calling setupInitialStates() to handle bias related parameters to be configured properly
* @param[in] handler parameter handler
* @return True in case of success, false otherwise.
*/
bool setupInitialStates(std::weak_ptr<BipedalLocomotion::ParametersHandler::IParametersHandler> handler);
/**
* Setup initial state standard deviations. The parameters in the PriorsStdDev group are,
* - imu_orientation_quaternion_wxyz [PARAMETER|REQUIRED|Initial IMU orientation as wrt inertial frame]
* - imu_position [PARAMETER|REQUIRED|Initial IMU position deviation wrt inertial frame]
* - imu_linear_velocity [PARAMETER|REQUIRED|Initial IMU linear velocity deviation wrt inertial frame, in a mixed-velocity representation]
* - l_contact_frame_orientation_quaternion [PARAMETER|REQUIRED|Initial left foot contact frame orientation deviation wrt inertial frame]
* - l_contact_frame_position [PARAMETER|REQUIRED|Initial left foot contact frame position deviation wrt inertial frame]
* - r_contact_frame_orientation_quaternion [PARAMETER|REQUIRED|Initial right foot contact frame orientation deviation wrt inertial frame]
* - r_contact_frame_position [PARAMETER|REQUIRED|Initial right foot contact frame position deviation wrt inertial frame]
* - accelerometer_bias [PARAMETER|REQUIRED, if imuBiasEstimationEnabled is set to true|Initial accelerometer bias devitaion expressed in IMU frame]
* - gyroscope_bias [PARAMETER|REQUIRED, if imuBiasEstimationEnabled is set to true|Initial gyroscope bias deviation expressed in IMU frame]
* @note this is a recipe method that can be called by the derived class from within customInitialization()
* @note ensure to call setupOptions() before calling setupPriorDevs() to handle bias related parameters to be configured properly
* @param[in] handler parameter handler
* @return True in case of success, false otherwise.
*/
bool setupPriorDevs(std::weak_ptr<BipedalLocomotion::ParametersHandler::IParametersHandler> handler);
ModelComputations m_modelComp; /**< Model computations object */
FloatingBaseEstimators::Options m_options; /**< Struct holding estimator options */
FloatingBaseEstimators::Measurements m_meas, m_measPrev; /**< Struct holding the latest measurements that were set to the estimator */
FloatingBaseEstimators::InternalState m_state, m_statePrev; /**< Structs holding the curent and previous internal states of the estimation algorithm */
FloatingBaseEstimators::Output m_estimatorOut; /**< Struct holding outputs of the estimator */
FloatingBaseEstimators::StateStdDev m_priors, m_stateStdDev; /**< Struct holding standard deviations associated to prior state estimates */
FloatingBaseEstimators::SensorsStdDev m_sensorsDev; /**< Struct holding standard deviations associated to sensor measurements */
/**
* Enumerator used to determine the running state of the estimator
*/
enum class State
{
NotInitialized, /**< The estimator is not initialized yet call FloatingBaseEstimator::initialze
method to initialize it*/
Initialized, /**< The estimator is initialized and ready to be used */
Running /**< The estimator is running */
};
State m_estimatorState{State::NotInitialized}; /**< State of the estimator */
double m_dt{0.01}; /**< Fixed time step of the estimator, in seconds */
bool m_useIMUForAngVelEstimate{true}; /**< Use IMU measurements as internal state imu angular velocity by default set to true for strap down IMU based EKF implementations, if IMU measurements not used, corresponding impl can set to false */
bool m_useIMUVelForBaseVelComputation{true}; /**< Compute base velocity using inertnal state IMU velocity. by default set to true for strap down IMU based EKF implementations, if IMU measurements not used, corresponding impl can set to false */
bool m_useModelInfo{true}; /**< Flag to enable running the estimator without using the URDF model information*/
bool m_isInvEKF{false}; /**< Flag to maintain soon to be deprecated functionalities currently existing only in InvEKFBaseEstimator */
private:
/**
* Setup model related parameters
*
* @param[in] handler parameter handler
* @return True in case of success, false otherwise.
*/
bool setupModelParams(std::weak_ptr<BipedalLocomotion::ParametersHandler::IParametersHandler> handler);
/**
* Setup parameter vector
* @param[in] param parameter name
* @param[in] prefix print prefix
* @param[in] handler parameter handler
* @param[out] vec parameter vector
* @return True in case of success, false otherwise.
*/
bool setupFixedVectorParamPrivate(const std::string& param, const std::string& prefix,
std::weak_ptr<BipedalLocomotion::ParametersHandler::IParametersHandler> handler,
std::vector<double>& vec);
};
} // namespace Estimators
} // namespace BipedalLocomotion
#endif // BIPEDAL_LOCOMOTION_ESTIMATORS_FLOATING_BASE_ESTIMATOR_H