In [108]:
%matplotlib inline
In [109]:
from __future__ import print_function
from __future__ import division
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
In [110]:
t1, t2, t3 = symbols('t1 t2 t3')
Lc, Lf, Lt = symbols('Lc Lf Lt')
The following class follows the traditional DH convention. 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. |
In [111]:
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):
return np.array([ # classic DH
[cos(theta), -sin(theta) * cos(alpha), sin(theta) * sin(alpha), cos(theta) * a],
[sin(theta), cos(theta) * cos(alpha), -cos(theta) * sin(alpha), sin(theta) * a],
[ 0, sin(alpha), cos(alpha), d],
[ 0, 0, 0, 1]
])
In [112]:
def eval(f, inputs):
h = []
for i in range(0, 3):
tmp = (f[i,3]).subs(inputs)
h.append(tmp.evalf())
return h
The parameters are:
| i | $a_i$ | $\alpha_i$ | $d_i$ | $theta_i$ |
|---|---|---|---|---|
| 1 | $L_{coxa}$ | 90 | 0 | $\theta_1$ |
| 2 | $L_{femur}$ | 0 | 0 | $\theta_2$ |
| 3 | $L_{tibia}$ | 0 | 0 | $\theta_3$ |
In [113]:
# a, alpha, d, theta
params = [
[Lc, pi/2, 0, t1],
[Lf, 0, 0, t2],
[Lt, 0, 0, t3]
]
dh = DH()
t = dh.fk(params)
t = eval(t,[])
In [114]:
for i in t:
print(simplify(i))
In [115]:
def fk(t1, t2, t3, Lc, Lf, Lt):
t1 = d2r(t1)
t2 = d2r(t2)
t3 = d2r(t3)
return np.array([
(Lc + Lf*cos(t2) + Lt*cos(t2 + t3))*cos(t1),
(Lc + Lf*cos(t2) + Lt*cos(t2 + t3))*sin(t1),
Lf*sin(t2) + 1.0*Lt*sin(t2 + t3)
])
Often, 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
The law of cosines for calculating one side of a triangle when the angle opposite and the other two sides are known. Can be used in conjunction with the law of sines to find all sides and angles.
So 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 $$
In [116]:
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)]
In [117]:
pts = fk(0.0,45.,-60., 10., 40., 100.)
# print(type(pts[1]))
print('pts: {:.3f} {} {:.3f}'.format(*pts))
angles = ik(pts[0], pts[1], pts[2], 10, 40, 100)
print('angles: {:.1f} {:.1f} {:.1f}'.format(*angles))
In [118]:
# from random import uniform
# for i in range(10):
# a=uniform(-90,90)
# b=uniform(0,90)
# c=uniform(-180,0)
# pts = fk(a,b,c, 10, 40, 100)
# angles = ik(pts[0], pts[1], pts[2], 10, 40, 100)
# if angles:
# if a-angles[0]>0.1 or b-angles[1]>0.1 or c-angles[2]>0.1:
# print('in: {:.1f} {:.1f} {:.1f} out: {:.1f} {:.1f} {:.1f}'.format(a,b,c, *angles))
In [119]:
# def printError(pts, pts2, angles, angles2):
# print('****************************************************')
# print('angles (orig):', angles)
# print('angles2 from ik(pts): {:.2f} {:.2f} {:.2f}'.format(*angles2))
# print('pts from fk(orig): {:.2f} {:.2f} {:.2f}'.format(*pts))
# print('pts2 from fk(angle2): {:.2f} {:.2f} {:.2f}'.format(*pts2))
# print('diff [deg]: {:.2f}'.format(np.linalg.norm(np.array(angles) - np.array(angles2))))
# print('diff [mm]: {:.2f}'.format(np.linalg.norm(pts - pts2)))
# print('\nExiting\n')
# print('****************************************************')
# def fk_ik():
# cox = 10
# fem = 40
# tib = 100
# for a in range(-45, 45, 5):
# for b in range(0,90,5):
# for g in range(-90,0,5):
# print('------------------------------------------------')
# a1 = [a,b,g]
# pts = fk(a1[0],a1[1],a1[2], cox, fem, tib)
# a2 = ik(pts[0], pts[1], pts[2], cox, fem, tib)
# pts2 = fk(a2[0], a2[1], a2[2], cox, fem, tib)
# print('points:', pts)
# # print(type(pts))
# # print(pts - pts2)
# # print(np.linalg.norm(pts - pts2))
# # print(a1)
# # print(type(a1))
# # print(a2)
# # print(type(a2))
# angle_error = np.linalg.norm(np.array(a1) - np.array(a2))
# pos_error = np.linalg.norm(pts - pts2)
# # print(angle_error, pos_error)
# if angle_error > 0.0001:
# print('Angle Error')
# printError(pts, pts2, angles, angles2)
# exit()
# elif pos_error > 0.0001:
# print('Position Error')
# printError(pts, pts2, angles, angles2)
# exit()
# else:
# print('Angle: {:.1f} {:.1f} {:.1f}'.format(angles[0], angles[1], angles[2]))
# print('Pos: {:.1f} {:.1f} {:.1f}'.format(pts[0], pts[1], pts[2]))
# print('Error(deg,mm): {:.2f} {:.2f}\n'.format(angle_error, pos_error))
# fk_ik()
In [142]:
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))
In [138]:
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)
Out[138]:
In [ ]:
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');
In [141]:
# X = np.arange(-5, 5, 1)
# Y = np.arange(-5, 5, 1)
# X, Y = np.meshgrid(X, Y)
# Z = X*Y**2
print(min(Z))
print(max(Z))
In [136]:
# print(Z.shape)
print(Z)
This work is licensed under a Creative Commons Attribution-ShareAlike 4.0 International License.