claudia-lat / mapest Goto Github PK

View Code? Open in Web Editor NEW

3.0 3.0 8.0 89.72 MB

Maximum A Posteriori estimation for human experiments.

MATLAB 100.00%

mapest's Introduction

Hi there, I am Claudia 👋

🔎 Researcher at the Artificial and Mechanical Intelligence (AMI)

📍 Istituto Italiano di Tecnologia, Genoa, Italy

🍅 Spare-time blogger at Pomodori al Sole

My interests are:

human modelling and biomechanics
human-robot physical collaboration algorithms development
cooking, reading and photography

How to reach me 📫 :

website: https://claudia-lat.github.io/
CV: https://claudia-lat.github.io/cv_en_CL.pdf

mapest's People

Contributors

Stargazers

Watchers

Forkers

martalorenzini traversaro marialazzaroni binbinccnu nunoguedelha fjandrad kouroshd isorrentino

mapest's Issues

Modify suitSensorPosition computation for MVN 2018

The Matlab function for computing the sensor position w.r.t. each link in MAPest has to be modified considering that the tag sensorAcceleration doesn't exists anymore in the new XML file.

Can we remove iDynTreeID.m ?

The iDynTreeID functions uses the deprecated iDynTree.DynamicsComputations, that has been deprecated in favor of iDynTree.KinDynComputations . To avoid problems in the future, we can either migrate iDynTreeID to use iDynTree.KinDynComputations, or just remove it if it is not used anymore.

Function printBerdyDynVariables_floating.m does not print properly the variables

I found this wrong behaviour when dealing with the floating-base analysis. The function printBerdyDynVariables_floating.m is a MAPest function that prints the dynamics variables in the vector d, their position and their range in the vector itself, like this (for a model with 49 links, 48 joints):

........
........
.

In the case of the floating-base formalism, the vector d seems to have (from the above-list) the following order:
\begin{equation*}
d = [ a_1, f^x_1, a_2, f^x_2, \cdots, a_{N_B} f^x_{N_B},f_1,f_2, \cdots, f_{N_B} ]
\end{equation*}
where:

is the 6D acceleration corresponding to the iDynTree.LINK_BODY_PROPER_CLASSICAL_ACCELERATION type,
is the net external wrench corresponding to the iDynTree.NET_INT_AND_EXT_WRENCHES_ON_LINK_WITHOUT_GRAV type,
is the joint wrench (force exchanged from two links through the joint) corresponding to iDynTree.JOINT_WRENCH type

As you can see from the picture, the vector contains 876 variables
( 49x6 + 49x6 + 48x6)
but if you compute the number of variables from the berdy model (via berdy.getNrOfDynamicVariables()) you obtain 924 that is exactly the dimension if you add the joint acceleration $\ddot q$ mapped from iDynTree.DOF_ACCELERATION type. But this function is not able to print it.

Should an OSIM model be fixed base or floating base?

Currently the model we generated is fixed base, i.e. we have the following joint:

<WeldJoint name="jGround_LeftFoot">
    <!--Name of the parent body to which this joint connects its owner body.-->
    <parent_body>ground</parent_body>
    <!--Location of the joint in the parent body specified in the parent reference frame. Default is (0,0,0).-->
    <location_in_parent>0 0 0</location_in_parent>
    <!--Orientation of the joint in the parent body specified in the parent reference frame. Euler XYZ body-fixed rotation angles are used to express the orientation. Default is (0,0,0).-->
    <orientation_in_parent>0 0 0</orientation_in_parent>
    <!--Location of the joint in the child body specified in the child reference frame. For SIMM models, this vector is always the zero vector (i.e., the body reference frame coincides with the joint). -->
    <location>0 0 -0.0493000000</location>
    <!--Orientation of the joint in the owing body specified in the owning body reference frame.  Euler XYZ body-fixed rotation angles are used to express the orientation. -->
    <orientation>0 0 0</orientation>
    <!--Set holding the generalized coordinates (q's) that parmeterize this joint.-->
    <CoordinateSet>
        <objects/>
        <groups />
    </CoordinateSet>
    <!--Whether the joint transform defines parent->child or child->parent.-->
    <reverse>false</reverse>
    <!--Defines how the child body moves with respect to the parent as a function of the generalized coordinates.-->
    <SpatialTransform>
    </SpatialTransform>
</WeldJoint>

If we use a 6dof joint, i.e. the following example, the model becomes free-floating.

<Joint>
    <CustomJoint name="jGround_leftFoot">
        <parent_body> ground </parent_body>
        <location_in_parent>       0.00000000       0.00000000       0.00000000 </location_in_parent>
        <orientation_in_parent>       0.00000000       0.00000000       0.00000000 </orientation_in_parent>
        <location>       0 0 -0.0493000000 </location>
        <orientation>       0.00000000       0.00000000       0.00000000 </orientation>
        <!--Generalized coordinates parameterizing this joint.-->
        <CoordinateSet name="">
            <objects>
                <Coordinate name="jLeftFoot_tilt">
                    <!--Cooridnate can describe rotational, translational, or coupled values.
                    Defaults to rotational.-->
                    <motion_type> rotational </motion_type>
                    <default_value>       0.00000000 </default_value>
                    <default_speed_value>       0.00000000 </default_speed_value>
                    <initial_value>       0.00000000 </initial_value>
                    <range>      -1.57079633       1.57079633 </range>
                    <clamped> true </clamped>
                    <locked> false </locked>
                    <prescribed_function/>
                </Coordinate>
                <Coordinate name="jLeftFoot_list">
                    <!--Cooridnate can describe rotational, translational, or coupled values.
                    Defaults to rotational.-->
                    <motion_type> rotational </motion_type>
                    <default_value>       0.00000000 </default_value>
                    <default_speed_value>       0.00000000 </default_speed_value>
                    <initial_value>       0.00000000 </initial_value>
                    <range>      -1.57079633       1.57079633 </range>
                    <clamped> true </clamped>
                    <locked> false </locked>
                    <prescribed_function/>
                </Coordinate>
                <Coordinate name="jLeftFoot_rotation">
                    <!--Cooridnate can describe rotational, translational, or coupled values.
                    Defaults to rotational.-->
                    <motion_type> rotational </motion_type>
                    <default_value>       0.00000000 </default_value>
                    <default_speed_value>       0.00000000 </default_speed_value>
                    <initial_value>       0.00000000 </initial_value>
                    <range>      -1.57079633       1.57079633 </range>
                    <clamped> true </clamped>
                    <locked> false </locked>
                    <prescribed_function/>
                </Coordinate>
                <Coordinate name="jLeftFoot_tx">
                    <!--Cooridnate can describe rotational, translational, or coupled values.
                    Defaults to rotational.-->
                    <motion_type> translational </motion_type>
                    <default_value>       0.00000000 </default_value>
                    <default_speed_value>       0.00000000 </default_speed_value>
                    <initial_value>       0.00000000 </initial_value>
                    <range>      -5.00000000       5.00000000 </range>
                    <clamped> true </clamped>
                    <locked> false </locked>
                    <prescribed_function/>
                </Coordinate>
                <Coordinate name="jLeftFoot_ty">
                    <!--Cooridnate can describe rotational, translational, or coupled values.
                    Defaults to rotational.-->
                    <motion_type> translational </motion_type>
                    <default_value>       0.00000000 </default_value>
                    <default_speed_value>       0.00000000 </default_speed_value>
                    <initial_value>       0.00000000 </initial_value>
                    <range>      -1.00000000       2.00000000 </range>
                    <clamped> true </clamped>
                    <locked> false </locked>
                    <prescribed_function/>
                </Coordinate>
                <Coordinate name="jLeftFoot_tz">
                    <!--Cooridnate can describe rotational, translational, or coupled values.
                    Defaults to rotational.-->
                    <motion_type> translational </motion_type>
                    <default_value>       0.00000000 </default_value>
                    <default_speed_value>       0.00000000 </default_speed_value>
                    <initial_value>       0.00000000 </initial_value>
                    <range>      -3.00000000       3.00000000 </range>
                    <clamped> true </clamped>
                    <locked> false </locked>
                    <prescribed_function/>
                </Coordinate>
            </objects>
            <groups/>
        </CoordinateSet>
        <reverse> false </reverse>
        <SpatialTransform name="">
            <!--3 Axes for rotations are listed first.-->
            <TransformAxis name="rotation1">
                <function>
                    <LinearFunction name="">
                        <coefficients>       1.00000000       0.00000000 </coefficients>
                    </LinearFunction>
                </function>
                <coordinates> jLeftFoot_tilt </coordinates>
                <axis>       0.00000000       0.00000000       1.00000000 </axis>
            </TransformAxis>
            <TransformAxis name="rotation2">
                <function>
                    <LinearFunction name="">
                        <coefficients>       1.00000000       0.00000000 </coefficients>
                    </LinearFunction>
                </function>
                <coordinates>jLeftFoot_list </coordinates>
                <axis>       1.00000000       0.00000000       0.00000000 </axis>
            </TransformAxis>
            <TransformAxis name="rotation3">
                <function>
                    <LinearFunction name="">
                        <coefficients>       1.00000000       0.00000000 </coefficients>
                    </LinearFunction>
                </function>
                <coordinates> jLeftFoot_rotation </coordinates>
                <axis>       0.00000000       1.00000000       0.00000000 </axis>
            </TransformAxis>
            <!--3 Axes for translations are listed next.-->
            <TransformAxis name="translation1">
                <function>
                    <LinearFunction name="">
                        <coefficients>       1.00000000       0.00000000 </coefficients>
                    </LinearFunction>
                </function>
                <coordinates> jLeftFoot_tx </coordinates>
                <axis>       1.00000000       0.00000000       0.00000000 </axis>
            </TransformAxis>
            <TransformAxis name="translation2">
                <function>
                    <LinearFunction name="">
                        <coefficients>       1.00000000       0.00000000 </coefficients>
                    </LinearFunction>
                </function>
                <coordinates> jLeftFoot_ty </coordinates>
                <axis>       0.00000000       1.00000000       0.00000000 </axis>
            </TransformAxis>
            <TransformAxis name="translation3">
                <function>
                    <LinearFunction name="">
                        <coefficients>       1.00000000       0.00000000 </coefficients>
                    </LinearFunction>
                </function>
                <coordinates>jLeftFoot_tz </coordinates>
                <axis>       0.00000000       0.00000000       1.00000000 </axis>
            </TransformAxis>
        </SpatialTransform>
    </CustomJoint>
</Joint>

The question is: should we use the fixed base or the floating one?
Regarding the inverse kinematics it seems that the best results are obtained by using the free-floating model, probably because we don't have to match beforehand the experimental and model coordinate systems. Does this have some drawbacks in other uses of the models?

cc @traversaro @MartaLorenzini

How should we define the jGroundBase joint location?

I found a typo in the jGroundBase definition here.
There is a hard-coded value in the location of the the joint in the Osim template. In the template, this location is defined as

!--Location of the joint in the child body specified in the child reference frame. For SIMM models, this vector is always the zero vector (i.e., the body reference frame coincides with the joint).

so it is the location of the jGroundJoint w.r.t. the base (i.e., the Pelvis in the model). It is clear that this value is some very specific test-related quantity and for sure it has to be fixed in the template, but the question is: which value has to be considered here? Should we put [0 0 0] or something else (e.g., the height of the subject pelvis as suggested by @lucaTagliapietra )?

Marker position on the OSIM model

Comparing an old OSIM model of @MartaLorenzini with the new one generated with the code here I noticed an offset on the markers position.

Add link angular acceleration into the measurement vector

The MAP algorithm

takes as input a 3D linear acceleration for each link where an IMU is located,
outputs a 6D acceleration (linear + angular) for all the links of the model.

Let's make a consideration on the linear acceleration:
This variable is measured by the IMU (i.e., is in the vector $y$) and estimated via MAP. The reliability of the measurement is weighted by the sensor covariance $\Sigma_{y,linAcc}$ (usually low). The estimation is weighted by the prior covariance $\Sigma_{d}$ (usually very high).
It goes without saying that the reliability given by $\Sigma_{y,linAcc}$ is predominant w.r.t. $\Sigma_{d}$ and it implicitly has a logic consistency.

Let's make a consideration on the angular acceleration:
This variable is NOT measured by the IMU (i.e., is NOT in the vector $y$) but estimated via MAP. Thus, there's no $\Sigma_{y,angAcc}$ but the only valuable covariance is $\Sigma_{d}$ (usually very high).

The aim of the issue is to embed the angular acceleration as a measurement into the vector $y$.
It is already foreseen by the C++ code:

It needs only to be integrated into the Matlab code.

Read robot positions

To correctly map robot external wrenches to human external wrenches, we need to pass the robot joint positions for each instant to the processRobotWrenches function. The joint positions can be extracted from the log of the stateExt:o ports.
To see how to extract the joint positions from the logs, you can copy some logic from the readStateExt in @fjandrad's repository :

https://github.com/robotology-playground/insitu-ft-analysis/blob/87f7f2315bcc2fedbe54f38e21e781ac39ffd914/utils/readStateExt.m

And by looking how this function is used:

https://github.com/robotology-playground/insitu-ft-analysis/search?utf8=%E2%9C%93&q=readStateExt

Fix errors when using subject05 urdfs

On branch: floatingWithIWear

When using the 66dof model I get:

When using the 48dof model I get:

I obtained the urdfs by coping the files from here

Anthropometric mass doesn't match with the URDF/OSIM one

There is a mismatch between the mass value computed in findAnthropometricParams (via anthropometric tables) and the real mass M of the subject passed to the same function.
Example: if I pass to the function a mass M = 61 kg, the sum of output boxes mass is 57.398 kg.
Since the wrong mass is copied in URDF and OSIM templates clearly they are not reliable.

Allowed range of motion for the 48-DoF human model

Here it is the list of joints with the relative DoFs, allowed movements and ranges of motions.

Thanks to @MariaLazzaroni @MartaLorenzini

Wrong estimation on link acceleration

There is a mismatch between the measured acceleration by Xsens IMUs and the estimated accelerations in the vector mu_dgiveny. The estimate of the acceleration gives a strange value of its norm as in picture:

This is the estimate acceleration (from mu_dgiveny) in the link RightFoot considering an experiment of bowing task with feet fixed on the force plates. The second picture is the norm of the acceleration and - at the initial frames where the subject is standing up without bending his body - its value is around 6.68 !
cc @traversaro @francesco-romano

How to improve the computational cost of MAP estimation

I started working on how to improve the computational cost in MAP estimation with the commit d560b77. Since most of the code time is spent on the inversion of the Sigma_dgiveny matrix, I ran main.m by considering 50 samples and by writing different code in MAPcomputation.m