jeljaik commented 9 years ago

Paper tasks

CHECK INDIVIDUAL SECTIONS

[x] Abstract
[x] Introduction
[x] Proposed Framework
- [x] Extended Kalman Filter
- [x] Dynamic Model
- [x] Sensors and Measurement Model
- [x] Skin Calibration and Covariance Matrix
- [x] Computation of Foot Rotation Index (FRI)
- [x] Multisensory stability indices estimation
- [x] Induced probability on FRI
[x] Experiments and Results
[x] Conclusions and Future Work
[x] References
Other tasks
[x] Slightly reduce sizes of bayesian network and foot schematics figures.
[x] Update values in Table.
[x] Change labels in Figure 4 to correspond to the FT sensor local reference frame.
[x] New sensor schematic diagram
[x] Section on foot skin and identification
[x] Remove simulation results
[x] Photographs of toppling scenario! using redICub
[x] Submit Abstract to http://biomechanics.osu.edu/dynamic-walking/AbstractSubmission.html
02-03-2015 Naveen
[x] In symbolic.m remember to add the "skin normal force" version of line 45.
[x] In dynamicsFunction/rigidBodyOutput.m change line 43 to take into account skin's output.
[x] 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
[x] 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?)
[x] Transform accelerometer gyroscope measurement to foot COM ref. frame.
[x] Obtain mass and inertia matrix of the foot from wholeBodyModel. (Waiting for Silvio Reply)
[x] 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.
[x] Bayesian linear regression.
[x] Covariance matrix for skin.
[ ] :memo: Work on the Introduction of the paper.
[ ] Push the necessary code for estimation.
[ ] In computeTotalForce.m finish the forceTorques option.
[x] Compare total normal force computed with the skin against the total normal force measured with the FT sensor.
[x] Add proper force computation in realMeasurement_withSkin.m
[x] Add sensor offsets.
[x] Identify IMU channels :point_right: http://wiki.icub.org/images/8/82/XsensMtx.pdf
[x] 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?
[x] Remove inactive taxels from Xts and yts
[x] Recompute the stiffness vector based on skin normal forces only.
[x] 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).
[x] 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.
[x] 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.
[x] Change plot_weights_on_skin() such that the foot drawing does not contain those awful black lines of the mesh.
[x] Update code to compute total normal force through the skin using the latest stiffness vector.
[x] Add function axisAngle2rotationMatrix
[x] 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.
[x] Choose the two experiments and filter starting and ending points of the dataset.
General Pending Tasks
[x] Recomputation of all measurements in FOOT COM REF FRAME
[x] Extract new Tmatrix from CAD files for right foot.
[x] Integration of skin data in realMeasurent.m separating right and left foot.
[x] Skin stiffness covariance estimate through Bayesian Network.
[ ] Use stiffness matrix which can be found in https://github.com/jeljaik/extended-kalman-filter/commit/fb1e74e3f49df7f5c293249029bac1aaed4ad017 to compute (total?) external force applied on the foot sole and COP.
[x] Review orientation estimation.
[x] Incorporate IMU with connection to head. Consider mounting position on foot in order to have repeatable experiments
[x] Redo simulation to have longer durations without hitting singularities.
[x] Experiment with torque inputs which maintain an orientation trajectory within singularity limits
Data collection 18-02-2015
[ ] Prepare experimental setups.
- [x] IMU rototranslation matrix tuning.
- [x] Tipping scenario.
- [ ] ~~Orientation perturbation.~~
[x] 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)
[x] Clean up the repository.

jeljaik commented 9 years ago

Luca's paper on skin stiffness matrix. lucasPaperIROS14.pdf | uploaded via ZenHub

jeljaik commented 9 years ago

Experimental Setups

Tipping Scenario

To launch the robot without a particular part (for whatever maintenance reason), remember you can do in pc104: robotInterface --config icub_all_norightarm_headtest.xml. Those particular configuration files can be found in ~/.local/share/yarp/robots/iCubGenova01 @pc104
Sequence forward tipping scenario torso from 0 0 0 to 0 0 60 left_arm -home position- right_leg 25 0 0 0 -2 -2 left_leg 25 0 0 0 0 0
Sequence backward tipping scenario torso from 0 0 20 to 0 0 -18.1 in 1.0 sec right_leg -10 0 0 0 -2 -2 left_leg -10 0 0 0 0 0
To dump data, let us use Silvio's script https://gist.github.com/traversaro/c0ba8ed463c9ecd34725 as: python dataDumperAppGenerator.py --ports /icub/right_leg/state:o /icub/left_leg/state:o /icub/torso/state:o /icub/inertial /icub/right_leg/analog:o /icub/left_leg/analog:o /icub/left_foot/analog:o /icub/skin/right_foot --host icub15 --name "tippingExperiments
Our script was generated as: python dataDumperAppGenerator.py --ports /icub/left_leg/analog:o /icub/skin/left_foot /icub/inertial /icub/torso/state:o --host icub15 --name "forwardTippingTest01" and the corresponding xml has been pushed in https://github.com/jeljaik/extended-kalman-filter/commit/6b91c554b9c6a2b18ed7c6cc26f2f4e4553069e0
[x] REMEMBER Ask for skin to be put back on the robot feet.

jeljaik commented 9 years ago

Using The Stiffness Vector

Skin raw readings must be processed as done in load_n_filter.m and get_input_output.m, i.e.:

Assume dataSkin is a matrix of size n x 384 (from the third column onwards, if read via yarpdatadumper.
processedDataSkin = 256 - dataSkin( : , 3:end);
processedDataSkin = (processedDataSkin ./ 255)

Use new functions in localParamEKF/skinFunctions/ as, e.g. totalForceFromSkinData('backwardTipping', 4, 'left');

jeljaik commented 9 years ago

Offsets

Output of wholeBodyDynamicsTree:

icub@icub15:~$ wholeBodyDynamicsTree        
||| clearing context
||| adding context [wholeBodyDynamicsTree]
||| configuring
||| no policy found
||| default config file specified as wholeBodyDynamicsTree.ini
||| checking [/home/icub/wholeBodyDynamicsTree.ini] (pwd)
||| checking [/home/icub/.config/yarp/robots/iCubGenova01] (robot YARP_CONFIG_HOME)
...
...
wholeBodyDynamicsThread started
[INFO] wholeBodyDynamicsThread: complete calibration at system time : 24-02-2015 10:54:50
[INFO] wholeBodyDynamicsThread: new calibration for FT 0 is 84.4066 -18.5809 251.86 -0.844597 -5.08643 0.273151
[INFO] wholeBodyDynamicsThread: new calibration for FT 1 is 146.851 -13.9138 125.512 0.137384 -1.66051 0.109277
[INFO] wholeBodyDynamicsThread: new calibration for FT 2 is 23.0772 92.4128 -102.178 -7.17163 0.225915 -1.79807
[INFO] wholeBodyDynamicsThread: new calibration for FT 3 is -20.8565 -16.5023 11.3506 -0.204595 1.28343 -0.0546408
[INFO] wholeBodyDynamicsThread: new calibration for FT 4 is 5.64907 4.74285 -51.5791 -0.67231 1.58887 0.00297924
[INFO] wholeBodyDynamicsThread: new calibration for FT 5 is 0.875248 -0.913526 -9.40195 0.0152946 0.165867 -0.0175137
wholeBodyDynamicsTree: double support calibration for allrequested
wholeBodyDynamicsThread::calibrateOffsetOnDoubleSupport called with code all
wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 0 is 84.4066 -18.5809 251.86 -0.844597 -5.08643 0.273151
wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 1 is 146.851 -13.9138 125.512 0.137384 -1.66051 0.109277
wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 2 is 23.0772 92.4128 -102.178 -7.17163 0.225915 -1.79807
wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 3 is -20.8565 -16.5023 11.3506 -0.204595 1.28343 -0.0546408
wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 4 is 5.64907 4.74285 -51.5791 -0.67231 1.58887 0.00297924
wholeBodyDynamicsThread::calibrateOffset: current calibration for FT 5 is 0.875248 -0.913526 -9.40195 0.0152946 0.165867 -0.0175137
wholeBodyDynamicsThread::calibrateOffset all called successfully, starting calibration.
wholeBodyDynamicsThread: new calibration for FT 0 is 84.1018 -18.3825 250.834 -0.822744 -5.09786 0.276386
wholeBodyDynamicsThread: new calibration for FT 1 is 146.262 -13.7526 124.946 0.142152 -1.67094 0.107521
wholeBodyDynamicsThread: new calibration for FT 2 is -4.11735 111.148 -131.104 -13.423 1.36924 -1.04304
wholeBodyDynamicsThread: new calibration for FT 3 is -52.0296 -5.64247 -18.0923 -0.516052 9.91345 0.717567
wholeBodyDynamicsThread: new calibration for FT 4 is -5.65084 -10.5031 -24.6174 1.95779 2.61084 0.0036913
wholeBodyDynamicsThread: new calibration for FT 5 is -17.4992 -0.910681 18.0613 -0.824831 3.32657 -0.252851
wholeBodyDynamicsThread::waitCalibrationDone() returning: calibration finished with success

Which translates into:

% Left arm
FT0 = [84.1018 -18.3825 250.834 -0.822744 -5.09786 0.276386];
% Right arm
FT1 = [146.262 -13.7526 124.946 0.142152 -1.67094 0.107521];
% Left leg
FT2 = [-4.11735 111.148 -131.104 -13.423 1.36924 -1.04304];
% LEft Foot
FT3 = [-52.0296 -5.64247 -18.0923 -0.516052 9.91345 0.717567];
% Right Leg
FT4 = [-5.65084 -10.5031 -24.6174 1.95779 2.61084 0.0036913];
% Right foot
FT5 = [-17.4992 -0.910681 18.0613 -0.824831 3.32657 -0.252851];

jeljaik commented 9 years ago

Current Foot Sole with Skin Sensors

Taxels positions are not correct as these locations saved in Tmatrix.mat come from a calibration procedure which gives this output. footandtaxels

jeljaik commented 9 years ago

Parent/Child relationship in the URDF Format

For future references: The parent/child relationship between links and joints in URDF format is as follows:

parentchild

jeljaik commented 9 years ago

Start/End Time of Our Experiments

@naveenoid I checked the output of the IMU and according to the datasheet the output signal is composed like this: [ euler angles (deg) ] [ accelerometer (m/s^2)] [ gyroscope (m/s)] [ magnetometer ] This has been confirmed from both the datasheet and the iCub wiki http://wiki.icub.org/wiki/Inertial_Sensor http://wiki.icub.org/images/8/82/XsensMtx.pdf

Also assigned labels and legends to the corresponding plot and it's now all clear.

imu

Since I think this is our most sensitive sensor I would choose the start and end time of our experiments based on those readings. As it can be seen from the angular speed, we can consider the experiment started after roughly 7 seconds. We could choose 6 secs as the starting point and I would go till the very end, i.e. ~9 secs, because I usted to stop the experiment right after the robot tipped (about 1 sec after. Yeah, I Know, I'm damn fast! :runner:)

jeljaik commented 9 years ago

Accelerometer

NOTE: The linear 3D accelerometers measure all accelerations, including the acceleration due to gravity. This is inherent to all accelerometers. Therefore, if you wish to use the 3D linear accelerations output by the MTi / MTx to estimate the “free” acceleration (i.e. 2nd derivative of position) gravity must first be subtracted

jeljaik / extended-kalman-filter

TODO List for IROS 2015 paper #2

Paper tasks

CHECK INDIVIDUAL SECTIONS

Other tasks

02-03-2015 Naveen

02-03-2015 Jorhabib

General Pending Tasks

Data collection 18-02-2015

Experimental Setups

Tipping Scenario

Using The Stiffness Vector

Offsets

Current Foot Sole with Skin Sensors

Parent/Child relationship in the URDF Format

Start/End Time of Our Experiments

Accelerometer