c++ implementation and Python bindings of the free foward dynamics (differential action model) - [merged]

[wxmerkt]

In GitLab by @cmastalli on Aug 1, 2019, 15:37

_Merges topic/cpp-free-fwddyn -> cppdevel

This PR mainly adds the free forward dynamics used for the talos_arm example. Addtionally, it adds a method to share the data with the cost sum. Finally, it adds unit-test for this differential action model.

[wxmerkt]

In GitLab by @cmastalli on Aug 1, 2019, 15:38

Thi is WIP, I am still debugging an issue. The DDP solution is not the same, and sometimes it crashes the Python program. I think there is problem with memory management.

[wxmerkt]

In GitLab by @cmastalli on Aug 1, 2019, 21:14

added 4 commits

• 15d73f46 - [cleanup] Cleaned up a bit the code
• b3dafa40 - [refact] Used const boost::shared_ptr in the cost classes (as in the action ones)
• 3b43f9d2 - [bugfix] Since the reference can be changed, we need to use the getter functions
• 9242711d - [cleanup] Removed Python code used for reference

Compare with previous version

[wxmerkt]

In GitLab by @cmastalli on Aug 6, 2019, 12:03

added 44 commits

• 9242711d...e3e5d68d - 4 commits from branch loco-3d:cpp_devel
• bc3d238b - [state][bindings] Getter method for the pinocchio model in state multibody
• a1289421 - [cleanup] Formatted the c++ code
• d87a722b - [action][bindings][wip] Added partially the free forward dynamics diff-action
• 5a506a73 - [action] Created method to share the diff-action data with an external cost data
• 73e3ce89 - [cleanup] Formatted the code for free forward dynamics
• 7ac2189d - [action] Fully implemented the free forward dynamics model
• f0692fca - [bindings] Added proper documentation of the free forward dynamics
• 6e254117 - [bindings] Missed ,
• 5a2c860a - [action] Reused the Pinocchio data for derivatives + solved a error in RNEA...
• 859936ca - [refact][action] Refactored action-based calc and calcDiff, using const...
• ec112e20 - [doc][bindings] Added missed description of some members for the action data
• ddc49103 - [bindings] Added the Python bindings of shareCostMemory of differential action abstract
• afa5afb8 - [action] Python bindings of internal data in free fwd dynamics + cleaned up a bit the code
• bfe1b721 - [bugfix] Fixed error in memory sharing in differential action abs
• aaab520f - [unittest] Added Python bindings unit-test for the free forward dynamics
• 990c3941 - [integrator] Added function and its Python bidings for get and set dt
• b2907e7b - [cleanup] Cleaned up the format for the Python utils
• 24b0039e - [wip] Debugging the talos arm example, later this will become a benchmark code
• b5d5a11d - [bugfix][action] M is represents as triangular matrix + used Pinocchio...
• 5b08784e - [cleanup] Use tail instead topRows
• ca73ff95 - [shooting] Adding some nice asserts
• 967a91d3 - [bugfix] Fixed an error in the legacy manipulator diff action
• 2e01fb28 - [debug][wip] Modified the script to debug the problem
• 2a0356bb - [cleanup] Cleaned up a bit the code
• 0d1fdf12 - [refact] Used const boost::shared_ptr in the cost classes (as in the action ones)
• b289c744 - [bugfix] Since the reference can be changed, we need to use the getter functions
• 380f4f5f - [cleanup] Removed Python code used for reference
• e9a5831a - [bugfix] Fixed a bug in the euler integrator (computation of Fx and Fu)
• 815294cc - [bugfix] Fixed wrong dimension of dq in integrate and Jintegrate (StateMultibody)
• cea22956 - [action] nr has to be at least equals 1, not 0
• 80cd031a - [bugfix] Wrong initialization of J matrix in CostModelFrameTranslation
• 5d87e7cc - [cleanup] Improved the assert messages which follows the our standard
• d839fd8e - [cleanup] Added other assert for different action and cost models
• 3da05ddf - [shooting] In rollout it's updated the terminal data
• 7cbb9839 - [action] Needed to use an extra matrix for Minv and setZero (free fwrd dyns)
• 87ece4cf - [bugfix] Fixed problem in block operations inside CostModelSum
• aaa62452 - [cost] Now addCost keeps the pointer in the Python bidings
• f8cea101 - [bindings] Read and write iter from SolverAbstract
• 6577c8a5 - [unittest] Added Python bindings unit-test for manipulator in action and solver
• fbcd5be3 - [wip] Latest modification of the benchmark script for talos arm

Compare with previous version

[wxmerkt]

In GitLab by @cmastalli on Aug 6, 2019, 16:01

added 1 commit

• 82684007 - [benchmark] Added a full benchmark of the talos arm (i.e. free-forward dynamics)

Compare with previous version

[wxmerkt]

In GitLab by @cmastalli on Aug 6, 2019, 16:07

I have solved various bug to make it work properly. I also included a benchmark for the talos arm (~300 Hz for 100 nodes).

In the benchmark there is hard-code the method for loading the talos arm. This needs to be tackle in example-robot-data. For more details see: https://gepgitlab.laas.fr/gepetto/example-robot-data/issues/12

[wxmerkt]

In GitLab by @cmastalli on Aug 6, 2019, 16:07

unmarked as a Work In Progress

[wxmerkt]

In GitLab by @cmastalli on Aug 6, 2019, 16:07

merged

[wxmerkt]

In GitLab by @cmastalli on Aug 6, 2019, 16:07

mentioned in commit 4ebae725f562b54068640f007064d3cd6bc5d67f