未知

最新推荐文章于 2024-05-31 10:50:16 发布
最新推荐文章于 2024-05-31 10:50:16 发布
阅读量283 收藏
点赞数
分类专栏： python learing
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/weixin_44287946/article/details/114273592
版权 
import numpy

def cos(theta):

    return numpy.cos(theta)
def sin(theta):
    return  numpy.sin(theta)
'''设置上面两个函数避免麻烦'''
'''变换矩阵的参数'''
theta1=0#30/180*numpy.pi
theta2=0#30/180*numpy.pi
theta3=0#30/180*numpy.pi
theta4=30/180*numpy.pi
theta5=30/180*numpy.pi
theta6=30/180*numpy.pi
d1=0.3
a2=0.3
a3=0.03
d4=0.2
d6=0.03
'''伪惯量矩阵'''

i1xx=i1yy=i1zz=5
i1xy=i1xz=i1yz=0
m1=1
i2xx =i2yy =i2zz=5
i2xy =i2xz =i2yz=0
m2=1
i3xx =i3yy =i3zz =5
i3xy =i3xz =i3yz=0
m3=1
i4xx =i4yy =i4zz =i4xy =i4xz =i4yz=0.5
m4=1
i5xx =i5yy =i5zz =i5xy =i5xz =i5yz=0.5
m5=1
i6xx =i6yy =i6zz =i6xy =i6xz =i6yz=0.5
m6=1

c1=numpy.cos(theta1)
c2=numpy.cos(theta2)
c3=numpy.cos(theta3)
c4=numpy.cos(theta4)
c5=numpy.cos(theta5)
c6=numpy.cos(theta6)

s1=numpy.sin(theta1)
s2=numpy.sin(theta2)
s3=numpy.sin(theta3)
s4=numpy.sin(theta4)
s5=numpy.sin(theta5)
s6=numpy.sin(theta6)
'''

'''
xc1=0
yc1=0
zc1=0.1
''''''
xc2=0
yc2=0
zc2=0.3
''''''
xc3=0
yc3=0
zc3=0.6
''''''
xc4=0.3
yc4=0
zc4=0.63
''''''
xc5=0.3
yc5=0
zc5=0.63
''''''
xc6=0.33
yc6=0
zc6=0.63

#I的公式的内容

gg=numpy.mat([0 ,0 ,10,0])
'''标准DH参数'''
T1=numpy.mat([[c1   ,0  ,  s1  ,   0],      [s1 ,   0    ,  -c1  ,    0],    [0  ,  1  ,   0  ,  d1],       [0 ,0 ,  0 , 1]]     )
T2=numpy.mat([[c2 , -s2  , 0  ,  a2*c2] ,   [s2 ,   c2    ,   0  ,a2*s2],    [0  ,  0  ,   1   ,  0],       [0,   0 ,  0 , 1]]   )
T3=numpy.mat([[c3  , 0   , s3  , a3*c3]   , [s3  ,   0 ,    -c3 , a3*s3],    [0  ,   1 ,   0   ,  0] ,      [0 ,  0 ,  0 , 1]]   )
T4=numpy.mat([[c4  , 0   ,-s4  ,     0] ,   [s4  ,   0 ,    c4  ,    0 ],    [0  ,   -1 ,  0   , d4] ,      [0  , 0  , 0  ,1]]   )
T5=numpy.mat([[c5 ,  0   , s5 ,      0] ,   [s5  ,   0 ,    -c5 ,    0 ],    [0  ,   1  ,  0   ,  0] ,      [0  , 0  , 0 , 1]]   )
T6=numpy.mat([[c6 ,-s6  ,  0  ,  0],       [s6   , c6  ,    0   ,   0] ,     [0  ,   0   , 1   ,  d6],      [0  , 0  , 0 , 1]]   )
T16=T1*T2*T3*T4*T5*T6
'''rp'''
r1=numpy.mat([xc1,yc1,zc1,1]).transpose()
r2=T1*r1
r3=T2*r2
'''惯性矩阵'''
I1=[[(-i1xx+i1yy+i1zz)/2  ,    i1xy      ,         i1xz       ,     m1*xc1],
    [    i1xy     ,     (i1xx-i1yy+i1zz)/2  ,    i1yz       ,     m1*yc1],
    [      i1xz    ,       i1yz      ,      (i1xx+i1yy-i1zz)/2  ,  m1*zc1],
    [    m1*xc1    ,       m1*yc1     ,           m1*zc1        ,    m1]]
I2=[      [(-i2xx+i2yy+i2zz)/2 ,     i2xy   ,             i2xz  ,            m2*xc2],
          [i2xy    ,            (i2xx-i2yy+i2zz)/2    ,   i2yz     ,         m2*yc2],
          [i2xz    ,                i2yz      ,      (i2xx+i2yy-i2zz)/2  ,   m2*zc2],
          [m2*xc2 ,                 m2*yc2   ,             m2*zc2    ,       m2] ]
I3=[      [(-i3xx+i3yy+i3zz)/2   ,   i3xy       ,        i3xz      ,      m3*xc3],
          [i3xy      ,    (i3xx-i3yy+i3zz)/2    ,  i3yz      ,      m3*yc3],
          [i3xz      ,      i3yz        ,    (i3xx+i3yy-i3zz)/2   , m3*zc3],
          [m3*xc3    ,       m3*yc3      ,          m3*zc3     ,       m3]  ]
I4=[[(-i4xx+i4yy+i4zz)/2  ,    i4xy      ,         i4xz     ,       m4*xc4],
     [     i4xy    ,      (i4xx-i4yy+i4zz)/2 ,     i4yz      ,      m4*yc4],
     [    i4xz   ,         i4yz       ,     (i4xx+i4yy-i4zz)/2  ,  m4*zc4],
     [  m4*xc4    ,       m4*yc4      ,          m4*zc4        ,    m4] ]
I5=[[(-i5xx+i5yy+i5zz)/2  ,    i5xy      ,         i5xz      ,      m5*xc5],
     [     i5xy   ,       (i5xx-i5yy+i5zz)/2   ,   i5yz        ,     m5*yc5],
      [    i5xz    ,        i5yz        ,    (i5xx+i5yy-i5zz)/2  ,   m5*zc5],
      [  m5*xc5    ,       m5*yc5       ,         m5*zc5         ,   m5   ] ]
I6=[[(-i6xx+i6yy+i6zz)/2  ,    i6xy      ,         i6xz     ,       m6*xc6],
          [i6xy    ,      (i6xx-i6yy+i6zz)/2   ,   i6yz    ,        m6*yc6],
          [i6xz     ,       i6yz       ,     (i6xx+i6yy-i6zz)/2  ,  m6*zc6],
          [m6*xc6   ,        m6*yc6    ,            m6*zc6      ,      m6 ]]

'''求速度的变换矩阵的导数'''
u11=numpy.mat([[-s1   ,0  ,  c1  ,   0],
               [c1 ,  0  ,  s1  ,    0],
               [0  ,  0  ,   0  ,    0],
               [0 ,    0 ,   0 ,     0]])
u21=numpy.mat([[c2*-s1,s1*s2,c1,a2*-s1*c2],
               [c2*c1,-c1*s2,s1,a2*c1*c2],
               [0,0,0,0],
               [0,0,0,0]])
u22=numpy.mat([[-c1*s1,-c1*c2,0,-a2*c1*s2],
              [-s1*s2,-c2*s1,0,-a2*s1*s2],
              [c2,-s2,0,a2*c2],
              [0,0,0,0]])
u31=numpy.mat([[ s1*s2*s3 - c2*c3*s1, c1, - c2*s1*s3 - c3*s1*s2, a3*s1*s2*s3 - a3*c2*c3*s1 - a2*c2*s1],
[ c1*c2*c3 - c1*s2*s3, s1,   c1*c2*s3 + c1*c3*s2, a2*c1*c2 + a3*c1*c2*c3 - a3*c1*s2*s3],
[ 0,  0, 0,    0],
[ 0,  0,   0,  0]])
u32=numpy.mat([[ - c1*c2*s3 - c1*c3*s2, 0, c1*c2*c3 - c1*s2*s3, - a2*c1*s2 - a3*c1*c2*s3 - a3*c1*c3*s2],
[ - c2*s1*s3 - c3*s1*s2, 0, c2*c3*s1 - s1*s2*s3, - a2*s1*s2 - a3*c2*s1*s3 - a3*c3*s1*s2],
[c2*c3 - s2*s3, 0,  c2*s3 + c3*s2,  a2*c2 + a3*c2*c3 - a3*s2*s3],
[0, 0,  0,  0]])
u33=numpy.mat([[ - c1*c2*s3 - c1*c3*s2, 0, c1*c2*c3 - c1*s2*s3, - a3*c1*c2*s3 - a3*c1*c3*s2],
[ - c2*s1*s3 - c3*s1*s2, 0, c2*c3*s1 - s1*s2*s3, - a3*c2*s1*s3 - a3*c3*s1*s2],
[ c2*c3- s2*s3, 0,  c2*s3+ c3*s2,  a3*c2*c3 - a3*s2*s3],
[  0, 0,   0,    0]])
'''
u41=numpy.mat([[ c1*s4 + c4*(s1*s2*s3 - c2*c3*s1),   c2*s1*s3 + c3*s1*s2, c1*c4 - s4*(s1*s2*s3 - c2*c3*s1), a3*s1*s2*s3 - a2*c2*s1 - a3*c2*c3*s1 - d4*(c2*s1*s3 + c3*s1*s2)],
               [ s1*s4 - c4*(c1*s2*s3 - c1*c2*c3), - c1*c2*s3 - c1*c3*s2, c4*s1 + s4*(c1*s2*s3 - c1*c2*c3), d4*(c1*c2*s3 + c1*c3*s2) + a2*c1*c2 + a3*c1*c2*c3 - a3*c1*s2*s3],
               [   0,   0,    0,  0],
               [   0,   0,   0,   0]])
u42=numpy.mat([[ -c4*(c1*c2*s3 + c1*c3*s2), c1*s2*s3 - c1*c2*c3, s4*(c1*c2*s3 + c1*c3*s2), - d4*(c1*s2*s3 - c1*c2*c3) - a2*c1*s2 - a3*c1*c2*s3 - a3*c1*c3*s2],
[ -c4*(c2*s1*s3 + c3*s1*s2), s1*s2*s3 - c2*c3*s1, s4*(c2*s1*s3 + c3*s1*s2), - d4*(s1*s2*s3 - c2*c3*s1) - a2*s1*s2 - a3*c2*s1*s3 - a3*c3*s1*s2],
[ c4*(c2*c3 - s2*s3),   - c2*s3 - c3*s2,  -s4*(c2*c3 - s2*s3),  d4*(c2*s3 + c3*s2) + a2*c2 + a3*c2*c3 - a3*s2*s3],
[   0,     0,       0,    0]])
u43=numpy.mat([[ c4*s1 + s4*(c1*s2*s3 - c1*c2*c3), 0, c4*(c1*s2*s3 - c1*c2*c3) - s1*s4, 0],
[ s4*(s1*s2*s3 - c2*c3*s1) - c1*c4, 0, c1*s4 + c4*(s1*s2*s3 - c2*c3*s1), 0],
[ -s4*(c2*s3 + c3*s2), 0,  -c4*(c2*s3 + c3*s2), 0],
[   0, 0,    0, 0]])
u44=numpy.mat([[ c4*s1 + s4*(c1*s2*s3 - c1*c2*c3), 0, c4*(c1*s2*s3 - c1*c2*c3) - s1*s4, 0],
[ s4*(s1*s2*s3 - c2*c3*s1) - c1*c4, 0, c1*s4 + c4*(s1*s2*s3 - c2*c3*s1), 0],
[ -s4*(c2*s3 + c3*s2), 0, -c4*(c2*s3 + c3*s2), 0],
[  0, 0,   0, 0]])
u51=numpy.mat([[ cos(theta5)*(cos(theta1)*sin(theta4) + cos(theta4)*(sin(theta1)*sin(theta2)*sin(theta3) - cos(theta2)*cos(theta3)*sin(theta1))) + sin(theta5)*(cos(theta2)*sin(theta1)*sin(theta3) + cos(theta3)*sin(theta1)*sin(theta2)), cos(theta1)*cos(theta4) - sin(theta4)*(sin(theta1)*sin(theta2)*sin(theta3) - cos(theta2)*cos(theta3)*sin(theta1)), sin(theta5)*(cos(theta1)*sin(theta4) + cos(theta4)*(sin(theta1)*sin(theta2)*sin(theta3) - cos(theta2)*cos(theta3)*sin(theta1))) - cos(theta5)*(cos(theta2)*sin(theta1)*sin(theta3) + cos(theta3)*sin(theta1)*sin(theta2)), a3*sin(theta1)*sin(theta2)*sin(theta3) - a2*cos(theta2)*sin(theta1) - a3*cos(theta2)*cos(theta3)*sin(theta1) - d4*(cos(theta2)*sin(theta1)*sin(theta3) + cos(theta3)*sin(theta1)*sin(theta2))],
[ cos(theta5)*(sin(theta1)*sin(theta4) - cos(theta4)*(cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)*cos(theta2)*cos(theta3))) - sin(theta5)*(cos(theta1)*cos(theta2)*sin(theta3) + cos(theta1)*cos(theta3)*sin(theta2)), cos(theta4)*sin(theta1) + sin(theta4)*(cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)*cos(theta2)*cos(theta3)), sin(theta5)*(sin(theta1)*sin(theta4) - cos(theta4)*(cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)*cos(theta2)*cos(theta3))) + cos(theta5)*(cos(theta1)*cos(theta2)*sin(theta3) + cos(theta1)*cos(theta3)*sin(theta2)), d4*(cos(theta1)*cos(theta2)*sin(theta3) + cos(theta1)*cos(theta3)*sin(theta2)) + a2*cos(theta1)*cos(theta2) + a3*cos(theta1)*cos(theta2)*cos(theta3) - a3*cos(theta1)*sin(theta2)*sin(theta3)],
[     0,  0,   0,   0],
[     0,   0,   0,  0]])
u52=numpy.mat([[ sin(theta5)*(cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)*cos(theta2)*cos(theta3)) - cos(theta4)*cos(theta5)*(cos(theta1)*cos(theta2)*sin(theta3) + cos(theta1)*cos(theta3)*sin(theta2)), sin(theta4)*(cos(theta1)*cos(theta2)*sin(theta3) + cos(theta1)*cos(theta3)*sin(theta2)), - cos(theta5)*(cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)*cos(theta2)*cos(theta3)) - cos(theta4)*sin(theta5)*(cos(theta1)*cos(theta2)*sin(theta3) + cos(theta1)*cos(theta3)*sin(theta2)), - d4*(cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)*cos(theta2)*cos(theta3)) - a2*cos(theta1)*sin(theta2) - a3*cos(theta1)*cos(theta2)*sin(theta3) - a3*cos(theta1)*cos(theta3)*sin(theta2)],
[ sin(theta5)*(sin(theta1)*sin(theta2)*sin(theta3) - cos(theta2)*cos(theta3)*sin(theta1)) - cos(theta4)*cos(theta5)*(cos(theta2)*sin(theta1)*sin(theta3) + cos(theta3)*sin(theta1)*sin(theta2)), sin(theta4)*(cos(theta2)*sin(theta1)*sin(theta3) + cos(theta3)*sin(theta1)*sin(theta2)), - cos(theta5)*(sin(theta1)*sin(theta2)*sin(theta3) - cos(theta2)*cos(theta3)*sin(theta1)) - cos(theta4)*sin(theta5)*(cos(theta2)*sin(theta1)*sin(theta3) + cos(theta3)*sin(theta1)*sin(theta2)), - d4*(sin(theta1)*sin(theta2)*sin(theta3) - cos(theta2)*cos(theta3)*sin(theta1)) - a2*sin(theta1)*sin(theta2) - a3*cos(theta2)*sin(theta1)*sin(theta3) - a3*cos(theta3)*sin(theta1)*sin(theta2)],
[ cos(theta4)*cos(theta5)*(cos(theta2)*cos(theta3) - sin(theta2)*sin(theta3)) - sin(theta5)*(cos(theta2)*sin(theta3) + cos(theta3)*sin(theta2)),  -sin(theta4)*(cos(theta2)*cos(theta3) - sin(theta2)*sin(theta3)),    cos(theta5)*(cos(theta2)*sin(theta3) + cos(theta3)*sin(theta2)) + cos(theta4)*sin(theta5)*(cos(theta2)*cos(theta3) - sin(theta2)*sin(theta3)),                                                               d4*(cos(theta2)*sin(theta3) + cos(theta3)*sin(theta2)) + a2*cos(theta2) + a3*cos(theta2)*cos(theta3) - a3*sin(theta2)*sin(theta3)],
[    0,        0,         0,      0]])
u53=numpy.mat([[  cos(theta5)*(cos(theta4)*sin(theta1) + sin(theta4)*(cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)*cos(theta2)*cos(theta3))), cos(theta4)*(cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)*cos(theta2)*cos(theta3)) - sin(theta1)*sin(theta4),  sin(theta5)*(cos(theta4)*sin(theta1) + sin(theta4)*(cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)*cos(theta2)*cos(theta3))), 0],
[ -cos(theta5)*(cos(theta1)*cos(theta4) - sin(theta4)*(sin(theta1)*sin(theta2)*sin(theta3) - cos(theta2)*cos(theta3)*sin(theta1))), cos(theta1)*sin(theta4) + cos(theta4)*(sin(theta1)*sin(theta2)*sin(theta3) - cos(theta2)*cos(theta3)*sin(theta1)), -sin(theta5)*(cos(theta1)*cos(theta4) - sin(theta4)*(sin(theta1)*sin(theta2)*sin(theta3) - cos(theta2)*cos(theta3)*sin(theta1))), 0],
[ -cos(theta5)*sin(theta4)*(cos(theta2)*sin(theta3) + cos(theta3)*sin(theta2)),  -cos(theta4)*(cos(theta2)*sin(theta3) + cos(theta3)*sin(theta2)),   -sin(theta4)*sin(theta5)*(cos(theta2)*sin(theta3) + cos(theta3)*sin(theta2)), 0],
[   0,      0,     0, 0]])
u54=numpy.mat([[  cos(theta5)*(cos(theta4)*sin(theta1) + sin(theta4)*(cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)*cos(theta2)*cos(theta3))), cos(theta4)*(cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)*cos(theta2)*cos(theta3)) - sin(theta1)*sin(theta4),  sin(theta5)*(cos(theta4)*sin(theta1) + sin(theta4)*(cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)*cos(theta2)*cos(theta3))), 0],
[ -cos(theta5)*(cos(theta1)*cos(theta4) - sin(theta4)*(sin(theta1)*sin(theta2)*sin(theta3) - cos(theta2)*cos(theta3)*sin(theta1))), cos(theta1)*sin(theta4) + cos(theta4)*(sin(theta1)*sin(theta2)*sin(theta3) - cos(theta2)*cos(theta3)*sin(theta1)), -sin(theta5)*(cos(theta1)*cos(theta4) - sin(theta4)*(sin(theta1)*sin(theta2)*sin(theta3) - cos(theta2)*cos(theta3)*sin(theta1))), 0],
[  -cos(theta5)*sin(theta4)*(cos(theta2)*sin(theta3) + cos(theta3)*sin(theta2)),     -cos(theta4)*(cos(theta2)*sin(theta3) + cos(theta3)*sin(theta2)),   -sin(theta4)*sin(theta5)*(cos(theta2)*sin(theta3) + cos(theta3)*sin(theta2)), 0],
[   0,     0,      0, 0]])
u55=numpy.mat([[ - sin(theta5)*(sin(theta1)*sin(theta4) - cos(theta4)*(cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)*cos(theta2)*cos(theta3))) - cos(theta5)*(cos(theta1)*cos(theta2)*sin(theta3) + cos(theta1)*cos(theta3)*sin(theta2)), 0,   cos(theta5)*(sin(theta1)*sin(theta4) - cos(theta4)*(cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)*cos(theta2)*cos(theta3))) - sin(theta5)*(cos(theta1)*cos(theta2)*sin(theta3) + cos(theta1)*cos(theta3)*sin(theta2)), 0],
[   sin(theta5)*(cos(theta1)*sin(theta4) + cos(theta4)*(sin(theta1)*sin(theta2)*sin(theta3) - cos(theta2)*cos(theta3)*sin(theta1))) - cos(theta5)*(cos(theta2)*sin(theta1)*sin(theta3) + cos(theta3)*sin(theta1)*sin(theta2)), 0, - cos(theta5)*(cos(theta1)*sin(theta4) + cos(theta4)*(sin(theta1)*sin(theta2)*sin(theta3) - cos(theta2)*cos(theta3)*sin(theta1))) - sin(theta5)*(cos(theta2)*sin(theta1)*sin(theta3) + cos(theta3)*sin(theta1)*sin(theta2)), 0],
[                                                                               cos(theta5)*(cos(theta2)*cos(theta3) - sin(theta2)*sin(theta3)) - cos(theta4)*sin(theta5)*(cos(theta2)*sin(theta3) + cos(theta3)*sin(theta2)), 0,                                                                               sin(theta5)*(cos(theta2)*cos(theta3) - sin(theta2)*sin(theta3)) + cos(theta4)*cos(theta5)*(cos(theta2)*sin(theta3) + cos(theta3)*sin(theta2)), 0],
[     0, 0,   0, 0]])
u61=numpy.mat([
[ sin(theta6)*(cos(theta1)*cos(theta4) - sin(theta4)*(sin(theta1)*sin(theta2)*sin(theta3) - cos(theta2)*cos(theta3)*sin(theta1))) + cos(theta6)*(cos(theta5)*(cos(theta1)*sin(theta4) + cos(theta4)*(sin(theta1)*sin(theta2)*sin(theta3) - cos(theta2)*cos(theta3)*sin(theta1))) + sin(theta5)*(cos(theta2)*sin(theta1)*sin(theta3) + cos(theta3)*sin(theta1)*sin(theta2))), cos(theta6)*(cos(theta1)*cos(theta4) - sin(theta4)*(sin(theta1)*sin(theta2)*sin(theta3) - cos(theta2)*cos(theta3)*sin(theta1))) - sin(theta6)*(cos(theta5)*(cos(theta1)*sin(theta4) + cos(theta4)*(sin(theta1)*sin(theta2)*sin(theta3) - cos(theta2)*cos(theta3)*sin(theta1))) + sin(theta5)*(cos(theta2)*sin(theta1)*sin(theta3) + cos(theta3)*sin(theta1)*sin(theta2))), sin(theta5)*(cos(theta1)*sin(theta4) + cos(theta4)*(sin(theta1)*sin(theta2)*sin(theta3) - cos(theta2)*cos(theta3)*sin(theta1))) - cos(theta5)*(cos(theta2)*sin(theta1)*sin(theta3) + cos(theta3)*sin(theta1)*sin(theta2)), d6*(sin(theta5)*(cos(theta1)*sin(theta4) + cos(theta4)*(sin(theta1)*sin(theta2)*sin(theta3) - cos(theta2)*cos(theta3)*sin(theta1))) - cos(theta5)*(cos(theta2)*sin(theta1)*sin(theta3) + cos(theta3)*sin(theta1)*sin(theta2))) - d4*(cos(theta2)*sin(theta1)*sin(theta3) + cos(theta3)*sin(theta1)*sin(theta2)) - a2*cos(theta2)*sin(theta1) - a3*cos(theta2)*cos(theta3)*sin(theta1) + a3*sin(theta1)*sin(theta2)*sin(theta3)],
[ sin(theta6)*(cos(theta4)*sin(theta1) + sin(theta4)*(cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)*cos(theta2)*cos(theta3))) + cos(theta6)*(cos(theta5)*(sin(theta1)*sin(theta4) - cos(theta4)*(cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)*cos(theta2)*cos(theta3))) - sin(theta5)*(cos(theta1)*cos(theta2)*sin(theta3) + cos(theta1)*cos(theta3)*sin(theta2))), cos(theta6)*(cos(theta4)*sin(theta1) + sin(theta4)*(cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)*cos(theta2)*cos(theta3))) - sin(theta6)*(cos(theta5)*(sin(theta1)*sin(theta4) - cos(theta4)*(cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)*cos(theta2)*cos(theta3))) - sin(theta5)*(cos(theta1)*cos(theta2)*sin(theta3) + cos(theta1)*cos(theta3)*sin(theta2))), sin(theta5)*(sin(theta1)*sin(theta4) - cos(theta4)*(cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)*cos(theta2)*cos(theta3))) + cos(theta5)*(cos(theta1)*cos(theta2)*sin(theta3) + cos(theta1)*cos(theta3)*sin(theta2)), d6*(sin(theta5)*(sin(theta1)*sin(theta4) - cos(theta4)*(cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)*cos(theta2)*cos(theta3))) + cos(theta5)*(cos(theta1)*cos(theta2)*sin(theta3) + cos(theta1)*cos(theta3)*sin(theta2))) + d4*(cos(theta1)*cos(theta2)*sin(theta3) + cos(theta1)*cos(theta3)*sin(theta2)) + a2*cos(theta1)*cos(theta2) + a3*cos(theta1)*cos(theta2)*cos(theta3) - a3*cos(theta1)*sin(theta2)*sin(theta3)],
[                                                                                                                                                                                                                                                                                                                                                                         0,                                                                                                                                                                                                                                                                                                                                                                         0,                                                                                                                                                                                                                         0,                                                                                                                                                                                                                                                                                                                                                                                                                              0],
[                                                                                                                                                                                                                                                                                                                                                                         0,                                                                                                                                                                                                                                                                                                                                                                         0,                                                                                                                                                                                                                         0,                                                                                                                                                                                                                                                                                                                                                                                                                              0]])
u62=numpy.mat([[ cos(theta6)*(sin(theta5)*(cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)*cos(theta2)*cos(theta3)) - cos(theta4)*cos(theta5)*(cos(theta1)*cos(theta2)*sin(theta3) + cos(theta1)*cos(theta3)*sin(theta2))) + sin(theta4)*sin(theta6)*(cos(theta1)*cos(theta2)*sin(theta3) + cos(theta1)*cos(theta3)*sin(theta2)), cos(theta6)*sin(theta4)*(cos(theta1)*cos(theta2)*sin(theta3) + cos(theta1)*cos(theta3)*sin(theta2)) - sin(theta6)*(sin(theta5)*(cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)*cos(theta2)*cos(theta3)) - cos(theta4)*cos(theta5)*(cos(theta1)*cos(theta2)*sin(theta3) + cos(theta1)*cos(theta3)*sin(theta2))), - cos(theta5)*(cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)*cos(theta2)*cos(theta3)) - cos(theta4)*sin(theta5)*(cos(theta1)*cos(theta2)*sin(theta3) + cos(theta1)*cos(theta3)*sin(theta2)), - d4*(cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)*cos(theta2)*cos(theta3)) - d6*(cos(theta5)*(cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)*cos(theta2)*cos(theta3)) + cos(theta4)*sin(theta5)*(cos(theta1)*cos(theta2)*sin(theta3) + cos(theta1)*cos(theta3)*sin(theta2))) - a2*cos(theta1)*sin(theta2) - a3*cos(theta1)*cos(theta2)*sin(theta3) - a3*cos(theta1)*cos(theta3)*sin(theta2)],
[ cos(theta6)*(sin(theta5)*(sin(theta1)*sin(theta2)*sin(theta3) - cos(theta2)*cos(theta3)*sin(theta1)) - cos(theta4)*cos(theta5)*(cos(theta2)*sin(theta1)*sin(theta3) + cos(theta3)*sin(theta1)*sin(theta2))) + sin(theta4)*sin(theta6)*(cos(theta2)*sin(theta1)*sin(theta3) + cos(theta3)*sin(theta1)*sin(theta2)), cos(theta6)*sin(theta4)*(cos(theta2)*sin(theta1)*sin(theta3) + cos(theta3)*sin(theta1)*sin(theta2)) - sin(theta6)*(sin(theta5)*(sin(theta1)*sin(theta2)*sin(theta3) - cos(theta2)*cos(theta3)*sin(theta1)) - cos(theta4)*cos(theta5)*(cos(theta2)*sin(theta1)*sin(theta3) + cos(theta3)*sin(theta1)*sin(theta2))), - cos(theta5)*(sin(theta1)*sin(theta2)*sin(theta3) - cos(theta2)*cos(theta3)*sin(theta1)) - cos(theta4)*sin(theta5)*(cos(theta2)*sin(theta1)*sin(theta3) + cos(theta3)*sin(theta1)*sin(theta2)), - d4*(sin(theta1)*sin(theta2)*sin(theta3) - cos(theta2)*cos(theta3)*sin(theta1)) - d6*(cos(theta5)*(sin(theta1)*sin(theta2)*sin(theta3) - cos(theta2)*cos(theta3)*sin(theta1)) + cos(theta4)*sin(theta5)*(cos(theta2)*sin(theta1)*sin(theta3) + cos(theta3)*sin(theta1)*sin(theta2))) - a2*sin(theta1)*sin(theta2) - a3*cos(theta2)*sin(theta1)*sin(theta3) - a3*cos(theta3)*sin(theta1)*sin(theta2)],
[                                                                       - cos(theta6)*(sin(theta5)*(cos(theta2)*sin(theta3) + cos(theta3)*sin(theta2)) - cos(theta4)*cos(theta5)*(cos(theta2)*cos(theta3) - sin(theta2)*sin(theta3))) - sin(theta4)*sin(theta6)*(cos(theta2)*cos(theta3) - sin(theta2)*sin(theta3)),                                                                         sin(theta6)*(sin(theta5)*(cos(theta2)*sin(theta3) + cos(theta3)*sin(theta2)) - cos(theta4)*cos(theta5)*(cos(theta2)*cos(theta3) - sin(theta2)*sin(theta3))) - cos(theta6)*sin(theta4)*(cos(theta2)*cos(theta3) - sin(theta2)*sin(theta3)),                                                   cos(theta5)*(cos(theta2)*sin(theta3) + cos(theta3)*sin(theta2)) + cos(theta4)*sin(theta5)*(cos(theta2)*cos(theta3) - sin(theta2)*sin(theta3)),                                                                                                               d4*(cos(theta2)*sin(theta3) + cos(theta3)*sin(theta2)) + a2*cos(theta2) + d6*(cos(theta5)*(cos(theta2)*sin(theta3) + cos(theta3)*sin(theta2)) + cos(theta4)*sin(theta5)*(cos(theta2)*cos(theta3) - sin(theta2)*sin(theta3))) + a3*cos(theta2)*cos(theta3) - a3*sin(theta2)*sin(theta3)],
[                                                                                                                                                                                                                                                                                                                 0,                                                                                                                                                                                                                                                                                                                 0,                                                                                                                                                                                               0,                                                                                                                                                                                                                                                                                                                                                                                                    0]])
u63=numpy.mat([[ cos(theta5)*cos(theta6)*(cos(theta4)*sin(theta1) + sin(theta4)*(cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)*cos(theta2)*cos(theta3))) - sin(theta6)*(sin(theta1)*sin(theta4) - cos(theta4)*(cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)*cos(theta2)*cos(theta3))), - cos(theta6)*(sin(theta1)*sin(theta4) - cos(theta4)*(cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)*cos(theta2)*cos(theta3))) - cos(theta5)*sin(theta6)*(cos(theta4)*sin(theta1) + sin(theta4)*(cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)*cos(theta2)*cos(theta3))),  sin(theta5)*(cos(theta4)*sin(theta1) + sin(theta4)*(cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)*cos(theta2)*cos(theta3))),  d6*sin(theta5)*(cos(theta4)*sin(theta1) + sin(theta4)*(cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)*cos(theta2)*cos(theta3)))],
[ sin(theta6)*(cos(theta1)*sin(theta4) + cos(theta4)*(sin(theta1)*sin(theta2)*sin(theta3) - cos(theta2)*cos(theta3)*sin(theta1))) - cos(theta5)*cos(theta6)*(cos(theta1)*cos(theta4) - sin(theta4)*(sin(theta1)*sin(theta2)*sin(theta3) - cos(theta2)*cos(theta3)*sin(theta1))),   cos(theta6)*(cos(theta1)*sin(theta4) + cos(theta4)*(sin(theta1)*sin(theta2)*sin(theta3) - cos(theta2)*cos(theta3)*sin(theta1))) + cos(theta5)*sin(theta6)*(cos(theta1)*cos(theta4) - sin(theta4)*(sin(theta1)*sin(theta2)*sin(theta3) - cos(theta2)*cos(theta3)*sin(theta1))), -sin(theta5)*(cos(theta1)*cos(theta4) - sin(theta4)*(sin(theta1)*sin(theta2)*sin(theta3) - cos(theta2)*cos(theta3)*sin(theta1))), -d6*sin(theta5)*(cos(theta1)*cos(theta4) - sin(theta4)*(sin(theta1)*sin(theta2)*sin(theta3) - cos(theta2)*cos(theta3)*sin(theta1)))],
[                                                                                                       - cos(theta4)*sin(theta6)*(cos(theta2)*sin(theta3) + cos(theta3)*sin(theta2)) - cos(theta5)*cos(theta6)*sin(theta4)*(cos(theta2)*sin(theta3) + cos(theta3)*sin(theta2)),                                                                                                           cos(theta5)*sin(theta4)*sin(theta6)*(cos(theta2)*sin(theta3) + cos(theta3)*sin(theta2)) - cos(theta4)*cos(theta6)*(cos(theta2)*sin(theta3) + cos(theta3)*sin(theta2)),                                                     -sin(theta4)*sin(theta5)*(cos(theta2)*sin(theta3) + cos(theta3)*sin(theta2)),                                                     -d6*sin(theta4)*sin(theta5)*(cos(theta2)*sin(theta3) + cos(theta3)*sin(theta2))],
[                                                                                                                                                                                                                                                                             0,                                                                                                                                                                                                                                                                               0,                                                                                                                                0,                                                                                                                                   0]])
u64=numpy.mat([[ cos(theta5)*cos(theta6)*(cos(theta4)*sin(theta1) + sin(theta4)*(cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)*cos(theta2)*cos(theta3))) - sin(theta6)*(sin(theta1)*sin(theta4) - cos(theta4)*(cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)*cos(theta2)*cos(theta3))), - cos(theta6)*(sin(theta1)*sin(theta4) - cos(theta4)*(cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)*cos(theta2)*cos(theta3))) - cos(theta5)*sin(theta6)*(cos(theta4)*sin(theta1) + sin(theta4)*(cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)*cos(theta2)*cos(theta3))),  sin(theta5)*(cos(theta4)*sin(theta1) + sin(theta4)*(cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)*cos(theta2)*cos(theta3))),  d6*sin(theta5)*(cos(theta4)*sin(theta1) + sin(theta4)*(cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)*cos(theta2)*cos(theta3)))],
[ sin(theta6)*(cos(theta1)*sin(theta4) + cos(theta4)*(sin(theta1)*sin(theta2)*sin(theta3) - cos(theta2)*cos(theta3)*sin(theta1))) - cos(theta5)*cos(theta6)*(cos(theta1)*cos(theta4) - sin(theta4)*(sin(theta1)*sin(theta2)*sin(theta3) - cos(theta2)*cos(theta3)*sin(theta1))),   cos(theta6)*(cos(theta1)*sin(theta4) + cos(theta4)*(sin(theta1)*sin(theta2)*sin(theta3) - cos(theta2)*cos(theta3)*sin(theta1))) + cos(theta5)*sin(theta6)*(cos(theta1)*cos(theta4) - sin(theta4)*(sin(theta1)*sin(theta2)*sin(theta3) - cos(theta2)*cos(theta3)*sin(theta1))), -sin(theta5)*(cos(theta1)*cos(theta4) - sin(theta4)*(sin(theta1)*sin(theta2)*sin(theta3) - cos(theta2)*cos(theta3)*sin(theta1))), -d6*sin(theta5)*(cos(theta1)*cos(theta4) - sin(theta4)*(sin(theta1)*sin(theta2)*sin(theta3) - cos(theta2)*cos(theta3)*sin(theta1)))],
[                                                                                                       - cos(theta4)*sin(theta6)*(cos(theta2)*sin(theta3) + cos(theta3)*sin(theta2)) - cos(theta5)*cos(theta6)*sin(theta4)*(cos(theta2)*sin(theta3) + cos(theta3)*sin(theta2)),                                                                                                           cos(theta5)*sin(theta4)*sin(theta6)*(cos(theta2)*sin(theta3) + cos(theta3)*sin(theta2)) - cos(theta4)*cos(theta6)*(cos(theta2)*sin(theta3) + cos(theta3)*sin(theta2)),                                                     -sin(theta4)*sin(theta5)*(cos(theta2)*sin(theta3) + cos(theta3)*sin(theta2)),                                                     -d6*sin(theta4)*sin(theta5)*(cos(theta2)*sin(theta3) + cos(theta3)*sin(theta2))],
[                                                                                                                                                                                                                                                                             0,                                                                                                                                                                                                                                                                               0,                                                                                                                                0,                                                                                                                                   0]])
u65=numpy.mat([[ -cos(theta6)*(sin(theta5)*(sin(theta1)*sin(theta4) - cos(theta4)*(cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)*cos(theta2)*cos(theta3))) + cos(theta5)*(cos(theta1)*cos(theta2)*sin(theta3) + cos(theta1)*cos(theta3)*sin(theta2))),  sin(theta6)*(sin(theta5)*(sin(theta1)*sin(theta4) - cos(theta4)*(cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)*cos(theta2)*cos(theta3))) + cos(theta5)*(cos(theta1)*cos(theta2)*sin(theta3) + cos(theta1)*cos(theta3)*sin(theta2))),   cos(theta5)*(sin(theta1)*sin(theta4) - cos(theta4)*(cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)*cos(theta2)*cos(theta3))) - sin(theta5)*(cos(theta1)*cos(theta2)*sin(theta3) + cos(theta1)*cos(theta3)*sin(theta2)),  d6*(cos(theta5)*(sin(theta1)*sin(theta4) - cos(theta4)*(cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)*cos(theta2)*cos(theta3))) - sin(theta5)*(cos(theta1)*cos(theta2)*sin(theta3) + cos(theta1)*cos(theta3)*sin(theta2)))],
[  cos(theta6)*(sin(theta5)*(cos(theta1)*sin(theta4) + cos(theta4)*(sin(theta1)*sin(theta2)*sin(theta3) - cos(theta2)*cos(theta3)*sin(theta1))) - cos(theta5)*(cos(theta2)*sin(theta1)*sin(theta3) + cos(theta3)*sin(theta1)*sin(theta2))), -sin(theta6)*(sin(theta5)*(cos(theta1)*sin(theta4) + cos(theta4)*(sin(theta1)*sin(theta2)*sin(theta3) - cos(theta2)*cos(theta3)*sin(theta1))) - cos(theta5)*(cos(theta2)*sin(theta1)*sin(theta3) + cos(theta3)*sin(theta1)*sin(theta2))), - cos(theta5)*(cos(theta1)*sin(theta4) + cos(theta4)*(sin(theta1)*sin(theta2)*sin(theta3) - cos(theta2)*cos(theta3)*sin(theta1))) - sin(theta5)*(cos(theta2)*sin(theta1)*sin(theta3) + cos(theta3)*sin(theta1)*sin(theta2)), -d6*(cos(theta5)*(cos(theta1)*sin(theta4) + cos(theta4)*(sin(theta1)*sin(theta2)*sin(theta3) - cos(theta2)*cos(theta3)*sin(theta1))) + sin(theta5)*(cos(theta2)*sin(theta1)*sin(theta3) + cos(theta3)*sin(theta1)*sin(theta2)))],
[    cos(theta6)*(cos(theta5)*(cos(theta2)*cos(theta3) - sin(theta2)*sin(theta3)) - cos(theta4)*sin(theta5)*(cos(theta2)*sin(theta3) + cos(theta3)*sin(theta2))),   -sin(theta6)*(cos(theta5)*(cos(theta2)*cos(theta3) - sin(theta2)*sin(theta3)) - cos(theta4)*sin(theta5)*(cos(theta2)*sin(theta3) + cos(theta3)*sin(theta2))),                                                                               sin(theta5)*(cos(theta2)*cos(theta3) - sin(theta2)*sin(theta3)) + cos(theta4)*cos(theta5)*(cos(theta2)*sin(theta3) + cos(theta3)*sin(theta2)),    d6*(sin(theta5)*(cos(theta2)*cos(theta3) - sin(theta2)*sin(theta3)) + cos(theta4)*cos(theta5)*(cos(theta2)*sin(theta3) + cos(theta3)*sin(theta2)))],
[                                                 0,           0,        0,                                                                                            0]])
u66=numpy.mat([[ cos(theta6)*(cos(theta4)*sin(theta1) + sin(theta4)*(cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)*cos(theta2)*cos(theta3))) - sin(theta6)*(cos(theta5)*(sin(theta1)*sin(theta4) - cos(theta4)*(cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)*cos(theta2)*cos(theta3))) - sin(theta5)*(cos(theta1)*cos(theta2)*sin(theta3) + cos(theta1)*cos(theta3)*sin(theta2))), - sin(theta6)*(cos(theta4)*sin(theta1) + sin(theta4)*(cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)*cos(theta2)*cos(theta3))) - cos(theta6)*(cos(theta5)*(sin(theta1)*sin(theta4) - cos(theta4)*(cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)*cos(theta2)*cos(theta3))) - sin(theta5)*(cos(theta1)*cos(theta2)*sin(theta3) + cos(theta1)*cos(theta3)*sin(theta2))), 0, 0],
[ sin(theta6)*(cos(theta5)*(cos(theta1)*sin(theta4) + cos(theta4)*(sin(theta1)*sin(theta2)*sin(theta3) - cos(theta2)*cos(theta3)*sin(theta1))) + sin(theta5)*(cos(theta2)*sin(theta1)*sin(theta3) + cos(theta3)*sin(theta1)*sin(theta2))) - cos(theta6)*(cos(theta1)*cos(theta4) - sin(theta4)*(sin(theta1)*sin(theta2)*sin(theta3) - cos(theta2)*cos(theta3)*sin(theta1))),   sin(theta6)*(cos(theta1)*cos(theta4) - sin(theta4)*(sin(theta1)*sin(theta2)*sin(theta3) - cos(theta2)*cos(theta3)*sin(theta1))) + cos(theta6)*(cos(theta5)*(cos(theta1)*sin(theta4) + cos(theta4)*(sin(theta1)*sin(theta2)*sin(theta3) - cos(theta2)*cos(theta3)*sin(theta1))) + sin(theta5)*(cos(theta2)*sin(theta1)*sin(theta3) + cos(theta3)*sin(theta1)*sin(theta2))), 0, 0],
[                                                                                                                               - sin(theta6)*(sin(theta5)*(cos(theta2)*cos(theta3) - sin(theta2)*sin(theta3)) + cos(theta4)*cos(theta5)*(cos(theta2)*sin(theta3) + cos(theta3)*sin(theta2))) - cos(theta6)*sin(theta4)*(cos(theta2)*sin(theta3) + cos(theta3)*sin(theta2)),                                                                                                                                   sin(theta4)*sin(theta6)*(cos(theta2)*sin(theta3) + cos(theta3)*sin(theta2)) - cos(theta6)*(sin(theta5)*(cos(theta2)*cos(theta3) - sin(theta2)*sin(theta3)) + cos(theta4)*cos(theta5)*(cos(theta2)*sin(theta3) + cos(theta3)*sin(theta2))), 0, 0],
[                                                                                                                                                                                                                                                                                                                                                                         0,                                                                                                                                                                                                                                                                                                                                                                           0, 0, 0]])
'''
u111=numpy.mat([[ -cos(theta1), 0, -sin(theta1), 0],
[ -sin(theta1), 0,  cos(theta1), 0],
[            0, 0,            0, 0],
[            0, 0,            0, 0]])
u211=numpy.mat([[ -cos(theta1)*cos(theta2), cos(theta1)*sin(theta2), -sin(theta1), -a2*cos(theta1)*cos(theta2)],
[ -cos(theta2)*sin(theta1), sin(theta1)*sin(theta2),  cos(theta1), -a2*cos(theta2)*sin(theta1)],
[                        0,                       0,            0,                           0],
[                        0,                       0,            0,                           0]])
u212=numpy.mat([[  sin(theta1)*sin(theta2),  cos(theta2)*sin(theta1), 0,  a2*sin(theta1)*sin(theta2)],
[ -cos(theta1)*sin(theta2), -cos(theta1)*cos(theta2), 0, -a2*cos(theta1)*sin(theta2)],
[                        0,                        0, 0,                           0],
[                        0,                        0, 0,                           0]])
u221=numpy.mat([[ -cos(theta1)*cos(theta2), cos(theta1)*sin(theta2), 0, -a2*cos(theta1)*cos(theta2)],
[ -cos(theta2)*sin(theta1), sin(theta1)*sin(theta2), 0, -a2*cos(theta2)*sin(theta1)],
[             -sin(theta2),            -cos(theta2), 0,             -a2*sin(theta2)],
[                        0,                       0, 0,                           0]])
u222=numpy.mat([[ -cos(theta1)*cos(theta2), cos(theta1)*sin(theta2), 0, -a2*cos(theta1)*cos(theta2)],
[ -cos(theta2)*sin(theta1), sin(theta1)*sin(theta2), 0, -a2*cos(theta2)*sin(theta1)],
[             -sin(theta2),            -cos(theta2), 0,             -a2*sin(theta2)],
[                        0,                       0, 0,                           0]])
u311=numpy.mat([[ cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)*cos(theta2)*cos(theta3), -sin(theta1), - cos(theta1)*cos(theta2)*sin(theta3) - cos(theta1)*cos(theta3)*sin(theta2), a3*cos(theta1)*sin(theta2)*sin(theta3) - a3*cos(theta1)*cos(theta2)*cos(theta3) - a2*cos(theta1)*cos(theta2)],
[ sin(theta1)*sin(theta2)*sin(theta3) - cos(theta2)*cos(theta3)*sin(theta1),  cos(theta1), - cos(theta2)*sin(theta1)*sin(theta3) - cos(theta3)*sin(theta1)*sin(theta2), a3*sin(theta1)*sin(theta2)*sin(theta3) - a3*cos(theta2)*cos(theta3)*sin(theta1) - a2*cos(theta2)*sin(theta1)],
[                                                                         0,            0,                                                                           0,                                                                                                            0],
[                                                                         0,            0,                                                                           0,                                                                                                            0]])
u312=numpy.mat([[   cos(theta2)*sin(theta1)*sin(theta3) + cos(theta3)*sin(theta1)*sin(theta2), 0, sin(theta1)*sin(theta2)*sin(theta3) - cos(theta2)*cos(theta3)*sin(theta1),   a2*sin(theta1)*sin(theta2) + a3*cos(theta2)*sin(theta1)*sin(theta3) + a3*cos(theta3)*sin(theta1)*sin(theta2)],
[ - cos(theta1)*cos(theta2)*sin(theta3) - cos(theta1)*cos(theta3)*sin(theta2), 0, cos(theta1)*cos(theta2)*cos(theta3) - cos(theta1)*sin(theta2)*sin(theta3), - a2*cos(theta1)*sin(theta2) - a3*cos(theta1)*cos(theta2)*sin(theta3) - a3*cos(theta1)*cos(theta3)*sin(theta2)],
[                                                                           0, 0,                                                                         0,                                                                                                              0],
[                                                                           0, 0,                                                                         0,                                                                                                              0]])
u313=numpy.mat([[   cos(theta2)*sin(theta1)*sin(theta3) + cos(theta3)*sin(theta1)*sin(theta2), 0, sin(theta1)*sin(theta2)*sin(theta3) - cos(theta2)*cos(theta3)*sin(theta1),   a3*cos(theta2)*sin(theta1)*sin(theta3) + a3*cos(theta3)*sin(theta1)*sin(theta2)],
[ - cos(theta1)*cos(theta2)*sin(theta3) - cos(theta1)*cos(theta3)*sin(theta2), 0, cos(theta1)*cos(theta2)*cos(theta3) - cos(theta1)*sin(theta2)*sin(theta3), - a3*cos(theta1)*cos(theta2)*sin(theta3) - a3*cos(theta1)*cos(theta3)*sin(theta2)],
[                                                                           0, 0,                                                                         0,                                                                                 0],
[                                                                           0, 0,                                                                         0,                                                                                 0]])
u321=numpy.mat([[   cos(theta2)*sin(theta1)*sin(theta3) + cos(theta3)*sin(theta1)*sin(theta2), 0, sin(theta1)*sin(theta2)*sin(theta3) - cos(theta2)*cos(theta3)*sin(theta1),   a2*sin(theta1)*sin(theta2) + a3*cos(theta2)*sin(theta1)*sin(theta3) + a3*cos(theta3)*sin(theta1)*sin(theta2)],
[ - cos(theta1)*cos(theta2)*sin(theta3) - cos(theta1)*cos(theta3)*sin(theta2), 0, cos(theta1)*cos(theta2)*cos(theta3) - cos(theta1)*sin(theta2)*sin(theta3), - a2*cos(theta1)*sin(theta2) - a3*cos(theta1)*cos(theta2)*sin(theta3) - a3*cos(theta1)*cos(theta3)*sin(theta2)],
[                                                                           0, 0,                                                                         0,                                                                                                              0],
[                                                                           0, 0,                                                                         0,                                                                                                              0]])
u322=numpy.mat([[ cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)*cos(theta2)*cos(theta3), 0, - cos(theta1)*cos(theta2)*sin(theta3) - cos(theta1)*cos(theta3)*sin(theta2), a3*cos(theta1)*sin(theta2)*sin(theta3) - a3*cos(theta1)*cos(theta2)*cos(theta3) - a2*cos(theta1)*cos(theta2)],
[ sin(theta1)*sin(theta2)*sin(theta3) - cos(theta2)*cos(theta3)*sin(theta1), 0, - cos(theta2)*sin(theta1)*sin(theta3) - cos(theta3)*sin(theta1)*sin(theta2), a3*sin(theta1)*sin(theta2)*sin(theta3) - a3*cos(theta2)*cos(theta3)*sin(theta1) - a2*cos(theta2)*sin(theta1)],
[                       - cos(theta2)*sin(theta3) - cos(theta3)*sin(theta2), 0,                           cos(theta2)*cos(theta3) - sin(theta2)*sin(theta3),                                   - a2*sin(theta2) - a3*cos(theta2)*sin(theta3) - a3*cos(theta3)*sin(theta2)],
[                                                                         0, 0,                                                                           0,                                                                                                            0]])
u323=numpy.mat([[ cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)*cos(theta2)*cos(theta3), 0, - cos(theta1)*cos(theta2)*sin(theta3) - cos(theta1)*cos(theta3)*sin(theta2), a3*cos(theta1)*sin(theta2)*sin(theta3) - a3*cos(theta1)*cos(theta2)*cos(theta3)],
[ sin(theta1)*sin(theta2)*sin(theta3) - cos(theta2)*cos(theta3)*sin(theta1), 0, - cos(theta2)*sin(theta1)*sin(theta3) - cos(theta3)*sin(theta1)*sin(theta2), a3*sin(theta1)*sin(theta2)*sin(theta3) - a3*cos(theta2)*cos(theta3)*sin(theta1)],
[                       - cos(theta2)*sin(theta3) - cos(theta3)*sin(theta2), 0,                           cos(theta2)*cos(theta3) - sin(theta2)*sin(theta3),                       - a3*cos(theta2)*sin(theta3) - a3*cos(theta3)*sin(theta2)],
[                                                                         0, 0,                                                                           0,                                                                               0]])
u331=numpy.mat([[   cos(theta2)*sin(theta1)*sin(theta3) + cos(theta3)*sin(theta1)*sin(theta2), 0, sin(theta1)*sin(theta2)*sin(theta3) - cos(theta2)*cos(theta3)*sin(theta1),   a3*cos(theta2)*sin(theta1)*sin(theta3) + a3*cos(theta3)*sin(theta1)*sin(theta2)],
[ - cos(theta1)*cos(theta2)*sin(theta3) - cos(theta1)*cos(theta3)*sin(theta2), 0, cos(theta1)*cos(theta2)*cos(theta3) - cos(theta1)*sin(theta2)*sin(theta3), - a3*cos(theta1)*cos(theta2)*sin(theta3) - a3*cos(theta1)*cos(theta3)*sin(theta2)],
[                                                                           0, 0,                                                                         0,                                                                                 0],
[                                                                           0, 0,                                                                         0,                                                                                 0]])
u332=numpy.mat([[ cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)*cos(theta2)*cos(theta3), 0, - cos(theta1)*cos(theta2)*sin(theta3) - cos(theta1)*cos(theta3)*sin(theta2), a3*cos(theta1)*sin(theta2)*sin(theta3) - a3*cos(theta1)*cos(theta2)*cos(theta3)],
[ sin(theta1)*sin(theta2)*sin(theta3) - cos(theta2)*cos(theta3)*sin(theta1), 0, - cos(theta2)*sin(theta1)*sin(theta3) - cos(theta3)*sin(theta1)*sin(theta2), a3*sin(theta1)*sin(theta2)*sin(theta3) - a3*cos(theta2)*cos(theta3)*sin(theta1)],
[                       - cos(theta2)*sin(theta3) - cos(theta3)*sin(theta2), 0,                           cos(theta2)*cos(theta3) - sin(theta2)*sin(theta3),                       - a3*cos(theta2)*sin(theta3) - a3*cos(theta3)*sin(theta2)],
[                                                                         0, 0,                                                                           0,                                                                               0]])
u333=numpy.mat([[ cos(theta1)*sin(theta2)*sin(theta3) - cos(theta1)*cos(theta2)*cos(theta3), 0, - cos(theta1)*cos(theta2)*sin(theta3) - cos(theta1)*cos(theta3)*sin(theta2), a3*cos(theta1)*sin(theta2)*sin(theta3) - a3*cos(theta1)*cos(theta2)*cos(theta3)],
[ sin(theta1)*sin(theta2)*sin(theta3) - cos(theta2)*cos(theta3)*sin(theta1), 0, - cos(theta2)*sin(theta1)*sin(theta3) - cos(theta3)*sin(theta1)*sin(theta2), a3*sin(theta1)*sin(theta2)*sin(theta3) - a3*cos(theta2)*cos(theta3)*sin(theta1)],
[                       - cos(theta2)*sin(theta3) - cos(theta3)*sin(theta2), 0,                           cos(theta2)*cos(theta3) - sin(theta2)*sin(theta3),                       - a3*cos(theta2)*sin(theta3) - a3*cos(theta3)*sin(theta2)],
[                                                                         0, 0,                                                                           0,                                                                               0]])

'''求Di'''
D3=-m3*gg*u33*r3
D2=-m2*gg*u22*r2-m3*gg*u32*r3
D1=-m1*gg*u11*r1-m2*gg*u21*r2-m3*gg*u31*r3

'''求D1j'''
sd13=u33*I3*u31.transpose()
d13=0
for i in range(4):
    trace=sd13[i,i]
    d13=d13+trace

sd12=u22*I2*u21.transpose()
d12=0
for i in range(4):
    trace=sd12[i,i]
    d12=d12+trace

sd11=u11*I1*u11.transpose()
d11=0
for i in range (4):
    trace=sd11[i,i]
    d11=d11+trace

'''求D2j'''
sd23=u33*I3*u32.transpose()
d23=0
for i in range(4):
    trace=sd23[i,i]
    d23=d23+trace

sd22=u22*I2*u22.transpose()
d22=0
for i in range(4):
    trace=sd22[i,i]
    d22=d22+trace

sd21=u21*I2*u22.transpose()
d21=0
for i in range(4):
    trace=sd21[i,i]
    d21=d21+trace

'''求D3j'''
sd33=u33*I3*u33.transpose()
d33=0
for i in range(4):
    trace=sd33[i,i]
    d33=d33+trace

sd32=u32*I3*u33.transpose()
d32=0
for i in range(4):
    trace=sd32[i,i]
    d32=d32+trace

sd31=u31*I3*u33.transpose()
d31=0
for i in range(4):
    trace=sd31[i,i]
    d31=d31+trace


'''求加速度'''
jia111=u111*I1*u11.transpose()
d111=0
for i in range(4):
    trace=jia111[i,i]
    d111=d111+trace

jia112=u212*I2*u21.transpose()
d112=numpy.trace(jia112)

jia113=u313*I3*u31.transpose()
d113=numpy.trace(jia113)

jia121=u221*I2*u21.transpose()
d121=numpy.trace(jia121)

jia122=u222*I2*u21.transpose()
d122=numpy.trace(jia122)

jia123=u323*I3*u31.transpose()
d123=numpy.trace(jia123)

jia131= u331*I3*u31.transpose()
d131=numpy.trace(jia131)

jia132=u332*I3*u31.transpose()
d132=numpy.trace(jia132)

jia133=u333*I3*u31.transpose()
d133=numpy.trace(jia133)

jia211=u211*I2*u22.transpose()
d211=numpy.trace(jia211)

jia212=u212*I2*u22.transpose()
d212=numpy.trace(jia212)

jia213=u313*I3*u32.transpose()
d213=numpy.trace(jia213)

jia221=u221*I2*u22.transpose()
d221=numpy.trace(jia221)

jia222=u222*I2*u22.transpose()
d222=numpy.trace(jia222)

jia223=u322*I3*u32.transpose()
d223=numpy.trace(jia223)

jia231=u331*I3*u32.transpose()
d231=numpy.trace(jia231)

jia232=u332*I3*u32.transpose()
d232=numpy.trace(jia232)

jia233=u333*I3*u33.transpose()
d233=numpy.trace(jia233)

jia311=u311*I3*u33.transpose()
d311=numpy.trace(jia311)

jia312=u312*I3*u33.transpose()
d312=numpy.trace(jia312)

jia313=u313*I3*u33.transpose()
d313=numpy.trace(jia313)

jia321=u321*I3*u33.transpose()
d321=numpy.trace(jia321)

jia322=u322*I3*u33.transpose()
d322=numpy.trace(jia322)

jia323=u323*I3*u33.transpose()
d323=numpy.trace(jia323)

jia331=u331*I3*u33.transpose()
d331=numpy.trace(jia331)

jia332=u332*I3*u33.transpose()
d332=numpy.trace(jia332)

jia333=u333*I3*u33.transpose()
d333=numpy.trace(jia333)

a1=a2=a3=1
v1=v2=v3=3
'''总的公式n=3'''
T_=d11*a1+d12*a2+d13*a3
T__=d111*v1*v1+\
    d112*v1*v2+\
    d113*v1*v3+\
    d121*v2*v1+\
    d122*v2*v2+\
    d123*v2*v3+\
    d131*v3*v1+\
    d132*v3*v2+\
    d133*v3*v3
T___=D1
Tzhong=T_+T__+T___+20*a1
print(Tzhong)
'''算第一个关节的力'''
重智成城
关注
  • 0
    点赞
  • 踩
  • 0
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
专栏目录
matlab 求未知函数,求解具有未知参数的 BVP
weixin_29958449的博客
03-23 2368
编写方程代码创建一个函数以编写方程代码。此函数应具有签名 dydx = mat4ode(x,y,lambda)，其中：x 是自变量。y 是因变量。lambda 是表示特征值的未知参数。您可以用代换法 y1=y 和 y2=y′ 将马蒂厄方程写成一阶方程组，y1′=y2，y2′=-(λ-2qcos(2x))y1。则对应的函数是function dydx = mat4ode(x,y,lambda) % ...
matlab矩阵赋未知数,matlab设未知数
weixin_39750190的博客
03-18 5777
Matlab 解非线性超定方程组 3x+2/(5+y)=6, 4x+4/(5+y)=7, 9x+4/(8+y)=12 11x+2/(4+y)=15 x,y 是未知数 --- clc;clear; %其实楼主的问......二元不定方程整数解的 MATLAB 解法 赵祈芬,高丽,辛伟伟 【摘要】利用 MATLAB 数学软件丰富的函数定义和相关计算机语言,给出了 在一定范围内求二元不定方程整数解......
python实现共轭梯度算法
HelloWorldTM的博客
03-22 1万+
python实现共轭梯度优化算法一、共轭梯度算法简介二、实现共轭梯度方法的两块重要积木1.共轭方向的确定2.方向优化步长的确定note3.共轭梯度算法优化过程 一、共轭梯度算法简介 共轭梯度(Conjugate Gradient)方法是一种迭代算法，可用于求解无约束的优化问题，例如能量最小化。常见的优化算法还有梯度下降法，相比于梯度下降法，共轭梯度法具有收敛更快，以及算法稳定性更好的优点。 从上图可以看出来，梯度下降法优化过程中函数是沿着梯度的反方向逐步优化，后一步优化的结果会对前一步的优化结果造成影响，
未知USB设备（设备描述符请求失败）解决方法
热门推荐
qq_40233706的博客
03-14 29万+
问题如图： 第一步: windows键+r键 -》输入：service.msc 第二步： 在服务窗口中找到Plug and Play服务，双击Plug and Play，打开Plug and Play的属性窗口： 首先将Plug and Play服务的启动类型为：禁用，停止服务，点击应用，确定，此时启动类型为：禁用，服务状态为：已停止， 然后再点击 Plug and Play服...
未知usb设备设备描述符请求失败怎么解决
eli的博客
10-14 9万+
设备描述符请求失败
Windows中的未知账户删除|电脑未知账户怎么删除
最新发布
天涯的浪子
05-31 1584
有的时候，因为重装Windows系统，或者其它帐户类的操作导致产生未知账户，另外最近安装阿里云盘和语雀等软件后打不开没反应，也可用这种方法解决，这篇文章是本站给大家带来的Windows中的未知账户删除方法。
CVPR 2022 | 未知目标检测模块STUD：学习视频中的未知目标
CV技术指南（微信公众号）
06-06 1500
欢迎关注公众号CV技术指南，专注于计算机视觉的技术总结、最新技术跟踪、经典论文解读、CV招聘信息。论文：Unknown-Aware Object Detection: Learning What You Don't Know from Videos in the Wild论文：https://arxiv.org/abs/2203.03800代码：https://github.com/deeplearning-wisc/stud目标检测模型在被训练的已知上下文中已经取得了巨大的成功，但是在处理网络训练时没有接
计算机管理显示磁盘未知,磁盘未知，未初始化或未分配问题的解决方案
weixin_35657239的博客
07-10 3万+
大家是否遇到过此类问题，当将外部硬盘插入计算机并找到提示该驱动器已成功安装但没有出现在“此PC”上的通知？当进入Window的磁盘管理实用程序，发现磁盘是未知的，未初始化的，未分配的，那么如何修复未初始化的磁盘？还有丢失数据之后怎么找回？”第1部分：初始化磁盘意味着什么？初始化存储设备(非常类似于格式化过程)是一种擦除驱动器上的数据并允许其在以前不使用时可以再次使用的方法。要进行初始化，可以通过将...
AutoCAD输入命令突然显示 未知命令。按 F1查看帮助。
gengli2017的博客
12-12 1644
网上说的什么病毒，卸载重装等无效。结果发现输入的字符是全角的，不是半角的，就输入法的问题，不是CAD的问题。如果不知道什么是全/半角，网上搜一下。你也可以打开个记事本，输入英文字母试试，发现字符之间隔得很宽。把输入法改成半角就行了，不同输入法有不同快捷键，试试 Shift+空格、Win+空格、Ctrl+Shift、Ctrl+空格等组合，或者去查对应输入法的全/半角切换方式。CAD一直好的，突然坏了，不能输入命令了，其他功能正常。输入命令显示“未知命令XXX，按 F1 查看帮助。
c语言 未知大小数组,关于C#：大小未知的可变长度数组
weixin_39892309的博客
05-24 2259
在C语言中，由于我使用的是通过编译器生成的框架，因此需要使用全局可变长度数组。但是，直到运行时，我才能知道其尺寸的大小(例如，argv)。因此，我想声明一个未知大小的全局变量长度数组，然后定义其大小。我已经做到了：int (*a)[]; //global variable length arrayint main(){//defining it's sizea = (int(*)[2]) mall...
控制方向未知条件下不确定船舶航向鲁棒lambda调节控制
01-14
为了进一步解决模型存在控制方向未知和外界环境干扰的船舶航向保持控制问题，在Backstepping方法基础上，引入Nussbaum增益技术，提出一种适用于新的、能够人为指定稳态控制性能的鲁棒 调节控制方法。考虑到一般系统...
JavaScript中获取未知对象属性的代码
10-28
在JavaScript中，处理未知对象属性是一项常见的任务，尤其是在进行动态编程或者与第三方库交互时。JavaScript的对象机制允许我们灵活地创建和访问属性，即使在对象创建后也可以动态添加新的属性。这种灵活性来源于...
Android8.0系统允许安装未知来源权限
06-29
其中一个显著变化是针对应用程序安装来源的管理，特别是对于"未知来源"的安装权限控制。这个变化直接影响了用户如何安装非Google Play商店的应用程序，对开发者和用户都有重要影响。 在Android 8.0之前，用户可以在...
Mac App Store出现“未知错误”解决方法.docx
09-26
Mac App Store出现“未知错误”解决方法 Mac App Store是一个 macOS 操作系统中的应用程序商店，用户可以通过它下载和更新应用程序。但是，有时候用户可能会遇到“未知错误”的问题，无法下载或更新应用程序。今天...
未知输入观测器设计和仿真示例：LTI 系统的未知输入观测器的设计和仿真-matlab开发
05-29
在本文中，我们将深入探讨如何使用MATLAB进行未知输入观测器（Unknown Input Observer，简称UIO）的设计与仿真，特别是针对线性时不变（Linear Time-Invariant，LTI）系统的应用。未知输入观测器是一种特殊的状态...
激活函数的作用
weixin_44287946的博客
11-16 3857
使用pytorch弄一个单层网络，但是激活函数选择线性函数，不然永远达不到要求， 激活函数的作用，摘自知乎。 f(*)称为激活函数或激励函数（Activation Function），激活函数的主要作用是完成数据的非线性变换，解决线性模型的表达、分类能力不足的问题; 激活函数的主要作用是改变之前数据的线性关系，如果网络中全部是线性变换，则多层网络可以通过矩阵变换，直接转换成一层神经网络。所以激活函数的存在，使得神经网络的“多层”有了实际的意义，使网络更加强大，增加网络的能力，使它可以学习复杂的事物，复杂的数
基于pybullet的机械臂导纳控制实验
weixin_44287946的博客
12-09 2973
基于pybullet的机械臂导纳控制实验——根据知乎韩冰老师的文章 谢谢韩冰老师的原创图： 这副图大概的意思是： 对机械臂的末端执行器搭载的外部六维力矩传感器施加一个力，导纳控制公式，得到一个(x…加速度),加速度通过积分，可以使用梯形公式等数值积分方法得到(x.)速度，再通过积分得到(x)位置。上面的加速度，速度，位置都是施加力引起的位移变化，所以要在原来的位置上加上这些变化。 随着力的施加，位置会发生变化。当力释放时，这个也是计算过程，机械臂会弹回原来的位置。 所以要给在计算时，要给时间它复原。 下面
不用matlab，不用ros,用pybullet，完成机械臂的轨迹规划
weixin_44287946的博客
12-03 2788
import pybullet from time import sleep import pybullet_data,pybullet_envs,pybullet_robots,pybullet_utils from circle import draw_cirlce import numpy ############ physicsClient = pybullet.connect(pybullet.GUI) #物理连接 pybullet.setAdditionalSearchPath(pybull
python-opencv颜色识别，圆检测。
weixin_44287946的博客
02-10 1368
import cv2 import numpy as np cap = cv2.VideoCapture(0) while(1): # Take each frame _, frame = cap.read() frame1=frame.copy() # 将图像转换到 HSV 空间 hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV) # 设置 HSV 阈值到蓝色范围。 lower_blue = np.arr
aspackdie未知错误
01-20
对于aspackdie未知错误的情况，我们可以采取以下步骤来尝试解决问题： 1. 检查环境配置：确认系统是否满足aspackdie的运行要求，例如当前操作系统版本、所需依赖库是否已正确安装等。 2. 更新aspackdie版本：查找...
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值