# Import Libraries:
import numpy as np
import sympy
from numpy.linalg import inv
import matplotlib.pyplot as plt
from matplotlib.animation import FFMpegWriter
from matplotlib.patches import Rectangle
from matplotlib.patches import Circle
import math
!apt update -y
!apt install ffmpeg -y
Inverse Kinematics Function:
class KinematicTreeNode:
def __init__(self): # Constructor
self.translation = None
self.rotation = None
self.child = None
def RotZ(theta):
c = np.cos(theta)
s = np.sin(theta)
return np.array([[c, -s],[s, c]])
def EndPos(X):
if X.child == None:
return X.translation
else:
return np.column_stack((X.translation,X.translation + np.matmul(RotZ(X.rotation),EndPos(X.child))))
# Takes the base link and returns the end-effector position
def GetAllJoints(X):
if X.child == None:
return X.translation
else:
return np.column_stack((X.translation,X.translation + np.matmul(RotZ(X.rotation),GetAllJoints(X.child))))
def invKinematics(pd,th):
# Loop for Inverse Kinematics with Newton-Raphson
# Step 1: Initial guess
theta_cur = np.array([[th[0,0]],[th[1,0]]]) # avoid singularity for initial guess
for i in range(1,10):
# Step 2: Find f(theta)
base.rotation = theta_cur[0,0]
L1.rotation = theta_cur[1,0]
p = GetAllJoints(base)
# Linkage position array
pa_posX = p[0,2]
pa_posY = p[1,2]
pa = np.vstack((pa_posX,pa_posY)) # Actual end effector position
# Step 3: Find Jacobian of f wrt theta
jac = np.array([[0.0, 0.0],[0.0, 0.0]])
for j in range(0,2):
for k in range(0,2):
delta_theta = np.array([[0.0],[0.0]])
delta_theta[k,0] = 0.01
base.rotation = theta_cur[0,0] + delta_theta[0,0]
L1.rotation = theta_cur[1,0] + delta_theta[1,0]
p_pos_perturb = EndPos(base) # Array of each link's perterbation
p_pos_perturbX = p_pos_perturb[0,2]
p_pos_perturbY = p_pos_perturb[1,2]
p_pos_perturb = np.vstack((p_pos_perturbX,p_pos_perturbY))
theta_step = np.array([[base.rotation],[L1.rotation]])
partial_der = (p_pos_perturb-pa)/0.01
jac[j,k] = partial_der[j]
# Step 4: Find delta theta
delta_theta_step = np.matmul(inv(jac),(pd-pa))
theta_cur = theta_cur + delta_theta_step
return theta_cur
# Giving bas, L1, L2 acces to .translate, .rotate, and .child
base = KinematicTreeNode()
L1 = KinematicTreeNode()
L2 = KinematicTreeNode()
# Initializing each joint:
base.translation = np.array([[0],[0]]) # translation to the joint
base.rotation = np.pi # rotation of the joint
L1.translation = np.array([[1],[0]])
L1.rotation = np.pi/2
L2.translation = np.array([[1],[0]])
L2.rotation = 0
# Defining children of each joint:
base.child = L1
L1.child = L2
L2.child = None
Getting x, th1, th2 values:
# Define Time Vector
dt = 0.001 # Time Step (s)
sim_time = 5 # Length of Simulation (s)
t_vec = np.arange(0, sim_time, dt)
vec_size = len(t_vec)
#print(vec_size)
half_vec_size = int(vec_size/2)
#print(half_vec_size)
P = EndPos(base)
L2_p = P[:,2]
y = -1.65
x_start = -0.70
y_start = y
x_end = 0.70
y_end = y_start
start = np.array([[x_start],[y_start]])
end = np.array([[x_end],[y_end]])
dx = end[0,0] - start[0,0]
dy = end[1,0] - start[1,0]
D = dx
r = D/2
center = np.array([[dx/2],[y]])
x = np.zeros(half_vec_size)
y = np.zeros(half_vec_size)
r = np.zeros(half_vec_size)
th = np.array([[np.pi/4],[np.pi/2]]) # Initial Guess
th1 = np.zeros(vec_size) # Will be used for graphs
th2 = np.zeros(vec_size)
phi = np.linspace(0,np.pi,half_vec_size)
#phi = phi[::-1]
a = dx/2
b = 0.5
# CREATING THE ELLIPSE PATH FOR FOOT
for i in range(0,half_vec_size):
r[i] = ( 1/( ((np.cos(phi[i])**2)/(a**2)) + ((np.sin(phi[i])**2)/(b**2)) ) )**(1/2)
x[i] = r[i]*np.cos(phi[i])
y[i] = r[i]*np.sin(phi[i]) + y_start
pdx = x[i]
pdy = y[i]
pd = np.array([[pdx], [pdy]]) # Array holding the x,y components of the endEff at each iteration
th = invKinematics(pd,th) # Grabing the thetas at each iteration
th1[i] = th[0,0]
th2[i] = th[1,0]
th = np.array([[th1[i]], [th2[i]]])
# DEFINGING VARIABLES FOR THE FLAT PATH OF FOOT
x_flat_start = 0.70
y_flat_start = -1.65
x_flat_end = -0.70
y_flat_end = -1.65
flat_start = np.array([[x_start],[y_start]])
flat_end = np.array([[x_end],[y_end]])
dx_flat = flat_end[0,0] - flat_start[0,0]
dy = end[1,0] - start[1,0]
x_flat = np.arange(flat_start[0,0], flat_end[0,0], dx_flat/half_vec_size)
# Setting initial condition for th1 and th2
th = np.array([[th1[half_vec_size-1]],[th2[half_vec_size-1]]])
# CREATING THE FLAT PATH FOR FOOT
for i in range(0,half_vec_size):
pdx = x_flat[i]
pdy = y_start
pd = np.array([[pdx], [pdy]]) # Array holding the x,y components of the endEff at each iteration
th = invKinematics(pd,th) # Grabing the thetas at each iteration
th1[half_vec_size+i] = th[0,0]
th2[half_vec_size+i] = th[1,0]
th = np.array([[th1[half_vec_size+i]], [th2[half_vec_size+i]]])
# FINALIZING x & y ARRAYS
y_flat = y_flat_start*np.ones(half_vec_size)
x_final = np.concatenate((x, x_flat), axis=None) # Concatenates ellips and flat values
y_final = np.concatenate((y, y_flat), axis=None) # Concatenates ellips and flat values
t_vec_NEW = np.arange(0, sim_time, dt)
# Plot all theta values over time:
plt.plot(x_final, y_final)
plt.title("x vs. y")
plt.axis("equal")
plt.show()
plt.plot(t_vec_NEW, th1)
plt.title("th1 vs. time")
plt.show()
plt.plot(t_vec_NEW, th2)
plt.title("th2 vs. time")
plt.show()
Dynamics using Lagrangian EOM solved symbolically
# Dynamics using Lagrangian EOM solved symbolically
# Redefining angular values from kinematics to desired values for dynamics
x = x_final
y = y_final
th1_des = np.zeros(vec_size)
th2_des = np.zeros(vec_size)
for i in range(0,vec_size):
th1_des[i] = th1[i]
th2_des[i] = th2[i]
# Pendulum Parameters:
# For this model, treat the system as a double pendulum to represent the legs
m_1 = 1 # kg
m_2 = 1 # kg
l_1 = 1 # m
l_2 = 1 # m
g = 9.8 # m/s^2
# Create Symbols for Time:
t = sympy.Symbol('t') # Creates symbolic variable t
tau1 = sympy.Symbol('tau1')
tau2 = sympy.Symbol('tau2')
# Create Generalized Coordinates as a function of time: q = [theta_1, theta_2, theta_3]
th1 = sympy.Function('th1')(t)
th2 = sympy.Function('th2')(t)
# Position Equation: r = [x, y]
r1 = np.array([l_1 * sympy.sin(th1), -l_1 * sympy.cos(th1)]) # Position of first pendulum
r2 = np.array([l_2 * sympy.sin(th2) + r1[0], -l_2 * sympy.cos(th2) + r1[1]]) # Position of second pendulum
# Velocity Equation: d/dt(r) = [dx/dt, dy/dt]
v1 = np.array([r1[0].diff(t), r1[1].diff(t)]) # Velocity of first pendulum
v2 = np.array([r2[0].diff(t), r2[1].diff(t)]) # Velocity of second pendulum
# Energy Equations:
T = (1/2 * m_1 * np.dot(v1, v1)) + (1/2 * m_2 * np.dot(v2, v2)) # Kinetic Energy
V = m_1 * g * r1[1] + m_2 * g * r2[1] # Potential Energy
L = T - V # Lagrangian
# Lagrange Terms:
dL_dth1 = L.diff(th1)
dL_dth1_dt = L.diff(th1.diff(t)).diff(t)
dL_dth2 = L.diff(th2)
dL_dth2_dt = L.diff(th2.diff(t)).diff(t)
# Euler-Lagrange Equations: dL/dq - d/dt(dL/ddq) = 0
th1_eqn = dL_dth1 - dL_dth1_dt - tau1
th2_eqn = dL_dth2 - dL_dth2_dt - tau2
# Replace Time Derivatives and Functions with Symbolic Variables:
ddth1 = sympy.Symbol('ddth1')
ddth2 = sympy.Symbol('ddth2')
replacements = [(th1.diff(t).diff(t), sympy.Symbol('ddth1')), (th1.diff(t), sympy.Symbol('dth1')), (th1, sympy.Symbol('th1')),
(th2.diff(t).diff(t), sympy.Symbol('ddth2')), (th2.diff(t), sympy.Symbol('dth2')), (th2, sympy.Symbol('th2'))]
th1_eqn = th1_eqn.subs(replacements)
th2_eqn = th2_eqn.subs(replacements)
r1 = r1[0].subs(replacements), r1[1].subs(replacements)
r2 = r2[0].subs(replacements), r2[1].subs(replacements)
# Simplfiy then Force SymPy to Cancel factorization: [Sometimes needed to use .coeff()]
th1_eqn = sympy.simplify(th1_eqn)
th2_eqn = sympy.simplify(th2_eqn)
th1_eqn = th1_eqn.cancel()
th2_eqn = th2_eqn.cancel()
# Solve for control torques
tau1 = sympy.solve(th1_eqn,tau1)
tau2 = sympy.solve(th2_eqn,tau2)
# Solve for accelerations
ddth1 = sympy.solve(th1_eqn,ddth1)
ddth2 = sympy.solve(th2_eqn,ddth2)
# Generate Lambda Functions for Torque and Position Equations:
replacements = (sympy.Symbol('th1'), sympy.Symbol('dth1'), sympy.Symbol('ddth1'), sympy.Symbol('th2'), sympy.Symbol('dth2'), sympy.Symbol('ddth2'))
tau1 = sympy.utilities.lambdify(replacements, tau1, "numpy")
tau2 = sympy.utilities.lambdify(replacements, tau2, "numpy")
replacements = (sympy.Symbol('th1'), sympy.Symbol('dth1'), sympy.Symbol('ddth1'), sympy.Symbol('tau1'), sympy.Symbol('th2'), sympy.Symbol('dth2'), sympy.Symbol('ddth2'), sympy.Symbol('tau2'))
ddth1 = sympy.utilities.lambdify(replacements, ddth1, "numpy")
ddth2 = sympy.utilities.lambdify(replacements, ddth2, "numpy")
replacements = (sympy.Symbol('th1'), sympy.Symbol('th2'))
r1 = sympy.utilities.lambdify(replacements, r1, "numpy")
r2 = sympy.utilities.lambdify(replacements, r2, "numpy")
# Simulate Dynamic System Control:
# Initial: th1, dth1, th2, dth2, th3, dth3
x0 = th1_des[0], 0.0, th2_des[0], 0.0
# Control gains: These have not yet been tuned for optimum perfomance
Kp = 150
Kd = 10
# Initialize Arrays:
th1_vec = np.zeros(vec_size)
dth1_vec = np.zeros(vec_size)
th2_vec = np.zeros(vec_size)
dth2_vec = np.zeros(vec_size)
# Evaluate Initial Conditions:
th1_vec[0] = x0[0]
dth1_vec[0] = x0[1]
th2_vec[0] = x0[2]
dth2_vec[0] = x0[3]
# Inverse Dynamics:
for i in range(1, vec_size):
# PD Control of accelerations
ddtheta1 = Kp*(th1_des[i-1]-th1_vec[i-1]) + Kd*(0-dth1_vec[i-1])
ddtheta2 = Kp*(th2_des[i-1]-th2_vec[i-1]) + Kd*(0-dth2_vec[i-1])
# Solving for torques
t1 = tau1(th1_vec[i-1], dth1_vec[i-1], ddtheta1, th2_vec[i-1], dth2_vec[i-1], ddtheta2)
t2 = tau2(th1_vec[i-1], dth1_vec[i-1], ddtheta1, th2_vec[i-1], dth2_vec[i-1], ddtheta2)
# Update theta from Euler step
th1_vec[i] = th1_vec[i-1] + dth1_vec[i-1]*dt
th2_vec[i] = th2_vec[i-1] + dth2_vec[i-1]*dt
# Informed update of velcoity value from EOM and tau
accth1 = ddth1(th1_vec[i-1], dth1_vec[i-1], ddtheta1, t1[0], th2_vec[i-1], dth2_vec[i-1], ddtheta2, t2[0])
accth2 = ddth2(th1_vec[i-1], dth1_vec[i-1], ddtheta1, t1[0], th2_vec[i-1], dth2_vec[i-1], ddtheta2, t2[0])
dth1_vec[i] = dth1_vec[i-1] + accth1[0]*dt
dth2_vec[i] = dth2_vec[i-1] + accth2[0]*dt
Animation: Walking Motion (1 cycle)
!apt update -y
!apt install ffmpeg -y
import math
import matplotlib.pyplot as plt
from matplotlib.animation import FFMpegWriter
from matplotlib.patches import Rectangle
from matplotlib.patches import Circle
#*************************************************
# Create animation
# Setup figure:
fig, ax = plt.subplots()
p, = ax.plot([],[],color='dimgray')
q, = ax.plot([],[],color='dimgray')
min_lim = -3
max_lim = 3
ax.axis('equal')
ax.set_xlim([min_lim, max_lim])
ax.set_ylim([min_lim, max_lim])
ax.set_xlabel('X')
ax.set_ylabel('Y')
ax.set_title('Robot Dog Leg motion')
title = "Robot Dog Leg motion"
# Setup animation writer
FPS = 20
sample_rate = int(1/(dt*FPS)) # Real time playback
dpi = 300
writerObj = FFMpegWriter(fps=FPS)
# Initialize Patch: (Base 1)
width = 0.5 # Width of Cart
height = width/2 # Height of Cart
xy_cart = (-width/2, -height/2) # Bottom Left Corner of Cart
r = Rectangle(xy_cart, width, height, color='black') # Rectangle Patch
ax.add_patch(r) # Add Patch to Plot
P = EndPos(base)
P_L2 = P[:,2]
xfoot = P_L2[0]
yfoot = P_L2[1]
# Initialize Patch: (Foot 1)
c1 = Circle((-xfoot,yfoot), radius=0.1, color='cornflowerblue') # Draws a circle of radius 0.1 at (X, Y) location (0, 0)
ax.add_patch(c1) # Add the patch to the axes
# Draw the Ground:
ground = ax.hlines(y_start - 0.1, -100, 100, colors='black')
height_hatch = 0.25
width_hatch = 200
xy_hatch = (-100, y_start-height_hatch-0.1)
ground_hatch = Rectangle(xy_hatch, width_hatch, height_hatch, facecolor='None', linestyle='None', hatch='/')
ax.add_patch(ground_hatch)
# Plot and Create Animation
with writerObj.saving(fig, title+".mp4", dpi):
# Sample only a few frames
for i in range(0, vec_size, sample_rate):
# thetas from Inverse Dynamics
base.rotation = th1_vec[i]
L1.rotation = th2_vec[i]
P = EndPos(base)
# Defining each point along kinematic tree
P_base = P[:,0]
P_L1 = P[:,1] # Position of leg 1
P_L2 = P[:,2] # Position of leg 2
# Defining and drawing each link
x1 = [P_base[0], P_L1[0], P_L2[0]]
y1 = [P_base[1], P_L1[1], P_L2[1]]
p.set_data(x1, y1)
# Foot Patch:
foot_center = P_L2[0], P_L2[1] # x,y components of the 2nd leg (end-effector)
c1.center = foot_center
'''
# Hatch Patch: Makes the system look as if it is moving horizontally
xy_hatch = -100 - x[i], y_start-height_hatch-0.1
ground_hatch.set(xy=xy_hatch)
'''
fig.canvas.draw()
writerObj.grab_frame()
plt.show()
from IPython.display import Video
Video("/work/Robot Dog Leg motion.mp4",embed=True,width=640,height=480)