def generate_symmetric_curve(t=None,
x_func=n.cos,
y_func=n.sin,
curve_factor=2 * n.pi,
freq=1,
ampl_factor=0.1,
num_points=50,
offset=0):
"""
Genrates a Homogenous point-curve with z=0 in the following matrix form:
[[x1, y1, 0, 1},
..... ,
[xn, yn, 0, 1}]
in local (unransformed) plane-coordinates in metres.
parameters:
curve_factor : ...
freq : ...
ampl_factor : 'diameter' of the curve
num_points : ...
"""
if t is None:
t = n.linspace(0, 1, num_points)
le = len(t)
y = y_func(t / t[-1] * curve_factor * freq)
y = y / n.max(n.abs(y))
x = x_func(t / t[-1] * curve_factor * freq)
x = x / n.max(n.abs(x))
ampl_factor = ampl_factor / 2.0
point_matrix = mat(
zip(x * ampl_factor, y * ampl_factor, n.zeros(le), n.ones(le)))
return point_matrix
def generate_curve(xmin=-0.25,
xmax=0.25,
x_func=None,
y_func=n.sin,
curve_factor=n.pi,
freq=1,
ampl_factor=0.25,
num_points=50,
offset=0):
"""
Genrates a Homogenous point-curve with z=0 in the following matrix form:
[[x1, y1, 0, 1},
..... ,
[xn, yn, 0, 1}]
in local (unransformed) plane-coordinates in metres.
"""
if x_func is None:
if xmin > xmax:
xmin, xmax = xmax, xmin
x_func = n.linspace(xmin, xmax, num_points)
le = len(x_func)
y = y_func(x_func / n.abs(xmax) * curve_factor * freq)
if n.max(n.abs(y)) > 0:
y = y / n.max(n.abs(y))
point_matrix = mat(
zip(x_func + offset, y * ampl_factor, n.zeros(le), n.ones(le)))
return point_matrix
def filter_solutions(solutions, filter_function = check_solution):
result = []
for s in solutions.T:
if filter_function(*s) == True:
result.append( s )
# returns non-flipped, flipped
return mat(zip(*result))
def find_single_path(ik_curve):
result = ik_curve
total = []
with utils.timing.Timer() as t:
for index in xrange(len(result[0])):
path = __find_path(result, index)
if path:
return mat(path)
def test_forward_kinematics_general(self):
print '\ntest_forward_kinematics_general'
for counter in xrange(10000):
fcounter = (counter / 10000.0)*100
if fcounter % 1.0 == 0.0:
print fcounter
j1 = rand_range(-180, 180)
j2 = rand_range(-90, 110)
j3 = rand_range(-230, 50)
j4 = rand_range(-200, 200)
j5 = rand_range(-115, 115)
j6 = rand_range(-400, 400)
# makes sure we never end up at a singular point
while (abs(j3) - 90) < 1e-7:
j3 = rand_range(-230, 50)
s0 = j1,j2,j3,j4,j5,j6
robot_info = forward_kinematics(j1, j2, j3, j4, j5, j6, **DH_TABLE)
T44, debug1 = robot_info['flange'], robot_info['robot_geometry_local']
while norm(calc_wcp(T44,L=0.065)[:2]) < 1e-7:
j2 = rand_range(-90, 110)
T44, debug1 = forward_kinematics(j1, j2, j3, j4, j5, j6, **DH_TABLE)
sol = mat( inverse_kinematics_irb140(DH_TABLE, T44) )
num_valid_solutions = 0
for s in sol.T:
robot_info = forward_kinematics(*s, **DH_TABLE)
A, debug2 = robot_info['flange'], robot_info['robot_geometry_local']
num_valid_solutions += check_solution(*s)
error = norm(A - T44)
if not n.isnan(error):
self.assertAlmostEqual(error, 0)
self.assertGreaterEqual(num_valid_solutions, 1)
self.assertEqual(num_valid_solutions, calc_valid_raw_invkin_irb140(T44).shape[0])
L = []
for s in iterdim(sol,1):
if check_solution(*s) == True:
L.append(s)
L = mat(L)
self.assertTrue(norm(calc_valid_raw_invkin_irb140(T44) - L) == 0.0)
def plot_robot_geometry(robot_info, color='k'):
global_robot_frames = mat(robot_info['robot_geometry_global'])
plot(global_robot_frames[:, 0, 3],
global_robot_frames[:, 2, 3],
color,
linewidth=2)
tool_pos = nzip(robot_info['tcp'][:3, 3], robot_info['flange'][:3, 3]).T
plot(tool_pos[:, 0], tool_pos[:, 2], color='g', linewidth=3)
def find_paths(ik_curve):
result = ik_curve
total = []
with utils.timing.Timer() as t:
for index in xrange(len(result[0])):
path = __find_path(result, index)
if path:
print 'FOUND ONE'
total.append(list(path))
return mat(total)
def __inverse_kinematics_pose(T44, filtering=False, raw_solutions=False):
# perform inverse kinematics on a single frame
angle_solutions = inverse_kinematics_irb140(DH_TABLE, T44)
if not raw_solutions:
extra = [angle_solutions]
for index in xrange(6):
extra.append( generate_modulo_solutions(angle_solutions, index, 360.0))
extra.append( generate_modulo_solutions(angle_solutions, index, -360.0))
pass
angle_solutions = merge_solutions(*extra)
if filtering:
angle_solutions = filter_solutions(angle_solutions)
return mat(angle_solutions.T)
def inverse_kinematics_irb140(dh_table, T44):
'''
Wrapper function that calculates the 20 analytical solutions
to the IRB140 robot.
'''
if type(T44) is list:
T44 = mat(T44)
dim = T44.shape
if len(dim) != 2:
raise ArithmeticError('Forward-kinematics must be a 4x4 matrix!')
if dim[0] != dim[1]:
raise ArithmeticError('Forward-kinematics must be square!')
if dim[0] != 4:
raise ArithmeticError('Forward-kinematics must have dimension of 4!')
if not dh_table.has_key('tool'):
dh_table['tool'] = None
tool = dh_table['tool']
if not (tool is None):
T44 = T44.dot( inv(tool) )
# x5 for each elb_x
sol_elbup = inverse_kinematics_elbow_up(dh_table, T44)
sol_elbdown = inverse_kinematics_elbow_down(dh_table, T44)
sol_elbup_backward = inverse_kinematics_elbow_up(dh_table, T44, flipped = True)
sol_elbdown_backward = inverse_kinematics_elbow_down(dh_table, T44, flipped = True)
#first columnt is first solution and so forth
## ret = mat(zip(sol_elbup1, sol_elbdown1, sol_elbup1_fl, sol_elbdown1_fl,
## sol_elbup2, sol_elbdown2, sol_elbup2_fl, sol_elbdown2_fl,
## sol_elbup3, sol_elbdown3, sol_elbup3_fl, sol_elbdown3_fl))
#first columnt is first solution and so forth
ret = mat( zip( sol_elbup, sol_elbdown, sol_elbup_backward, sol_elbdown_backward ) )
k,m,n = ret.shape
ret = ret.reshape(k*m,n)
return ret.T
def define_plane_from_directions(origin, dirx, diry, system_type='local'):
"""
This function returns a coordinate system frame (NxN matrix),
for a plane defined by a matrix containing a point of origin (t)
and a set of basis vectors (R) all of dimension N: [[R, t,]
[0, 1]]
"""
if not system_type in ['local', 'global']:
raise ArithmeticError(
'Only local or global coordinate system descriptions allowed')
origin, dirx, diry = convert_to_mat(origin, dirx, diry)
diry = gram_schmith_step(diry, dirx)
diry = diry / norm(diry)
dirx = dirx / norm(dirx)
normal = mat([rand(), rand(), rand()])
normal = gram_schmith_step(normal, dirx)
normal = gram_schmith_step(normal, diry)
plane_transform = expand_matrix(mat([dirx, diry, normal, origin]).T, 1, 0)
if system_type == 'global':
return mat_flip(plane_transform)
else:
return plane_transform
def transform_to_next(A, alpha, D, theta, convention='standard'):
"""
Calculats transform from frame J = I-1 to I
in order AI = Rzj( theta )Tzj( D )Txi( A )Rxi( alpha )
and parameters are given in order A, alpha, D, theta,
according to the standard convention:
matrix = mat([[ct, -st*ca, st*sa, A*ct],
[st, ct*ca, -ct*sa, A*st],
[ 0, sa, ca, D ],
[ 0, 0, 0, 1 ]]);
If convention is changed to modified then the transformation
is remapped according to the equivalent modified Denivit-hartenberg,
and performs the mapping from I to I+1.
"""
Rz_J = homogenous_rotation_z(theta)
Tz_J = homogenous_translation_z(D)
Tx_I = homogenous_translation_x(A)
Rx_I = homogenous_rotation_x(alpha)
matrix = matmul(Rz_J, Tz_J, Tx_I, Rx_I)
if convention.lower() == 'standard':
return matrix
elif convention.lower() == 'modified':
modified = mat([
matrix[0, 0],
-matrix[1, 0],
matrix[2, 0],
A,
-matrix[0, 1],
matrix[1, 1],
-matrix[2, 1],
-matrix[2, 1] * D,
matrix[0, 2],
-matrix[1, 2],
matrix[2, 2],
matrix[2, 2] * D,
0,
0,
0,
1,
]).reshape((4, 4))
return modified
else:
raise ArithmeticError(
"As of writing this function only two conventions are allowed: 'standard' or 'modified'."
)
def test_elbow_up_backward_facing(self):
print '\ntest_elbow_up_backward'
for _ in xrange(100):
j1 = rand_range(-180, 180)
j2 = -40
j3 = -100
j4 = rand_range(-200, 200)
j5 = rand_range(-115, 115)
j6 = rand_range(-400, 400)
robot_info = forward_kinematics(j1,j2,j3,j4,j5,j6,**DH_TABLE)
A, debug = robot_info['flange'], robot_info['robot_geometry_local']
sol = mat([j1,j2,j3,j4,j5,j6])
s = inverse_kinematics_elbow_up(DH_TABLE, A, flipped = True)
self.assertNotEqual(n.isnan(n.sum(s)), True)
a,b,c = s[0][0:3]
self.assertAlmostEqual(a, j1)
self.assertAlmostEqual(b, j2)
self.assertAlmostEqual(c, j3)
a,b,c = s[1][0:3]
self.assertAlmostEqual(a, j1)
self.assertAlmostEqual(b, j2)
self.assertAlmostEqual(c, j3)
s = inverse_kinematics_elbow_down(DH_TABLE, A, flipped = True)
a,b,c = s[0][0:3]
self.assertAlmostEqual(a, j1)
self.assertNotAlmostEqual(b, j2)
self.assertNotAlmostEqual(c, j3)
a,b,c = s[1][0:3]
self.assertAlmostEqual(a, j1)
self.assertNotAlmostEqual(b, j2)
self.assertNotAlmostEqual(c, j3)
# check if they are stored in the right order i.e.
# elbow_up, elbow_down, elbow_up_backward, delbow_down_backward
sols = inverse_kinematics_irb140(DH_TABLE, A)
for i in xrange(2, len(sols.T), 4):
a,b,c = sols[:,i][:3]
self.assertAlmostEqual(a, j1)
self.assertAlmostEqual(b, j2)
self.assertAlmostEqual(c, j3)
def test_just_barely_reach_flipped_configs(self):
print '\ntest_just_barely_reach_flipped_configs'
for _ in xrange(0,100):
j1 = rand_range(-180, 180)
j2 = -90
j3 = -89
j4 = rand_range(-200, 200)
j5 = rand_range(-115, 115)
j6 = rand_range(-400, 400)
s0 = j1,j2,j3,j4,j5,j6
robot_info = forward_kinematics(*s0, **DH_TABLE)
T44, debug1 = robot_info['flange'], robot_info['robot_geometry_local']
sol = mat( inverse_kinematics_irb140(DH_TABLE, T44) )
for s in sol.T:
robot_info = forward_kinematics(*s, **DH_TABLE)
A, debug2 = robot_info['flange'], robot_info['robot_geometry_local']
self.assertAlmostEqual(norm(A-T44), 0)
def backward_facing_elbow_down(dh_table, T44):
'''
Calculates backward-facing elbow-down.
Note: This is in reality an elbow-down solution,
the name merely implies it is "flipped" up-side-down
due to the base have turned 180 degrees.
'''
#Geometrical paramaters
wcp = calc_wcp(T44, 0.065)
#First angle - j1, used to adjust a point-position
j1 = calc_j1(wcp, flipped=True)
p0 = mat([70e-3, 0, 352e-3])
p0 = homogenous_rotation_z(j1)[0:3,0:3].dot(p0)
x0 = norm(wcp[0:2] - p0[0:2])
h1 = p0[2]
h2 = wcp[2]
s = h2 - h1
x1 = norm(p0 - wcp)
beta = 380e-3
alpha = 360e-3
th3 = ang_sats2(x1, alpha, beta)
j3 = -90 + th3
th21 = atan2(s, x0)
th22 = atan2(beta*sin2(th3), alpha + beta*cos2(th3))
j2 = -90 + th21 - th22
#packing the solutions in a dynamic way
## j4, j5, j6,\
## j41,j51,j61, \
## j42,j52,j62 = inverse_kinematics_spherical_wrist(dh_table, j1, j2, j3, T44)
##
## return (j1, j2, j3, j4, j5, j6),\
## (j1, j2, j3, j41, j51, j61), \
## (j1, j2, j3, j42, j52, j62)
result = pack_elbow_and_wrists(dh_table, j1, j2, j3, T44)
return result
def define_plane_from_angles(origin, r, t, s, system_type='local'):
"""
This function returns a coordinate system frame (NxN matrix),
for a plane defined by a matrix containing a point of origin (t)
and a set of basis vectors (R) all of dimension N: [[R, t,]
[0, 1]]
"""
if not system_type in ['local', 'global']:
raise ArithmeticError(
'Only local or global coordinate system descriptions allowed')
R = rotation_matrix_rot_tilt_skew(r, t, s)
plane_transform = expand_matrix(R)
plane_transform[:3, 3] = origin
plane_transform = expand_matrix(
mat([R[:, 0], R[:, 1], R[:, 2], origin]).T, 1, 0)
if system_type == 'global':
return mat_flip(plane_transform)
else:
return plane_transform
def elbow_down(dh_table, T44):
'''
Calculates forward-facing elbow-down.
'''
#Geometrical paramaters
wcp = calc_wcp(T44, 0.065)
#First angle - j1, used to adjust a point-position
j1 = calc_j1(wcp, flipped=False)
p0 = mat([70e-3, 0, 352e-3])
p0 = homogenous_rotation_z(j1)[0:3,0:3].dot(p0)
x0 = norm(wcp[0:2] - p0[0:2])
h1 = p0[2]
h2 = wcp[2]
s = h2 - h1
x1 = norm(p0 - wcp)
beta = 380e-3
alpha = 360e-3
th3 = ang_sats2(x1, alpha, beta)
j3 = -th3 - 90
th21 = atan2(s, x0)
th22 = atan2(beta*sin2(th3), alpha + beta*cos2(th3))
j2 = 90 - (th21 - th22)
if norm(wcp[:2])-norm(p0[:2]) < 0:
j2 = -90 + (th21+th22)
## j3 = -90-th3
## j4, j5, j6,\
## j41,j51,j61, \
## j42,j52,j62 = inverse_kinematics_spherical_wrist(dh_table, j1, j2, j3, T44)
##
## return (j1, j2, j3, j4, j5, j6),\
## (j1, j2, j3, j41, j51, j61), \
## (j1, j2, j3, j42, j52, j62)
result = pack_elbow_and_wrists(dh_table, j1, j2, j3, T44)
return result
def test_non_reach_config(self):
print '\ntest_non_reach_configs'
for _ in xrange(0,100):
j1 = rand_range(-180, 180)
j2 = 90
j3 = -89
j4 = rand_range(-200, 200)
j5 = rand_range(-115, 115)
j6 = rand_range(-400, 400)
s0 = j1,j2,j3,j4,j5,j6
robot_info = forward_kinematics(*s0, **DH_TABLE)
T44, debug1 = robot_info['flange'], robot_info['robot_geometry_local']
sol = mat( inverse_kinematics_irb140(DH_TABLE, T44) )
sol = sol.T
for i,s in enumerate(sol):
robot_info = forward_kinematics(*s, **DH_TABLE)
A, debug2 = robot_info['flange'], robot_info['robot_geometry_local']
if i in [l+m*8 for m,_ in enumerate(range(0, len(sol), 8)) for l in [0,1,4,5]]: #all non-flipped solutions only
self.assertAlmostEqual(norm(A-T44), 0)
else:
self.assertTrue(n.isnan(norm(A-T44)))
def ang(R):
return mat(rot_tilt_skew(R))
def inverse_kinematics_curve(trans_frames):
# perform inverse kinematics over a curve and collect all solutions
all_solutions = []
for point_frame in trans_frames:
all_solutions.append(__inverse_kinematics_pose(point_frame, filtering=True))
return mat(all_solutions)
d,e,f = rots2.shape
Y = rots1.reshape(a*b, c)
Z = rots2.reshape(d*e, f)
B = Y.T.dot(Y)
C = Y.T.dot(Z)
A = solve(B,C)
return fit_to_ori(A)
# ranges:
# rot = (-pi, pi)
# tilt = (-pi/2, pi/2)
# skew = (-pi, pi)
e = lambda mag: mat([rnd(-1, 1)*(10**mag) for _ in xrange(9)]).reshape(3,3)
def ang(R):
return mat(rot_tilt_skew(R))
def apply_noise(R, mag=-4):
return R + e(mag)
if __name__ == "__main__":
res = {"solerr1":[],
"solerr2":[],
"solerr32":[],
"angerr1":[],
"angerr2":[],
"angerr32":[],
}
def generate_modulo_solutions(solution_matrix, index, modulo=360.0):
return mat(zip(*__modulo_solutions(solution_matrix, index, modulo)))
def mat_flip3(M):
transf_flip = mat([[1, 0, 0], [0, -1, 0], [0, 0, -1]])
return transf_flip.dot(M)
def forward_kinematics(*joint_values, **kwargs):
"""
Performs the denavit-hartenberg algorithm
and calculates T44 = A0 * A1 * A2 * ... * An multiplication.
The parameters are entered in the order A, alpha, D, theta repeatedly
if no order is specified. However, is another order specified then
the values must be entered in that order.
"""
# check so that only supported parameters
# exist in kwargs and abort if any other parameter
# had been created by mistake due to typos or similar
if not kwargs.has_key('tool'):
kwargs['tool'] = None
print 'NO TOOL'
tool = kwargs['tool']
input_param_diff = set(kwargs) - set(
['order', 'convention', 'unit', 'table', 'tool'])
if len(input_param_diff) > 0:
raise Exception('Unsupported parameters: ' + str(*input_param_diff))
# handle which unit is being used
if not kwargs.has_key('unit'):
kwargs['unit'] = 'metre'
elif kwargs['unit'] == 'm':
kwargs['unit'] = 'metre'
elif kwargs['unit'] == 'mm':
kwargs['unit'] = 'millimetre'
# supply a standard order
if not kwargs.has_key('order'):
kwargs['order'] = ['A', 'alpha', 'D', 'theta']
# supply the standard denivit-hartenberg if no convention given
if not kwargs.has_key('convention'):
kwargs['convention'] = 'standard'
row_length = 5
nbr_of_joints = len(joint_values)
if len(kwargs['table']) == 1 and type(kwargs['table'][0]) in [list, tuple]:
raise ArithmeticError(
"Function does not use lists or tuples, please unpack using * operator."
)
elif not (len(kwargs['table']) % row_length == 0):
raise ArithmeticError(
"Invalid number of Denavit-Hartenberg parameters - you also need to supply type of joint"
)
matrices = []
for k in xrange(0, nbr_of_joints):
# Performing the operation
# A, alpha, D, theta = params['A'], params['alpha'], params['D'], params['theta']
# in a very general way
var_names, var_values, joint_type = kwargs['order'],\
kwargs['table'][row_length*k : row_length*k + row_length-1],\
kwargs['table'][row_length*k + row_length-1]
for i in xrange(row_length - 1):
exec('%s = %0.16f' % (var_names[i], var_values[i]))
if kwargs['unit'] == 'mm' or kwargs['unit'] == 'millimetre':
A = A * 1e-3 #converts mm to meters
D = D * 1e-3 #convers mm to meters
elif kwargs['unit'] == 'm' or kwargs['unit'] == 'metre':
pass
else:
raise ArithmeticError("Unknown unit of length, only meters('m' or 'metre')"+\
+" or millimeters ('mm' or 'millimetre') allowed.")
if joint_type == 'R':
matrices.append(
transform_to_next(A, alpha, D, theta + joint_values[k]))
elif joint_type == 'P':
matrices.append(
transform_to_next(A, alpha, D + joint_values[k], theta))
# collect information about the Denivit-Hartenberg table
dh_table = {
'table': kwargs['table'],
'unit': kwargs['unit'],
'convention': kwargs['convention'],
'order': kwargs['order'],
'tool': kwargs['tool']
}
global_geometry = matmul_series(*matrices)
global_geometry.insert(0, homogenous_matrix(0, 0, 0, 0, 0, 0))
flange = matmul(*matrices)
if tool is None:
tcp = mat(flange)
else:
tcp = flange.dot(tool)
result = {
'flange': flange,
'tcp': tcp,
'robot_geometry_local': matrices,
'robot_geometry_global': global_geometry,
'dh_table': dh_table
}
# perform matrix chain-multiplication / serial-multiplication with matrix product
return result
def merge_solutions(*args):
result = []
for m in args:
result += zip(*m)
return mat(zip(*result))
