robotology-legacy / mex-wholebodymodel Goto Github PK

When switching from a world reference link (like "l_sole") for example to world reference frame (computed from get state), the quaternion returned after that point on is not a valid quaternion.

A lot of users have been confused by the semantics of the setWorldLink function #26 and now it is possible to use all the functions by directly passing the complete state #27 .

If no code it is supposed to use this function, perhaps it is a better idea to just remove it from the interface, to avoid confusion/misuse by the users?

After that, for symmetry with all existing calls, it could make sense to have an updateState in which you pass all the state, with also the base position (basically a combination of setWorldFrame + updateState ).

In this way we would be more consistent with all the existing interfaces.

The floating-base, mechanical system

M(q) dot(v) + C(q,v) v + g(q) = S tau

must comply with:

i) The mass matrix M is symmetric, positive definite;

ii) dot(M) - 2C must be skew-symmetric.

The point i) has been debugged sufficiently in my opinion. The main problem for debugging ii) is that you would in general require the time derivative of the mass matrix, which is not trivial in general.

This time differentiation, however, may be avoided. In particular, ii) implies that the kinetic energy of the system is constant when

a) tau = 0

b) g(q) = 0

Then, I would launch simulations with random initial conditions (q,v)(0) and plot

Ek = v^t M(q) v

If Ek is not constant under a) and b), then there is a problem in the model itself.

See https://github.com/robotology/mex-wholebodymodel/pull/57/files#r44255064 .

TL;DR : the rotation matrix in input is not properly converted from column major storage order (Matlab) to row major storage order (WBI).

Fix:

• propagate the fix in https://github.com/robotology/mex-wholebodymodel/blob/efc77a2feb1b8ec13f9693770cf3c1ce07cc2f1a/mex-wholebodymodel/library/src/modelsetworldframe.cpp#L88 to all the functions handling rotations. To avoid repeating mistakes like that, consider to implement that conversion in a function that is used by all the components (see #42 ).
• Change all the code that is assuming the wrong behavior.

I think it make sense that we do the fix directly in the https://github.com/robotology/mex-wholebodymodel/tree/controllers_update branch.

@naveenoid @gabrielenava Do yo think you can solve this in Genoa before Wednesday ?
If you have other priorities just let me know, so I can plan to work on this myself.

I was wondering in the simulator in the matlab-src/mex-wholebodymodel_balancing, which joint ordering is used?

To be more clear, suppose you initialize wholeBodyModel as:
wholeBodyModel('model-initialise', 'icub') but you're not connected to the robot, then you wanna switch initialization (without restarting Matlab) to wholeBodyModel('model-initialize', 'icubGazeboSim') what you get is the default error message for unexisting component.

And to avoid problems, it should be added to the .gitignore file.

Some logic that is currently copy-pasted across a lot of functions/scripts, and need to be consolidated in functions:

Constrained Dynamics with Torque as a input

• Tick when fixed

This function is already available in the forwardDynamics function in https://github.com/robotology/mex-wholebodymodel/blob/master/matlab-src/forwardDynamics.m , but for some reason is not used in the balancing/controller simulation. Instead a forwardDynamics function with the same signature but with different semantics ( O_o ) is implemented in https://github.com/robotology/mex-wholebodymodel/blob/master/matlab-src/mex-wholebodymodel_balancing/forwardDynamics.m . We definitively need to consolidate this.

Quaternion to Rotation conversion

• Tick when fixed

Duplicated in at least 6 files :
https://github.com/robotology/mex-wholebodymodel/search?utf8=%E2%9C%93&q=2*qt_b_mod_s*skew%28qt_b_mod_r%29&type=Code

Simulation to mex-wholebodymodel (and reverse) conversion

• Tick when fixed

The position of the robot is a (7+n_J) vector for the simulation code (
containing the quaternion of the base orientation) and a triple (RotationMatrix, BaseOriginPosition,
JointPos ) (where RotationMatrix is a 3x3 matrix, BaseOriginPosition a 3 vector and JointPos a n_J vector). We hardcode how this two representation are mapped, we need to write two functions to encapsulate this mapping.

Duplicated in at least 8 files :
https://github.com/robotology/mex-wholebodymodel/search?utf8=%E2%9C%93&q=x_b++%3D+chi%281%3A3%2C%3A%29%3B++qt_b+%3D+chi%284%3A7%2C%3A%29%3B

Apparently the following lines:
%Setting State to random values
q = rand(32,1);dq = rand(32,1);dxb = rand(6,1);
needs to be changed into:
%Setting State to random values
q = rand(25,1);dq = rand(25,1);dxb = rand(6,1);
to complete the test successfully.

Currently the reference frame is that of a chosen link which causes inconsistency between reference frames for different scenarios.

I think that it makes sense to remove all the .mat files from the repo.

.mat files are binaries for which it is quite difficult to track the modifications, for example while checking the repo history.

I think that all the .mat committed files can be replaced by .m files produced by Simulink.saveVars function. We will lose some precision in the stored results, but the overall benefit seems clear to me.

The matlab controllers need to be parameterised with structs and cell arrays with real-time components to simplify testing.

Matlab serialises matrices in Column Major ordering while wbi uses the Row major ordering.

Functions must take this issue in consideration (currently I think only the Jacobian computation is affected).

The functions forwardDynTest.m and run_forwardDyn.m seem use addpath. Any addpath call should be moved in a configuration script (something like init_wbim.m).

When using this interface, all vectors are assumed to be column vectors. So, if the user mistakenly passes a row vector, MATLAB complains with

Error using wholeBodyModel
Malformed state argument dimensions in ModelGeneralisedForces call

Probably, it would be better to add further information, so that to help the user recognize the error.

When launching timeTest.m, results seems to be slower that what they were in the past. At present results on macOs everything compiled using make (default compile options) are:

Starting optimised mode trial 
-------------------------- 
mexWholeBodyModel started with robot : icub, Num of Joints : 25 
Robot name icub and World Reference Frame set about l_sole 
Initialisation time : 9.065052e-02 secs 
 Starting Trial...
 Optimised-Mode Trial Total Time : 11.998686 secs
Optimised-Mode Trial Average Time : 1.199869e-03 secs 

When compiling with Xcode, again with default options:

Starting optimised mode trial 
-------------------------- 
mexWholeBodyModel started with robot : icub, Num of Joints : 25 
Robot name icub and World Reference Frame set about l_sole 
Initialisation time : 8.941473e-02 secs 
 Starting Trial...
 Optimised-Mode Trial Total Time : 2.509663 secs
Optimised-Mode Trial Average Time : 2.509663e-04 secs 

@DanielePucci I searched it (quickly) in
https://www-sop.inria.fr/act_recherche/formulaire/uploads/phd-425.pdf but I could not find it.

Hi all,

in both testing scripts "forwardDynamicsConsistencyTest.m" and "integrateForwardDynamics.m" there is an initial gravity vector created for the control torques with,

gInit = wbm_generalisedBiasForces(R_b,p_b,params.qjInit,zeros(25,1),zeros(6,1));

but will never used. For the time-dependent parameter tau it will be used the zeros-function instead of gInit. Is zeros in this case just an initial dummy?
I've found this also in the forwardDynamics function at line 54. Internally g will be calculated but afterwards it will never used or returned.
Can somebody explain me what was the intention, the thought, to calculate there at these places "g" and "gInit"?
Thanks.

If in my code I decide at some point to change the world reference frame from the default left leg to the right leg, is it currently possible?

The instance of the ComponentManager is allocated in https://github.com/robotology/mex-wholebodymodel/blob/77139ce83e22b766085d83542c7a1caedde74cdf/src/componentmanager.cpp#L40 , but is never properly deleted.

The visualiser, especially the GUI components of run_forwardDyn and the graphics drawing components of visualize_forwardDyn need cleanup and commenting. Its currently filled with too much kludge.

Also possible bug with the new WBI ID but I am currently investigating this. Without cleanup its basically impossible for me to debug.

When computing the floating base mass matrix, the mex-wholebodymodel currently takes the joint-angles as input (https://github.com/robotology-playground/mex-wholebodymodel/blame/master/matlab-src/timeTest.m#L26). For a floating base system, the floating base mass matrix should be a function of the joint-angles and the floating base configuration. In the code the latter (i.e. the floating base configuration) is computed internally by assuming that the world to root rigid motion is known (https://github.com/robotology-playground/mex-wholebodymodel/blame/master/src/modelmassmatrix.cpp#L129-L132). In particular, it is assumed that the left foot reference corresponds to the world frame (https://github.com/robotology-playground/mex-wholebodymodel/blob/master/src/modelcomponent.cpp#L35-L45). This is an assumption that should be removed, especially since we already have other pieces of code that should not make this assumption (see for example the integration of forward dynamics by @barrelback).

implement a "fixed-link" free API for setting WorldToBase RotoTranslation mirroring WBI.

Matlab functions should be copied into the install folder.

(Let's say that in general everything that should be used should stay in the install folder and not in the build)

Hi all,

I want to announce that the new WBM-Class branch is ready for merging into the master. This new wrapper-structure can be considered as the next generation for the mex-WBM.

What is new there:

• OO approach.
• Namespaces instead of paths. This prevents also errors when other methods have by accident the same name.
• Rewritten and optimized the utilities methods
• The initialization of the model is now clear defined and easier to use than before
• Error-checks (dimensions, size, etc.) at the most important positions in the class
• Speed optimized for run-time (thanks to the book: Accelerating MATLAB Performance: 1001 tips to speed up MATLAB programs [I love this book! ] )
• Removed all Matlab-warnings while plotting the results (since R2014b, Matlab has changed a lot in the background for the graphics-GUI, i.e. using OpenGL, and replaced several functions and variables).
• Introduced a fastForwardDynamics function which is decoupled from the class to get more speed for time-critical computations.

What is still missing:

• visualizeForwadDynamics function.
• Full documentation in M2HTML.

(Both will be done after Christmas.)

To check:

• Since "chi" can getting very large in the end, use a GPU-matrix/array for chi instead of a normal. This can maybe increase the performance for further computations afterwards.

One open bug:

There is still one bug during the initialization process, which I'm not able to remove in Linux. This error happens also in the Matlab-files of Naveen. Im using Linux Mint 17.3 Rosa and I tested it under Matlab R2014b and R2015b. I get the same error in both versions. I don't know if the error is also under MacOS and MS-Windows. We have to check it. The error happens when wbm_modelInitialise(); is called in the optimized mode. If no name is set then the function should normally read the default robot name from environment variable YARP_ROBOT_NAME. But it doesn't do it under Linux and I get the following error message:

...
[ERROR]yarpWholeBodyModel: robot option not found 
WBI unable to initialise (usually means unable to connect to chosen robot)
...

I have also checked my configuration and the environment variables under ~/.profile. I have everything set what was given in the install instruction.
Do I still miss something?

Here is the excerpt of my configuration in .profile:

# Environment variables for the CoDyCo-Superbuild project:
export CODYCO_SUPERBUILD_ROOT=/home/ganymed/Library/codyco-superbuild
export PATH=$PATH:$CODYCO_SUPERBUILD_ROOT/build/install/bin
export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:$CODYCO_SUPERBUILD_ROOT/build/install/lib
export YARP_DATA_DIRS=$YARP_DATA_DIRS:$CODYCO_SUPERBUILD_ROOT/build/install/share/codyco
# environmental variable for the WBI-Toolbox:
export YARP_ROBOT_NAME="icubGazeboSim"

In my repository I have made a temporary file under matlab-src/tests, called timeTest_2.m, to reproduce the error without changing other files. There I have deactivated the normal method wbm_modelInitialise('icubGazeboSim');
and replaced it with wbm_modelInitialise();. Please feel free to test it also under your OS, to see if you are getting the same error as me.
Thanks.

P.S. By the way, if we don't see us, I wish you a merry Christmas and a Happy New Year!

I was trying to use the timeTest.m but it seems that it has been no longer updated. The error I am getting is:

Error in number of arguments, check docs

Error in timeTest (line 26)
    M = wholeBodyModel('mass-matrix',q);

I think it would be useful include it among tests since it always good to have an idea of the time required to make basic computations. As for now, I would not require stringent limit on computations.

FullState versions are needed of all major function calls.. i.e. no more just passing qj but q to all the relelvant functions

It would be useful if help functionality is added to the mex-wholebody functions

Hi,
I tried to run the simulator from the matlab-src folder on my desktop pc running MATLAB 2013b, by running the run_forwardDyn script, but I get an error on the "Sf" function that computes the skew symmetric matrix:

Cannot find an exact (case-sensitive) match for 'Sf'
The closest match is: sf in /usr/local/MATLAB/R2013b/toolbox/stateflow/stateflow/sf.mexa64

Error in controller_BalancingTorque (line 37)
A  = [ eye(3),   zeros(3),eye(3), zeros(3);
      -Sf(xcom),  eye(3), Sf(Pr), eye(3) ];

This error doesn't occur in my laptop running MATLAB 2013a. Is this a default function in Matlab or is it a custom function?
I thought it might be a problem of path not well set, but I checked and it seems that on my desktop and laptop there are the same path settings.

As discussed with @traversaro having a function like wbm_setInitialFloatingBaseState accepting qj and a reference link might eliminate #43 and solve issues brought on by #47

I will go for a full matlab implementation once other things in the PR are fixed.

In the repo I found a lot of istances of wbm_modelInitialise('icubGazeboSim') , and I assume that it was a way to instantiate the mex-wholebodymodel with a robot with a given YARP_ROBOT_NAME. Unfortunatly I notice while installing the software for the TU/e master student that the script was not working at all if the YARP_ROBOT_NAME environmental variable was not set. I checked the code and apparently this happens because the argument of wbm_modelInitialise is passed as the robot parameter to the yarpWholeBodyModel constructor.

Unfortunately the robot parameter is the yarp prefix of ports used to access the robot controlboard/sensors, and it does not have nothing to do with the YARP_ROBOT_NAME enviromental variable!

I then guess that all people working with the simulation script (@Ganimed , @gabrielenava , @naveenoid ) has also the YARP_ROBOT_NAME environmental variable set, and for this reason that did not see this bug.

The fix is not easy, because there is not "local" way to set the YARP_ROBOT_NAME in the code. A possibility is to set the environmental variable from C++ or Matlab, but it is not a great idea to have code that change the environment. A proper solution involve adding a way for setting the YARP_ROBOT_NAME through the code, but without modify the environment.. I will open an issue in YARP to discuss this.

Another convenient feature to have (that is related to this) would be to directly specify the urdf file to load and the list of joints in matlab. The mex-wholebodymodel can then properly configure the yarpWholeBodyModel, disregarding at all the installed configuration files. I will open a separate issue for that.

Hi everyone, one of the source of confusion related to this repository is that it contains mex/functions/scripts fulfilling two different purposes:

• C++ code and matlab code in matlab-src/whole_body_model_functions : Provide a matlab interface to the yarpWholeBodyModel C++ class (original purpose of the repo, mex-wholebodymodel properly said)
• All the other matlab code: implement simulation and control code in matlab, using the above mentioned interface

To clarify this I propose to move all the code in two directories :

• mex-wholebodymodel (like the repo itself) for all the interface code.
• simulation for all the simulation & control related code.

With respect to #20 , I think it then make sense to properly install all the interface-related mex / code .
It could make sense instead to avoid installation of simulation code, so that it can be directly edited and used from the repo.

It could make also sense to move all the functions in proper MATLAB packages, but I guess it would be a big step and I would then prefer to defer it.

It would be nice to be able to initialize the mex-wholebodymodel by:

• passing directly a path to an urdf file to load the interface (in this case, all the joints of the model would be automatically added).
• passing directly a path to an urdf file and a list of joints to use
  This would drastically simplify its use for new user/new robots.

Currently for adding a new robot to the mex-wholebodymodel you have to properly create a set of configuration files and copy/install it in a way that can be found by the Yarp ResourceFinder, an impossible task for a newbie.

Ehen launching integrateForwardDynamics, an error is produced.

Undefined function 'wbm_initialise' for input arguments of type 'char'.

For @Eric092 consistency checks.

In the balancing controller, the orientation of the feet are obtained from the interface as quaternions (with real-imaginary serialization) [1] but they are used as axis-angle [2] [3].

At the beginning I was expecting at least the serialization to be consistent (the real part of the quaternion is the cosine of the rotation angle divided by two, while the imaginary is a 3d vector obtained multiplying the axis by the sine of the rotation angle diveded by two) but apparently (if I am not wrong) this is not the case.
I guess then that things works because those rotation are really close to the identity most of the time (they are just used to be sure that there is no integration drift) and hence for a 0 angle rotation the axis can be arbitrary, and you still get more and less the identity.
Nevertheless this must be corrected, preferably by consolidating the function for the conversion between the different representations of SO(3) (see #42) .

[1] : https://github.com/robotology/mex-wholebodymodel/blob/00e222b6ef681095fc7c29c94c6e493ddd24bc5b/matlab-src/mex-wholebodymodel_balancing/forwardDynamics.m#L114
[2] : https://github.com/robotology/mex-wholebodymodel/blob/00e222b6ef681095fc7c29c94c6e493ddd24bc5b/matlab-src/mex-wholebodymodel_balancing/controllerFCN.m#L36
[3] : b9d71b9#commitcomment-13939900

I need to compute numerically the partial derivatives both the mass matrix and the jacobian with respect of a given steady state position.
Seems that using wbm functions is possible to compute a variation in mass matrix and jacobian with respect of each joints variable but not with respect of the position and rotation of the floating base.

This is necessary because a random user could just add to the path all the directories in mex-wholebodymodel recursivly, and thus there would be in ambiguity on which function/script it is called, that could cause hideous bugs (ask to @claudia-lat).

I repeat: NEVER EVER EVER [1] use the same name for two different matlab files, even if they are in two different folders.

Examples of duplicates files (tick when solved):

• forwardDynamics.m (this is one is tragic because the different versions have totally different semantic meaning)
• integrateForwardDynamics.m
• forwardDynamics_zeroExternalForces.m

[1] : https://youtu.be/o7MF-FHra4o?t=2m21s

Hi guys,

I've seen in the master branch that there are still some files which I know that they are deprecated or experimental. So they should be deleted or moved to another place.

• Under matlab/utilities:

  • stldemo.m
  • stlread.m
  • quat2rot.m

  Maybe they should be moved into a folder outside of the release directory "mex-wholebodymodel". For example in "matlab-src/experimental".

• And under matlab/wrappers, there is still the file wbm_setWorldLink.m which will be never used again.

As discussed with @traversaro, this repository should be moved to robotology

cc-issue: robotology/robotology.github.io#2

If params.leftArmInit is set (for example) to :

params.leftArmInit  = [ -15   15  0.0  15.9   0.0]';          

Then in the visualizer the right arm is modified instead.

In particular, the initial configuration of the right arm appears to me modified in all the visualization, except for the one on the top right (that instead seems to be correct).

I suggest that for supporting Matlab 2015a we move to the official FindMatlab.cmake provided in the new CMake 3.3 . Given that on Linux users will have to stick with the CMake version provided by their distributions, we can import it locally as done for WBI-Toolbox in robotology-playground/WBI-Toolbox#84 and for iDynTree in robotology/idyntree@692f4a5 .

cc @naveenoid
Originally reported by @prashanthr05

I wanted to consolidate the tests such that they could be executed in a systematic way, but I tried
to run the forwardDynamicsConsistencyTest and I get this output:

>> forwardDynamicsConsistencyTest
Robot name icubGazeboSim and World Reference Frame set about l_sole 
Random Initial configuration
  Columns 1 through 11

    1.0179    0.3310   -0.2220   -0.5140    0.4804    1.0425    0.1518   -0.3893   -1.5775    0.2726    1.3232

  Columns 12 through 22

    1.3148   -0.3192    1.5566   -0.2707   -0.1986   -1.2349    0.5591    0.3063   -0.1948    0.6448   -0.1784

  Columns 23 through 25

   -0.5732    0.4852    0.2465

Random Initial velocity
  Columns 1 through 11

    2.2397   -1.7970   -1.5510    3.3739    3.8100    0.0164   -4.5974    1.5961   -0.8527    2.7619   -2.5127

  Columns 12 through 22

    2.4490   -0.0596   -1.9908   -3.9090   -4.5929   -0.4722    3.6138    3.5071    2.4249   -3.4072    2.4572

  Columns 23 through 25

   -3.1428    2.5648   -4.2926

starting integration
Warning: Failure at t=6.837546e-01.  Unable to meet integration tolerances without reducing the step size below the
smallest value allowed (1.776357e-15) at time t. 
> In ode15s at 730
  In forwardDynamicsConsistencyTest at 72 

In many functions there is the following installation specific line:

addpath('./../build');

This line should be removed because it really depends on how the installation was performed. We should define a proper installation as done by @jeljaik in the robotology-playground/WBI-toolbox

It appears like that (just for the visualizer) the base body is assumed to be fixed, and so the foot appears as moving even if their position is constant.

It easy to reproduce the problem by setting a sine trajectory in the input to the desired value of the torso yaw.

In particular, the base state was undefined because it was taken from the returned value of a function that was not returning anything (see 0437e08#commitcomment-13937981 ).
I guess the function is always returning the identity transform in practice.
Fixed in 0437e08 , still need to be fixed in master.

Hi guys, as you may know a new master student (supervised by Ale Saccon) a TU/e will start working with this code base (especially the simulation + controller) to try to integrate our control strategies with some based on hybrid systems [1] [2].

I will have to help him from the technical point of view, but given that I have a limited experience on this repo I would like to ask a bit of help from all of you.

@gabrielenava @naveenoid @DanielePucci do you think it is feasible to write a little README.md on how to run the balancing controller with simulation in matlab + visualization, given a working codyco-superbuild + Matlab installation?

[1] : https://scholar.google.com/citations?view_op=view_citation&hl=it&user=9jACRLUAAAAJ&sortby=pubdate&citation_for_view=9jACRLUAAAAJ:7PzlFSSx8tAC
[2] : https://scholar.google.com/citations?view_op=view_citation&hl=it&user=9jACRLUAAAAJ&sortby=pubdate&citation_for_view=9jACRLUAAAAJ:IWHjjKOFINEC

Apparently the wholeBodyModel has still some YARP dependencies since when launching wholeBodyModel('model-initialise','icubGazeboSim'); I obtain the following messages on the shell where matlab was launched (yarserver running)

yarp: Port /wbiTestmodel/torso/rpc:o active at tcp://192.168.7.1:10002
yarp: Port /wbiTestmodel/torso/command:o active at tcp://192.168.7.1:10003
yarp: Port /wbiTestmodel/torso/state:i active at tcp://192.168.7.1:10004
Problems instantiating the device driver torso

If yarpserver is not running the test script timeTest.m crashes Matlab. If yarpserver is running everything goes smoothly.