This estimator uses the kinematics, IMU mounted on the head and the contact forces information to estimate the floating base of the robot. The pose of floating base with respect to the inertial frame is computed through legged odometry and is fused along with the attitude estimates from the head IMU. The base velocity is obtained from the contact Jacobian computed using the kinematics information by enforcing the unilateral constraint of the foot.
-
Set the
YARP_ROBOT_NAME
environment variable according to the chosen Gazebo model:export YARP_ROBOT_NAME="iCubGazeboV2_5"
-
Run
yarpserver
yarpserver --write
-
Run gazebo and drag and drop iCub (e.g. icubGazeboSim or iCubGazeboV2_5):
gazebo -slibgazebo_yarp_clock.so
-
Prior to launching the
baseEstimatorV1
, launch thewholeBodyDynamics
device.YARP_CLOCK=/clock yarprobotinterface --config launch-wholebodydynamics.xml
-
Reset the offset of the FT sensors. Open a terminal and write
yarp rpc /wholeBodyDynamics/rpc >> resetOffset all
Note that this step can be avoided if a flag
startWithZeroFTSensorOffsets
is set to true in thewholeBodyDynamics
configuration file (This is applicable only in the simulation.). -
Run
yarprobotinterface
with corresponding device configuration file.
-
To run
baseEstimatorV1
,YARP_CLOCK=/clock yarprobotinterface --config launch-fbe-analogsens.xml
This launches the floating base estimation device.
-
communicate with the
base-estimator
through RPC service calls:yarp rpc /base-estimator/rpc
the following commands are allowed:
startFloatingBaseFilter
: starts the estimator, this needs to be run after the FT sensors are reset;
other optional commands include,
setContactSchmittThreshold lbreak lmake rbreak rmake
: used to set contact force thresholds for feet contact detection;setPrimaryFoot foot
: set the foot to the one that does not break the contact initially during walking,foot
can beleft
orright
;useJointVelocityLPF flag
: use a low pass filter on the joint velocities,flag
can betrue
orfalse
;setJointVelocityLPFCutoffFrequency freq
: set the cut-off frequency for the low pass filter on the joint velocities;resetLeggedOdometry
: reset the floating base pose and reset the legged odometry to the inital state;resetLeggedOdometryWithRefFrame frame x y z roll pitch yaw
: reset the legged odometry by mentioning an initial reference to the world frame with respect to the initial fixed frame;getRefFrameForWorld
: get the initial fixed frame with respect to which the world frame was set;
The configuration file for the estimator can be found app/robots/${YARP_ROBOT_NAME}/fbe-analogsens.xml
.
The attitude estimation for the IMU can be chosen to be either a QuaternionEKF or a non-linear complementary filter. The gains should be tuned accordingly. The choice of which IMU to use can also be chosen, however please read the note below.
Before run yarprobotinterface
check if dump_data
is set to true
If true
, run the Logger Module
YARP_CLOCK=/clock WalkingLoggerModule
All the data will be saved in the current folder inside a txt
named Dataset_YYYY_MM_DD_HH_MM_SS.txt
You can follow the same instructions of the simulation section without using YARP_CLOCK=/clock
. Make sure your YARP_ROBOT_NAME
is coherent with the name of the robot (e.g. iCubGenova04)
Currently the supported robots are only:
iCubGenova04
icubGazeboV2_5
Note
If you would like to use root link IMU instead of head IMU (default is Head IMU), please uncomment the following lines in launch-fbe-analogsens.xml
<device name="xsens_inertial" type="genericSensorClient">
<param name="remote">/icubSim/xsens_inertial</param>
<param name="local">/baseestimation/waist/imu:i</param>
</device>
and the following line in fbe-analogsens.xml
<elem name="root_link_imu_acc">xsens_inertial</elem>
In order to check if the baseEstimatorV1
device has been properly installed and YARP
is being able to identify the location of the device library, we can run
yarpdev --list
to check if baseEstimatorV1
plugin has been listed with the set of YARP devices with the following message
[INFO]Device "baseEstimatorV1", available on request
In case this is not seen, check again properly if the YARP_DATA_DIRS
has been properly updated.
If the following error message is seen,
[Warning]Device "baseEstimatorV1", Wrong library name
then, it can be fixed by adding the following line in your .bashrc
file
export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:${WBDEstimator_INSTALL_DIR}/lib/yarp
and run sudo ldconfig
on your terminal.