In [1]:

    

from sympy import *
from sympy.abc import *
from IPython.display import display
from sympy.interactive import printing
from sympy.matrices import *
import numpy as np
import math
printing.init_printing(pretty_print=True,use_latex=True)
var('x1 x2 x3 x4 y1 y2 y3 y4 z1 z2 z3 z4 q k v n t g e s')
bonds=[x1,y1,z1,x2,y2,z2]


    

In [2]:

    

#Derivatives of harmonic oscillator (for sanity) as well as the expected energy, gradient and Hessian for a
#harmonic oscillator at the point q={3.0,2.0,1.0} with parameters k={6.0,7.0,8.0} and r0={1.5,1.0,0.5}
ho=1.0/2.0*k*(q-t)**2
display(ho)
dedq=diff(ho,q)
display(dedq)
de2dq2=diff(dedq,q)
display(de2dq2)
f=lambdify([k,q,t],ho,"numpy")
print("Energy is:",sum(f(np.array([6.0,7.0,8.0]),np.array([3.0,2.0,1.0]),np.array([1.5,1.0,0.5]))))
f=lambdify([k,q,t],dedq,"numpy")
print("Gradient is:",f(np.array([6.0,7.0,8.0]),np.array([3.0,2.0,1.0]),np.array([1.5,1.0,0.5])))
f=lambdify([k,q,t],de2dq2,"numpy")
print("Diagonal of Hessian is:",f(np.array([6.0,7.0,8.0]),np.array([3.0,2.0,1.0]),np.array([1.5,1.0,0.5])))


    

    






$$0.5 k \left(q - t\right)^{2}$$






    






$$0.5 k \left(2 q - 2 t\right)$$






    






$$1.0 k$$






    




Energy is: 11.25
Gradient is: [ 9.  7.  4.]
Diagonal of Hessian is: [ 6.  7.  8.]

In [3]:

    

#Derivatives of Coulomb's law as well as the expected energy, gradient and Hessian for a
#point charges at the point q={3.0,2.0,1.0} with parameters t={6.0,7.0,8.0}
cl=t/q
display(cl)
dedq=diff(cl,q)
display(dedq)
de2dq2=diff(dedq,q)
display(de2dq2)
f=lambdify([t,q],cl,"numpy")
print("Energy is:",sum(f(np.array([6.0,7.0,8.0]),np.array([3.0,2.0,1.0]))))
f=lambdify([t,q],dedq,"numpy")
print("Gradient is:",f(np.array([6.0,7.0,8.0]),np.array([3.0,2.0,1.0])))
f=lambdify([t,q],de2dq2,"numpy")
print("Diagonal of Hessian is:",f(np.array([6.0,7.0,8.0]),np.array([3.0,2.0,1.0])))


    

    






$$\frac{t}{q}$$






    






$$- \frac{t}{q^{2}}$$






    






$$\frac{2 t}{q^{3}}$$






    




Energy is: 13.5
Gradient is: [-0.66666667 -1.75       -8.        ]
Diagonal of Hessian is: [  0.44444444   1.75        16.        ]

In [4]:

    

#Derivatives of Fourier Series as well as the expected energy, gradient and Hessian for a
#amps={3.2,2.2,1.2}, phases={PI/2.0,PI,PI/4} periods={2.0,3.0,4.0} and angles={2.0,3.10,0.6}
fs=v*(1.0+cos(n*t-g))
display(fs)
dedt=diff(fs,t)
display(dedt)
de2dt2=diff(dedt,t)
display(de2dt2)
angles,amps,periods,phases=np.array([2.0,3.10,0.6]),np.array([3.2,2.2,1.2]),\
                           np.array([2.0,3.0,4.0]),np.array([math.pi/2.0,math.pi,math.pi/4.0])
f=lambdify([v,n,t,g],fs,"numpy")
print("Energy is:",sum(f(amps,periods,angles,phases)))
f=lambdify([v,n,t,g],dedt,"numpy")
print("Gradient is:",f(amps,periods,angles,phases))
f=lambdify([v,n,t,g],de2dt2,"numpy")
print("Diagonal of Hessian is:",f(amps,periods,angles,phases))


    

    






$$v \left(\cos{\left (g - n t \right )} + 1.0\right)$$






    






$$n v \sin{\left (g - n t \right )}$$






    






$$- n^{2} v \cos{\left (g - n t \right )}$$






    




Energy is: 6.30857792951
Gradient is: [-4.18331917  0.8213992  -4.79539532]
Diagonal of Hessian is: [  9.68707194 -19.64606144   0.84079682]

In [5]:

    

#Derivatives of Lennard Jones as well as the expected energy, gradient and Hessian for a
#r={3.2,2.2,1.2}, sigma={PI/2.0,PI,PI/4} epsilon={2.0,3.0,4.0}
lj=e*((s/q)**12-2*(s/q)**6)
display(lj)
dedq=diff(lj,q)
display(dedq)
de2dq2=diff(dedq,q)
display(de2dq2)
dist,epsilon,sigma=np.array([3.2,2.2,1.2]),np.array([2.0,3.0,4.0]),np.array([math.pi/2.0,math.pi,math.pi/4.0])
f=lambdify([e,s,q],lj,"numpy")
print("Energy is:",sum(f(epsilon,sigma,dist)))
f=lambdify([e,s,q],dedq,"numpy")
print("Gradient is:",f(epsilon,sigma,dist))
f=lambdify([e,s,q],de2dq2,"numpy")
print("Hessian is:",f(epsilon,sigma,dist))


    

    






$$e \left(- \frac{2 s^{6}}{q^{6}} + \frac{s^{12}}{q^{12}}\right)$$






    






$$e \left(\frac{12 s^{6}}{q^{7}} - \frac{12 s^{12}}{q^{13}}\right)$$






    






$$e \left(- \frac{84 s^{6}}{q^{8}} + \frac{156 s^{12}}{q^{14}}\right)$$






    




Energy is: 164.162849653
Gradient is: [  1.03457493e-01  -1.03778526e+03   2.89706016e+00]
Hessian is: [ -2.23560931e-01   6.51078520e+03  -1.56637591e+01]

In [6]:

    

#Derivatives of distance
dist=sqrt((x1-x2)**2+(y1-y2)**2+(z1-z2)**2)
r1,r2,r3=np.array([0.0,0.0,0.0]),np.array([0.85,0.1,0.0]),np.array([0.1,0.74,0.0])
r12,r13=r1-r2,r1-r3
f=lambdify([x1,y1,z1,x2,y2,z2],dist,"numpy")
qs_=f(np.array([r1[0],r1[0]]),np.array([r1[1],r1[1]]),np.array([r1[2],r1[2]]),
      np.array([r2[0],r3[0]]),np.array([r2[1],r3[1]]),np.array([r2[2],r3[2]]))
print("Distances are",qs_)

dx,dy,dz=symbols('dx dy dz')
drdq=[diff(dist,i).subs(dist,q).subs(x1-x2,dx).subs(y1-y2,dy).subs(z1-z2,dz) for i in bonds]
display(drdq)

f=lambdify([dx,dy,dz,q],drdq,"numpy")
dq1=np.array([f(r12[0],r12[1],r12[2],qs_[0]),f(r13[0],r13[1],r13[2],qs_[1])])
print("1st deriv is:",dq1)
###Hessian
dr2dqdq=[[diff(diff(dist,i),j).subs(dist,q).subs(x1-x2,dx).subs(y1-y2,dy).subs(z1-z2,dz) for j in bonds] for i in bonds]
display(dr2dqdq)

f=lambdify([dx,dy,dz,q],dr2dqdq,"numpy")
dq2=np.array([f(r12[0],r12[1],r12[2],qs_[0]),f(r13[0],r13[1],r13[2],qs_[1])])
print("2nd deriv is:",dq2)


    

    




Distances are [ 0.85586214  0.74672619]






    






$$\left [ \frac{dx}{q}, \quad \frac{dy}{q}, \quad \frac{dz}{q}, \quad - \frac{dx}{q}, \quad - \frac{dy}{q}, \quad - \frac{dz}{q}\right ]$$






    




1st deriv is: [[-0.9931506  -0.11684125  0.          0.9931506   0.11684125 -0.        ]
 [-0.1339179  -0.99099243  0.          0.1339179   0.99099243 -0.        ]]






    






$$\left [ \left [ - \frac{dx^{2}}{q^{3}} + \frac{1}{q}, \quad - \frac{dx dy}{q^{3}}, \quad - \frac{dx dz}{q^{3}}, \quad \frac{dx^{2}}{q^{3}} - \frac{1}{q}, \quad \frac{dx dy}{q^{3}}, \quad \frac{dx dz}{q^{3}}\right ], \quad \left [ - \frac{dx dy}{q^{3}}, \quad - \frac{dy^{2}}{q^{3}} + \frac{1}{q}, \quad - \frac{dy dz}{q^{3}}, \quad \frac{dx dy}{q^{3}}, \quad \frac{dy^{2}}{q^{3}} - \frac{1}{q}, \quad \frac{dy dz}{q^{3}}\right ], \quad \left [ - \frac{dx dz}{q^{3}}, \quad - \frac{dy dz}{q^{3}}, \quad - \frac{dz^{2}}{q^{3}} + \frac{1}{q}, \quad \frac{dx dz}{q^{3}}, \quad \frac{dy dz}{q^{3}}, \quad \frac{dz^{2}}{q^{3}} - \frac{1}{q}\right ], \quad \left [ \frac{dx^{2}}{q^{3}} - \frac{1}{q}, \quad \frac{dx dy}{q^{3}}, \quad \frac{dx dz}{q^{3}}, \quad - \frac{dx^{2}}{q^{3}} + \frac{1}{q}, \quad - \frac{dx dy}{q^{3}}, \quad - \frac{dx dz}{q^{3}}\right ], \quad \left [ \frac{dx dy}{q^{3}}, \quad \frac{dy^{2}}{q^{3}} - \frac{1}{q}, \quad \frac{dy dz}{q^{3}}, \quad - \frac{dx dy}{q^{3}}, \quad - \frac{dy^{2}}{q^{3}} + \frac{1}{q}, \quad - \frac{dy dz}{q^{3}}\right ], \quad \left [ \frac{dx dz}{q^{3}}, \quad \frac{dy dz}{q^{3}}, \quad \frac{dz^{2}}{q^{3}} - \frac{1}{q}, \quad - \frac{dx dz}{q^{3}}, \quad - \frac{dy dz}{q^{3}}, \quad - \frac{dz^{2}}{q^{3}} + \frac{1}{q}\right ]\right ]$$






    




2nd deriv is: [[[ 0.01595102 -0.1355837   0.         -0.01595102  0.1355837  -0.        ]
  [-0.1355837   1.15246145  0.          0.1355837  -1.15246145 -0.        ]
  [ 0.          0.          1.16841248 -0.         -0.         -1.16841248]
  [-0.01595102  0.1355837  -0.          0.01595102 -0.1355837   0.        ]
  [ 0.1355837  -1.15246145 -0.         -0.1355837   1.15246145  0.        ]
  [-0.         -0.         -1.16841248  0.          0.          1.16841248]]

 [[ 1.31516212 -0.17772461  0.         -1.31516212  0.17772461 -0.        ]
  [-0.17772461  0.02401684  0.          0.17772461 -0.02401684 -0.        ]
  [ 0.          0.          1.33917896 -0.         -0.         -1.33917896]
  [-1.31516212  0.17772461 -0.          1.31516212 -0.17772461  0.        ]
  [ 0.17772461 -0.02401684 -0.         -0.17772461  0.02401684  0.        ]
  [-0.         -0.         -1.33917896  0.          0.          1.33917896]]]

In [7]:

    

#Derivatives of angle
v1,v2,v3=Matrix([x1,y1,z1]),Matrix([x2,y2,z2]),Matrix([x3,y3,z3])
DoF=[x1,y1,z1,x2,y2,z2,x3,y3,z3]
#Definition of our two vectors
A,B=v1-v2,v3-v2
#Their unit norm
n=A.cross(B)
angle=atan2(sqrt(n.dot(n)),A.dot(B))
nx,ny,nz=symbols('nx ny nz')
Ax,Ay,Az,Bx,By,Bz=symbols('Ax Ay Az Bx By Bz')
adotB,acrossB2=symbols('A{\cdot}B'),('n2')
angle_A=angle.subs(v1[0]-v2[0],Ax).subs(v1[1]-v2[1],Ay).subs(v1[2]-v2[2],Az)
angle_AB=angle_A.subs(v3[0]-v2[0],Bx).subs(v3[1]-v2[1],By).subs(v3[2]-v2[2],Bz)
angle_f=angle_AB.subs(Ax*Bx+Ay*By+Az*Bz,adotB).subs(Ax*By-Ay*Bx,nz).subs(Az*Bx-Ax*Bz,ny).subs(Ay*Bz-Az*By,nx)
display(angle_f.subs(nx*nx+ny*ny+nz*nz,acrossB2))
r2,r1,r3=np.array([0.0,0.0,0.0]),np.array([0.85,0.1,0.0]),np.array([0.1,0.74,0.0])
f=lambdify(DoF,angle,"numpy")
qs_=f(r1[0],r1[1],r1[2],r2[0],r2[1],r2[2],r3[0],r3[1],r3[2])
print(qs_)
drdq=[diff(angle,i) for i in DoF]
for i in range(len(DoF)):
    drdq_A=drdq[i].subs(v1[0]-v2[0],Ax).subs(v1[1]-v2[1],Ay).subs(v1[2]-v2[2],Az)
    drdq_AB=drdq_A.subs(v3[0]-v2[0],Bx).subs(v3[1]-v2[1],By).subs(v3[2]-v2[2],Bz)
    drdq_dn=drdq_AB.subs(Ax*Bx+Ay*By+Az*Bz,adotB).subs(Ax*By-Ay*Bx,nz).subs(Az*Bx-Ax*Bz,ny).subs(Ay*Bz-Az*By,nx)
    drdq_dB=drdq_dn.subs(nx*nx+ny*ny+nz*nz,acrossB2).subs(2*v2[2]-2*v3[2],-2*Bz).subs(2*v2[1]-2*v3[1],-2*By).subs(2*v2[0]-2*v3[0],-2*Bx)
    drdq_f=drdq_dB.subs(2*v1[2]-2*v2[2],2*Az).subs(2*v1[1]-2*v2[1],2*Ay).subs(2*v1[0]-2*v2[0],2*Ax)
    display(drdq_f)
f=lambdify(DoF,drdq,"numpy")
qs_=f(r1[0],r1[1],r1[2],r2[0],r2[1],r2[2],r3[0],r3[1],r3[2])
print(qs_)


    

    






$$\operatorname{atan_{2}}{\left (\sqrt{n_{2}},A{\cdot}B \right )}$$






    




1.31936614028






    






$$\frac{A{\cdot}B \left(By nz - Bz ny\right)}{\sqrt{n_{2}} \left(A{\cdot}B^{2} + n_{2}\right)} - \frac{Bx \sqrt{n_{2}}}{A{\cdot}B^{2} + n_{2}}$$






    






$$\frac{A{\cdot}B \left(- Bx nz + Bz nx\right)}{\sqrt{n_{2}} \left(A{\cdot}B^{2} + n_{2}\right)} - \frac{By \sqrt{n_{2}}}{A{\cdot}B^{2} + n_{2}}$$






    






$$\frac{A{\cdot}B \left(Bx ny - By nx\right)}{\sqrt{n_{2}} \left(A{\cdot}B^{2} + n_{2}\right)} - \frac{Bz \sqrt{n_{2}}}{A{\cdot}B^{2} + n_{2}}$$






    






$$\frac{A{\cdot}B}{\sqrt{n_{2}} \left(A{\cdot}B^{2} + n_{2}\right)} \left(\frac{ny}{2} \left(- 2 z_{1} + 2 z_{3}\right) + \frac{nz}{2} \left(2 y_{1} - 2 y_{3}\right)\right) - \frac{\sqrt{n_{2}}}{A{\cdot}B^{2} + n_{2}} \left(- x_{1} + 2 x_{2} - x_{3}\right)$$






    






$$\frac{A{\cdot}B}{\sqrt{n_{2}} \left(A{\cdot}B^{2} + n_{2}\right)} \left(\frac{nx}{2} \left(2 z_{1} - 2 z_{3}\right) + \frac{nz}{2} \left(- 2 x_{1} + 2 x_{3}\right)\right) - \frac{\sqrt{n_{2}}}{A{\cdot}B^{2} + n_{2}} \left(- y_{1} + 2 y_{2} - y_{3}\right)$$






    






$$\frac{A{\cdot}B}{\sqrt{n_{2}} \left(A{\cdot}B^{2} + n_{2}\right)} \left(\frac{nx}{2} \left(- 2 y_{1} + 2 y_{3}\right) + \frac{ny}{2} \left(2 x_{1} - 2 x_{3}\right)\right) - \frac{\sqrt{n_{2}}}{A{\cdot}B^{2} + n_{2}} \left(- z_{1} + 2 z_{2} - z_{3}\right)$$






    






$$- \frac{Ax \sqrt{n_{2}}}{A{\cdot}B^{2} + n_{2}} + \frac{A{\cdot}B \left(- Ay nz + Az ny\right)}{\sqrt{n_{2}} \left(A{\cdot}B^{2} + n_{2}\right)}$$






    






$$- \frac{Ay \sqrt{n_{2}}}{A{\cdot}B^{2} + n_{2}} + \frac{A{\cdot}B \left(Ax nz - Az nx\right)}{\sqrt{n_{2}} \left(A{\cdot}B^{2} + n_{2}\right)}$$






    






$$- \frac{Az \sqrt{n_{2}}}{A{\cdot}B^{2} + n_{2}} + \frac{A{\cdot}B \left(- Ax ny + Ay nx\right)}{\sqrt{n_{2}} \left(A{\cdot}B^{2} + n_{2}\right)}$$






    




[0.13651877133105803, -1.1604095563139933, 0.0, 1.190597441007536, 0.98106952761958865, 0.0, -1.3271162123385942, 0.1793400286944046, 0.0]

In [12]:

    

#Derivatives of torsion
v1,v2,v3,v4=Matrix([x1,y1,z1]),Matrix([x2,y2,z2]),Matrix([x3,y3,z3]),Matrix([x4,y4,z4])
DoF=[x1,y1,z1,x2,y2,z2,x3,y3,z3,x4,y4,z4]

#Definition of our vectors
r21,r31,r23,r34,r42=v2-v1,v3-v1,v2-v3,v3-v4,v4-v2
n1,n2=r21.cross(r23),r34.cross(r23)
A=n1.cross(n2)
angle=atan2(sqrt(A.dot(A)),n1.dot(n2))
n1x,n1y,n1z,n2x,n2y,n2z=symbols('n{_{1x}} n{_{1y}} n{_{1z}} n{_{2x}} n{_{2y}} n{_{2z}}')
r21x,r21y,r21z,r31x,r31y,r31z,r23x,r23y,r23z=symbols('r{_{21x}} r{_{21y}} r{_{21z}} r{_{31x}} r{_{31y}} r{_{31z}} r{_{23x}} r{_{23y}} r{_{23z}}')
r34x,r34y,r34z,r42x,r42y,r42z=symbols('r{_{34x}} r{_{34y}} r{_{34z}} r{_{42x}} r{_{42y}} r{_{42z}}')
Ax,Ay,Az=symbols('A{_x} A{_y} A{_z}')
A2=symbols("A{^2}")
n1dotn2=symbols("n{_1}{\\cdot}n{_2}")
angle_r21=angle.subs(v2[0]-v1[0],r21x).subs(v2[1]-v1[1],r21y).subs(v2[2]-v1[2],r21z)
angle_r23=angle_r21.subs(v2[0]-v3[0],r23x).subs(v2[1]-v3[1],r23y).subs(v2[2]-v3[2],r23z)
angle_r34=angle_r23.subs(v3[0]-v4[0],r34x).subs(v3[1]-v4[1],r34y).subs(v3[2]-v4[2],r34z)
angle_n1=angle_r34.subs(r21y*r23z-r21z*r23y,n1x).subs(r21z*r23x-r21x*r23z,n1y).subs(r21x*r23y-r21y*r23x,n1z)
angle_n2=angle_n1.subs(r34y*r23z-r34z*r23y,n2x).subs(r34z*r23x-r34x*r23z,n2y).subs(r34x*r23y-r34y*r23x,n2z)
angle_A=angle_n2.subs(n1y*n2z-n1z*n2y,Ax).subs(n1z*n2x-n1x*n2z,Ay).subs(n1x*n2y-n1y*n2x,Az)
display(angle_A.subs(Ax*Ax+Ay*Ay+Az*Az,A2).subs(n1x*n2x+n1y*n2y+n1z*n2z,n1dotn2))
r=np.array([[1.1851,-0.0039,0.9875],[0.7516,-0.0225,-0.0209],[1.1669,0.8330,-0.5693],[1.1155,-0.9329,-0.5145],
           [-0.7516,0.0225,0.0209],[-1.1669,-0.8334,0.5687],[-1.1157,0.9326,0.5151],[-1.1850,0.0044,-0.9875]])
f=lambdify(DoF,angle,"numpy")
for i in [0,2,3]:
    for j in [5,6,7]:
        qs_=f(r[i][0],r[i][1],r[i][2],r[1][0],r[1][1],r[1][2],r[4][0],r[4][1],r[4][2],r[j][0],r[j][1],r[j][2])
        print(qs_)
drdq=[diff(angle,i) for i in DoF]
pf=symbols('\\alpha')
n2cr23x,n2cr23y,n2cr23z=symbols("(n{_2}{\\times}r{_{23}}){_x} (n{_2}{\\times}r{_{23}}){_y} (n_{2}{\\times}r{_{23}}){_z}")
n2cr21x,n2cr21y,n2cr21z=symbols("(n{_2}{\\times}r{_{21}}){_x} (n{_2}{\\times}r{_{21}}){_y} (n_{2}{\\times}r{_{21}}){_z}")
n1cr34x,n1cr34y,n1cr34z=symbols("(n{_1}{\\times}r{_{34}}){_x} (n{_1}{\\times}r{_{34}}){_y} (n_{1}{\\times}r{_{34}}){_z}")
n1cr42x,n1cr42y,n1cr42z=symbols("(n{_1}{\\times}r{_{42}}){_x} (n{_1}{\\times}r{_{42}}){_y} (n_{1}{\\times}r{_{42}}){_z}")
n2cr31x,n2cr31y,n2cr31z=symbols("(n{_2}{\\times}r{_{31}}){_x} (n{_2}{\\times}r{_{31}}){_y} (n_{2}{\\times}r{_{31}}){_z}")
for i in range(len(DoF)):
    angle_r21=drdq[i].subs(v2[0]-v1[0],r21x).subs(v2[1]-v1[1],r21y).subs(v2[2]-v1[2],r21z)
    angle_r23=angle_r21.subs(v2[0]-v3[0],r23x).subs(v2[1]-v3[1],r23y).subs(v2[2]-v3[2],r23z)
    angle_r34=angle_r23.subs(v3[0]-v4[0],r34x).subs(v3[1]-v4[1],r34y).subs(v3[2]-v4[2],r34z)
    angle_r24=angle_r34.subs(v4[0]-v2[0],r42x).subs(v4[1]-v2[1],r42y).subs(v4[2]-v2[2],r42z)
    angle_r31=angle_r24.subs(v3[0]-v1[0],r31x).subs(v3[1]-v1[1],r31y).subs(v3[2]-v1[2],r31z)
    angle_n1=angle_r31.subs(r21y*r23z-r21z*r23y,n1x).subs(r21z*r23x-r21x*r23z,n1y).subs(r21x*r23y-r21y*r23x,n1z)
    angle_n2=angle_n1.subs(r34y*r23z-r34z*r23y,n2x).subs(r34z*r23x-r34x*r23z,n2y).subs(r34x*r23y-r34y*r23x,n2z)
    angle_A=angle_n2.subs(n1y*n2z-n1z*n2y,Ax).subs(n1z*n2x-n1x*n2z,Ay).subs(n1x*n2y-n1y*n2x,Az)
    angle_A2=angle_A.subs(Ax*Ax+Ay*Ay+Az*Az,A2).subs(n1x*n2x+n1y*n2y+n1z*n2z,n1dotn2)
    angle_pf=angle_A2.subs(1/(A2+n1dotn2**2),pf)
    angle_n2cB=angle_pf.subs(n2y*r23z-n2z*r23y,n2cr23x).subs(n2z*r23x-n2x*r23z,n2cr23y).subs(n2x*r23y-n2y*r23x,n2cr23z)
    angle_n2cr31=angle_n2cB.subs(n2y*r31z-n2z*r31y,n2cr31x).subs(n2z*r31x-n2x*r31z,n2cr31y).subs(n2x*r31y-n2y*r31x,n2cr31z)
    angle_n2cr21=angle_n2cr31.subs(n2y*r21z-n2z*r21y,n2cr21x).subs(n2z*r21x-n2x*r21z,n2cr21y).subs(n2x*r21y-n2y*r21x,n2cr21z)
    angle_n1cr34=angle_n2cr21.subs(n1y*r34z-n1z*r34y,n1cr34x).subs(n1z*r34x-n1x*r34z,n1cr34y).subs(n1x*r34y-n1y*r34x,n1cr34z)
    angle_n1cr42=angle_n1cr34.subs(n1y*r42z-n1z*r42y,n1cr42x).subs(n1z*r42x-n1x*r42z,n1cr42y).subs(n1x*r42y-n1y*r42x,n1cr42z)
    display(angle_n1cr42)
for i in [0,2,3]:
    for j in [5,6,7]:
        for k in range(len(DoF)):
            f=lambdify(DoF,drdq[k],"numpy")
            qs_=f(r[i][0],r[i][1],r[i][2],r[1][0],r[1][1],r[1][2],r[4][0],r[4][1],r[4][2],r[j][0],r[j][1],r[j][2])
            print(str(qs_),end=",")
        print("")


    

    






$$\operatorname{atan_{2}}{\left (\sqrt{A{^2}},n{_1}{\cdot}n{_2} \right )}$$






    




1.04786541775
1.04663519795
3.14110029443
3.14088699457
1.0477976969
1.04666739958
1.04643698771
3.14093760341
1.04778260729






    






$$- (n{_2}{\times}r{_{23}}){_x} \sqrt{A{^2}} \alpha + \frac{\alpha n{_1}{\cdot}n{_2}}{\sqrt{A{^2}}} \left(\frac{A{_x}}{2} \left(2 n{_{2y}} r{_{23y}} + 2 n{_{2z}} r{_{23z}}\right) - A{_y} n{_{2x}} r{_{23y}} - A{_z} n{_{2x}} r{_{23z}}\right)$$






    






$$- (n{_2}{\times}r{_{23}}){_y} \sqrt{A{^2}} \alpha + \frac{\alpha n{_1}{\cdot}n{_2}}{\sqrt{A{^2}}} \left(- A{_x} n{_{2y}} r{_{23x}} + \frac{A{_y}}{2} \left(2 n{_{2x}} r{_{23x}} + 2 n{_{2z}} r{_{23z}}\right) - A{_z} n{_{2y}} r{_{23z}}\right)$$






    






$$- (n_{2}{\times}r{_{23}}){_z} \sqrt{A{^2}} \alpha + \frac{\alpha n{_1}{\cdot}n{_2}}{\sqrt{A{^2}}} \left(- A{_x} n{_{2z}} r{_{23x}} - A{_y} n{_{2z}} r{_{23y}} + \frac{A{_z}}{2} \left(2 n{_{2x}} r{_{23x}} + 2 n{_{2y}} r{_{23y}}\right)\right)$$






    






$$- \sqrt{A{^2}} \alpha \left((n{_1}{\times}r{_{34}}){_x} + (n{_2}{\times}r{_{31}}){_x}\right) + \frac{\alpha n{_1}{\cdot}n{_2}}{\sqrt{A{^2}}} \left(\frac{A{_x}}{2} \left(- 2 n{_{1y}} r{_{34y}} - 2 n{_{1z}} r{_{34z}} + 2 n{_{2y}} r{_{31y}} + 2 n{_{2z}} r{_{31z}}\right) + \frac{A{_y}}{2} \left(2 n{_{1x}} r{_{34y}} - 2 n{_{2x}} r{_{31y}}\right) + \frac{A{_z}}{2} \left(2 n{_{1x}} r{_{34z}} - 2 n{_{2x}} r{_{31z}}\right)\right)$$






    






$$- \sqrt{A{^2}} \alpha \left((n{_1}{\times}r{_{34}}){_y} + (n{_2}{\times}r{_{31}}){_y}\right) + \frac{\alpha n{_1}{\cdot}n{_2}}{\sqrt{A{^2}}} \left(\frac{A{_x}}{2} \left(2 n{_{1y}} r{_{34x}} - 2 n{_{2y}} r{_{31x}}\right) + \frac{A{_y}}{2} \left(- 2 n{_{1x}} r{_{34x}} - 2 n{_{1z}} r{_{34z}} + 2 n{_{2x}} r{_{31x}} + 2 n{_{2z}} r{_{31z}}\right) + \frac{A{_z}}{2} \left(2 n{_{1y}} r{_{34z}} - 2 n{_{2y}} r{_{31z}}\right)\right)$$






    






$$- \sqrt{A{^2}} \alpha \left((n_{1}{\times}r{_{34}}){_z} + (n_{2}{\times}r{_{31}}){_z}\right) + \frac{\alpha n{_1}{\cdot}n{_2}}{\sqrt{A{^2}}} \left(\frac{A{_x}}{2} \left(2 n{_{1z}} r{_{34x}} - 2 n{_{2z}} r{_{31x}}\right) + \frac{A{_y}}{2} \left(2 n{_{1z}} r{_{34y}} - 2 n{_{2z}} r{_{31y}}\right) + \frac{A{_z}}{2} \left(- 2 n{_{1x}} r{_{34x}} - 2 n{_{1y}} r{_{34y}} + 2 n{_{2x}} r{_{31x}} + 2 n{_{2y}} r{_{31y}}\right)\right)$$






    






$$- \sqrt{A{^2}} \alpha \left((n{_1}{\times}r{_{42}}){_x} - (n{_2}{\times}r{_{21}}){_x}\right) + \frac{\alpha n{_1}{\cdot}n{_2}}{\sqrt{A{^2}}} \left(\frac{A{_x}}{2} \left(- 2 n{_{1y}} r{_{42y}} - 2 n{_{1z}} r{_{42z}} - 2 n{_{2y}} r{_{21y}} - 2 n{_{2z}} r{_{21z}}\right) + \frac{A{_y}}{2} \left(2 n{_{1x}} r{_{42y}} + 2 n{_{2x}} r{_{21y}}\right) + \frac{A{_z}}{2} \left(2 n{_{1x}} r{_{42z}} + 2 n{_{2x}} r{_{21z}}\right)\right)$$






    






$$- \sqrt{A{^2}} \alpha \left((n{_1}{\times}r{_{42}}){_y} - (n{_2}{\times}r{_{21}}){_y}\right) + \frac{\alpha n{_1}{\cdot}n{_2}}{\sqrt{A{^2}}} \left(\frac{A{_x}}{2} \left(2 n{_{1y}} r{_{42x}} + 2 n{_{2y}} r{_{21x}}\right) + \frac{A{_y}}{2} \left(- 2 n{_{1x}} r{_{42x}} - 2 n{_{1z}} r{_{42z}} - 2 n{_{2x}} r{_{21x}} - 2 n{_{2z}} r{_{21z}}\right) + \frac{A{_z}}{2} \left(2 n{_{1y}} r{_{42z}} + 2 n{_{2y}} r{_{21z}}\right)\right)$$






    






$$- \sqrt{A{^2}} \alpha \left((n_{1}{\times}r{_{42}}){_z} - (n_{2}{\times}r{_{21}}){_z}\right) + \frac{\alpha n{_1}{\cdot}n{_2}}{\sqrt{A{^2}}} \left(\frac{A{_x}}{2} \left(2 n{_{1z}} r{_{42x}} + 2 n{_{2z}} r{_{21x}}\right) + \frac{A{_y}}{2} \left(2 n{_{1z}} r{_{42y}} + 2 n{_{2z}} r{_{21y}}\right) + \frac{A{_z}}{2} \left(- 2 n{_{1x}} r{_{42x}} - 2 n{_{1y}} r{_{42y}} - 2 n{_{2x}} r{_{21x}} - 2 n{_{2y}} r{_{21y}}\right)\right)$$






    






$$- \sqrt{A{^2}} \alpha \left(n{_{1y}} r{_{23z}} - n{_{1z}} r{_{23y}}\right) + \frac{\alpha n{_1}{\cdot}n{_2}}{\sqrt{A{^2}}} \left(\frac{A{_x}}{2} \left(- 2 n{_{1y}} r{_{23y}} - 2 n{_{1z}} r{_{23z}}\right) + A{_y} n{_{1x}} r{_{23y}} + A{_z} n{_{1x}} r{_{23z}}\right)$$






    






$$- \sqrt{A{^2}} \alpha \left(- n{_{1x}} r{_{23z}} + n{_{1z}} r{_{23x}}\right) + \frac{\alpha n{_1}{\cdot}n{_2}}{\sqrt{A{^2}}} \left(A{_x} n{_{1y}} r{_{23x}} + \frac{A{_y}}{2} \left(- 2 n{_{1x}} r{_{23x}} - 2 n{_{1z}} r{_{23z}}\right) + A{_z} n{_{1y}} r{_{23z}}\right)$$






    






$$- \sqrt{A{^2}} \alpha \left(n{_{1x}} r{_{23y}} - n{_{1y}} r{_{23x}}\right) + \frac{\alpha n{_1}{\cdot}n{_2}}{\sqrt{A{^2}}} \left(A{_x} n{_{1z}} r{_{23x}} + A{_y} n{_{1z}} r{_{23y}} + \frac{A{_z}}{2} \left(- 2 n{_{1x}} r{_{23x}} - 2 n{_{1y}} r{_{23y}}\right)\right)$$






    




0.0284653010523,0.97900805365,-0.0302947816383,-0.0464913630787,-1.38062723188,-0.185588314476,0.0565934655447,0.916101300855,1.0489650447,-0.0385674035183,-0.514482122622,-0.833081948582,
-0.0284653010523,-0.97900805365,0.0302947816383,0.033405923143,1.36724213856,-0.270576855663,0.00514738517599,-0.852739339491,1.10312965726,-0.0100880072668,0.464505254578,-0.86284758324,
-0.0284653010523,-0.97900805365,0.0302947816383,0.0284630554296,0.97905488329,-0.0304259527828,0.0284818783645,0.978992682927,-0.0296820855074,-0.0284796327418,-0.979039512568,0.0298132566519,
0.0385758170692,0.515082499748,0.832738175353,-0.0385788138454,-0.515253755027,-0.832661578856,-0.0385644067421,-0.514310867344,-0.833158545079,0.0385674035183,0.514482122622,0.833081948582,
-0.0385758170692,-0.515082499748,-0.832738175353,0.051664253781,0.528638848346,1.288826749,-0.0231764439786,0.45094890598,-1.31893615688,0.0100880072668,-0.464505254578,0.86284758324,
0.0385758170692,0.515082499748,0.832738175353,-0.0566071214945,-0.916826103619,-1.04867584612,0.0465109371671,1.38078311644,0.186124414111,-0.0284796327418,-0.979039512568,0.0298132566519,
-0.0101134825722,0.463945620371,-0.863161242086,0.0232043811408,-0.450345967831,1.31929172926,-0.0516583020869,-0.528081775162,-1.28921243576,0.0385674035183,0.514482122622,0.833081948582,
-0.0101134825722,0.463945620371,-0.863161242086,0.0101189412052,-0.46373106115,0.863126559125,0.0100825486338,-0.464719813799,0.862882266201,-0.0100880072668,0.464505254578,-0.86284758324,
0.0101134825722,-0.463945620371,0.863161242086,-0.00517607349175,0.851918316423,-1.10327746201,-0.0334170418223,-1.36701220862,0.269929476571,0.0284796327418,0.979039512568,-0.0298132566519,

In [ ]: