Adapting DDP-Generator with Drifting Vehicle Dynamic Model

[cyrusxyl]

Hello @jgeisler0303, Thank you very much for sharing your code! I am a MRSD student at CMU's robotic institute. Our team is currently working on an autonomous drift r/c car. We have integrated Yuval Tassa's iLQG algorithm (Matlab code) with a dynamic model of vehicles under drifting. The algorithm produce good trajectories, but we are looking for performance improvement. We found your code extremely helpful for our purpose and we appreciate that you are sharing your great implementation.

In attempts to adapt your implementation, I defined our dynamic model in Maxima. I believe the dynamics functions are defined properly, as they produce correct results from Maxima console. However, I'm getting a compilation error: Undefined symbols for architecture x86_64: "_derivative", referenced from: _bp_derivsL in iLQG_func.o ld: symbol(s) not found for architecture x86_64 clang: error: linker command failed with exit code 1 (use -v to see invocation)

I can't really tell if this is a complier issue, or an issue cause by the dynamic model defined in .mac file. I wonder if you have an intuition of how to solve this problem. I attached my generated files and the dynamics defined in .mac file. Driftcar.zip

Thank you very much, Cyrus

[jgeisler0303]

The error comes from using the fmod function, the fabs function and lots of nested if-then-else. Maxima does not know how to calculate the derivative of fmod or fabs and thus just generates a function call to derivative which gets mangled by the compiler to _derivative. This function of course does not exist and then the liker complains, or in my case octave complains because it uses dynamic (run-time) linking.

As a solution you could add this line to your problem definition: gradef(fmod(x, y), 1, 0);. For the fabs something similar might work but the discontinuity might upset the optimizer. So, maybe better use the sqrtabs function. As for the nested if-then-else's, those should be handled by my code. But some how it doesn't work in all cases. I will take deeper look into this problem tomorrow.

I also fixed two problems that might become bugs some day. So, please consider updating your repo clone.

If you want to go with numerical differentiation, like Yuval's code, you also might want to take a look at the openmp_eigen3 branch. There is experimental code for much faster matrix computations or multi-core processing or finite difference differentiation. All these are currently still separate but I plan to merge them into a flexible framework where each feature can be enabled separately and all can be mixed in any combination.

[cyrusxyl]

@jgeisler0303 Thank you very much for replying. I was initially using mod() and abs() functions with maxima, but then the C compiler complains about mod() and abs() not defined in iLQG_func.c, that's why I was trying fmod() and fabs() functions.

While using mod() and abs() with maxima, I also noticed that prefix the auxillari value with apostrophe will compile the problem successfully. s: dx([x_,y_,phi,Ux,Uy,r],[thr,steer]); f[x_]: x_ + 's[1]*h; f[y_]: y_ + 's[2]*h; However, in this case 's' got recognized by the compiler as a parameter rather than an auxillari value. And the other parameters used in dx() function are not recognized. Driftcar_aother_attempt.zip

Again, thanks a lot for your help. I'll update my repo and try out the openmp_eigen3 branch.

[jgeisler0303]

1. The c compiler doesn't know the mod() and abs() functions and Maxima doesn't know to translate them correctly. You could add a #define mod fmod to the generated code.
2. Maxima knows how to calculate the derivative of abs. For abs this works well, albeit not very efficient (diff(abs(x), x)= x/abs(x)). It would be more efficient to declare your own abs function like if x>0 then x else -x.
3. Maxima knows how to calculate the derivative of mod. But the result includes the derivative of floor which is not known. So, I doubt or at least wonder how code generated from using mod could work, with or without auxiliaries.
4. Using auxiliaries is restricted to scalar values. Arrays can only be used for parameters. That's why Maxima thinks your auxiliary is a parameter. In the definition of s as an array also probably lies the problem with the other parameters. Try to define s1, and s2 not as a function.
5. I don't have time to look at your second attempt right now.
6. For using the openmp_eigen3 please take a look at this comment: https://github.com/jgeisler0303/DDP-Generator/issues/1#issuecomment-281164563.
7. I didn't have time to investigate the problem with nested conditionals. Maybe tonight...

[jgeisler0303]

I found the problem: defining gradef using x doesn't work because x is already defined as the state vector. Therefore you have to declare this at the beginning of you problem definition: gradef(fmod(xxxx, yyyy), 1, 0); gradef(fabs(xxxx), if xxxx>0 then 1 else -1);

Also, I committed another small change, I'm not sure if it does make a difference but you should pull it anyway.

Now I can produce your initial example without errors.

Finally, please beware that the changes discussed here are not yet ported to the openmp_eigen3 branch.

[cyrusxyl]

Thanks for clearing my confusions! I followed your instruction on defining gradef for fmod() and fabs(). There is still the_derivative issue, and they seems to be involved with a lot of mcond(). I suspect this is caused by the nested conditions. I also tried to define s1,...,s6 as separate variables without calling the dx() function. This helps when I use s1,...,s6 as substitution, there will be less warnings generated by the compiler. Using s1,...,s6 as auxiliaries will create an error before iLQG_func.c gets generated:

circular definition of dependencies involving  r  and  dep
#0: recurse_deps(d_work=r,parents=[s4,r]) (genenerator_main.mac line 174)
#1: recurse_deps(d_work=s4,parents=[s4]) (genenerator_main.mac line 179)
#2: recurse_deps(d_work=top_level_aux,parents=[]) (genenerator_main.mac line 179)
 -- an error. To debug this try: debugmode(true);

This error doesn't limited to s4 but happens to all s1,...,s6 if I tried to use them as auxiliaries.

For using the eigen branch, I am still trying to setup the car parking demo, while I am getting several errors from eigen like this one:

/usr/local/include/eigen3/Eigen/src/Core/DenseCoeffsBase.h:406:7: error: static_assert failed "THE_BRACKET_OPERATOR_IS_ONLY_FOR_VECTORS__USE_THE_PARENTHESIS_OPERATOR_INSTEAD"
      EIGEN_STATIC_ASSERT(Derived::IsVectorAtCompileTime,
      ^                   ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
/usr/local/include/eigen3/Eigen/src/Core/util/StaticAssert.h:32:40: note: expanded from macro 'EIGEN_STATIC_ASSERT'
    #define EIGEN_STATIC_ASSERT(X,MSG) static_assert(X,#MSG);
                                       ^             ~
/Users/Cyrus/Documents/MATLAB/DDP-Generator-openmp_eigen3/examples/CarParking/Car_ddp_eigen/iLQG_func.cpp:516:10: note: in instantiation of member function 'Eigen::DenseCoeffsBase<Eigen::Matrix<double, 4, 4, 0, 4, 4>, 1>::operator[]' requested here
    t->fx[8]=-sin(x[2])*aux_s;

Thank you very much for taking your time to help me!

[jgeisler0303]

Ah sorry, there was a bug in the generated code for eigen3 target. I pushed a fix.

Is the version of your car you have the trouble with one of those you uploaded?

[cyrusxyl]

Hi @jgeisler0303, Thanks for helping! For eigen branch I was just trying out the car parking demo problem. For the dependence problem I was using my first attempt, with gradef(fmod(xxxx, yyyy), 1, 0); gradef(fabs(xxxx), if xxxx>0 then 1 else -1); at the beginning of the file. I changed s: dx([x_,y_,phi,Ux,Uy,r],[thr,steer]); to [s1,s2,s3,s4,s5,s6]: dx([x_,y_,phi,Ux,Uy,r],[thr,steer]);

I also tried to flatten the function outside, but it will produce the same error messages.

[jgeisler0303]

Oh, there was a kind of bug: r was used as local variable and maxima cannot discriminate between your variables and its own local variables. I must really refactor all code to prefix all of the generators local variables.

I pushed a workaround. It worked for me but compiled very long.

[cyrusxyl]

Thanks a lot for the help, the program compiles successfully after the fix! But I notice that alpha_F and alpha_R were mistaken as parameters in iLQG_func.c. I tried define alpha_F and alpha_R separately and they are now recognized correctly. But with this I got some new errors involve mcond and is,sec,unknown, etc. err.log.txt

[jgeisler0303]

You can tackle is and sec like this in the generated source code: #define is(x) (x) resp. #define sec(x) (1.0/cos(x)). just add these manually after generation for now.

But unknown means that Maxima thinks that some conditional statement cannot be resolved. So, you should check again how you defined alpha_F and alpha_R. As this problem didn't occur before.

[cyrusxyl]

Thanks! With sec(x) defined and some minor changes regrading alpha_F and alpha_R, I was able to compile successfully and it is producing trajectories similar to what we got from our MATLAB implementation, and it runs about 60x faster!

But I notice that I sometime got errors like this:

@k 0: aux_s2 in line 597 is nan or inf: nan
where
 aux_s2=sqrt(pow(x[3],2.0)+pow(x[4],2.0))*sin(x[2]+mcond(x[3]<0.0&&x[4]>0.0,3.141592653589793-atan(x[4]/abs(x[3])),1,mcond(x[3]<0.0&&x[4]<0.0,-3.141592653589793-atan(x[4]/abs(x[3])),1,atan(x[4]/abs(x[3])))));

There is division by zero, when x[3]==0. Part of the nested condition in our dynamic definition intends to take care of this:

beta:
    if Ux=0 and Uy=0
    then 0
    else if Ux=0 and Uy>0
    then %pi/2
    else if Ux=0 and Uy<0
    then -%pi/2

But I think these conditions in the form of if a=b are not recognized, since I cannot find their mcond equivalent in iLQG_func.c. I tried to define == operator in Maxima and use that instead, or use is(Ux=0), it did not make any difference. Do you have any intuition on how to fix this?

[cyrusxyl]

We are also thinking of generating command line executable instead of mex file with matlab. Is this possible? Would you give us some pointers on this?

[jgeisler0303]

Strange. When I do gentran(a: (if x=0 and y=0 then 1 else 2));in Maxima I get a=mcond(x==0&&y==0,1,1,2);

Command line executable is possible of course. It should be quite straight forward to adapt from iLQG_mex.c. How do you intend to do input and output? You want to do MPC? Then you should do something where the program stays open and has some means to receive new values to process.

[jgeisler0303]

I just realized that when you do an assignment with if-statements they are evaluated immediately and the condition always becomes false because the variables involved don't exist: beta: if Ux=0 and Uy=0 then 0 else if Ux=0 and Uy>0 then %pi/2 else if Ux=0 and Uy<0 then -%pi/2; --> gentran(eval(beta)); --> 0. So the expression defaults to the last else value. So, you need to apostrophe the conditions: beta: if '(Ux=0 and Uy=0) then 0 else if '(Ux=0 and Uy>0) then %pi/2 else if '(Ux=0 and Uy<0) then -%pi/2; (but should there be a final else?) --> gentran(beta: eval(beta)); --> beta=mcond(Ux==0&&Uy==0,0,1,mcond(Ux==0&&Uy>0,%pi/2.0,1,mcond(Ux==0&&Uy<0,-1.0/2.0*%pi,1,0)));

[cyrusxyl]

I reduced the amount of nested conditions and avoided the a=b conditions. I think it resolves most of the problems and is running faster. I'm trying to further increase the performance by flatten our the functions and make use of auxiliaries, but having some trouble. I'm wondering how the auxiliaries and their derivatives are ordered in iLQG_func.c. For instance, I have

#define aux_Fyr t->Fyr
#define aux_dr t->dr
#define aux_Fyf t->Fyf

while dr depends on both Fyr and Fyf. When the program generate the derivative of dr, it knows to replace derivative(aux_Fyr, x[3], 1) with daux_diff_Fyr_Ux, but it cannot interpret derivative(aux_Fyf, x[3], 1) because daux_diff_Fyf_Ux is defined later in the file. I'm not sure if manual replace the term will do the work, as it is not assigned at the moment of access. Is there a way to manipulate the ordering of the auxiliaries?

[jgeisler0303]

a=b is not the problem. You can use it, but you need to enclose it in apostrophed parenthesis.

derivative(...) expressions must not be found anywhere in the generated code. If they are, something is still wrong. Manual editing must never be necessary and should never be performed.

The order of calculating aux and daux is taken care of. The define statements are only a renaming. The actual calculation takes place elsewhere.

Maybe you need to upload another example.

[cyrusxyl]

Thanks for the clarification! I uploaded my generated files. In optDefDriftCarFlat.mac I flattened the functions, this one compiles and is working. I followed your instruction to apostrophe the conditions, but still don't see the mcond equivalents in iLQG_func.c. In optDefDriftCarFlat_aux.mac, I put apostrophes in front of the reusable variables to make them auxiliaries, this one does not compile. DriftCar_flat.zip

[jgeisler0303]

I looked at the iLQG_func.c from optDefDriftCarFlat.mac the and found many mcond'. Could you point out which one exactly you are missing?

You didn't apostrophe all conditional statements. But maybe that's not necessary. It might even introduce aux variables when you don't want it. I dug a bit deeper and found that only and, or, not are causing problems because they force evaluation. So, you might try not to apostrophe anything but rather use mand(a, b) instead of a and b. But then you need to #define mand(x, y) (x && y) and respectively for or and not in the generated code. I have not tried this yet. When it works I will add the defines automatically in the generated code.

I noticed that there are some constant numbers in integer c-notation (without any dot). The generator should take care of this, but somehow it didn't. This might cause problems. So, until it's fixed you should replace all 0, 1, -1 etc. with 0.0, 1.0, -1.0.

The optDefDriftCarFlat_aux.mac does not compile because the generator somehow does not know how the calculate the derivative of some aux vars. I not sure why this is. Will investigate further later.

[cyrusxyl]

Thanks a lot for the help! I believe the missing conditions are if ('gamma_F = 0) and if '(gamma_R = 0)

[jgeisler0303]

Now I think I found the problem. = is not the same as == in c. = test if two objects are the same, so a variable and constant will never be the same. You have to use equal(a, b) instead. That's the theory. I have not tried it yet though.

[jgeisler0303]

Regarding the problem with the aux version: I found a fundamental bug in the sorting of aux vars that leads to to the observed problem Undefined symbols for architecture x86_64: "_derivative"...

It will take me a bit longer to fix this.

I also noticed that aux variables in If-statements are replaced in derivatives by what the aux var stands for. This I will also need to fix.

[jgeisler0303]

I pushed a totally reworked version that seems to compile your "aux" model correctly. I have not tested any more. But maybe you want to see if it works for you.

Only, you need to change parameter name h in the problem definition because that name is reserved for the input constraints.

[cyrusxyl]

Awesome! Thank you very much! I'll check it out.