<jupyter_start><jupyter_text>Quadruped Forward/Inverse Kinematics---<NAME>Created: 24 Oct 2016  Denavit-Hartenberg (DH)Development of the DH forward kinematics for a quadruped robot. We will use the DH method to develop the symbolic equations and use python to simplify them.![](https://github.com/walchko/pyGeckoRobots/blob/master/quadruped/robotis/docs/spider.png?raw=true)<jupyter_code>%matplotlib inline
%reload_ext version_information
%version_information numpy, matplotlib
from __future__ import print_function
from __future__ import division
from matplotlib import pyplot as plt
import numpy as np
from sympy import symbols, sin, cos, pi, simplify
from math import radians as d2r
from math import degrees as r2d
from math import atan2, sqrt, acos, fabs
t1, t2, t3 = symbols('t1 t2 t3')
Lc, Lf, Lt = symbols('Lc Lf Lt')<jupyter_output><empty_output><jupyter_text>The following class follows the traditional [DH convention](https://en.wikipedia.org/wiki/Denavit%E2%80%93Hartenberg_parameters). Where:| Parameter | Definition||-----------|-----------|| $\alpha_i$   | angle about common normal, from old z axis to new z axis (twist)|| $d_i$        | offset along previous z to the common normal || $\theta_i$   | angle about previous z, from old x to new x || $a_i$        | length of the common normal. Assuming a revolute joint, this is the radius about previous z. |<jupyter_code>class DH(object):
    def __init__(self):
        pass

    def fk(self, params):
        t = np.eye(4)
        for p in params:
            t = t.dot(self.makeT(*p))
        return t

    def makeT(self, a, alpha, d, theta):
        # create a modified DH homogenious matrix
        # this is the same matrix as above
        return np.array([
            [           cos(theta),           -sin(theta),           0,             a],
            [sin(theta)*cos(alpha), cos(theta)*cos(alpha), -sin(alpha), -d*sin(alpha)],
            [sin(theta)*sin(alpha), cos(theta)*sin(alpha),  cos(alpha),  d*cos(alpha)],
            [                    0,                     0,           0,             1]
        ])
    @staticmethod
    def eom(f, inputs):
        h = []
        for i in range(0, 3):
            tmp = (f[i,3]).subs(inputs)
            h.append(simplify(tmp.evalf()))
        return h
# def eval(f, inputs):
#     h = []
#     for i in range(0, 3):
#         tmp = (f[i,3]).subs(inputs)
#         h.append(tmp.evalf())
#     return h<jupyter_output><empty_output><jupyter_text>The parameters are:| i |$a_i$        | $\alpha_i$   | $d_i$   | $theta_i$  ||---|-------------|--------------|---------|------------|| 1 | $L_{coxa}$  | 0            | 0       | $\theta_1$ || 2 | 0           | 90           | 0       | $\theta_2$ || 3 | $L_{femur}$ | 0            | 0       | $\theta_3$ || 4 | $L_{tibia}$ | 0            | 0       |            |<jupyter_code># a, alpha, d, theta
# params = [
#     [Lc,    0, 0, t1],
#     [ 0, pi/2, 0, t2],
#     [Lf,    0, 0, t3],
#     [Lt,    0, 0,  0]
# ]
params = [
    [ 0,    0, 0, t1],
    [Lc, pi/2, 0, t2],
    [Lf,    0, 0, t3],
    [Lt,    0, 0,  0]
]
dh = DH()
t = dh.fk(params)
t = dh.eom(t,[])
for i in t:
    print(i)
import math
def fk(t1, t2, t3, Lc, Lf, Lt):
    t1 = d2r(t1)
    t2 = d2r(t2)
    t3 = d2r(t3)
#     return np.array([
#         Lc + Lf*math.cos(t1)*math.cos(t2) + Lt*math.cos(t1)*math.cos(t2 + t3),
#         (Lf*math.cos(t2) + Lt*math.cos(t2 + t3))*math.sin(t1),
#         Lf*math.sin(t2) + Lt*math.sin(t2 + t3)
#     ])
    return np.array([
        (Lc + Lf*math.cos(t2) + Lt*math.cos(t2 + t3))*math.cos(t1),
        (Lc + Lf*math.cos(t2) + Lt*math.cos(t2 + t3))*math.sin(t1),
        Lf*math.sin(t2) + Lt*math.sin(t2 + t3)
    ])
print(fk(0,0,0, 1,2,3))
print(fk(0,90,0, 1,2,3))
print(fk(0,0,-90, 1,2,3))
print(fk(90,0,0, 1,2,3))
print(fk(-90,0,0, 1,2,3))
print(fk(90,90,0, 1,2,3))
print(fk(90,45,-90, 1,2,2))
print(fk(90,45,45, 1,2,2))<jupyter_output>[ 6.  0.  0.]
[ 1.  0.  5.]
[ 3.  0. -3.]
[  3.67394040e-16   6.00000000e+00   0.00000000e+00]
[  3.67394040e-16  -6.00000000e+00   0.00000000e+00]
[  6.12323400e-17   1.00000000e+00   5.00000000e+00]
[  2.34423551e-16   3.82842712e+00   0.00000000e+00]
[  1.47827946e-16   2.41421356e+00   3.41421356e+00]<jupyter_text>Inverse KinematicsOften, we need to find the angles of the leg joints given a location (x,y,z) of the foot. To calculate this, remember the leg is composed of 3 revolute joints. The shoulder is in one plane and both the femur joint and tibia joint are in another plane.**insert picture here** Useful Trigonometry![](https://upload.wikimedia.org/wikipedia/commons/thumb/4/49/Triangle_with_notations_2.svg/200px-Triangle_with_notations_2.svg.png)The **law of cosines** for calculating one side of a triangle when the angleopposite and the other two sides are known. Can be used in conjunction withthe law of sines to find all sides and angles. DerivationSo let's start with the shoulder, look down from the top, we see the x-y plane. We can calculate the it's rotation $\theta_1$ by using x and y of the foot location.$$\theta_1 = atan2(y,x)$$Next, let's look at the plane which the next 2 joints lie in. Now we will calculate the $\theta_2$ and $\theta_3$ angles.$$ m = \sqrt{x^2 + y^2} $$$$ f = m - L_{coxa} $$$$ \beta_1 = atan2(f,z) $$$$ d = \sqrt{f^2 + z^2} $$$$ \beta_2 = \arccos( \frac{L_{femur}^2+d^2-L_{tibia}^2}{2 d L_{femur}} ) $$$$ \theta_2 = \beta_1 + \beta_2 $$$$\theta_3 = \arccos( \frac{L_{femur}^2+L_{tibia}^2-d^2}{2 L_{femur} L_{tibia}} ) $$Now finally, you should notice the definition is different between forward and reverse kinematics. So let's fix that ...$$ \theta_{2fk} = \theta_{2ik} - \pi $$$$ \theta_{3fk} = \theta_{3ik} - \pi/2 $$<jupyter_code>def ik(x, y, z, Lc, Lf, Lt):
    t1 = atan2(y, x)
    f = sqrt(x**2 + y**2) - Lc
    
    # depending on z, you have to do a different atan
    if z < 0.0:
        b1 = atan2(f, fabs(z))
    else:
        b1 = atan2(z, f)
    d = sqrt(f**2 + z**2)  
    b2 = acos((Lf**2 + d**2 - Lt**2) / (2.0 * Lf * d))
    t2 = b1 + b2
    t3 = acos((Lf**2 + Lt**2 - d**2) / (2.0 * Lf * Lt))

    t3 -= pi  # fix to align fk and ik frames
    if z < 0.0:
        t3 -= pi/2

    return [r2d(t1), r2d(t2),r2d(t3)]

import math

def cl(a,b,c):
    return acos((c**2-(a**2+b**2))/(-2*a*b))

def ik2(x, y, z, L1, L2, L3):
    t1 = atan2(y, x)
    m = sqrt((x)**2 + y**2)-L1
    e = atan2(z,m)
    print('e',e)
    r = sqrt(m**2+z**2)
    aa = cl(L2,r,L3)
    t2 = aa+e
    gg = cl(L2,L3,r)
    t3 = (math.pi + gg)
    
#     print('alt', r2d(-aa+e), r2d(-(math.pi + gg)))

    return [r2d(t1), r2d(t2),r2d(t3)]
pts = fk(0.0,0.,270., 10, 40, 10)
print('pts: {:.3f} {:.3f} {:.3f}'.format(*pts))

angles = ik2(pts[0], pts[1], pts[2], 10, 40, 10)
print('angles: {:.1f} {:.1f} {:.1f}'.format(*angles))
sa = []
sb = []
sc = []
try:
    for a in range(-45,45,5):
        for b in range(-45,45,5):
            for c in range(200,360,5):
                pts = fk(a,b,c, 10, 40, 100)
                angles = ik2(pts[0], pts[1], pts[2], 10, 40, 100)
                sa.append((a-angles[0]))
                sb.append((b-angles[1]))
                sc.append((c-angles[2]))
#                 if (a-angles[0]) > 10:
#                     print(a,b,c)
except Exception as e:
    print('fuck:', e)
    print(pts)
    print(a,b,c)
plt.subplot(3,1,1)
plt.plot(sa,'r-')
plt.ylabel('Error a')
plt.grid(True);
plt.subplot(3,1,2)
plt.plot(sb,'g-')
plt.ylabel('Error b')
plt.grid(True);
plt.subplot(3,1,3)
plt.plot(sc,'b-')
plt.ylabel('Error c')
plt.grid(True);
plt.plot(sa,'r-')
plt.ylabel('Error a')
plt.grid(True);
plt.plot(sb,'g-')
plt.ylabel('Error b')
plt.plot(sc,'b-')
plt.ylabel('Error c')
plt.grid(True);<jupyter_output><empty_output><jupyter_text>Loading<jupyter_code>def calcTorque(foot, Lc, Lf, Lt):
    """
    s     s
    +-----+--------+ CM
    |              |
    |              v
    |
        torque balance:  s + s = r W/3
    the servos are at location s, the center mass is at CM, the weight is W
    and r is the distance from the foot to CM. The shoulder servo works perpendicular
    to this plane and doesn't help to lift.
    """
    # servo = 0.15  # TG9e servo torque in Nm
    servo = 0.39  # XL-320 servo torque in Nm

    x = 0.001*sqrt(foot[0]**2 + foot[1]**2)  # convert to mm
    r = .144/2 + x

    # 2*servo = r F
    F = 2.0*servo/r  # 2 servos per leg lifting
    return 3.0*F  # 3 legs are lifting at all times

Lc = 40
Lf = 50
Lt = 100
robot_weight = 560
foot = fk(0,0,-45, Lc, Lf, Lt)
print('Foot location {:.3f} {} {:.3f}'.format(*foot))
N = calcTorque(foot, Lc, Lf, Lt)
print('This can lift {:.3f} N'.format(N))

# why the fuck do people confuse mass and weight!!! weight is N and mass is kg
# the the stupid scale I used was in grams for weight ... idiots!!!
G = 1000*N/9.81
print('This can lift {:.3f} gram force'.format(G))  # convert N to grams force
print('The robot is: {} gram force'.format(robot_weight))
print('Factor of safety: {:.2f}'.format(G/robot_weight))
from mpl_toolkits.mplot3d import Axes3D
from matplotlib import cm
from matplotlib.ticker import LinearLocator, FormatStrFormatter
import matplotlib.pyplot as plt
import numpy as np

# fig = plt.figure()
# ax = fig.gca(projection='3d')
x = np.arange(0, 45, 1.0)
y = np.arange(-135, -90, 1.0)
# X, Y = np.meshgrid(X, Y)
# x = range(0, 25)
# y = range(-115, -90)
Z = []
for femur in x:
    z=[]
    for tibia in y:
#         print('{} {}'.format(femur, tibia))
        pos = ik(0, femur, tibia, Lc, Lf, Lt)
        N = calcTorque(pos, Lc, Lf, Lt)
        G = 1000*N/9.81
        z.append(G/robot_weight)
    Z.append(z)

X, Y = np.meshgrid(x, y)
# ax.plot_surface(X, Y, Z, rstride=1, cstride=1, cmap=cm.coolwarm,
#                        linewidth=0, antialiased=False)
# ax.plot_surface(X, Y, Z);
plt.figure()
plt.contour(X,Y,Z)
import matplotlib.pyplot as plt

plt.subplot(2,2,1);
plt.plot(px)
plt.ylabel('x')

plt.subplot(2,2,2)
plt.plot(py)
plt.ylabel('y')

plt.subplot(2,2,3);
plt.plot(pz)
plt.ylabel('z')

plt.subplot(2,2,4);
plt.plot(px,py)
plt.ylabel('y')
plt.xlabel('x');
atan2(-3,4)<jupyter_output><empty_output>
{'fraction_non_alphanumeric': 0.18354618354618354, 'fraction_numerical': 0.06276606276606277, 'mean_word_length': 2.6285919540229883, 'pattern_counts': {'":': 0, '<': 23, '<?xml version=': 0, '>': 22, 'https://': 3, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 7, 'word_list_counts': {'cursed_substrings.json': 6, 'profanity_word_list.json': 1, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'chain_length': '5', 'dir': 'jupyter-structured-clean-dedup', 'id': '387700', 'license': 'MIT', 'n_tokens_mistral': 4532, 'n_tokens_neox': 4123, 'n_words': 1066, 'path': 'docs/DH.ipynb', 'repo_name': 'MultipedRobotics/quadruped-XL320', 'size': '10108'}