Python integration of UAV & UAM - [merged] #588

Closed wxmerkt closed 4 years ago

In GitLab by @jmarti on Oct 25, 2019, 11:15

_Merges UAMdevel -> devel

Python code to use Crocoddyl with UAVs (unmanned aerial vehicles) and UAMs (unmanned aerial manipulators)

In GitLab by @cmastalli on Oct 25, 2019, 11:20

This code is using legacy Python, we need to work on it! I will comment some of the changes that we need to do.

In GitLab by @cmastalli on Oct 25, 2019, 11:21

Import the new crocoddyl submodule

import crocoddyl

and then modify all the classes by appending crocoddyl of course

In GitLab by @cmastalli on Oct 25, 2019, 11:22

Remove inclusion of a2m and m2a. All the code should used NumPy.matrix. However, we might want to change this in future

In GitLab by @cmastalli on Oct 25, 2019, 11:23

As mentioned earlier you need to append crocoddyl so,

class DifferentialActionModelDoublePendulum(crocoddyl.DifferentialActionModelAbstract):

In GitLab by @cmastalli on Oct 25, 2019, 11:24

The API has changed! Do not pass the Pinocchio model, instead pass the StateMultibody. This class does the job of old StatePinocchio

In GitLab by @cmastalli on Oct 25, 2019, 11:24

Construct the abstract class by passing the state object

In GitLab by @cmastalli on Oct 25, 2019, 11:25

Remove this line. The state is by default when you construct the abstract class. For accessing to it in the rest of the class, you can do

self.state

In GitLab by @cmastalli on Oct 25, 2019, 11:26

Removed this function, it's not needed anymore!

In GitLab by @cmastalli on Oct 25, 2019, 11:27

Unfortunately, we cannot derived data classes in Python code. So internal data need to be stored inside the model class (e.g. data.M/pinocchio/Minv)

In GitLab by @cmastalli on Oct 25, 2019, 11:28

data.r has been removed from the common data. So you need to added as your internal data too

In GitLab by @cmastalli on Oct 25, 2019, 11:29

Renamed this variable to

self.enableAba

In GitLab by @cmastalli on Oct 25, 2019, 11:31

Another undesired effect in Python code is that you cannot write internal data of Numpy matrices and vectors. In future it will be possible, there is an ongoing work on EigenPy. So you need to modify as:

data.Fx = np.hstack([data.pinocchio.ddq_dq, data.pinocchio.ddq_dv])

In GitLab by @cmastalli on Oct 25, 2019, 11:33

same here, i.e.

data.Fu = data.pinocchio.Minv

In GitLab by @cmastalli on Oct 25, 2019, 11:33

and of course here!

These are a lot of tiny modifications, please do it slowly and check every single modification

In GitLab by @cmastalli on Oct 25, 2019, 11:34

Remove this data class, you don't need anymore! Again all the internal data has to be stored inside model, sorry for that!

In GitLab by @cmastalli on Oct 25, 2019, 11:35

After solving and understanding each modification of pendulum problem, them do the same here.

In GitLab by @cmastalli on Oct 25, 2019, 11:35

And again remove this class, no needed anymore!

In GitLab by @cmastalli on Oct 25, 2019, 11:37

Moved this file inside bindings/python/crocoddyl/utils and renamed to uam.py

In GitLab by @cmastalli on Oct 25, 2019, 11:38

For running this example, then you need to include it in the CMakeFile inside example folder. We did it for the other examples.

In GitLab by @cmastalli on Oct 25, 2019, 11:39

import crocoddyl

In GitLab by @cmastalli on Oct 25, 2019, 11:40

Changed to

from crocoddyl.utils import displayTrajectory

In GitLab by @cmastalli on Oct 25, 2019, 11:41

We deprecated the robots.py. We should include this load function here: https://gepgitlab.laas.fr/gepetto/example-robot-data.

First we should create a PR there! I will give you feedback there

In GitLab by @cmastalli on Oct 25, 2019, 11:42

Now it's called StateMultibody

In GitLab by @cmastalli on Oct 25, 2019, 11:43

All the cost functions, action models, activations, etc have a different API. Instead of passing the Pinocchio model, you need to pass the StateMultibody object

In GitLab by @cmastalli on Oct 25, 2019, 11:44

The API has changed here. See this: https://gepgitlab.laas.fr/loco-3d/crocoddyl/blob/devel/bindings/python/crocoddyl/utils/biped.py#L194

In GitLab by @cmastalli on Oct 25, 2019, 11:45

Additionally, with the Python bindings, you can't pass arguments by using their names. You need to do it in a specific order as in c++

In GitLab by @cmastalli on Oct 25, 2019, 11:46

You need to modify all the cost construction as mentioned before. See the file that I shared with you for more details

In GitLab by @cmastalli on Oct 25, 2019, 11:46

same here

In GitLab by @cmastalli on Oct 25, 2019, 11:46

now it's renamed to dt, so

runningModel.dt = dt

In GitLab by @cmastalli on Oct 25, 2019, 11:47

The stack of callbacks need to be defined once throught setCallbacks function

In GitLab by @cmastalli on Oct 25, 2019, 11:50

There is not anymore the quasiStatic function inside the differential action model. Now it's inside the action model.

Previously it was a tailored quasiStatic computation, now I have implemented a general function based on numerical optimization. Please be aware that there are tiny numerical differences with respect to the previous version.

So do

m.quasiStatic(d, rmodel.defaultState)

In GitLab by @cmastalli on Oct 25, 2019, 11:51

You're not using the warm-start. You should remove the previous lines

In GitLab by @cmastalli on Oct 25, 2019, 11:52

Be aware that u is a numpy matrix, not array as before.

Then you will need to add your code accordingly.

In GitLab by @cmastalli on Oct 25, 2019, 11:52

Do the same modifications of kinton_flying.py

In GitLab by @cmastalli on Oct 25, 2019, 11:52

same here!

In GitLab by @cmastalli on Oct 25, 2019, 11:53

Don't forget to adapt your notebooks!

In GitLab by @cmastalli on Oct 25, 2019, 11:53

Push a notebook file which clean, i.e. without the printed messages generated when you run it

In GitLab by @cmastalli on Oct 25, 2019, 11:54

It's not needed since we're moving to new Python code

In GitLab by @cmastalli on Oct 25, 2019, 11:55

Revert this change, we should modify anything in the legacy code. With this, I can't check things back during this transition. In any case, you wouldn't use this piece of code

In GitLab by @cmastalli on Oct 25, 2019, 11:56

Follow same guides for the action model.

In GitLab by @cmastalli on Oct 25, 2019, 11:56

same here

In GitLab by @cmastalli on Oct 25, 2019, 11:56

not needed anymore

In GitLab by @cmastalli on Oct 25, 2019, 11:57

same here

In GitLab by @cmastalli on Oct 25, 2019, 11:57

not needed anymore

In GitLab by @cmastalli on Oct 25, 2019, 11:57

Again revert all modifications inside any Python legacy code :)

In GitLab by @cmastalli on Oct 25, 2019, 11:59

I think it's better if you encoded this code inside the model. We don't need to generalize it. Please move this code inside the model

In GitLab by @cmastalli on Oct 25, 2019, 11:59

not needed

In GitLab by @cmastalli on Oct 25, 2019, 12:00

I'm not sure why this is needed. Could you explain me?

In GitLab by @cmastalli on Oct 25, 2019, 12:00

Revert these modifications

In GitLab by @cmastalli on Oct 25, 2019, 12:01

This has to go to example-robot-data

loco-3d / crocoddyl

Python integration of UAV & UAM - [merged] #588