# ---
# jupyter:
#   jupytext:
#     text_representation:
#       extension: .py
#       format_name: light
#       format_version: '1.5'
#       jupytext_version: 1.14.4
#   kernelspec:
#     display_name: Python 2
#     language: python
#     name: python2
# ---

# Dynamics of offset diff drive vehicle
#
# Equations are from the paper written by <NAME> etal.
#
# https://www.jstage.jst.go.jp/article/jrsj1983/18/8/18_8_1166/_pdf
#
# This notebook is written by <NAME>

# # Preparation
#
# We use sympy to transform the equation:

# !sudo pip install -U sympy

from sympy import *

init_printing()

# # Symbols
#
# Following symbols are used to describe internal state of the vehicle:

(r, s, W, theta) = symbols(r"r s W \theta_s")

# $r$ : radius of each wheels
#
# $W$: wheel separation
#
# $s$ : wheel offset
#
# $\theta_s$ : current angle of the steering joint (described later)

# Joint space parameters:

(wr, wl, ws) = symbols('w_r w_l w_s')
uv = Matrix([wr, wl, ws])

# $w_r$ : angular velocity of the right wheel joint
#
# $w_l$ : angular velocity of the left wheel joint
#
# $w_s$: angular velocity of the steering joint

# Cartesian space parameters:

(vx, vy, vz) = symbols('v_x v_y v_z')
xv = Matrix([vx, vy, vz])

# $v_x$ : x axes velocity
#
# $v_y$ : y axes velocity
#
# $v_z$ : z axes velocity

# # Dynamics of diff drive vehicle

diff_drive = Matrix([
    [r/2, r/2],
    [0, 0],
    [r/W, -r/W]
])
Eq(xv, MatMul(diff_drive, Matrix(uv[0:2]), evaluate=False), evaluate=False)

# x axes velocity is generated from the combination of left and right wheel velocities.
#
# z axes velocity is generated from the difference of left and right wheel velocities.
#
# We can natually understand that we cannot directly obtain y axes velocity from the mechanism.

# # Dynamics of offset diff drive vehicle
#
# Here, we apply offset to the diff drive vehicle described above.
#
# We first, create offset transformation matrix:

offset = Matrix([
    [1, 0, 0],
    [0, 1, s],
    [0, 0, 1]
])
offset

# Apply offset to the jacobian matrix:

offset_diff_drive = offset * diff_drive
Eq(offset_diff_drive, MatMul(offset, diff_drive, evaluate=False), evaluate=False)

# Now, the equation will be as below:

Eq(xv, MatMul(offset_diff_drive, Matrix(uv[0:2]), evaluate=False), evaluate=False)

# What is interesting here, is we get y axes velocity by just adding the offset!
#
# By calculating the inverse matrix, we can realize omnidirectional movement from this mechanism:

inv_offset_diff_drive = Matrix(offset_diff_drive[0:2, :]).inv()
Eq(Matrix(uv[0:2]), MatMul(inv_offset_diff_drive, Matrix(xv[0:2]), evaluate=False), evaluate=False)

# Unfortunately z axes velocity is defined by holonomic constraints and not controllable by us. But we will add a steering joint to our vehicle to realize complete omnidirectional movement.

# # Dynamics of offset diff drive vehicle with a steering joint
#
# Here, we will add a steering joint to our vehicle to get controllable z axes velocity:

rotate = Matrix([
    [cos(theta), -sin(theta), 0],
    [sin(theta), cos(theta), 0],
    [0, 0, 1]
])
rotate

# By applying the rotation, jacobian matrix will be as follows:

abs_offset_diff_drive = rotate * offset_diff_drive
Eq(abs_offset_diff_drive, MatMul(rotate, offset_diff_drive, evaluate=False), evaluate=False)

# Final jacobian matrix is as follows:

fwd_dynamics = BlockMatrix([abs_offset_diff_drive, Matrix([0, 0, -1])]).as_explicit()
Eq(xv,MatMul(fwd_dynamics, uv, evaluate=False), evaluate=False)

# Inverse jacobian matrix will let us know how to calculate each joint velocity to realize omnidirectional movement:

inv_dynamics = fwd_dynamics.inv().simplify()
Eq(uv, MatMul(inv_dynamics, xv, evaluate=False), evaluate=False)



StarcoderdataJupyterScriptsDedupFiltered