tessellationrobot.github.io

System Dynamics

Our robot will be built of 4 identical legs. The diagram below shows how we specify several points, reference frames, and distances for one leg. The end stop is a conceptual model for the behavior of our leg design. The leg is made of a tessallation that is easily deformed in one direction but reaches a singularity when deformed in the oposite direction preventing futher motion in that direction. The leg joint is modeled with a spring and damper so it mimics the tessallation joint.

Below is a diagram of how the points are defined for the whole robot assembley. The details for the definitions of each leg are not shown on this diragram as they similar to thouse in the above diagram and would clutter this diagram making it hard to understand. Note the robot body that is defined as a beam from point A to point B is free from the orign marked in the lower left corner. This diagram is a side veiw of the robot and BEF and BCD legs on the real robot are sperated in the z direction.

We are modeling the beams that make up the legs as ridgid links as our expirements showed that they are rigid untill failure due to buckling. This will not affect our locamtion strategy as we did not plan for link compliance to store energy during locomotion.

Naming conventions:
Legs:
-BCD leg 1
-BEF leg 2
-AGH leg 3
-AIJ leg 4
Leg tips are always the letter that comes later in aphebetical order. A and B are points on the body.
The beams connected to A or B are the first leg beams and the others are the leg second beams.

!pip install pynamics
%matplotlib inline
import pynamics
from pynamics.frame import Frame
from pynamics.variable_types import Differentiable,Constant
from pynamics.system import System
from pynamics.body import Body
from pynamics.dyadic import Dyadic
from pynamics.output import Output,PointsOutput
from pynamics.particle import Particle
from pynamics.constraint import AccelerationConstraint
import pynamics.integration
import numpy
import sympy
import matplotlib.pyplot as plt
plt.ion()
from math import pi

Requirement already satisfied: pynamics in /usr/local/lib/python3.7/dist-packages (0.2.0)
Requirement already satisfied: matplotlib in /usr/local/lib/python3.7/dist-packages (from pynamics) (3.2.2)
Requirement already satisfied: scipy in /usr/local/lib/python3.7/dist-packages (from pynamics) (1.4.1)
Requirement already satisfied: sympy in /usr/local/lib/python3.7/dist-packages (from pynamics) (1.7.1)
Requirement already satisfied: numpy in /usr/local/lib/python3.7/dist-packages (from pynamics) (1.21.5)
Requirement already satisfied: kiwisolver>=1.0.1 in /usr/local/lib/python3.7/dist-packages (from matplotlib->pynamics) (1.3.2)
Requirement already satisfied: cycler>=0.10 in /usr/local/lib/python3.7/dist-packages (from matplotlib->pynamics) (0.11.0)
Requirement already satisfied: python-dateutil>=2.1 in /usr/local/lib/python3.7/dist-packages (from matplotlib->pynamics) (2.8.2)
Requirement already satisfied: pyparsing!=2.0.4,!=2.1.2,!=2.1.6,>=2.0.1 in /usr/local/lib/python3.7/dist-packages (from matplotlib->pynamics) (3.0.7)
Requirement already satisfied: six>=1.5 in /usr/local/lib/python3.7/dist-packages (from python-dateutil>=2.1->matplotlib->pynamics) (1.15.0)
Requirement already satisfied: mpmath>=0.19 in /usr/local/lib/python3.7/dist-packages (from sympy->pynamics) (1.2.1)

system=System()
pynamics.set_system(__name__,system)

#lenghts in m
lB = Constant(.15,'lB',system)
lC = Constant(.05,'lC',system)
lD = Constant(.05,'lD',system)
lE = Constant(.05,'lE',system)
lF = Constant(.05,'lF',system)
lG = Constant(.05,'lG',system)
lH = Constant(.05,'lH',system)
lI = Constant(.05,'lI',system)
lJ = Constant(.05,'lJ',system)

#masses in kg
mB = Constant(.5,'mB',system)
mC = Constant(.01,'mC',system)
mD = Constant(.01,'mD',system)
mE = Constant(.01,'mE',system)
mF = Constant(.01,'mF',system)
mG = Constant(.01,'mG',system)
mH = Constant(.01,'mH',system)
mI = Constant(.01,'mI',system)
mJ = Constant(.01,'mJ',system)

g = Constant(9.81,'g',system)     #Gravity 
b = Constant(1e-4,'b',system)
k = Constant(1e-2,'k',system)
kEndStop = Constant(1e1,'kEndStop',system)
bEndStop = Constant(1e-1,'bEndStop',system)
preload1 = Constant(-80*pi/180,'preload1',system)
preload2 = Constant(-15*pi/180,'preload2',system)

#end stop limits
limit = Constant(-(-0*pi/180),'limit',system)


#Inertial values that describe the dyatics/inertia of the rigid bodies about x, y and z axes 
Ixx_B = Constant(1e-1,'Ixx_B',system)
Iyy_B = Constant(1e-1,'Iyy_B',system)
Izz_B = Constant(1e-1,'Izz_B',system)
Ixx_C = Constant(1e-2,'Ixx_C',system)
Iyy_C = Constant(1e-2,'Iyy_C',system)
Izz_C = Constant(1e-2,'Izz_C',system)
Ixx_D = Constant(1e-2,'Ixx_D',system)
Iyy_D = Constant(1e-2,'Iyy_D',system)
Izz_D = Constant(1e-2,'Izz_D',system)
Ixx_E = Constant(1e-2,'Ixx_E',system)
Iyy_E = Constant(1e-2,'Iyy_E',system)
Izz_E = Constant(1e-2,'Izz_E',system)
Ixx_F = Constant(1e-2,'Ixx_F',system)
Iyy_F = Constant(1e-2,'Iyy_F',system)
Izz_F = Constant(1e-2,'Izz_F',system)
Ixx_G = Constant(1e-2,'Ixx_G',system)
Iyy_G = Constant(1e-2,'Iyy_G',system)
Izz_G = Constant(1e-2,'Izz_G',system)
Ixx_H = Constant(1e-2,'Ixx_H',system)
Iyy_H = Constant(1e-2,'Iyy_H',system)
Izz_H = Constant(1e-2,'Izz_H',system)
Ixx_I = Constant(1e-2,'Ixx_I',system)
Iyy_I = Constant(1e-2,'Iyy_I',system)
Izz_I = Constant(1e-2,'Izz_I',system)
Ixx_J = Constant(1e-2,'Ixx_J',system)
Iyy_J = Constant(1e-2,'Iyy_J',system)
Izz_J = Constant(1e-2,'Izz_J',system)


torque = Constant(0,'torque',system)
freq = Constant(3e0,'freq',system)  #3e0 means 3Hz

#3 state variables:
qC,qC_d,qC_dd = Differentiable('qC',system)
qD,qD_d,qD_dd = Differentiable('qD',system)
qE,qE_d,qE_dd = Differentiable('qE',system)
qF,qF_d,qF_dd = Differentiable('qF',system)
qG,qG_d,qG_dd = Differentiable('qG',system)
qH,qH_d,qH_dd = Differentiable('qH',system)
qI,qI_d,qI_dd = Differentiable('qI',system)
qJ,qJ_d,qJ_dd = Differentiable('qJ',system)

# state variables for body position and rotation
Ax,Ax_d,Ax_dd = Differentiable('Ax',system)
Ay,Ay_d,Ay_dd = Differentiable('Ay',system)
Aq,Aq_d,Aq_dd = Differentiable('Aq',system)

initialvalues = {}
initialvalues[Aq]=0*pi/180
initialvalues[Aq_d]=0*pi/180
initialvalues[Ax]=0
initialvalues[Ax_d]=0
initialvalues[Ay]=0.1
initialvalues[Ay_d]=0

initialvalues[qC]=-60*pi/180
initialvalues[qC_d]=0*pi/180
initialvalues[qD]=-15*pi/180
initialvalues[qD_d]=0*pi/180
initialvalues[qE]=-50*pi/180
initialvalues[qE_d]=0*pi/180
initialvalues[qF]=-15*pi/180
initialvalues[qF_d]=0*pi/180
initialvalues[qG]=-40*pi/180
initialvalues[qG_d]=0*pi/180
initialvalues[qH]=-15*pi/180
initialvalues[qH_d]=0*pi/180
initialvalues[qI]=-70*pi/180
initialvalues[qI_d]=0*pi/180
initialvalues[qJ]=-15*pi/180
initialvalues[qJ_d]=0*pi/180

statevariables = system.get_state_variables()
ini = [initialvalues[item] for item in statevariables]

#Defining reference frames of the system 
N = Frame('N',system)
A = Frame('A',system)

C = Frame('C',system)
D = Frame('D',system)
E = Frame('E',system)
F = Frame('F',system)
G = Frame('G',system)
H = Frame('H',system)
I = Frame('I',system)
J = Frame('J',system)

system.set_newtonian(N)
A.rotate_fixed_axis(N,[0,0,1],Aq,system)

#legs attacheced to body
C.rotate_fixed_axis(A,[0,0,1],qC,system)
E.rotate_fixed_axis(A,[0,0,1],qE,system)
G.rotate_fixed_axis(A,[0,0,1],qG,system)
I.rotate_fixed_axis(A,[0,0,1],qI,system)

#legs attacheced to legs
D.rotate_fixed_axis(C,[0,0,1],qD,system)
F.rotate_fixed_axis(E,[0,0,1],qF,system)
H.rotate_fixed_axis(G,[0,0,1],qH,system)
J.rotate_fixed_axis(I,[0,0,1],qJ,system)

pNO=0*N.x
#body
pNA = Ax*N.x + Ay*N.y
pAB = pNA + lB*A.x
#legs
pBC = pAB + lC*C.x
pCD = pBC + lD*D.x

pBE = pAB + lE*E.x
pEF = pBE + lF*F.x

pAI = pNA + lI*I.x
pIJ = pAI + lJ*J.x 

pAG = pNA + lG*G.x
pGH = pAG + lH*H.x 

#Center of mass
pAcm=pNA+lB/2*A.x  
pCcm = pBC - lC/2*C.x
pDcm = pCD - lD/2*D.x
pEcm = pBE - lE/2*E.x
pFcm = pEF - lF/2*F.x
pIcm = pAI - lI/2*I.x
pJcm = pIJ - lJ/2*J.x
pGcm = pAG - lG/2*G.x
pHcm = pGH - lH/2*H.x

#Angular velocity
wNA = N.get_w_to(A) 

wAC = A.get_w_to(C)
wAE = A.get_w_to(E)
wAI = A.get_w_to(I)
wAG = A.get_w_to(G)

wCD = C.get_w_to(D)
wEF = E.get_w_to(F)
wIJ = I.get_w_to(J)
wGH = G.get_w_to(H)

#vBtip = pBtip.time_derivative(N,system)

IA = Dyadic.build(A,Ixx_B,Iyy_B,Izz_B)
IC = Dyadic.build(C,Ixx_C,Iyy_C,Izz_C)
ID = Dyadic.build(D,Ixx_D,Iyy_D,Izz_D)
IE = Dyadic.build(E,Ixx_E,Iyy_E,Izz_E)
IF = Dyadic.build(F,Ixx_F,Iyy_F,Izz_F)
IG = Dyadic.build(G,Ixx_G,Iyy_G,Izz_G)
IH = Dyadic.build(H,Ixx_H,Iyy_H,Izz_H)
II = Dyadic.build(I,Ixx_I,Iyy_I,Izz_I)
IJ = Dyadic.build(J,Ixx_J,Iyy_J,Izz_J)


BodyA = Body('BodyA',A,pAcm,mB,IA,system)
BodyC = Body('BodyC',C,pCcm,mC,IC,system)
BodyD = Body('BodyD',D,pDcm,mD,ID,system)
BodyE = Body('BodyE',E,pEcm,mE,IE,system)
BodyF = Body('BodyF',F,pFcm,mF,IF,system)
BodyG = Body('BodyG',G,pGcm,mG,IG,system)
BodyH = Body('BodyH',H,pHcm,mH,IH,system)
BodyI = Body('BodyI',I,pIcm,mI,II,system)
BodyJ = Body('BodyJ',J,pJcm,mJ,IJ,system)

# Damper forces
system.addforce(-b*wAC,wAC)
system.addforce(-b*wAE,wAE)
system.addforce(-b*wAI,wAI)
system.addforce(-b*wAG,wAG)

system.addforce(-b*wCD,wCD)
system.addforce(-b*wEF,wEF)
system.addforce(-b*wIJ,wIJ)
system.addforce(-b*wGH,wGH)

<pynamics.force.Force at 0x7f13da67e910>

#Spring forces
system.add_spring_force1(k,(qC-preload1)*A.z,wAC)
system.add_spring_force1(k,(qD-preload2)*C.z,wCD)

system.add_spring_force1(k,(qE-preload1)*A.z,wAE)
system.add_spring_force1(k,(qF-preload2)*E.z,wEF)

system.add_spring_force1(k,(qI-preload1)*A.z,wAI)
system.add_spring_force1(k,(qJ-preload2)*C.z,wIJ)

system.add_spring_force1(k,(qG-preload1)*A.z,wAG)
system.add_spring_force1(k,(qH-preload2)*C.z,wGH)

(<pynamics.force.Force at 0x7f13da55d2d0>,
 <pynamics.spring.Spring at 0x7f13da55d550>)

#end Stops
system.add_spring_force1(kEndStop,((abs(qD+limit)+qD+limit)/(2*(qD+limit)))*(qD+limit)*C.z,wCD)
system.add_spring_force1(kEndStop,((abs(qF+limit)+qF+limit)/(2*(qF+limit)))*(qF+limit)*E.z,wEF)
system.add_spring_force1(kEndStop,((abs(qJ+limit)+qJ+limit)/(2*(qJ+limit)))*(qJ+limit)*I.z,wIJ)
system.add_spring_force1(kEndStop,((abs(qH+limit)+qH+limit)/(2*(qH+limit)))*(qH+limit)*G.z,wGH)

#end Stop Damping
system.addforce(-bEndStop*((abs(qD+limit)+qD+limit)/(2*(qD+limit)))*wCD,wCD)
system.addforce(-bEndStop*((abs(qF+limit)+qF+limit)/(2*(qF+limit)))*wEF,wEF)
system.addforce(-bEndStop*((abs(qJ+limit)+qJ+limit)/(2*(qJ+limit)))*wIJ,wIJ)
system.addforce(-bEndStop*((abs(qH+limit)+qH+limit)/(2*(qH+limit)))*wGH,wGH)

<pynamics.force.Force at 0x7f13da59ef50>

#constraning AB not to fall

eq = []
eq.append(.1*N.y - pNA)
eq.append(.1*N.y+.15*N.x - pAB)

eq_scalar = []
eq_scalar.append(eq[0].dot(N.x))
eq_scalar.append(eq[0].dot(N.y))
#eq_scalar.append(eq[1].dot(N.x))
eq_scalar.append(eq[1].dot(N.y))

eq_d = [item.time_derivative() for item in eq]
eq_dd = [item.time_derivative() for item in eq_d]
eq_dd_scalar = []
eq_dd_scalar.append(eq_dd[0].dot(N.x))
eq_dd_scalar.append(eq_dd[0].dot(N.y))
#eq_dd_scalar.append(eq_dd[1].dot(N.x))
eq_dd_scalar.append(eq_dd[1].dot(N.y))

system.add_constraint(AccelerationConstraint(eq_dd_scalar))

system.addforcegravity(-g*N.y)

f,ma = system.getdynamics()

2022-03-05 21:05:41,636 - pynamics.system - INFO - getting dynamic equations

f

[-b*qC_d + g*lC*mC*sin(Aq)*sin(qC)/2 - g*lC*mC*cos(Aq)*cos(qC)/2 + g*lC*mD*sin(Aq)*sin(qC) - g*lC*mD*cos(Aq)*cos(qC) - g*lD*mD*(-sin(qC)*sin(qD) + cos(qC)*cos(qD))*cos(Aq)/2 - g*lD*mD*(-sin(qC)*cos(qD) - sin(qD)*cos(qC))*sin(Aq)/2 - k*(-preload1 + qC), -b*qD_d - bEndStop*qD_d*(limit + qD + Abs(limit + qD))/(2*limit + 2*qD) - g*lD*mD*(-sin(qC)*sin(qD) + cos(qC)*cos(qD))*cos(Aq)/2 - g*lD*mD*(-sin(qC)*cos(qD) - sin(qD)*cos(qC))*sin(Aq)/2 - k*(-preload2 + qD) - kEndStop*(limit + qD)*(limit + qD + Abs(limit + qD))/(2*limit + 2*qD), -b*qE_d + g*lE*mE*sin(Aq)*sin(qE)/2 - g*lE*mE*cos(Aq)*cos(qE)/2 + g*lE*mF*sin(Aq)*sin(qE) - g*lE*mF*cos(Aq)*cos(qE) - g*lF*mF*(-sin(qE)*sin(qF) + cos(qE)*cos(qF))*cos(Aq)/2 - g*lF*mF*(-sin(qE)*cos(qF) - sin(qF)*cos(qE))*sin(Aq)/2 - k*(-preload1 + qE), -b*qF_d - bEndStop*qF_d*(limit + qF + Abs(limit + qF))/(2*limit + 2*qF) - g*lF*mF*(-sin(qE)*sin(qF) + cos(qE)*cos(qF))*cos(Aq)/2 - g*lF*mF*(-sin(qE)*cos(qF) - sin(qF)*cos(qE))*sin(Aq)/2 - k*(-preload2 + qF) - kEndStop*(limit + qF)*(limit + qF + Abs(limit + qF))/(2*limit + 2*qF), -b*qG_d + g*lG*mG*sin(Aq)*sin(qG)/2 - g*lG*mG*cos(Aq)*cos(qG)/2 + g*lG*mH*sin(Aq)*sin(qG) - g*lG*mH*cos(Aq)*cos(qG) - g*lH*mH*(-sin(qG)*sin(qH) + cos(qG)*cos(qH))*cos(Aq)/2 - g*lH*mH*(-sin(qG)*cos(qH) - sin(qH)*cos(qG))*sin(Aq)/2 - k*(-preload1 + qG), -b*qH_d - bEndStop*qH_d*(limit + qH + Abs(limit + qH))/(2*limit + 2*qH) - g*lH*mH*(-sin(qG)*sin(qH) + cos(qG)*cos(qH))*cos(Aq)/2 - g*lH*mH*(-sin(qG)*cos(qH) - sin(qH)*cos(qG))*sin(Aq)/2 - k*(-preload2 + qH) - kEndStop*(limit + qH)*(limit + qH + Abs(limit + qH))/(2*limit + 2*qH), -b*qI_d + g*lI*mI*sin(Aq)*sin(qI)/2 - g*lI*mI*cos(Aq)*cos(qI)/2 + g*lI*mJ*sin(Aq)*sin(qI) - g*lI*mJ*cos(Aq)*cos(qI) - g*lJ*mJ*(-sin(qI)*sin(qJ) + cos(qI)*cos(qJ))*cos(Aq)/2 - g*lJ*mJ*(-sin(qI)*cos(qJ) - sin(qJ)*cos(qI))*sin(Aq)/2 - k*(-preload1 + qI), -b*qJ_d - bEndStop*qJ_d*(limit + qJ + Abs(limit + qJ))/(2*limit + 2*qJ) - g*lJ*mJ*(-sin(qI)*sin(qJ) + cos(qI)*cos(qJ))*cos(Aq)/2 - g*lJ*mJ*(-sin(qI)*cos(qJ) - sin(qJ)*cos(qI))*sin(Aq)/2 - k*(-preload2 + qJ) - kEndStop*(limit + qJ)*(limit + qJ + Abs(limit + qJ))/(2*limit + 2*qJ), 0, -g*mB - g*mC - g*mD - g*mE - g*mF - g*mG - g*mH - g*mI - g*mJ, -g*lB*mB*cos(Aq)/2 - g*lB*mC*cos(Aq) - g*lB*mD*cos(Aq) - g*lB*mE*cos(Aq) - g*lB*mF*cos(Aq) + g*lC*mC*sin(Aq)*sin(qC)/2 - g*lC*mC*cos(Aq)*cos(qC)/2 + g*lC*mD*sin(Aq)*sin(qC) - g*lC*mD*cos(Aq)*cos(qC) - g*lD*mD*(-sin(qC)*sin(qD) + cos(qC)*cos(qD))*cos(Aq)/2 - g*lD*mD*(-sin(qC)*cos(qD) - sin(qD)*cos(qC))*sin(Aq)/2 + g*lE*mE*sin(Aq)*sin(qE)/2 - g*lE*mE*cos(Aq)*cos(qE)/2 + g*lE*mF*sin(Aq)*sin(qE) - g*lE*mF*cos(Aq)*cos(qE) - g*lF*mF*(-sin(qE)*sin(qF) + cos(qE)*cos(qF))*cos(Aq)/2 - g*lF*mF*(-sin(qE)*cos(qF) - sin(qF)*cos(qE))*sin(Aq)/2 + g*lG*mG*sin(Aq)*sin(qG)/2 - g*lG*mG*cos(Aq)*cos(qG)/2 + g*lG*mH*sin(Aq)*sin(qG) - g*lG*mH*cos(Aq)*cos(qG) - g*lH*mH*(-sin(qG)*sin(qH) + cos(qG)*cos(qH))*cos(Aq)/2 - g*lH*mH*(-sin(qG)*cos(qH) - sin(qH)*cos(qG))*sin(Aq)/2 + g*lI*mI*sin(Aq)*sin(qI)/2 - g*lI*mI*cos(Aq)*cos(qI)/2 + g*lI*mJ*sin(Aq)*sin(qI) - g*lI*mJ*cos(Aq)*cos(qI) - g*lJ*mJ*(-sin(qI)*sin(qJ) + cos(qI)*cos(qJ))*cos(Aq)/2 - g*lJ*mJ*(-sin(qI)*cos(qJ) - sin(qJ)*cos(qI))*sin(Aq)/2]

ma

[Izz_C*(Aq_dd + qC_dd) + Izz_D*(Aq_dd + qC_dd + qD_dd) + lB*lC*mC*Aq_d**2*sin(qC)/2 + lB*lC*mC*Aq_dd*cos(qC)/2 + lB*lC*mD*Aq_d**2*sin(qC) + lB*lC*mD*Aq_dd*cos(qC) + lC*lD*mD*(Aq_d + qC_d)**2*sin(qD)/2 - lC*lD*mD*(Aq_d + qC_d + qD_d)**2*sin(qD)/2 + lC*mC*(lC*Aq_dd/2 + lC*qC_dd/2)/2 + lC*mD*(lC*Aq_dd + lC*qC_dd) + lC*mD*(lD*Aq_dd/2 + lD*qC_dd/2 + lD*qD_dd/2)*cos(qD) + lC*(-mC*Ax_dd*sin(Aq) + mC*Ay_dd*cos(Aq))*cos(qC)/2 - lC*(mC*Ax_dd*cos(Aq) + mC*Ay_dd*sin(Aq))*sin(qC)/2 + lC*(-mD*Ax_dd*sin(Aq) + mD*Ay_dd*cos(Aq))*cos(qC) - lC*(mD*Ax_dd*cos(Aq) + mD*Ay_dd*sin(Aq))*sin(qC) + lD*mD*(lC*Aq_dd + lC*qC_dd)*cos(qD)/2 + lD*mD*(lD*Aq_dd/2 + lD*qC_dd/2 + lD*qD_dd/2)/2 + lD*(-sin(qC)*sin(qD) + cos(qC)*cos(qD))*(-mD*Ax_dd*sin(Aq) + mD*Ay_dd*cos(Aq))/2 + lD*(-sin(qC)*cos(qD) - sin(qD)*cos(qC))*(mD*Ax_dd*cos(Aq) + mD*Ay_dd*sin(Aq))/2 + lD*(lB*mD*Aq_d**2*sin(qC) + lB*mD*Aq_dd*cos(qC))*cos(qD)/2 - lD*(-lB*mD*Aq_d**2*cos(qC) + lB*mD*Aq_dd*sin(qC))*sin(qD)/2, Izz_D*(Aq_dd + qC_dd + qD_dd) + lC*lD*mD*(Aq_d + qC_d)**2*sin(qD)/2 + lD*mD*(lC*Aq_dd + lC*qC_dd)*cos(qD)/2 + lD*mD*(lD*Aq_dd/2 + lD*qC_dd/2 + lD*qD_dd/2)/2 + lD*(-sin(qC)*sin(qD) + cos(qC)*cos(qD))*(-mD*Ax_dd*sin(Aq) + mD*Ay_dd*cos(Aq))/2 + lD*(-sin(qC)*cos(qD) - sin(qD)*cos(qC))*(mD*Ax_dd*cos(Aq) + mD*Ay_dd*sin(Aq))/2 + lD*(lB*mD*Aq_d**2*sin(qC) + lB*mD*Aq_dd*cos(qC))*cos(qD)/2 - lD*(-lB*mD*Aq_d**2*cos(qC) + lB*mD*Aq_dd*sin(qC))*sin(qD)/2, Izz_E*(Aq_dd + qE_dd) + Izz_F*(Aq_dd + qE_dd + qF_dd) + lB*lE*mE*Aq_d**2*sin(qE)/2 + lB*lE*mE*Aq_dd*cos(qE)/2 + lB*lE*mF*Aq_d**2*sin(qE) + lB*lE*mF*Aq_dd*cos(qE) + lE*lF*mF*(Aq_d + qE_d)**2*sin(qF)/2 - lE*lF*mF*(Aq_d + qE_d + qF_d)**2*sin(qF)/2 + lE*mE*(lE*Aq_dd/2 + lE*qE_dd/2)/2 + lE*mF*(lE*Aq_dd + lE*qE_dd) + lE*mF*(lF*Aq_dd/2 + lF*qE_dd/2 + lF*qF_dd/2)*cos(qF) + lE*(-mE*Ax_dd*sin(Aq) + mE*Ay_dd*cos(Aq))*cos(qE)/2 - lE*(mE*Ax_dd*cos(Aq) + mE*Ay_dd*sin(Aq))*sin(qE)/2 + lE*(-mF*Ax_dd*sin(Aq) + mF*Ay_dd*cos(Aq))*cos(qE) - lE*(mF*Ax_dd*cos(Aq) + mF*Ay_dd*sin(Aq))*sin(qE) + lF*mF*(lE*Aq_dd + lE*qE_dd)*cos(qF)/2 + lF*mF*(lF*Aq_dd/2 + lF*qE_dd/2 + lF*qF_dd/2)/2 + lF*(-sin(qE)*sin(qF) + cos(qE)*cos(qF))*(-mF*Ax_dd*sin(Aq) + mF*Ay_dd*cos(Aq))/2 + lF*(-sin(qE)*cos(qF) - sin(qF)*cos(qE))*(mF*Ax_dd*cos(Aq) + mF*Ay_dd*sin(Aq))/2 + lF*(lB*mF*Aq_d**2*sin(qE) + lB*mF*Aq_dd*cos(qE))*cos(qF)/2 - lF*(-lB*mF*Aq_d**2*cos(qE) + lB*mF*Aq_dd*sin(qE))*sin(qF)/2, Izz_F*(Aq_dd + qE_dd + qF_dd) + lE*lF*mF*(Aq_d + qE_d)**2*sin(qF)/2 + lF*mF*(lE*Aq_dd + lE*qE_dd)*cos(qF)/2 + lF*mF*(lF*Aq_dd/2 + lF*qE_dd/2 + lF*qF_dd/2)/2 + lF*(-sin(qE)*sin(qF) + cos(qE)*cos(qF))*(-mF*Ax_dd*sin(Aq) + mF*Ay_dd*cos(Aq))/2 + lF*(-sin(qE)*cos(qF) - sin(qF)*cos(qE))*(mF*Ax_dd*cos(Aq) + mF*Ay_dd*sin(Aq))/2 + lF*(lB*mF*Aq_d**2*sin(qE) + lB*mF*Aq_dd*cos(qE))*cos(qF)/2 - lF*(-lB*mF*Aq_d**2*cos(qE) + lB*mF*Aq_dd*sin(qE))*sin(qF)/2, Izz_G*(Aq_dd + qG_dd) + Izz_H*(Aq_dd + qG_dd + qH_dd) + lG*lH*mH*(Aq_d + qG_d)**2*sin(qH)/2 - lG*lH*mH*(Aq_d + qG_d + qH_d)**2*sin(qH)/2 + lG*mG*(lG*Aq_dd/2 + lG*qG_dd/2)/2 + lG*mH*(lG*Aq_dd + lG*qG_dd) + lG*mH*(lH*Aq_dd/2 + lH*qG_dd/2 + lH*qH_dd/2)*cos(qH) + lG*(-mG*Ax_dd*sin(Aq) + mG*Ay_dd*cos(Aq))*cos(qG)/2 - lG*(mG*Ax_dd*cos(Aq) + mG*Ay_dd*sin(Aq))*sin(qG)/2 + lG*(-mH*Ax_dd*sin(Aq) + mH*Ay_dd*cos(Aq))*cos(qG) - lG*(mH*Ax_dd*cos(Aq) + mH*Ay_dd*sin(Aq))*sin(qG) + lH*mH*(lG*Aq_dd + lG*qG_dd)*cos(qH)/2 + lH*mH*(lH*Aq_dd/2 + lH*qG_dd/2 + lH*qH_dd/2)/2 + lH*(-sin(qG)*sin(qH) + cos(qG)*cos(qH))*(-mH*Ax_dd*sin(Aq) + mH*Ay_dd*cos(Aq))/2 + lH*(-sin(qG)*cos(qH) - sin(qH)*cos(qG))*(mH*Ax_dd*cos(Aq) + mH*Ay_dd*sin(Aq))/2, Izz_H*(Aq_dd + qG_dd + qH_dd) + lG*lH*mH*(Aq_d + qG_d)**2*sin(qH)/2 + lH*mH*(lG*Aq_dd + lG*qG_dd)*cos(qH)/2 + lH*mH*(lH*Aq_dd/2 + lH*qG_dd/2 + lH*qH_dd/2)/2 + lH*(-sin(qG)*sin(qH) + cos(qG)*cos(qH))*(-mH*Ax_dd*sin(Aq) + mH*Ay_dd*cos(Aq))/2 + lH*(-sin(qG)*cos(qH) - sin(qH)*cos(qG))*(mH*Ax_dd*cos(Aq) + mH*Ay_dd*sin(Aq))/2, Izz_I*(Aq_dd + qI_dd) + Izz_J*(Aq_dd + qI_dd + qJ_dd) + lI*lJ*mJ*(Aq_d + qI_d)**2*sin(qJ)/2 - lI*lJ*mJ*(Aq_d + qI_d + qJ_d)**2*sin(qJ)/2 + lI*mI*(lI*Aq_dd/2 + lI*qI_dd/2)/2 + lI*mJ*(lI*Aq_dd + lI*qI_dd) + lI*mJ*(lJ*Aq_dd/2 + lJ*qI_dd/2 + lJ*qJ_dd/2)*cos(qJ) + lI*(-mI*Ax_dd*sin(Aq) + mI*Ay_dd*cos(Aq))*cos(qI)/2 - lI*(mI*Ax_dd*cos(Aq) + mI*Ay_dd*sin(Aq))*sin(qI)/2 + lI*(-mJ*Ax_dd*sin(Aq) + mJ*Ay_dd*cos(Aq))*cos(qI) - lI*(mJ*Ax_dd*cos(Aq) + mJ*Ay_dd*sin(Aq))*sin(qI) + lJ*mJ*(lI*Aq_dd + lI*qI_dd)*cos(qJ)/2 + lJ*mJ*(lJ*Aq_dd/2 + lJ*qI_dd/2 + lJ*qJ_dd/2)/2 + lJ*(-sin(qI)*sin(qJ) + cos(qI)*cos(qJ))*(-mJ*Ax_dd*sin(Aq) + mJ*Ay_dd*cos(Aq))/2 + lJ*(-sin(qI)*cos(qJ) - sin(qJ)*cos(qI))*(mJ*Ax_dd*cos(Aq) + mJ*Ay_dd*sin(Aq))/2, Izz_J*(Aq_dd + qI_dd + qJ_dd) + lI*lJ*mJ*(Aq_d + qI_d)**2*sin(qJ)/2 + lJ*mJ*(lI*Aq_dd + lI*qI_dd)*cos(qJ)/2 + lJ*mJ*(lJ*Aq_dd/2 + lJ*qI_dd/2 + lJ*qJ_dd/2)/2 + lJ*(-sin(qI)*sin(qJ) + cos(qI)*cos(qJ))*(-mJ*Ax_dd*sin(Aq) + mJ*Ay_dd*cos(Aq))/2 + lJ*(-sin(qI)*cos(qJ) - sin(qJ)*cos(qI))*(mJ*Ax_dd*cos(Aq) + mJ*Ay_dd*sin(Aq))/2, -lB*mB*Aq_d**2*cos(Aq)/2 - lB*mB*Aq_dd*sin(Aq)/2 - lB*mC*Aq_d**2*cos(Aq) - lB*mC*Aq_dd*sin(Aq) - lB*mD*Aq_d**2*cos(Aq) - lB*mD*Aq_dd*sin(Aq) - lB*mE*Aq_d**2*cos(Aq) - lB*mE*Aq_dd*sin(Aq) - lB*mF*Aq_d**2*cos(Aq) - lB*mF*Aq_dd*sin(Aq) + mB*Ax_dd + mC*Ax_dd + mD*Ax_dd + mE*Ax_dd + mF*Ax_dd + mG*Ax_dd + mH*Ax_dd + mI*Ax_dd + mJ*Ax_dd + (-sin(Aq)*sin(qC) + cos(Aq)*cos(qC))*(-lD*mD*(Aq_d + qC_d + qD_d)**2*cos(qD)/2 - mD*(lD*Aq_dd/2 + lD*qC_dd/2 + lD*qD_dd/2)*sin(qD)) + (-sin(Aq)*sin(qE) + cos(Aq)*cos(qE))*(-lF*mF*(Aq_d + qE_d + qF_d)**2*cos(qF)/2 - mF*(lF*Aq_dd/2 + lF*qE_dd/2 + lF*qF_dd/2)*sin(qF)) + (-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*(-lH*mH*(Aq_d + qG_d + qH_d)**2*cos(qH)/2 - mH*(lH*Aq_dd/2 + lH*qG_dd/2 + lH*qH_dd/2)*sin(qH)) + (-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*(-lJ*mJ*(Aq_d + qI_d + qJ_d)**2*cos(qJ)/2 - mJ*(lJ*Aq_dd/2 + lJ*qI_dd/2 + lJ*qJ_dd/2)*sin(qJ)) + (-sin(Aq)*cos(qC) - sin(qC)*cos(Aq))*(-lD*mD*(Aq_d + qC_d + qD_d)**2*sin(qD)/2 + mD*(lD*Aq_dd/2 + lD*qC_dd/2 + lD*qD_dd/2)*cos(qD)) + (-sin(Aq)*cos(qE) - sin(qE)*cos(Aq))*(-lF*mF*(Aq_d + qE_d + qF_d)**2*sin(qF)/2 + mF*(lF*Aq_dd/2 + lF*qE_dd/2 + lF*qF_dd/2)*cos(qF)) + (-sin(Aq)*cos(qG) - sin(qG)*cos(Aq))*(-lH*mH*(Aq_d + qG_d + qH_d)**2*sin(qH)/2 + mH*(lH*Aq_dd/2 + lH*qG_dd/2 + lH*qH_dd/2)*cos(qH)) + (-sin(Aq)*cos(qI) - sin(qI)*cos(Aq))*(-lJ*mJ*(Aq_d + qI_d + qJ_d)**2*sin(qJ)/2 + mJ*(lJ*Aq_dd/2 + lJ*qI_dd/2 + lJ*qJ_dd/2)*cos(qJ)) - (-lC*mC*(Aq_d + qC_d)**2*sin(qC)/2 + mC*(lC*Aq_dd/2 + lC*qC_dd/2)*cos(qC))*sin(Aq) + (-lC*mC*(Aq_d + qC_d)**2*cos(qC)/2 - mC*(lC*Aq_dd/2 + lC*qC_dd/2)*sin(qC))*cos(Aq) - (-lC*mD*(Aq_d + qC_d)**2*sin(qC) + mD*(lC*Aq_dd + lC*qC_dd)*cos(qC))*sin(Aq) + (-lC*mD*(Aq_d + qC_d)**2*cos(qC) - mD*(lC*Aq_dd + lC*qC_dd)*sin(qC))*cos(Aq) - (-lE*mE*(Aq_d + qE_d)**2*sin(qE)/2 + mE*(lE*Aq_dd/2 + lE*qE_dd/2)*cos(qE))*sin(Aq) + (-lE*mE*(Aq_d + qE_d)**2*cos(qE)/2 - mE*(lE*Aq_dd/2 + lE*qE_dd/2)*sin(qE))*cos(Aq) - (-lE*mF*(Aq_d + qE_d)**2*sin(qE) + mF*(lE*Aq_dd + lE*qE_dd)*cos(qE))*sin(Aq) + (-lE*mF*(Aq_d + qE_d)**2*cos(qE) - mF*(lE*Aq_dd + lE*qE_dd)*sin(qE))*cos(Aq) - (-lG*mG*(Aq_d + qG_d)**2*sin(qG)/2 + mG*(lG*Aq_dd/2 + lG*qG_dd/2)*cos(qG))*sin(Aq) + (-lG*mG*(Aq_d + qG_d)**2*cos(qG)/2 - mG*(lG*Aq_dd/2 + lG*qG_dd/2)*sin(qG))*cos(Aq) - (-lG*mH*(Aq_d + qG_d)**2*sin(qG) + mH*(lG*Aq_dd + lG*qG_dd)*cos(qG))*sin(Aq) + (-lG*mH*(Aq_d + qG_d)**2*cos(qG) - mH*(lG*Aq_dd + lG*qG_dd)*sin(qG))*cos(Aq) - (-lI*mI*(Aq_d + qI_d)**2*sin(qI)/2 + mI*(lI*Aq_dd/2 + lI*qI_dd/2)*cos(qI))*sin(Aq) + (-lI*mI*(Aq_d + qI_d)**2*cos(qI)/2 - mI*(lI*Aq_dd/2 + lI*qI_dd/2)*sin(qI))*cos(Aq) - (-lI*mJ*(Aq_d + qI_d)**2*sin(qI) + mJ*(lI*Aq_dd + lI*qI_dd)*cos(qI))*sin(Aq) + (-lI*mJ*(Aq_d + qI_d)**2*cos(qI) - mJ*(lI*Aq_dd + lI*qI_dd)*sin(qI))*cos(Aq), -lB*mB*Aq_d**2*sin(Aq)/2 + lB*mB*Aq_dd*cos(Aq)/2 - lB*mC*Aq_d**2*sin(Aq) + lB*mC*Aq_dd*cos(Aq) - lB*mD*Aq_d**2*sin(Aq) + lB*mD*Aq_dd*cos(Aq) - lB*mE*Aq_d**2*sin(Aq) + lB*mE*Aq_dd*cos(Aq) - lB*mF*Aq_d**2*sin(Aq) + lB*mF*Aq_dd*cos(Aq) + mB*Ay_dd + mC*Ay_dd + mD*Ay_dd + mE*Ay_dd + mF*Ay_dd + mG*Ay_dd + mH*Ay_dd + mI*Ay_dd + mJ*Ay_dd + (-sin(Aq)*sin(qC) + cos(Aq)*cos(qC))*(-lD*mD*(Aq_d + qC_d + qD_d)**2*sin(qD)/2 + mD*(lD*Aq_dd/2 + lD*qC_dd/2 + lD*qD_dd/2)*cos(qD)) + (-sin(Aq)*sin(qE) + cos(Aq)*cos(qE))*(-lF*mF*(Aq_d + qE_d + qF_d)**2*sin(qF)/2 + mF*(lF*Aq_dd/2 + lF*qE_dd/2 + lF*qF_dd/2)*cos(qF)) + (-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*(-lH*mH*(Aq_d + qG_d + qH_d)**2*sin(qH)/2 + mH*(lH*Aq_dd/2 + lH*qG_dd/2 + lH*qH_dd/2)*cos(qH)) + (-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*(-lJ*mJ*(Aq_d + qI_d + qJ_d)**2*sin(qJ)/2 + mJ*(lJ*Aq_dd/2 + lJ*qI_dd/2 + lJ*qJ_dd/2)*cos(qJ)) + (sin(Aq)*cos(qC) + sin(qC)*cos(Aq))*(-lD*mD*(Aq_d + qC_d + qD_d)**2*cos(qD)/2 - mD*(lD*Aq_dd/2 + lD*qC_dd/2 + lD*qD_dd/2)*sin(qD)) + (sin(Aq)*cos(qE) + sin(qE)*cos(Aq))*(-lF*mF*(Aq_d + qE_d + qF_d)**2*cos(qF)/2 - mF*(lF*Aq_dd/2 + lF*qE_dd/2 + lF*qF_dd/2)*sin(qF)) + (sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*(-lH*mH*(Aq_d + qG_d + qH_d)**2*cos(qH)/2 - mH*(lH*Aq_dd/2 + lH*qG_dd/2 + lH*qH_dd/2)*sin(qH)) + (sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*(-lJ*mJ*(Aq_d + qI_d + qJ_d)**2*cos(qJ)/2 - mJ*(lJ*Aq_dd/2 + lJ*qI_dd/2 + lJ*qJ_dd/2)*sin(qJ)) + (-lC*mC*(Aq_d + qC_d)**2*sin(qC)/2 + mC*(lC*Aq_dd/2 + lC*qC_dd/2)*cos(qC))*cos(Aq) + (-lC*mC*(Aq_d + qC_d)**2*cos(qC)/2 - mC*(lC*Aq_dd/2 + lC*qC_dd/2)*sin(qC))*sin(Aq) + (-lC*mD*(Aq_d + qC_d)**2*sin(qC) + mD*(lC*Aq_dd + lC*qC_dd)*cos(qC))*cos(Aq) + (-lC*mD*(Aq_d + qC_d)**2*cos(qC) - mD*(lC*Aq_dd + lC*qC_dd)*sin(qC))*sin(Aq) + (-lE*mE*(Aq_d + qE_d)**2*sin(qE)/2 + mE*(lE*Aq_dd/2 + lE*qE_dd/2)*cos(qE))*cos(Aq) + (-lE*mE*(Aq_d + qE_d)**2*cos(qE)/2 - mE*(lE*Aq_dd/2 + lE*qE_dd/2)*sin(qE))*sin(Aq) + (-lE*mF*(Aq_d + qE_d)**2*sin(qE) + mF*(lE*Aq_dd + lE*qE_dd)*cos(qE))*cos(Aq) + (-lE*mF*(Aq_d + qE_d)**2*cos(qE) - mF*(lE*Aq_dd + lE*qE_dd)*sin(qE))*sin(Aq) + (-lG*mG*(Aq_d + qG_d)**2*sin(qG)/2 + mG*(lG*Aq_dd/2 + lG*qG_dd/2)*cos(qG))*cos(Aq) + (-lG*mG*(Aq_d + qG_d)**2*cos(qG)/2 - mG*(lG*Aq_dd/2 + lG*qG_dd/2)*sin(qG))*sin(Aq) + (-lG*mH*(Aq_d + qG_d)**2*sin(qG) + mH*(lG*Aq_dd + lG*qG_dd)*cos(qG))*cos(Aq) + (-lG*mH*(Aq_d + qG_d)**2*cos(qG) - mH*(lG*Aq_dd + lG*qG_dd)*sin(qG))*sin(Aq) + (-lI*mI*(Aq_d + qI_d)**2*sin(qI)/2 + mI*(lI*Aq_dd/2 + lI*qI_dd/2)*cos(qI))*cos(Aq) + (-lI*mI*(Aq_d + qI_d)**2*cos(qI)/2 - mI*(lI*Aq_dd/2 + lI*qI_dd/2)*sin(qI))*sin(Aq) + (-lI*mJ*(Aq_d + qI_d)**2*sin(qI) + mJ*(lI*Aq_dd + lI*qI_dd)*cos(qI))*cos(Aq) + (-lI*mJ*(Aq_d + qI_d)**2*cos(qI) - mJ*(lI*Aq_dd + lI*qI_dd)*sin(qI))*sin(Aq), Izz_B*Aq_dd + Izz_C*(Aq_dd + qC_dd) + Izz_D*(Aq_dd + qC_dd + qD_dd) + Izz_E*(Aq_dd + qE_dd) + Izz_F*(Aq_dd + qE_dd + qF_dd) + Izz_G*(Aq_dd + qG_dd) + Izz_H*(Aq_dd + qG_dd + qH_dd) + Izz_I*(Aq_dd + qI_dd) + Izz_J*(Aq_dd + qI_dd + qJ_dd) + lB**2*mB*Aq_dd/4 + lB**2*mC*Aq_dd + lB**2*mD*Aq_dd + lB**2*mE*Aq_dd + lB**2*mF*Aq_dd + lB*lC*mC*Aq_d**2*sin(qC)/2 + lB*lC*mC*Aq_dd*cos(qC)/2 - lB*lC*mC*(Aq_d + qC_d)**2*sin(qC)/2 + lB*lC*mD*Aq_d**2*sin(qC) + lB*lC*mD*Aq_dd*cos(qC) - lB*lC*mD*(Aq_d + qC_d)**2*sin(qC) + lB*lE*mE*Aq_d**2*sin(qE)/2 + lB*lE*mE*Aq_dd*cos(qE)/2 - lB*lE*mE*(Aq_d + qE_d)**2*sin(qE)/2 + lB*lE*mF*Aq_d**2*sin(qE) + lB*lE*mF*Aq_dd*cos(qE) - lB*lE*mF*(Aq_d + qE_d)**2*sin(qE) - lB*mB*Ax_dd*sin(Aq)/2 + lB*mB*Ay_dd*cos(Aq)/2 - lB*mC*Ax_dd*sin(Aq) + lB*mC*Ay_dd*cos(Aq) + lB*mC*(lC*Aq_dd/2 + lC*qC_dd/2)*cos(qC) - lB*mD*Ax_dd*sin(Aq) + lB*mD*Ay_dd*cos(Aq) + lB*mD*(lC*Aq_dd + lC*qC_dd)*cos(qC) - lB*mE*Ax_dd*sin(Aq) + lB*mE*Ay_dd*cos(Aq) + lB*mE*(lE*Aq_dd/2 + lE*qE_dd/2)*cos(qE) - lB*mF*Ax_dd*sin(Aq) + lB*mF*Ay_dd*cos(Aq) + lB*mF*(lE*Aq_dd + lE*qE_dd)*cos(qE) + lB*(-lD*mD*(Aq_d + qC_d + qD_d)**2*sin(qD)/2 + mD*(lD*Aq_dd/2 + lD*qC_dd/2 + lD*qD_dd/2)*cos(qD))*cos(qC) + lB*(-lD*mD*(Aq_d + qC_d + qD_d)**2*cos(qD)/2 - mD*(lD*Aq_dd/2 + lD*qC_dd/2 + lD*qD_dd/2)*sin(qD))*sin(qC) + lB*(-lF*mF*(Aq_d + qE_d + qF_d)**2*sin(qF)/2 + mF*(lF*Aq_dd/2 + lF*qE_dd/2 + lF*qF_dd/2)*cos(qF))*cos(qE) + lB*(-lF*mF*(Aq_d + qE_d + qF_d)**2*cos(qF)/2 - mF*(lF*Aq_dd/2 + lF*qE_dd/2 + lF*qF_dd/2)*sin(qF))*sin(qE) + lC*lD*mD*(Aq_d + qC_d)**2*sin(qD)/2 - lC*lD*mD*(Aq_d + qC_d + qD_d)**2*sin(qD)/2 + lC*mC*(lC*Aq_dd/2 + lC*qC_dd/2)/2 + lC*mD*(lC*Aq_dd + lC*qC_dd) + lC*mD*(lD*Aq_dd/2 + lD*qC_dd/2 + lD*qD_dd/2)*cos(qD) + lC*(-mC*Ax_dd*sin(Aq) + mC*Ay_dd*cos(Aq))*cos(qC)/2 - lC*(mC*Ax_dd*cos(Aq) + mC*Ay_dd*sin(Aq))*sin(qC)/2 + lC*(-mD*Ax_dd*sin(Aq) + mD*Ay_dd*cos(Aq))*cos(qC) - lC*(mD*Ax_dd*cos(Aq) + mD*Ay_dd*sin(Aq))*sin(qC) + lD*mD*(lC*Aq_dd + lC*qC_dd)*cos(qD)/2 + lD*mD*(lD*Aq_dd/2 + lD*qC_dd/2 + lD*qD_dd/2)/2 + lD*(-sin(qC)*sin(qD) + cos(qC)*cos(qD))*(-mD*Ax_dd*sin(Aq) + mD*Ay_dd*cos(Aq))/2 + lD*(-sin(qC)*cos(qD) - sin(qD)*cos(qC))*(mD*Ax_dd*cos(Aq) + mD*Ay_dd*sin(Aq))/2 + lD*(lB*mD*Aq_d**2*sin(qC) + lB*mD*Aq_dd*cos(qC))*cos(qD)/2 - lD*(-lB*mD*Aq_d**2*cos(qC) + lB*mD*Aq_dd*sin(qC))*sin(qD)/2 + lE*lF*mF*(Aq_d + qE_d)**2*sin(qF)/2 - lE*lF*mF*(Aq_d + qE_d + qF_d)**2*sin(qF)/2 + lE*mE*(lE*Aq_dd/2 + lE*qE_dd/2)/2 + lE*mF*(lE*Aq_dd + lE*qE_dd) + lE*mF*(lF*Aq_dd/2 + lF*qE_dd/2 + lF*qF_dd/2)*cos(qF) + lE*(-mE*Ax_dd*sin(Aq) + mE*Ay_dd*cos(Aq))*cos(qE)/2 - lE*(mE*Ax_dd*cos(Aq) + mE*Ay_dd*sin(Aq))*sin(qE)/2 + lE*(-mF*Ax_dd*sin(Aq) + mF*Ay_dd*cos(Aq))*cos(qE) - lE*(mF*Ax_dd*cos(Aq) + mF*Ay_dd*sin(Aq))*sin(qE) + lF*mF*(lE*Aq_dd + lE*qE_dd)*cos(qF)/2 + lF*mF*(lF*Aq_dd/2 + lF*qE_dd/2 + lF*qF_dd/2)/2 + lF*(-sin(qE)*sin(qF) + cos(qE)*cos(qF))*(-mF*Ax_dd*sin(Aq) + mF*Ay_dd*cos(Aq))/2 + lF*(-sin(qE)*cos(qF) - sin(qF)*cos(qE))*(mF*Ax_dd*cos(Aq) + mF*Ay_dd*sin(Aq))/2 + lF*(lB*mF*Aq_d**2*sin(qE) + lB*mF*Aq_dd*cos(qE))*cos(qF)/2 - lF*(-lB*mF*Aq_d**2*cos(qE) + lB*mF*Aq_dd*sin(qE))*sin(qF)/2 + lG*lH*mH*(Aq_d + qG_d)**2*sin(qH)/2 - lG*lH*mH*(Aq_d + qG_d + qH_d)**2*sin(qH)/2 + lG*mG*(lG*Aq_dd/2 + lG*qG_dd/2)/2 + lG*mH*(lG*Aq_dd + lG*qG_dd) + lG*mH*(lH*Aq_dd/2 + lH*qG_dd/2 + lH*qH_dd/2)*cos(qH) + lG*(-mG*Ax_dd*sin(Aq) + mG*Ay_dd*cos(Aq))*cos(qG)/2 - lG*(mG*Ax_dd*cos(Aq) + mG*Ay_dd*sin(Aq))*sin(qG)/2 + lG*(-mH*Ax_dd*sin(Aq) + mH*Ay_dd*cos(Aq))*cos(qG) - lG*(mH*Ax_dd*cos(Aq) + mH*Ay_dd*sin(Aq))*sin(qG) + lH*mH*(lG*Aq_dd + lG*qG_dd)*cos(qH)/2 + lH*mH*(lH*Aq_dd/2 + lH*qG_dd/2 + lH*qH_dd/2)/2 + lH*(-sin(qG)*sin(qH) + cos(qG)*cos(qH))*(-mH*Ax_dd*sin(Aq) + mH*Ay_dd*cos(Aq))/2 + lH*(-sin(qG)*cos(qH) - sin(qH)*cos(qG))*(mH*Ax_dd*cos(Aq) + mH*Ay_dd*sin(Aq))/2 + lI*lJ*mJ*(Aq_d + qI_d)**2*sin(qJ)/2 - lI*lJ*mJ*(Aq_d + qI_d + qJ_d)**2*sin(qJ)/2 + lI*mI*(lI*Aq_dd/2 + lI*qI_dd/2)/2 + lI*mJ*(lI*Aq_dd + lI*qI_dd) + lI*mJ*(lJ*Aq_dd/2 + lJ*qI_dd/2 + lJ*qJ_dd/2)*cos(qJ) + lI*(-mI*Ax_dd*sin(Aq) + mI*Ay_dd*cos(Aq))*cos(qI)/2 - lI*(mI*Ax_dd*cos(Aq) + mI*Ay_dd*sin(Aq))*sin(qI)/2 + lI*(-mJ*Ax_dd*sin(Aq) + mJ*Ay_dd*cos(Aq))*cos(qI) - lI*(mJ*Ax_dd*cos(Aq) + mJ*Ay_dd*sin(Aq))*sin(qI) + lJ*mJ*(lI*Aq_dd + lI*qI_dd)*cos(qJ)/2 + lJ*mJ*(lJ*Aq_dd/2 + lJ*qI_dd/2 + lJ*qJ_dd/2)/2 + lJ*(-sin(qI)*sin(qJ) + cos(qI)*cos(qJ))*(-mJ*Ax_dd*sin(Aq) + mJ*Ay_dd*cos(Aq))/2 + lJ*(-sin(qI)*cos(qJ) - sin(qJ)*cos(qI))*(mJ*Ax_dd*cos(Aq) + mJ*Ay_dd*sin(Aq))/2]

func1,lambda1 = system.state_space_post_invert(f,ma,return_lambda = True)

2022-03-05 21:05:47,232 - pynamics.system - INFO - solving a = f/m and creating function
2022-03-05 21:05:55,003 - pynamics.system - INFO - substituting constrained in Ma-f.
2022-03-05 21:05:56,788 - pynamics.system - INFO - done solving a = f/m and creating function
2022-03-05 21:05:56,791 - pynamics.system - INFO - calculating function for lambdas

tol = 1e-5

tinitial = 0
tfinal = 10
fps = 30
tstep = 1/fps
t = numpy.r_[tinitial:tfinal:tstep]

states=pynamics.integration.integrate(func1,ini,t,rtol=tol,atol=tol, args=({'constants':system.constant_values},))

2022-03-05 21:05:57,270 - pynamics.integration - INFO - beginning integration
2022-03-05 21:05:57,278 - pynamics.system - INFO - integration at time 0000.00
2022-03-05 21:06:03,642 - pynamics.system - INFO - integration at time 0003.32
2022-03-05 21:06:09,849 - pynamics.system - INFO - integration at time 0007.40
2022-03-05 21:06:15,816 - pynamics.system - INFO - integration at time 0008.04
2022-03-05 21:06:16,002 - pynamics.integration - INFO - finished integration

plt.figure()
artists = plt.plot(t,states[:,:2])
plt.legend(artists,['qC','qD'])

<matplotlib.legend.Legend at 0x7f13d11d5450>

KE = system.get_KE()
PE = system.getPEGravity(pNA) - system.getPESprings()
energy_output = Output([KE-PE],system)
energy_output.calc(states,t)
energy_output.plot_time()

2022-03-05 21:06:17,567 - pynamics.output - INFO - calculating outputs
2022-03-05 21:06:17,794 - pynamics.output - INFO - done calculating outputs

points = [pAB,pBC,pCD,pBC,pAB,pBE,pEF,pBE,pAB,pNA,pAI,pIJ,pAI,pNA,pAG,pGH]
points_output = PointsOutput(points,system)
y = points_output.calc(states,t)
points_output.plot_time(20)

2022-03-05 21:06:18,537 - pynamics.output - INFO - calculating outputs
2022-03-05 21:06:18,744 - pynamics.output - INFO - done calculating outputs





<matplotlib.axes._subplots.AxesSubplot at 0x7f13ccbdb8d0>

points_output.animate(fps = fps,movie_name = 'triple_pendulum.mp4',lw=2,marker='o',color=(1,0,0,1),linestyle='-')

<matplotlib.axes._subplots.AxesSubplot at 0x7f13cc5f38d0>

from matplotlib import animation, rc
from IPython.display import HTML
HTML(points_output.anim.to_html5_video())

We are yet to modify and optimize the code. There is a scope of inclusion of a motor controls section. We will also be considering ground frictrional forces.

Parameter Identification Plan

The goal of this assignment is to decide as a group how we will divide up the various parameter ID tasks. Following is the division of tasks amongst the group members.

(Note: Some tasks may get overlapped as they will be completed as a team or in groups)

Task / Teammate	Manan	Eric	Meet	Disha
Materials			X	X
Mass and Inertia Properties				X
Link, joint stiffness		X
Prototyping	X	X	X	X
Model Fitting using Python (Drawing comparison)	X
Updating the code	X	X
Updating the website			X
Report Compilation	X	X	X	X

Description:

Materials:

Identification of the materials and servos to be used in construction of the robot. Identification of the relevant material parameters, especially material thickness and weight per unit area. Material weight per unit area will be used to find the rotational inertia of the links. The important parameters of the motor will also need to be determined, especially its holding torque. Disha and Meet are exploring for some thin, stiff and lightweight sheet materials for the laminate to make the origami structure. Currently we are studying the characteristics of a material named copper foil paper.

Mass and Inertia Properties:

After identifying the materials and the combination of laminates, we would have to study the mass and inertia properties of the laminated material (self weight of the material) to understand how the motion of the actual prototype will be affected due to the mass and inertia of the laminate. We will be doing load tests to identify these parameters.

Prototyping:

The initial prototyping tasks involve the testing of construction techniques in order to build prototype legs. These leg prototypes will be used to identify the other important leg parameters such as link and joint stiffness. Prototyping activity is divided into a few parts. The CAD designing for the laser cutting will be prepared by Eric, the laser cutting task will be done by Manan and Meet and Disha will be looking after the assembling part.

Link, joint stiffness:

The stiffness of the legs and the joints needs to be calculated. We plan to do this by applying lateral load to the link and then calculating the corresponding displacement.

Model Fitting using Python (Drawing comparison):

This involves finding the optimal parameters and then mapping it to see how well our model fits that data.

Updating the code:

Implementation of vertical ground reaction forces is needed. Implementation of directionally controlled friction forces or friction approximations is needed. Implementation of motors to control the limbs of the model is needed and the motor parameters need to be matched to the motors to be used. The motor actuation pattern also needs to be determined and implemented.

Updating the website:

The Github website needs to be updated regularly once any modifications are made. It will help us in easily finding the past experiment data to compare with new.

Data extraction approach:

Physical experiments with loads and external parameters would be used to extract data. Video recording may be implemented for better results.

Cardstock was used for the initial testing of the material. Cardstock was utilized for Layers 0 and 4, adhesive was used for Layers 1 and 3, and layer 2 was composed of a flexible plastic. We deduced that the final laminated system showed much stiffer properties than we expected. We concluded that the finished laminated structure had far stiffer qualities than we had anticipated. We are now proceeding to construct a 5-layer system using paper (70 GSM) layers 0 and 4.

Note: We are conducting additional testing and will update the website accordingly.