Embed Ackermann Physics into JaxSim

[lucaslibshutz]

The goal of this is to possibly get a 3D rigid body posed ackermann car into a JaxSim framework that can be used for simulation/training/etc. Relevant theory and documentation will be posted here.

[lucaslibshutz]

Working on rigid body model of the car with slip ratios modeled, will post a more detailed walkthrough of the dynamics most likely tomorrow during worksession

[lucaslibshutz]

First version of full physics model has been implemented in cornellev/ackermann-jax@38eb8fe - theoretical breakdown below.

Detailed Theoretical Breakdown

1) State and Frames

Reference Frames

• World/Interial: $\mathcal{I}$
• Body/chassis: $\mathcal{B}$ attached to COM of chassis

State

• $\mathbf{p}^\mathcal{I} \in \mathbb{R}^3$: chassis position in world frame
• $R \equiv {}^\mathcal{I} R_\mathcal{B} \in SO(3)$: rotation matrix that maps body vectors into world frame
• $\mathbf{v}^\mathcal{I}\in \mathbb{R}^3$: linear velocity of chassis in world frame
• $\boldsymbol{\omega}^\mathcal{B} \in \mathbb{R}^3$: chassis angular velocity in body frame (should only be about shared $z$ axis, as changes in tilt and stuff should be negligible, but keeping it in full rigid body dynamics compensates for anything we may encounter)
• $\boldsymbol{\omega}_w \in \mathbb{R}^4$: spin rates of each wheel

Therefore, the state is defined as:

$$ \mathbf{x} = \begin{bmatrix} \mathbf{p}^\mathcal{I} & R & \mathbf{v}^\mathcal{I} & \boldsymbol{\omega}^\mathcal{B} & \boldsymbol{\omega}_w \end{bmatrix}^\top $$

2) Inputs & Ackermann Steering

Input

• $\delta_i$: steering angle command
• $\tau_i$ for wheels, with $i \in \{ \text{FL,FR,RL,RR} \}$

And for each motor, I've put in a hasMotor configuration, so that we can change between AWD/RWD/FWD if we want to in the future. For now, we'll keep it as just RWD.

And, for front wheel steering only, we have $\delta_{FL}=\delta_{FR}=\delta$, and $\delta_{RL}=\delta_{RR}=0$, which yields $\delta_i \in \mathbb{R}^4$ in general (but will only use justthe first two for mapping)

3) Wheel Contact Kinematics (for roll & pitch / ground plane stuff, which will be later)

Each wheel has a contact point in the body frame (w.r.t COM), as $r_i^\mathcal{B} \in \mathbb{R}^3$. (i.e., $[+a,\pm W/2,-h]$, for one set of wheels, etc). This is already written in the code.

To translate both position and velocity into the world frame, we can use the following equations:

$$ \begin{gather*} p_i^\mathcal{I} = p^\mathcal{I} + R r_i^\mathcal{B} \\ v_i^\mathcal{I} = v^\mathcal{I} + R(\omega^\mathcal{B} \times r_i^\mathcal{B}) \end{gather*} $$

We have to embed the cross product here, as this is exactly what will encode rotation in full 3 rotational DOF.

4) Ground contact model

We define ground plane as $z=0$,and if we define $z_i=\hat{\mathbf{e}}_3 \cdot p_i^\mathcal{I}$, then:

$$ d_i = \max(0,-z_i) $$

And define penetration rate $\dot{d}_i$:

$$ \dot{d}_i = \begin{cases} (-v_i^\mathcal{I})_z, & d_i \gt 0 \\ 0, & d_i = 0 \end{cases} $$

From these quantities, we can essentially treat our contact force as a spring-damper model, with:

$$ F_{z,i} = \max \left(0, k_nd_i + c_n\dot{d}_i\right) $$

where we can define $k_n$ and $c_n$ to suit our needs. Furthermore, because $d_i$ depends on the height of each corner, if the car ends up tilting, the $F_{z,i}$ responds accordingly. This is also crucial in producing the roll/pitch dynamics that we need.

5) Tire slip & forces

For each wheel, we define tangent and normal vectors using $\delta_i$:

$$ \hat{t}_i^\mathcal{B} = \begin{bmatrix} \cos(\delta_i) \\ \sin(\delta_i) \\ 0 \end{bmatrix}, \hat{n}_i^\mathcal{B} = \begin{bmatrix} -\sin(\delta_i) \\ \cos(\delta_i) \\ 0 \end{bmatrix} $$

And to map these to world vectors, we can simply left multiply by our DCM $R$, with $\hat{t}_i^\mathcal{I} = R\hat{t}_i^\mathcal{B}$, and the same thing for $\hat{n}_i^\mathcal{B}$. From these vectors, we can simply dot these with our wheel velocities to get our tangential and normal velocities in the inertial/world frame:

$$ v_{t,i} = \left(\hat{t}_i^\mathcal{I}\right)^\top v_i^\mathcal{I}, v_{n,i} = \left(\hat{n}_i^\mathcal{I}\right)^\top v_i^\mathcal{I} $$

Where we define $\mathbf{a} \cdot \mathbf{b} = \mathbf{a}^\top \mathbf{b}$ in general coordinates.

Modeling Wheel Slip

To model slip ratios, we can simply find the difference between rotational and linear velocity, via the following equation:

$$ \kappa_i = \frac{r_w \omega_{w,i}-v_{t,i}}{\max(\epsilon,|v_{t,i}|)} $$

Similarly, for wheel angle slip:

$$ \alpha_i = \mathrm{atan2}(v_{n,i},\max(\epsilon,|v_{t,i}|)) $$

Tire Force Laws

For unsaturated forces, we have:

$$ F_{x,i}^\star = C_\kappa \kappa_i, F_{y,i}^\star = -C_\alpha \alpha_i $$

And for a friction limit, we have the friction cone defined as:

$$ \sqrt{F_{x,i}^2+F_{y,i}^2} \leq \mu F_{z,i} $$

And $F_x$ and $F_y$ are defined in individual wheel frames, and need to be transformed to world axes.

6) Assembling Equations of Motion

For each wheel, we can define the contact force as:

$$ f_i^\mathcal{I} = F_{x,i}\hat{t}_i^\mathcal{I} + F_{y,i}\hat{n}_i^\mathcal{I} + F_{z,i} \hat{z}^\mathcal{I} $$

And the total wheel force will be:

$$ F^\mathcal{I} = \sum_i f_i^\mathcal{I} $$

And similarly, for torque:

$$ \tau^\mathcal{I} = \sum_i (p_i^\mathcal{I} - p^\mathcal{I})\times f_i^\mathcal{I} $$

And then we have to convert it to $\tau^\mathcal{B} = R^\top \tau^\mathcal{I}$, as we use body frame angular velocity along with the wheels, so it's a bit easier to work that way.

From these, we can assemble our full equations of motion:

Translation

$$ \begin{gather*} \dot{p}^\mathcal{I}=v^\mathcal{I} \\ m\dot{v}^\mathcal{I} = F^\mathcal{I} + mg^\mathcal{I} \end{gather*} $$

Rotation

$$ \mathbb{I}\dot{\omega}^\mathcal{B} + \omega^\mathcal{B} \times (\mathbb{I} \omega^\mathcal{B})=\tau^\mathcal{B} $$

Which leads to:

$$ \dot{\omega}^\mathcal{B} = \mathbb{I}^{-1}\left(\tau^\mathcal{B}-\omega^\mathcal{B}\times(\mathbb{I}\omega^\mathcal{B})\right) $$

Furthermore, for wheel dynamics, we also have a similar equation:

$$ I_w \dot{\omega}_{w,i} = \tau_i - r_w F_{x,i} - b_w \omega_{w,i} $$

7) Using Euler Integration

For semi-implicit Euler integration, the process is as follows:

1. Compute forces/torques from $\mathbf{x}_k$
2. Compute accelerations $\dot{v}_k,\dot{\omega}k,\dot{\omega}{w,k}$
3. Update velocities:

$$v_{k+1}=v_k+\dot{v}_k \Delta t, \omega_{k+1}=\omega_k + \dot{\omega}_k \Delta t$$

4. Update pose using velocities (position/orientation)

$$ p_{k+1} = p_k + v_{k+1}\Delta t, R_{k+1} = R_k \mathrm{exp}(\omega_{k+1}\Delta t) $$

[lucaslibshutz]

Things I found weird the first time

Here are some things that I found non-intuitive from a dynamics standpoint, and some subtleties that are important to mention.

1) How does $F_{x,i}=C_\kappa \kappa_i$ and $F_{y,i} = -C_\alpha \alpha_i$?

For a tire, the force is not generated from torque, it is generated from shearing rubber with respect to the ground plane. This is exactly why we are encoding slip ratios in forces, as shear forces come from a mismatch between what the wheel wants to do and how it is actually performing.

For example, if $\kappa=0$, this means that we are rolling without slip. If $\kappa\gt 0$, then the rubber is being stretched backwards, which means that our car is being pushed forwards (and the reverse for $\kappa\lt 0$).

So, we are effectively making a Hooke's Law approximation for the contact, and $C_\kappa$ will have units of Newtons/unit slip.

Similarly, for $\alpha$, the same logic applies, except the negative sign encodes a restorative direction (i.e., if the tire is angled, then the car wants to rotate the other way)

However, this only works until we violate friction, so our constraint is:

$$ \sqrt{F_x^2+F_y^2} \leq \mu F_z $$

Without this, we would get infinite grip, which doesn't make sense. So, if we violate this limit, we simply scale down our individual tire forces to ensure our linear model matches real-world behaviors.

So, $\kappa$ and $\alpha$ are shear deformation rates, while $C_\kappa$ and $C_\alpha$ are stiffnesses, which together form a first-order Taylor expression of tire behavior.

2) How does $\mathrm{exp}(\omega_{k+1}\Delta t)$ encode orientation update?

While for position, we have $p_{k+1} = p_k + v\Delta t$, we cannot do the same for rotation:

$$ R_{k+1} \neq R_k + \dot{R} \delta t $$

This is because our rotation matrix $R$ needs to stay in the group of $SO(3)$, such that we do not violate the properties of the group.

In reality, we define the derivative of the rotation matrix as:

$$ \dot{R} = R[\omega^\mathcal{B}]_\times $$

[JaidenAGrimminck]

ok wait so from what I can understand, the frontend needs to provide chassis position (and orientation), velocity, spin rates of each wheel, and angular velocity? is there anything about terrain that I would need to provide, like friction etc?

[lucaslibshutz]

@JaidenAGrimminck I think just initial conditions should work for the state input for the first time, and for the terrain I think most likely geometry and just the friction coefficient of the surface. The other stuff (i.e. tire parameters, slip constants, etc) should be handled on the sim end

[lucaslibshutz]

Running a little late on this bc of interview prep. Main model is done, just pending testing/other stuff.

[lucaslibshutz]

delaying again unfortunately :(

[lucaslibshutz]

Current progress: I've been trying to debug a test script meant to test a basic control of going straight and turning right, but I've run into a couple of problems. First, it seems the contact parameters that I had for my physics model were initially way too high, causing the wheels to not interact with the ground properly, but now that should be fixed, in cornellev/ackermann-jax@a57159b .

Now, I'm debugging the car not staying straight/one position, which is most likely due to the way in which my code is mapping the wheel/torques forces, but that is just my hypothesis at this point. I'm going to spend the next few days (barring a shit ton of HW) looking into that and performing some unit tests.

[lucaslibshutz]

Current update: I fixed the "jumpiness" issue in the position, as I actually found in this commit that I set a tolerance of $\epsilon=1\times 10^6$ instead of $10^{-6}$. Once I did that, the discrepancy of the front wheel angles function that was initially outputting two different front wheel angles $\delta_{FL}$ and $\delta_{FR}$ was solved, for this line:

    def ackermann_front_angles(self, delta: Array, eps: float = 1e-6) -> Array:

Currenlty, I'm working on fixing some problems while turning, which I'm observing for trying to command a $14º$ angle, with the following plots.

Now I'm working through to see if my tire slip modeling/parameters are the culprit, or if it's the way in which I have represented the inertia of the wheels improperly - will keep posted in this thread.

[lucaslibshutz]

I'm trying to debug more of the longitudal forces and how they're represented, as I've seen some braking force being applied.

[lucaslibshutz]

Ok, this is a bit late, from what this looks like it seems that the physics are working as expected, just the steering control input just needs to be fine tuned, which is most likely a separate project. Everything is up to date in the current main branch, cornellev/ackermann-jax@209821a .

Next steps for me are to work with @smriti-kumar on getting either a UKF/EKF implemented using this framework, which already has a step and xdot function implemented, but needs to handle orientation integration.