@jeljaik @prashanthr05 I will try to list out some objectives for the real measurement that we need to perform on 30/06/2015.

• Fit IMU in a reasonably similar way to foot, as firm as possible, using double side tape or whatever means
• Fit skin under foot
• Place robot vertically, perform macumba, try to obtain computed offset values.
• Perform toppling on hard ground with skin underneath, with foot IMU plugged in inertial port.
• datadump all leg and torso joint angles, F/T sensors in both legs and skin.
• Repeat trials with soft mat placed below robot foot (try to first find new toplling angle and velocity ranges
• Fold mat once, repeat trial. (:point_up: Remember: Each time redo Macumba on flat surface first)
• Fold mat twice, repeat trial
• Fold mat third time, repeat trial

This line looks like it may be a typo, in the (reference?) Tools/ekf.m file you use Qd = [Q11 Q12; Q12' Q22];
You might want to check.

I also realise this project may not be in active development any more so no worries if not.

I installed the EKF library and tried to run the script testEKF_completeLegWithSkin.m under the EKF_EulerAngles folder of dynamic-tests branch. I can see the plots being plotted but it suspends showing the following error,

"Attempt to reference field of non-structure array.

Error in plotResultsOutput_withSkin (line 19)
figPreN = figPreN.Number - 1;

Error in plotAndSaveFigs (line 2)
plotResultsOutput_withSkin(dataBaseFolder);

Error in testEKF_completeLegWithSkin (line 227)
plotAndSaveFigs(dataBaseFolder,plotFigBaseFolder);"

@jeljaik @DanielePucci
Overall goal : Balancing controller working with compliant skin.

• Balancing controller with different foot orientations (balancing on a flat non-moving surface that is at an angle with ground).
• Imu attached to surface for absolute orientation of the fixed surface.
• Foot skin board (including accelro and gyro) install open ports
• Jorhs fusion on foot skin board data (compare with IMU)
• Test controller with small variations of fixed surface...also test Jors fusion
• Test the behaviour change when including skin

Reference: http://54.183.65.232/docs/indigo/api/bfl/html/smoother__test_8cpp_source.html

Paper tasks

CHECK INDIVIDUAL SECTIONS

• Abstract
• Introduction
• Proposed Framework
  • Extended Kalman Filter
  • Dynamic Model
  • Sensors and Measurement Model
  • Skin Calibration and Covariance Matrix
  • Computation of Foot Rotation Index (FRI)
  • Multisensory stability indices estimation
  • Induced probability on FRI
• Experiments and Results
• Conclusions and Future Work
• References

Other tasks

• Slightly reduce sizes of bayesian network and foot schematics figures.
• Update values in Table.
• Change labels in Figure 4 to correspond to the FT sensor local reference frame.
• New sensor schematic diagram
• Section on foot skin and identification
• Remove simulation results
• Photographs of toppling scenario! using redICub
• Submit Abstract to http://biomechanics.osu.edu/dynamic-walking/AbstractSubmission.html

02-03-2015 Naveen

• In symbolic.m remember to add the "skin normal force" version of line 45.
• In dynamicsFunction/rigidBodyOutput.m change line 43 to take into account skin's output.
• Extract rototranslation matrix from F/T sensor to CoP on sole foot CoM
  (Waiting for Silvio Reply)
  Transformation matrix from F/T sensor to ankle can be found in http://wiki.icub.org/images/9/97/NewiCubRefFrames1.png
• Discuss with @iron76 how to properly define the ankle F/T sensor reading (should it be a combination of the GRF and the dynamics of the robott?)
• Transform accelerometer gyroscope measurement to foot COM ref. frame.
• Obtain mass and inertia matrix of the foot from wholeBodyModel. (Waiting for Silvio Reply)
• Obtain real data Covariance matrix R from the first "calibration" part of the dataset.

02-03-2015 Jorhabib

• Verify correctness of the leg inertia tensor extraction.
• Bayesian linear regression.
• Covariance matrix for skin.
• 📝 Work on the Introduction of the paper.
• Push the necessary code for estimation.
• In computeTotalForce.m finish the forceTorques option.
• Compare total normal force computed with the skin against the total normal force measured with the FT sensor.
• Add proper force computation in realMeasurement_withSkin.m
• Add sensor offsets.
• Identify IMU channels 👉 http://wiki.icub.org/images/8/82/XsensMtx.pdf
• Exact relationship between ankle F/T readings and skin in the normalForce version as done in load_n_filter.m.
  answer: What has to be multiplied by the stiffness vector is just the raw third component of the force measurement as seen in line 38 of get_input_output.m. Should we use the norm instead?
• Remove inactive taxels from Xts and yts
• Recompute the stiffness vector based on skin normal forces only.
• Solve the rank defficiency induced by subsampling of the training dataset.
  answer Temporarily solved by choosing a subsample for which the submatrix is full column rank(Xtr).
• Save idxTaxelsNeverActive from load_n_filter() in test_on_large_data() to remove the same taxels in our collected skin data, as in this estimation the always inactive taxels have been removed. Solution The function for estimation must be called in the following way [best_w, best_w_lsqlin, activeTaxelsIndeces] = test_on_large_data() and activeTaxelsIndeces which corresponds to a vector of all the taxels that were activated at least once in the dataset MUST be stored an added to /localParamEKF/skinFunctions.
• Fix function plot_weights_on_skin() to plot foot with the new reduced weights vector containing taxels whose stiffnes is not estimated (zero is being assigned to those taxels). Solution: A new vector best_w_padded was created containing the weights (stiffness values) of the taxels for which the estimation was performed, and zero for all the others.
• Change plot_weights_on_skin() such that the foot drawing does not contain those awful black lines of the mesh.
• Update code to compute total normal force through the skin using the latest stiffness vector.
• Add function axisAngle2rotationMatrix
• Obtain roto-translation from upperleg (thigh sensor) to WRF and ankle sensor to WRF for the initial leg configuration we used in the experiments. Solution: in /localParamEKF/utils you will find a simulink model called mdlForRotoTransMatrices which uses iCubHeidelberg01 and retrieves the desired rototranslations saving them in .mat files.
• Choose the two experiments and filter starting and ending points of the dataset.

General Pending Tasks

• Recomputation of all measurements in FOOT COM REF FRAME
• Extract new Tmatrix from CAD files for right foot.
• Integration of skin data in realMeasurent.m separating right and left foot.
• Skin stiffness covariance estimate through Bayesian Network.
• Use stiffness matrix which can be found in fb1e74e to compute (total?) external force applied on the foot sole and COP.
• Review orientation estimation.
• Incorporate IMU with connection to head. Consider mounting position on foot in order to have repeatable experiments
• Redo simulation to have longer durations without hitting singularities.
• Experiment with torque inputs which maintain an orientation trajectory within singularity limits

Data collection 18-02-2015

• Prepare experimental setups.
  • IMU rototranslation matrix tuning.
  • Tipping scenario.
  • Orientation perturbation.
• Prepare scripts for retrieving experimental data [dataDumper, macumba, ecc]
• Record spare video to replicate experiments next week (FROM RED ROBOT SINCE BLACKIE IS ON HOLIDAYS)
• Clean up the repository.

This cmake module should be able to find XSens installed headers and libraries for the correct architecture. A first version working on Linux would be highly appreciated.

Extended Abstract TODO

• Abstract sections update
• Photographs of Balancing, sensorized skin image

Technical Work TODO

• Contact Diego Torazza, RBCS Mechanical Design head Robotics, Brain and Cognitive Sciences for new 3D printed foot.
• Gyroscope status and timetable verify with Maggiali and Nori
• Review Kalman Filters libraries and pick best
• Consider scalable design of library fitting inside yarpWholeBodyState

Theoretical Work TODO#

• Dani on stability analysis of existing controller (maybe with assumptions on error bounds of sensors?)
• Expand model to consider a compliant contact with world?

1. Introduction to Random Signals and Applied Kalman Filtering (With Matlab Exercises and Solutions).
2. Online MIT Course on Stochastic Estimation and Control http://ocw.mit.edu/courses/aeronautics-and-astronautics/16-322-stochastic-estimation-and-control-fall-2004/readings/
3. Distributed Inertial Sensing Documentation http://wiki.icub.org/wiki/Distributed_Inertial_sensing

• Identity matrix and column vector from quaternion: jeljaik/orocos-bfl-berdy#6

MT Software Suite can be obtained from https://www.xsens.com/mt-software-suite/

• If the IMU is recognized via USB, you should read this after lsusb
Bus 002 Device 006: ID 0403:d38b Future Technology Devices International, Ltd
• In the directory MT_Software_Suite_Linux_4.3 you want to run mtsdk_linux_4.3.sh which is the MT SDK.
• If you run into this error message

'realpath' could not be found. It is usually installed with the 'realpath' package
'uudecode' could not be found. It is usually installed with the 'sharutils' package

Simply do:

sudo apt-get install realpath && sudo apt-get install sharutils

• If you want to install in the default directory /usr/local just hit Enter when it asks to Enter the installation directory. Afterwards you will find in this directory all the libraries, headers and examples necessary for you to start collecting data straight from the sensor.
• The first example you wanna try is in /usr/local/xsens/examples/mtsdk/src_cpp. To compile do: make src_cpp. Enter src_cpp cd src_cpp and run ./example
• If it recognizes the device but it can't access the device do the following as found in README.MTSDK

The device is recognized, but I cannot ever access the device -

    Make sure you are in the correct group (often dialout or uucp) in order to
    access the device. You can test this with

        $ ls -l /dev/ttyUSB0
        crw-rw---- 1 root dialout 188, 0 May  6 16:21 /dev/ttyUSB0
        $ groups
        dialout audio video usb users plugdev

    If you aren't in the correct group, you can fix this in two ways.

    1. Add yourself to the correct group
        You can add yourself to it by using your distributions user management
        tool, or call

            $ sudo usermod -G dialout -a $USER

        Be sure to replace dialout with the actual group name if it is
        different. After adding yourself to the group, either relogin to your
        user, or call

            $ newgrp dialout

        to add the current terminal session to the group.

It is strongly recommended to log out and back in after these changes for them to take effect

• The output of the example module will look like this:
  W:-0.72,X: 0.22,Y:-93.12,Z: 0.00,Roll: -82.13,Pitch: 90.00,Yaw: -89.80^C0.00,Yaw: -90.000w: 90.00: -90.000

We need to repo ready for access and download (for paper readers) and new collaborators (eg. Dani, Prashanth).

• All of blog stuff move to subfolder "blog"
• Move EKF quaternion to some kind of "experimental" folder
• Rename localParamEKF to "EKF_EulerAngle"
• Copy EKF in two and freeze "EKF_IROS_2015".
• All calibration and experimental data should be in a common root folder "data"
• Keep cad files, photos etc in another common root folder "other"

The module as of today is fully written and should be "functional". This doesn't mean that the estimate is correct. This issue has been created simply to analyze in an organized way the filtering process in this module. Up next, the different stages by which it is composed

• Correct initialization
• All the filter parameters are passed and set correctly to their initial values in the corresponding configuration file.
• Initial gaussian system noise uncertainty (mean and covariance) makes sense.
• Initial gaussian measurement noise uncertainty (mean and covariance) makes sense.
• Priors make sense (mu and covariance)
• Online Estimation
• Convert read angular velocity to rad/s
• nonLinearAnalyticConditionalGaussian::ExpectedValueGet() should return the state prior prediction.
• nonLinearAnalyticConditionalGaussian::dfGet() should return the transition matrix.
• nonLinearMeasurementGaussianPdf::ExpectedValueGet()` should return the acceleration model output.
• nonLinearMeasurementGaussianPdf::dfGet() should return the jacobian evaluated at the current estimate.

test

CURRENTLY working on it. Very very soon pushing initial code and brief report describing the method.

TODO List

• Write 3D visualizer.
• Write summary on Quest Attitude Determination.
• Write model function.
• Write measurement derivative function.
• Finish coding simplified version of the quaternion-based EKF.
• Push code when first full skeleton is ready.
• Notes on theory and implementation.
• Update website with the first corrected draft version of my notes.
• Unify notation.
• Online estimation with visualization

The CSS was not being rendered online due the wrong value for url in _config.yml which should be set to url: /extended-kalman-filter prior to the commit of your website. If on the other hand it is being deployed on localhost:4000 the previous line should be commented or set to "".

Remember also that to deploy your site locally you must run:
bundle exec jekyll serve --watch --baseurl ''

@prashanthr05 For a sanity check on the results, it is recommended that the following checklist is followed:

• Check IMU rotation
• Check CoM frame position and orientation
• Check sensor homogenous transforms in CoM frame (com_X_imu, com_X_ft)
• verify raw data signs
• sanity check on raw data magnitudes
• verify calibrated and rotated data signs
• sanity check on calibrated and rotated data magnitudes
• verify process model equations
• verify measurement model equations

Tuning must be performed only after each of these are verified through checking the measured data. Both the calibration phase (robot standing) and dynamic phase (robot toppling) parts must be verified.