[action] Addded actuation to DAMFreeFwdDynamics - [merged] #598

Closed wxmerkt closed 4 years ago

In GitLab by @jmarti on Nov 8, 2019, 07:48

_Merges DifferentialActionModelFreeFwdDynamicsoverloaded -> devel

Current behavior: The DAMFreeFwdDynamics (DAMFFD) assumes that the actuation model is the fully actuated one.

Desired behavior: Pass the actuation model to the DAMFFD as a parameter in the constructor.

In GitLab by @jmarti on Nov 8, 2019, 07:49

changed the description

In GitLab by @cmastalli on Nov 8, 2019, 09:58

If you're missing that the actuation model might depends on $\mathbf{x}$, i.e, $\boldsymbol{\tau}(\mathbf{x},\mathbf{u})$. Therefore you need to add the following lines of code:

 d->Fx.leftCols(nv).noalias() = d->pinocchio.Minv * d->pinocchio.dtau_dq;
 d->Fx.rightCols(nv).noalias() = d->pinocchio.Minv * d->pinocchio.dtau_dv;

In GitLab by @cmastalli on Nov 8, 2019, 09:59

Eigen creates an internal object with this expression, to avoid that we use noalias(), i.e.:

d->Fu.noalias() = d->pinocchio.Minv * d->actuation->dtau_du;

In GitLab by @cmastalli on Nov 8, 2019, 10:07

same as https://gepgitlab.laas.fr/loco-3d/crocoddyl/merge_requests/262#note_7824. Write all these lines as:

    d->Fx.leftCols(nv).noalias() = d->pinocchio.Minv * d->actuation->dtau_dq;
    d->Fx.leftCols(nv).noalias() -= d->Minv * d->pinocchio.dtau_dq;
    d->Fx.rightCols(nv).noalias() = d->pinocchio.Minv * d->actuation->dtau_dv;
    d->Fx.rightCols(nv).noalias() -= d->Minv * d->pinocchio->dtau_dv;
    d->Fu.noalias() = d->pinocchio.Minv * d->actuation->dtau_du;

In GitLab by @cmastalli on Nov 8, 2019, 10:20

There are other actions to be done:

In GitLab by @cmastalli on Nov 8, 2019, 10:28

assigned to @cmastalli

In GitLab by @jmarti on Nov 8, 2019, 11:20

don't you have to add also d->pinocchio.ddq_dq?

i.e.: d->Fx.leftCols(nv) = d->pinocchio.ddq_dq - d->pinocchio.Minv * d->pinocchio.dtau_dq

In GitLab by @jmarti on Nov 8, 2019, 12:16

changed this line in version 2 of the diff

In GitLab by @jmarti on Nov 8, 2019, 12:16

changed this line in version 2 of the diff

In GitLab by @jmarti on Nov 8, 2019, 12:16

added 2 commits

1b28eff2 - [action] modified python bindings
1a92c089 - Bindings modified

Compare with previous version

In GitLab by @jmarti on Nov 8, 2019, 12:18

added 1 commit

d9d312ef - Added the actuationModel to the arm example

Compare with previous version

In GitLab by @jmarti on Nov 8, 2019, 12:22

added 1 commit

8118c7e6 - [example cpp]

Compare with previous version

In GitLab by @jmarti on Nov 8, 2019, 12:48

added 1 commit

6da29fa8 - [arm manipulation ipynb] Added actuation to DAMFFD

Compare with previous version

In GitLab by @jmarti on Nov 8, 2019, 13:08

added 1 commit

7af4cb8e - Added actuation

Compare with previous version

In GitLab by @cmastalli on Nov 9, 2019, 15:28

Yes, you're right. It was a typing error, this is what I meant:

 d->Fx.leftCols(nv).noalias() += d->pinocchio.Minv * d->actuation->dtau_dq;
 d->Fx.rightCols(nv).noalias() += d->pinocchio.Minv * d->actuation->dtau_dv;

In GitLab by @cmastalli on Nov 9, 2019, 15:30

I updated these lines. I also had the typing error

In GitLab by @jmarti on Nov 12, 2019, 07:02

added 1 commit

2ff5cbce - Action model derivatives

Compare with previous version

In GitLab by @cmastalli on Nov 12, 2019, 16:58

added 50 commits

2ff5cbce...e8b1db38 - 42 commits from branch loco-3d:devel
3eca6463 - [action] Addded actuation to DAMFreeFwdDynamics
1a89b219 - [action] modified python bindings
8c32863a - Bindings modified
f1810573 - Added the actuationModel to the arm example
2a180fd7 - [example cpp]
c4f05c24 - [arm manipulation ipynb] Added actuation to DAMFFD
221e65ff - Added actuation
1e64e744 - Action model derivatives

Compare with previous version

In GitLab by @cmastalli on Nov 12, 2019, 18:47

added 1 commit

134c2378 - [action] Added last details of actuation model + fixed all examples, benchmarks and notebooks

Compare with previous version

In GitLab by @cmastalli on Nov 12, 2019, 18:47

resolved all threads

In GitLab by @cmastalli on Nov 12, 2019, 18:48

@jmarti I have finished the remaining task in this PR:

Added the derivatives of the actuation model for the RNEA case (c++ and Python)
Fixed all benchmarks, examples and notebooks

In GitLab by @cmastalli on Nov 12, 2019, 22:58

merged

In GitLab by @cmastalli on Nov 12, 2019, 22:58

mentioned in commit f75bb8aa30126c005d94e61f37e42693c0957532

In GitLab by @jmarti on Nov 14, 2019, 08:11

mentioned in merge request !251

loco-3d / crocoddyl

[action] Addded actuation to DAMFreeFwdDynamics - [merged] #598