Cleanup/Documentation of simulation and controller code

[traversaro]

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

[DanielePucci]

Yes Silvio, it is feasible, though it seems to me that @gabrielenava does not have stable code yet. Probably, he can provide us with additional details. @naveenoid should tell us the status of the code on the repository.

[traversaro]

:+1:

@gabrielenava anything that I can do to help reaching "stability" (that is a relative concept by the way :D)

I cleaned all the branches in the repo, transforming some stale branches in tags. Just two branch remain:

• One by @gabrielenava , that I guess contains the refactor we were speaking about.
• The other by @Ganimed , but I don't know its content. @Ganimed what is your branch about?

[naveenoid]

The back-end is now stable and has been merged. So I can work on the readme over this weekend. @barrelback 's old controller needs work on porting if you want a working balancing controller to be available to the user. Currently fully functional dynamics and a visualisation are available. Maybe @gabrielenava can finalise and have a working example in the main matlab_src folder utilising a balancing controller in closed loop. @Ganimed 's work is about building a matlab class interface to the functions so it is not going to affect the rest.

[gabrielenava]

The balancing controller is still under developement. We discovered some issues just few days ago, but I can check the code this weekend and push a "stable" version in feature/balancing_controller and a README for the controller in the same feature.

[traversaro]

Ok, perfect.

In the meanwhile I feel more confident if we add some regression tests to the repo, so that the fact that everything is properly working can just be checked by anyone with a make test. I think it make sense to port the simple structure that we have in iDynTree [1] [2].

[1] : https://github.com/robotology/idyntree/blob/master/bindings/matlab/tests/CMakeLists.txt [2] : https://github.com/robotology/idyntree/blob/master/bindings/matlab/tests/runiDynTreeTests.m

[traversaro]

I checked out @Ganimed class and it is actually quite clean. I was just wondering... why the iCubWBM name ? The software is not iCub-specific in anyway.

[Ganimed]

This is an information which I didn't know! I've always thought that the WBM is only for the iCub! But names are changeable. :) I've made something in my branch. But it is quite small. You will find everything in the folder matlab-src/+iCubWBM. This is a Matlab-package. There are the new classes with all your functions concentrated together in some few files. Everything else outside of this folder is untouched. My goal is, that these classes will replace in the future all your functions where each of them is stored in a single file. These classes should make a clear structure of the WBM. Then it is easier to maintenance and easier to use. Especially for those who are not mechanic-engineers ... But at the moment is everything under construction and I'm always merging with your changes that I'm on the actual level.

[Ganimed]

P.S. I've made also a class-diagram. There you can see the structure. It's under matlab-src/+iCubWBM/UML. Feedbacks are welcome!

[traversaro]

Mhh... @Ganimed can you find an alternative not-iCub specific name? Thanks!

I implemented what the test infrastructure that I discussed in https://github.com/robotology-playground/mex-wholebodymodel/issues/29#issuecomment-143193083 . Some details about the tests have been added in the README :
Not all tests have been ported (see https://github.com/robotology-playground/mex-wholebodymodel/issues/30) but I think that it can be already be useful.

Unfortunately we still don't have automatic testing of software using matlab at the moment, so I would kindly ask to everyone working on this repo (@naveenoid @gabrielenava @Ganimed) to always run the tests (It is sufficient to just run ctest in the build directory) before commiting any modification in the repo.

Furthermore, it would be great if we could add more test to be sure that everything is working properly even in the future. Adding a test is easy (and even fun : ) ), it is sufficient to have a script that raise an exception (for example using an assert call) if the result is not the expected one ( see for example https://github.com/robotology/mex-wholebodymodel/blob/master/tests/baseLinkFrameTest.m ) . Once you have a script like that, you can add it to the tests directory and to the list of the tests to run contained in the https://github.com/robotology-playground/mex-wholebodymodel/blob/master/tests/WBMTests.m file.

[Ganimed]

No, problem. In the next time I will rename everything such that every file/variable is without "iCub" and use general names.

[Ganimed]

Is the WBM considered in general only for humanoid robots or for any robot which would also include for example the Kuka or a robot with 6 legs. This information is quite important for me when I'm making an adequate renaming for the classes and others. Thx.

[traversaro]

It is designed with floating base robots in mind (so also robots with six legs, see https://github.com/robotology-playground/wholebodyinterface ) but it can be used even for fixed base robots.

[Ganimed]

Thanks! Good to know! Then I know how to proceed in renaming. Perhaps I have to rethink some parts of the class structure.

[gabrielenava]

I've done the ctest and then I pushed in my branch the last version of the balancing controller. it 's tested and seems everything is working well. I added a README about the controller part of the code which is in the mexwholebodymodel_balancing folder.

[traversaro]

Thanks @gabrielenava ! Two things :

• Unless it is necessary for some reason, avoid to save binary .mat files in the repository, as they are saved in the history of the repo and will remain there in saecula saeculorum.
• Can we merge it? As far as I can see everything is self-contained in an own directory, so we don't create any problem by merging.

[traversaro]

@gabrielenava I don't really understand what you did with the tests.. you moved them in matlab-src/tests ? Consider also that ctest is only useful to assess that everything is working on a given branch!

However I noticed a lot of fixes copy-pasted by hand between master and feature/balancing_controller, so I am going to merge feature/balancing_controller in master as soon as possible. As soon I merged, can you switch to master and make sure that everything is working from there? Thanks!

[gabrielenava]

regarding ctest, I followed the instructions before and went in the build directory in codyco_superbuild, but I don't know if I did it properly. the entire balancing controller is in the folder mexwholebodymodel_balancing so I think that if master is updated to the last version is not a problem to merge the branch. if you merge it, I'll do the test this afternoon

[traversaro]

I tried to cleanup a bit the directory of matlab code, by moving some unused directory in the deprecated directory : https://github.com/robotology-playground/mex-wholebodymodel/commit/9144496e21d69ddddfd5fb79ee17fe118b6817e8 . If they are not used anymore we can simply cancel them, if anyone wants to get them they can be accessed in the git history.

Furthermore, I think we should try to remove any script present in the root of the matlab-src directory and move the rest of the code to three directories: whole_body_model_functions containing the interface functions, worker_functions for the utilities function, and simulation for the simulation related functions. My original idea was to further separate the simulation and controller functions, but as far as I can see at the moment the simulation and the controller code I tightly coupled, right @gabrielenava ?

[traversaro]

By the way I got the simulator + controller to run correctly, so good job @gabrielenava !

[gabrielenava]

yes, for now simulation and controller are coupled because for visualize the graphics of contact forces, torques, ecc... is necessary to use forwardDynamics.m. I think is not compicated to separate them but is quite a long work.

However, I did the following things until now: -update the bal. controller to integrate the latest changes -test the controller in my branch -push the code in my branch -accept the pull request and merge the bal. controller in master -update master on my local repository and test again the balancing controller

and after the last test seems that there're no problems and everything works. If something doesn't work, tell me and I'll try to solve it in this week

[traversaro]

Added an entry point to the simulation+controller in main readme in https://github.com/robotology-playground/mex-wholebodymodel/commit/3ac3b6f30105193126713a8bb7213676651e9a95 .

[traversaro]

@gabrielenava apparently you already merged the PR at noon, but I checked and everything seems ok.

[Ganimed]

I have a question about the function wbm_getState, which is important for me for understanding. The function wbm_getState returns following values:

qj - joint angles (NumDoF x 1)xTb - base rototranslation (7 x 1) with 3 for position of frame and 4 for orientation quaternion
dqj - joint velocities (NumDoF x 1)
vxb - floating base velocity (6 x 1)

This is everything clear for me until the parameter vxb which is a vector of 6 elements. Does this vector describes the,

dx_b:     the cartesian velocity of the base (R^3)    and
omega_b:  the velocity describing the orientation of the base (so(3)),

such that d_xb = vxb(1:3) and omega_b = vxb(4:6)? Is my thought correct?

Additionally the initialization process is still not clear for me! This is very important for the constructor that it is doing the right thing!! When I'm using wbm_modelInitialise in the optimized mode, then I'm forced to set also wbm_setWorldLink into the optimized mode?

Francesco told to me, that for all other functions it is not obligatory to use in the optimized mode if wbm_modelInitialise is set into the optimized mode. The only thing is urgent, when a function is used in the optimized mode, that a state must be set by wbm_updateState BEFORE calling the optimized function.

Is that right?! Because this information is for me very important for the constructor and the structure of the class! Thanks!

[Ganimed]

I've renamed everything now.

[gabrielenava]

you're right about what vxb is, but be careful because wbm_getState simpy takes the values you setted before in wbm_updateState. I'm not using wbm_modelinitialize and wbm_setworldlink in optimized mode, however I think that: -wbm_initialize just sets the values related to the robot you're working on, such as number of links, joints DOF, ecc... -wbm_setworldlink decides the link where the world reference frame is. For exemple if you set the left foot to be the world link and then compute wbm_forwardkinematics('l_sole') the result will be [0;0;0;1;0;0;0] so it's now the origin of the world reference frame. since wbm_setworldlink and wbm_modelinitialize are two completely different things, I don't think that if you're using one of them in optimized mode then you're forced to use the other in opt. mode too. But is my opinion. yes, if you don't update the state before, every wbm function in optimized mode can't see your changes in the state and returns always the same values. If you are not sure, is better to use the normal mode.

[gabrielenava]

sorry, i pushed the wrong key :-)

[Ganimed]

Thanks for the info! :)

[traversaro]

Thanks to @gabrielenava docs on how to run the system+controller simulation is available at https://github.com/robotology/mex-wholebodymodel#controller-simulation . They can always be improved/corrected, but I guess that was the main point of this issue, so we can close it. Further cleanup of the repo can be discussed in specific issues, like https://github.com/robotology/mex-wholebodymodel/issues/20 .

[iron76]

:+1: Thanks at @gabrielenava for the documentation!