在做正运动学推导时碰到一题六个旋转矩阵相乘的计算题,而其中矩阵又全部是由未知量组成。苦于手算,况且目前只想到用numpy处理又处理不了未知量,只得自己实现一个含有未知量矩阵的相乘运算程序
示例
矩阵a:
矩阵b
结果:
代码
# import numpy as np
def matMul(a, b):
rowLen = len(a)
colLen = len(a[0])
for i in range(rowLen):
for j in range(colLen):
a[i][j] = str(a[i][j])
b[j][i] = str(b[j][i])
mat1 = divideMat(a)
mat2 = divideMat(b)
# print(mat1)
# print(mat2)
answer = [[[] for j in range(rowLen)] for i in range(rowLen)]
# 进行乘法
for i in range(rowLen):
tar1 = mat1[i]
for j in range(rowLen):
tar2 = [mat2[k][j] for k in range(colLen)]
res = 0
# for k in range(Len):
# res += tar1[k] * tar2[k]
res = merge(tar1, tar2)
answer[i][j] = res
# show(answer)
rt = recoverMat(answer)
return rt
def divideMat(mat):
rowLen = len(mat)
colLen = len(mat[0])
rt = [[[] for j in range(colLen)] for i in range(rowLen)]
for i in range(rowLen):
for j in range(colLen):
t = mat[i][j]
# 在减号前面补上加号
start = 1
while True:
start = t.find('-', start)
if start == -1:
break
t = t[0:start] + '+' + t[start:]
start += 2
# 按+号分割,将一个单元分割开来
for target in t.split('+'):
k = 0
c = 1 #系数
if target[k] == '-':
k += 1
c *= -1
n = ''
while '0' <= target[k] <= '9':
n = n + target[k]
k += 1
if k >= len(target):
break
if n == '':
n = '1' #没有系数部分的话系数设为1
c = c*int(n)
rt[i][j].append({'coe':c, 'var':target[k:]})
return rt
def recoverMat(mat):
rt = []
for i in mat:
row = []
for j in i:
s = ''
for k in j:
coe = k['coe']
var = k['var']
if coe < 0:
if coe == -1 and var:
s = s + '-' + var
elif coe == -1 and not var:
s = s + '-1'
else:
s = s + str(coe) + var
elif coe == 0:
s = s + '+' + str(coe)
else:
if coe == 1 and var:
s = s + '+' + var
elif coe == 1 and not var:
s = s + '+1'
else:
s = s + '+' + str(coe) + var
if s[0] == '+':
s = s[1:]
row.append(s)
rt.append(row)
return rt
def merge(m1, m2):
rt = [] # 该项计算结果
# print(m1, m2)
for i in range(len(m1)):
tar1 = m1[i]
tar2 = m2[i] # [{'coe': 1, 'var': 'c2'}]
coe = 0
var = ''
# print(tar1, tar2)
for j in tar1:
for k in tar2:
coe = j['coe'] * k['coe']
if not coe:
continue
var = j['var'] + k['var']
rt.append({'coe':coe, 'var':var})
if not rt:
rt.append({'coe':0, 'var':''})
return rt
def show(mat):
for i in mat:
for j in range(len(i)):
print(i[j], ' ' ,end='')
print()
if __name__ == '__main__':
a = [
['c1','0','-s1','0'],
['s1','0','c1','0'],
['0','-1','0','13'],
['0','0','0','1']
]
b = [
['c2','-s2','0','8c2'],
['s2','c2','0','8s2'],
['0','0','1','ds'],
['0','0','0','1']
]
c = [
['c3', '0', 's3', '8c3'],
['s3', '0', '-c3', '8s3'],
['0', '1', '0', '0'],
['0', '0', '0', '1']
]
d = [
['c4', '0', '-s4', '0'],
['s4', '0', 'c4', '0'],
['0', '-1', '0', 'de'],
['0', '0', '0', '1']
]
e = [
['c5', '0', '-s5', '0'],
['s5', '0', 'c5', '0'],
['0', '-1', '0', '0'],
['0', '0', '0', '1']
]
f = [
['c6', '-s6', '0', '0'],
['s6', 'c6', '0', '0'],
['0', '0' ,'1' , '0'],
['0', '0', '0', '1']
]
k1 = matMul(a,b)
show(k1)
k2 = matMul(k1,c)
show(k2)
k3 = matMul(k2,d)
show(k3)
k4 = matMul(k3,e)
k5 = matMul(k4,f)
show(k5)
test1 = [
[1,2],
[1,2],
[3,4]
]
test2 = [
[2,5,1],
[2,3,2]
]
ans = matMul(test1, test2)
show(ans)
其实矩阵纯数字运算没有什么,同时加上变量后就涉及到两个不同运算法则的处理。
我的处理办法就是将矩阵中一个位置的元素按+/-
拆开来存储,例如:2b+3c
就是{'coe':2, 'var':'b'}
和{'coe':3, 'var':'c'}
。这样对应位相乘时只需计算系数与系数相乘,变量直接以字符串相加即可。
并且能够对纯数字进行运算,只不过得到得结果需要自己加法计算一下
一劳永逸