This post will review more details in kinematics.
Notations
\[\begin{aligned} q &= \text{Joint state, position} \\ \dot{q} &= \text{Joint velocity} \\ \ddot{q} &= \text{Joint acceleration} \\ X^G &= f_{kin}^G(q) \\ dX^G &= J^G(q)dq \end{aligned}\]Angular Velocity and Rotation Matrix
Firstly, note that $\dot{q}$ contains two component, joint translational velocity and rotational velocity $\omega$.
\[\dot{q} = \begin{equation*}{}^AV^B_C = \begin{bmatrix} {}^A\omega^B_C \\ {}^A\text{v}^B_C \end{bmatrix}.\end{equation*}\]while velocity is straight-forward in 3-d space, angular velocity need proper introduction.
- Elementary School angular velocity: In 2d, angular movement can be parameterized as $(r, \phi)$, where angular velocity is $\dot{\phi}$. While note that the rotation axis is the imaginery z-axis.
- Undergrad angular velocity: In 3d, angular velocity is defined by a cross product of $r$ and $v$, intuitively rotating around the normal axis defined by the plane spanned by $r$ and $v$. It can be parametrized by a vector $(\omega_x, \omega_y, \omega_z)$ and $v = \omega \times r$.
For example, $\omega=(0,0,1)$ and $r=(1,0,0)$, which is $(0,1,0)$.
Now we can connect the angular velocity and rotation matrix. Consider a rotation matrix $R(t)$ that rotate points from a fixed point $p^A$ to $p^B$, both expressed in world frame.
\[p^B (t) = R(t) p^A\]then take derative on both side.
\[v^B = \dfrac{dR(t)}{dt} p^A + R(t) \dfrac{p^A}{dt} = \dot{R(t)} p^A\]While by definition of angular velocity
\[v^B = \omega \times p^B = \omega \times (R(t) p^A) = [\omega ]_{\times} R(t) p^A \quad [\omega]_\times = \begin{bmatrix} 0 & -\omega_z & \omega_y \\ \omega_z & 0 & -\omega_x \\ -\omega_y & \omega_x & 0 \end{bmatrix}\]Hence
\[\dot{R(t)} = [\omega ]_{\times} R(t) \quad [\omega ]_{\times} = \dot{R(t)} R(t)^T\]given $RR^T=I$ for any rotation matrix. We can use a planar robot to illustrate.
import numpy as np
import matplotlib.pyplot as plt
import sympy as sp
# --- Symbolic Verification ---
t = sp.symbols('t')
theta = sp.Function('theta')(t)
# Planar rotation around Z
R = sp.Matrix([
[sp.cos(theta), -sp.sin(theta), 0],
[sp.sin(theta), sp.cos(theta), 0],
[0, 0, 1]
])
# Time derivative of R
R_dot = R.diff(t)
# Compute Omega_skew = R_dot * R.T
Omega_skew = sp.simplify(R_dot * R.T)
print("Symbolic [omega]_x = R_dot * R^T:")
sp.pprint(Omega_skew)
# Symbolic [omega]_x = R_dot * R^T:
# ⎡ d ⎤
# ⎢ 0 -──(θ(t)) 0⎥
# ⎢ dt ⎥
# ⎢ ⎥
# ⎢d ⎥
# ⎢──(θ(t)) 0 0⎥
# ⎢dt ⎥
# ⎢ ⎥
# ⎣ 0 0 0⎦
Note the [0,1] is $-\dot{\theta}$, and we can plot the trajectory like and code below
# ==========================================
# PART 2: Numerical Simulation
# ==========================================
# Setup time and motion profile (Variable angular velocity)
time_steps = np.linspace(0.0*np.pi, 1.0*np.pi, 50)
theta_vals = 0.2 * time_steps**2 # Angle theta(t)
omega_vals = 0.4 * time_steps # Angular velocity omega(t)
# Fixed point in Body Frame (e.g., robot end-effector at rest)
pA = np.array([1.0, 0, 0])
# Lists to store history for plotting
pB_list = [] # Position trajectory
v_from_Rdot = [] # Velocity from R_dot * pA
v_from_Cross = [] # Velocity from omega x pB
for i, th in enumerate(theta_vals):
om = omega_vals[i]
# 1. Numerical Rotation Matrix
R_num = np.array([
[np.cos(th), -np.sin(th), 0],
[np.sin(th), np.cos(th), 0],
[0, 0, 1]
])
# 2. Numerical Derivative of R (R_dot)
# d(cos)/dt = -sin * theta_dot
R_dot_num = np.array([
[-np.sin(th)*om, -np.cos(th)*om, 0],
[ np.cos(th)*om, -np.sin(th)*om, 0],
[ 0, 0, 0]
])
# 3. Calculate Positions and Velocities
pB = R_num @ pA
# Method A: Matrix Derivative
v1 = R_dot_num @ pA
# Method B: Cross Product
omega_vec = np.array([0, 0, om])
v2 = np.cross(omega_vec, pB)
# Store results
pB_list.append(pB)
v_from_Rdot.append(v1)
v_from_Cross.append(v2)
# Convert lists to arrays for easier slicing
pB_arr = np.array(pB_list)
v_Rdot_arr = np.array(v_from_Rdot)
v_Cross_arr = np.array(v_from_Cross)
# ==========================================
# PART 3: Plotting Code
# ==========================================
plt.figure(figsize=(6, 5))
# Plot the path of the point
plt.plot(pB_arr[:, 0], pB_arr[:, 1], 'k--', alpha=0.6, label='Trajectory of pB')
# Plot velocity vectors at specific intervals (every 8th step)
indices = np.arange(0, len(time_steps), 6)
for idx in indices:
# Origin of arrow
x, y = pB_arr[idx, 0], pB_arr[idx, 1]
# Velocity components (scaled by 0.5 for visual clarity)
dx, dy = v_Rdot_arr[idx, 0] * 0.5, v_Rdot_arr[idx, 1] * 0.5
plt.arrow(x, y, dx, dy, head_width=0.05, color='r', length_includes_head=True)
# Add dummy entry for legend to represent the arrows
plt.plot([], [], 'r-', label='Velocity Vector (v)')
# Plot origin
plt.scatter([0], [0], color='black', label='Origin (Joint)')
plt.axis('equal')
plt.title('Planar Robot Motion')
plt.xlabel('X')
plt.ylabel('Y')
plt.legend()
plt.grid(True, linestyle=':', alpha=0.6)
plt.show()