-O3 obviously -DNDEBUG -march=native -fopenmp
Flags: Duration: -O3 103s -O3 -march=native -DNDEBUG 77s += fopenmp 79s Conclusion. No fopenmp
Still getting good accuracy over test datasets at .005s timestep When previously it was .001s. Very good. May not work as well with non flat terrain however.
Improved accuracy from urdf model
Same accuracy when using float. But much slower. Although, that could be due to the feenableexcept flags. Nope, still slower. Make sense, the templated floats have data members, which probably screws up stride length which ruins spatial locality and vectorization
Lets do it wrt the tire sinkage parameter n0. That sounds like fun. Managed to take a basic derivative with CppAD not going to take the derivative wrt to sinkage yet because CppAD requires dynamic sized vectors and not static sized. Which makes things a bit more difficutl for me. Nope, I found a simple way around it.
Need to do a forward pass with all input parameters, including vehicle start state, and all tire velocities. Then do reverse auto-diff but only wrt the sinkage param.
The problem is, if I do reverse auto diff in CppAD It will calculated the derivative of the loss wrt every input. Including the initial state and all the tire velocity commands. Which is a significant amount of extra computation.
Here’s what I’m gonna do. THe lazy way, just create a new auto diff function every time. And then call forward_zero, then call reverse. This is not working. Getting a NaN during call to reverse. Need to google it and try to find out why I could be seeing a Nan.
It has something to do with the conditionals, I believe. Also, shouldn’t reverse and forward mode return the same results? They aren’t. Will do a test. But also, both forward and reverse seemed to be exploding so, the problem is more fundamental.
Lol, I did a stupid. Oh man. Need to find out where to actually plugin the y0 then. You dont. You just plug in x0 during forward(0, x0). Forward and Reverse Derivatives now match.
Gradient still exploding though. Only some of the gradients. Hmm. Lets remove all conditional operators as a test. Still explodes. Stops exploding when I remove the division used to calculate slip_ratio. FUCK. How do I deal with this? Add a constant to the denominator. This is a bandaid, not a solution, still can divide by zero if we’re unlucky. It will serve as proof of concept at least hopefully.
With default params: CV3 MARE lin: 0.3308356 With optimized params: CV3 MARE lin: 0.2217555
When I use CppADCodeGen I can generate the full derivative, and then possibly remove the calculations for derivatives that I don’t need.
From float templated robcogen to CppAD templated Robcogen.
and combine them you know what I mean
This is the big gun. Lets see if I can make it work for derivative wrt sinkage.
Rewrite VerifyModel the functions getModelParams setModelParams.
Need to figure out the transforms.
Base link definitely follows the usual convention: x - forward y - left z - up
3 Homogenous Transforms per tire (not including inverses): See transforms.cpp and look at each function. q = <0,0,0,0> Type_fr_base_link_X_fr_front_left_wheel() # This is the transform from base [1 0 0 tx] # to joint frame. (Doesn’t rotate) [0 0 1 ty] [0 -1 0 tz] [0 0 0 1 ] Type_fr_base_link_X_fr_front_left_wheel_link() # Transform from base to tire frame. [1 0 0 tx] # Only aligns with joint frame when q=<0,0,0,0> [0 0 1 ty] [0 -1 0 tz] [0 0 0 1 ] Type_fr_base_link_X_fr_front_left_wheel_link_COM() # The rotation is in the y direction. [1 0 0 tx] # So this is like the tire link frame [0 1 0 ty] # except it rotates around y and not z. [0 0 1 tz] # That means it is aligned with the base frame at q=0. [0 0 0 1 ]
From the docs: A_X_B is a transform that takes vectors in frame b and expresses them in frame a. As in: v_a = A_X_B* v_b
Note this transform: fr_base_link_X_fr_base_link_COM from transforms.h base_link reference frame unfortunately does not align with the center of mass of the base_link
I’ve found out that the code only updates motionTransform when setJointStatus is called. In the forward dynamics code, forceTransform class isn’t used, they just use the transpose of motionTransform.