4 ms·
Working in the field of robotics, I often have to write big vectors (usually computed as the result of 3D transformations) and then compute their Jacobian (thei
by abricq 3y ago
Working in the field of robotics, I often have to write big vectors (usually computed as the result of 3D transformations) and then compute their Jacobian (their derivative with respect to several state-variables), which quickly becomes very nasty equations. I use sympy to (i) compute these big vectors, expressed in a very declarative way, (ii) compute the jacobian and (iii) export the results in C-code, immediately importable in my code case.
To illustrate what I mean by "expressing systems of equations in a declarative way", here is a toy-example of how to estimate the position of the a sensor with respect to a robot's center if you have access to a dataset containing robot positions and the sensor positions. The 'naive' approach (it very often works) is to solve an over-constrained system with a gradient-descent. To perform a gradient descent, you need a residual function and its jacobian. Here's how you would do to compute it with Sympy. (Note: you'd just have to define the `transform` and `invert` functions...)
# Pose of a sensor in robot frame (to be estimated)
xa, ya = symbols("xa, ya")
a = Matrix([xa, ya, ta])
# Position of the robot at time t
rx, ry, rt = symbols("rx, ry, rt")
rk = Matrix([rx, ry, rt])
# Measure of the sensor at time t
gx, gy = symbols("gx, gy")
gk = Matrix([gx, gy, 0])
# Estimated x (from the measures)
estimated_a = transform(invert(rk), gk)
# compute the norm of the gk, squared
n2_mat = norm2(estimated_a - a)
n2 = sympy.collect(sympy.expand(n2_mat[0, 0]), a).simplify()
# Compute the jacobian
J = n2_mat.jacobian([xa, ya, ta])
# Print what is necessary for Guass-Markov regression
print("\n\nres =", n2)
print("\nJacobian = ", J)
- exDM69 3y agoI've used SymPy in a very similar way, in my case it was some nasty derivatives that were needed for some orbital mechanics calculations. First write down the equations, then let SymPy do the laborious math part and turn the output into C code. I'm guessing this is why you don't see a lot of SymPy projects in the wild. It was used for some intermediate calculations, and the results were turned into the product's code and the symbolic code is thrown away.
- analog31 3y agoThere might be a better way, but my habit is to include the symbolic code as a comment in the C. I've thanked myself for doing this.
- abricq 3y agoI commit the symbolic code on its own file, in the same commit which adds the c code.
- lucioperca 3y agoI guess a hypermodern solution would be to produce the parts of the C code with CI/CD from the SymPy Code.
- tehsauce 3y agoHave you considered using jax? you can efficiently compute the jacobian (even using the gpu if you want) without leaving python at all. The api is also numpy compatible!
- abricq 3y agoWe target very low-level & specific hardware. I don't think it would be easy to deploy Jax on it. But it's an interesting idea, maybe for robots running linux !
- eutectic 3y agoI just did this exact thing today, writing a cloth simulator! I wanted to try updating each node position in turn using a Newton step, holding the other nodes fixed.
- fho 3y agoFunfact: you can probably JIT compile that using JAX for an easy performance gain.
- 6gvONxR4sf7o 3y agoBut then they would have to compile the jit-optimized XLA to C. Do you know if that’s straightforwardly doable?
- fho 3y agoHmm ... Good question. I would assume that JAX and XLA is based on LLVM (like everything else these days) and that you could probably grab the IR at some point to compile it to your target.
- nephanth 3y agoNumba would probably do the job too
- sampo 3y ago> I use sympy to (i) compute these big vectors, expressed in a very declarative way, (ii) compute the jacobian and (iii) export the results in C-code, immediately importable in my code case. You could maybe calculate the Jacobian in C code using automatic differentiation. Might be less C-code, might be less elementary operations done in the C code, and you would not need to be copy-pasting complex symbolically derived formulas from Python to C. https://en.wikipedia.org/wiki/Automatic_differentiation https://en.wikipedia.org/wiki/Automatic_differentiation
- whatshisface 3y agoI could be wrong, but I seriously doubt C macros can do this.
- sampo 3y agoWhy are you thinking of macros?
- whatshisface 3y agoIn an embedded environment you would want it to be pre-compiled and optimized.
- dimatura 3y agoYes, back in the day I saw people doing this with Maple, glad there's open source options now. There's a cool library (also using Sympy) to do this kind of thing in robotics and computer vision applications, symforce https://github.com/symforce-org/symforce https://github.com/symforce-org/symforce
- belzebalex 3y agoI tried to use SymPy for a similar problem: computing the Jacobian of a complicated integral using quaternion rotations. Problem was that the symbolic results were way too complicated and the simplify function didn't help much. So, I got back to manual differentiation.
- luxcem 3y agoI'm not familiar with this field but is this similar to an Extended Kalman Filter?
- mattivc 3y agoYou might find this library interesting: https://github.com/symforce-org/symforce https://github.com/symforce-org/symforce
- tonyarkles 3y agoWas just going to suggest that! I’ve only recently started using it but so far it has been excellent. One tip… the code generator seems to do a lot of cleaning and simplification. Just today I had some ugly equations that took up many pages of Jupyter output but the generated C++ code was only about 150 lines and didn’t look particularly inefficient. It seemed to me on inspection that it found repeated product and sum terms and properly computed them once and stored them in scratch variables.
- whyever 3y agoWhy are you calculating the Jacobian symbolically? AFAIK, for complex cases, the numerical Jacobian is often faster / more numerically stable.
- funks_ 3y agoEspecially if you actually require vector-Jacobian or Jacobian-vector products instead of the full Jacobian.
- eutectic 3y agoI though finite difference differentiation was notoriously unstable.
- legobmw99 3y agoPerhaps the commenter means something like reverse mode automatic differentiation? Finite differences does indeed have stability issues, and even if you apply some tricks will only give you about half float precision
- mitthrowaway2 3y agoThe poster is talking about using symbolic math to obtain a closed-form expression for the Jacobian that is then numerically evaluated. This will often be faster and more accurate. For example, if your function is sin(x), then the symbolic math tells you your derivative is cos(x), so you put that expression in your code and compile it. When calling this on the angle x = 0.123, your code then just evaluates numerically cosf(0.123), rather than (sinf(0.124) - sinf(0.122)).
- toolslive 3y agoThere are plenty of situations where the scenario goes like this: you first do symbolical calculations. then fill in some of the free variables, your resulting expression will simplify a lot. Then you compile that resulting expression to native code and evaluate the simplified specialized code for a zillion parameter vectors. Anyway,if your day job needs something like this you're better off using a lisp than python.