Inverse Dynamics

This document details the inverse dynamics formulation for the planar serial chain mechanism. Inverse dynamics answers the question: "What forces/torques are required to produce a desired acceleration?"

1. Mass Properties

For each link $i$, we define: - $m_i$: Mass of the link. - $(x_{Gi}, y_{Gi})$: Position of the center of mass (CoM). - $I_i$: Moment of inertia about the CoM.

The CoM is defined relative to the joint frame by offsets $r_{ix}$ (longitudinal) and $r_{iy}$ (perpendicular). $$ \begin{aligned} x_{Gi} &= x_{i-1} + r_{ix} \cos \theta_i - r_{iy} \sin \theta_i \ y_{Gi} &= y_{i-1} + r_{ix} \sin \theta_i + r_{iy} \cos \theta_i \end{aligned} $$

The velocity and acceleration of the CoM ($\\dot{x}_{Gi}, \\dot{y}_{Gi}, \\ddot{x}_{Gi}, \\ddot{y}_{Gi}$) are obtained by differentiating these positions with respect to time.

2. Equation of Motion for a Single Link

The dynamics of the $i$-th link are governed by:

\[ \begin{aligned} m_i \\ddot{x}_{Gi} &= f_{Gix} \\ m_i \\ddot{y}_{Gi} &= f_{Giy} \\ I_i \\ddot{\\theta}_i &= n_{Gi} \end{aligned} \]

where: - $(f_{Gix}, f_{Giy})$ is the net force acting on the CoM. - $n_{Gi}$ is the net torque acting on the link.

3. Force Analysis

Three types of forces act on each segment: 1. Actuation Force/Torque ($\\tau_i$): Generated by motors at the joints. 2. Constraint Force ($f_{ix}, f_{iy}$): Forces at the joints maintaining connection between links. 3. External Force ($f_{Eix}, f_{Eiy}$): Forces from the environment acting at a point $(x_{Ei}, y_{Ei})$.

The net force and torque on link $i$ are the sum of these interactions. Note that link $i$ is connected to link $i-1$ (at joint $i$) and link $i+1$ (at joint $i+1$).

Force Balance: $$ \begin{aligned} f_{Gix} &= f_{ix} - f_{(i+1)x} + f_{Eix} \ f_{Giy} &= f_{iy} - f_{(i+1)y} + f_{Eiy} \end{aligned} $$

Torque Balance: $$ \begin{aligned} n_{Gi} &= \tau_i - \tau_{i+1} \ &\quad + (x_{i-1} - x_{Gi})f_{iy} - (y_{i-1} - y_{Gi})f_{ix} \quad \text{(Force from joint } i) \ &\quad - (x_i - x_{Gi})f_{(i+1)y} + (y_i - y_{Gi})f_{(i+1)x} \quad \text{(Force from joint } i+1) \ &\quad + (x_{Ei} - x_{Gi})f_{Eiy} - (y_{Ei} - y_{Gi})f_{Eix} \quad \text{(External force)} \end{aligned} $$

4. Recursive Computation (Inverse Dynamics)

To find the joint torques $\\tau_i$, we compute recursively backward from the tip ($n$) to the base ($1$).

Base conditions (Tip): $$ f_{(n+1)x} = 0, \quad f_{(n+1)y} = 0, \quad \tau_{n+1} = 0 $$

Recursive step (for $i = n, \dots, 1$): First, solve for the constraint forces at joint $i$ required to support the link's motion and the forces from link $i+1$:

\[ \begin{aligned} f_{ix} &= m_i \\ddot{x}_{Gi} + f_{(i+1)x} - f_{Eix} \\ f_{iy} &= m_i \\ddot{y}_{Gi} + f_{(i+1)y} - f_{Eiy} \end{aligned} \]

Then, solve for the actuation torque $\\tau_i$:

\[ \begin{aligned} \\tau_i &= I_i \\ddot{\\theta}_i + \\tau_{i+1} \\ &\\quad - (x_{i-1} - x_{Gi})f_{iy} + (y_{i-1} - y_{Gi})f_{ix} \\ &\\quad + (x_i - x_{Gi})f_{(i+1)y} - (y_i - y_{Gi})f_{(i+1)x} \\ &\\quad - (x_{Ei} - x_{Gi})f_{Eiy} + (y_{Ei} - y_{Gi})f_{Eix} \end{aligned} \]

Or equivalently, using the joint positions directly in the torque balance:

$$ \begin{aligned} \tau_i &= n_{Gi} + \tau_{i+1} \ &\quad - (x_i - x_{i-1}) f_{(i+1)y} + (y_i - y_{i-1}) f_{(i+1)x} \ &\quad + (x_{Ei} - x_{i-1}) f_{Eiy} - (y_{Ei} - y_{i-1}) f_{Eix} \ &\quad - (x_{Gi} - x_{i-1}) f_{Giy} + (y_{Gi} - y_{i-1}) f_{Gix} \end{aligned} $$ (Note: The exact formulation in code may vary slightly in term grouping, but follows Newton-Euler logic.)

5. Implementation

The function skeleton_update_force implements this backward recursion. 1. Ensure kinematics (acceleration of CoM) are updated first. 2. Iterate from $i=n$ down to $1$. 3. Compute net forces/torques required ($f_{Gi}, n_{Gi}$) from Newton's laws. 4. Compute joint constraint forces $(f_{ix}, f_{iy})$. 5. Compute joint torque $\\tau_i$.