Skip to content

Latest commit

 

History

History
116 lines (97 loc) · 5.2 KB

README.md

File metadata and controls

116 lines (97 loc) · 5.2 KB
Raw

Overview

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.

💻 How to run the simulation

  1. Set the YARP_ROBOT_NAME environment variable according to the chosen Gazebo model:

    export YARP_ROBOT_NAME="iCubGazeboV2_5"

  2. Run yarpserver

    yarpserver --write

  3. Run gazebo and drag and drop iCub (e.g. icubGazeboSim or iCubGazeboV2_5):

    gazebo -slibgazebo_yarp_clock.so

  4. Prior to launching the baseEstimatorV1, launch the wholeBodyDynamics device.

     YARP_CLOCK=/clock yarprobotinterface --config launch-wholebodydynamics.xml

  5. 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 the wholeBodyDynamics configuration file (This is applicable only in the simulation.).

  6. 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.

  1. 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 be left or right;
    • useJointVelocityLPF flag: use a low pass filter on the joint velocities, flag can be true or false;
    • 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;

Configuration

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.

How to dump data

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

🏃 How to test on iCub

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)

⚠️ Warning

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>

Troubleshooting

yarprobotinterface closes with the message, "baseEstimatorV1 device not found"

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.