tessellationrobot.github.io

Parameter Identification: Inertia Calculation

By Disha Vyas

In our robot prototype we are going to use four lagged robot paper made rat type robot. I need to identify and measure the paper parameters and leg parameters which includes thickness of paper used, mass of each leg, length of each leg, moment of Inertia of 2 two beam and one joint of each leg and combine rotational inertia for each leg and find which component of inertia we should consider in our model.
Then I need to code the system dynamics and observe the system behavior.

Exprimental Setup:

To measure paper thinness of the paper I have used the vernier caliper as shown below. I used 0.1 mm thick paper.
To measure weight of the paper I have taken 72 sq Inches of paper and find the weight of that paper which is 6.4 gram.
Then to calculate mass for each part of leg, I have measure inches of each part of legs with includes upper beam, joint and lower beams. After that I calculated the area square of each part of the paper by multiplying length and width of the paper.
For rotational inertia for each leg we need to identify which component of inertia I should consider from Ixx, Iyy, Izz.
As shown in fig. below the rotation of the leg is in the z direction so I am considering the Izz component of the inertia.

To calculate the Inertia, I used the universal inertia formula for rectangle beams of the legs and the multiply them by the number of folds which is 8.
For inertia calculation of joints, I have considered the right-angle triangle inertia formula and then multiply by number fold per leg joints which is 16.
I have considered the joint inertia combined with the upper beam of the leg.

a1 = 0.022606
m1 = 0.000277
Izz1 = (m1*a1*a1)/12
Iz1 = Izz1*8
Iz1

9.437043491466668e-08

a2 = 0.022606
m2 = 0.000205
Izz2 = (m2*a2*a2)/12
Iz2 = Izz2 * 8
Iz2

6.984093558666668e-08

a3 = 7.15
m3 = 0.00124
#Izz3 = (m3*a3*a3)/12
#Izz3

a3 = 0.0321056
Izz3 = (a3 * a3 * a3 *a3)/36
Izz3

2.951349633363598e-08

Iz3 = Izz3 * 16
Iz3

4.722159413381757e-07

Iup = Iz2 + Iz3
Iup

5.420568769248424e-07

M1 = m1 * 8 
M1

0.002216

M2 = (m2 * 8) + m3
M2

0.0028799999999999997

a1 = 0.022606
m1 = 0.000277
Izz1 = (m1*a1*a1)/12
Iz1 = Izz1*8
Iz1

9.437043491466668e-08

a2 = 0.022606
m2 = 0.000205
Izz2 = (m2*a2*a2)/12
Iz2 = Izz2 * 8
Iz2

6.984093558666668e-08

a3 = 7.15
m3 = 0.00124
#Izz3 = (m3*a3*a3)/12
#Izz3

a3 = 0.0321056
Izz3 = (a3 * a3 * a3 *a3)/36
Izz3

2.951349633363598e-08

Iz3 = Izz3 * 16
Iz3

4.722159413381757e-07

Iup = Iz2 + Iz3
Iup

5.420568769248424e-07

M1 = m1 * 8 
M1

0.002216

M2 = (m2 * 8) + m3
M2

0.0028799999999999997

#lenghts in m
lB = Constant(0.15,'lB',system)
lC = Constant(0.06604,'lC',system)
lD = Constant(0.0889,'lD',system)
lE = Constant(0.06604,'lE',system)
lF = Constant(0.0889,'lF',system)
lG = Constant(0.06604,'lG',system)
lH = Constant(0.0889,'lH',system)
lI = Constant(0.06604,'lI',system)
lJ = Constant(0.0889,'lJ',system)

#masses in kg
mB = Constant(.5,'mB',system)
mC = Constant(0.0028799999999999997,'mC',system)
mD = Constant(0.002216,'mD',system)
mE = Constant(0.0028799999999999997,'mE',system)
mF = Constant(0.002216,'mF',system)
mG = Constant(0.0028799999999999997,'mG',system)
mH = Constant(0.002216,'mH',system)
mI = Constant(0.0028799999999999997,'mI',system)
mJ = Constant(0.002216,'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(5.420568769248424e-07,'Izz_C',system)
Ixx_D = Constant(1e-2,'Ixx_D',system)
Iyy_D = Constant(1e-2,'Iyy_D',system)
Izz_D = Constant(9.437043491466668e-08,'Izz_D',system)
Ixx_E = Constant(1e-2,'Ixx_E',system)
Iyy_E = Constant(1e-2,'Iyy_E',system)
Izz_E = Constant(5.420568769248424e-07,'Izz_E',system)
Ixx_F = Constant(1e-2,'Ixx_F',system)
Iyy_F = Constant(1e-2,'Iyy_F',system)
Izz_F = Constant(9.437043491466668e-08,'Izz_F',system)
Ixx_G = Constant(1e-2,'Ixx_G',system)
Iyy_G = Constant(1e-2,'Iyy_G',system)
Izz_G = Constant(5.420568769248424e-07,'Izz_G',system)
Ixx_H = Constant(1e-2,'Ixx_H',system)
Iyy_H = Constant(1e-2,'Iyy_H',system)
Izz_H = Constant(9.437043491466668e-08,'Izz_H',system)
Ixx_I = Constant(1e-2,'Ixx_I',system)
Iyy_I = Constant(1e-2,'Iyy_I',system)
Izz_I = Constant(5.420568769248424e-07,'Izz_I',system)
Ixx_J = Constant(1e-2,'Ixx_J',system)
Iyy_J = Constant(1e-2,'Iyy_J',system)
Izz_J = Constant(9.437043491466668e-08,'Izz_J',system)


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

##System Dynamics based on Ineria Calculation

!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: sympy in /usr/local/lib/python3.7/dist-packages (from pynamics) (1.7.1)
Requirement already satisfied: scipy in /usr/local/lib/python3.7/dist-packages (from pynamics) (1.4.1)
Requirement already satisfied: numpy in /usr/local/lib/python3.7/dist-packages (from pynamics) (1.21.5)
Requirement already satisfied: matplotlib in /usr/local/lib/python3.7/dist-packages (from pynamics) (3.2.2)
Requirement already satisfied: python-dateutil>=2.1 in /usr/local/lib/python3.7/dist-packages (from matplotlib->pynamics) (2.8.2)
Requirement already satisfied: cycler>=0.10 in /usr/local/lib/python3.7/dist-packages (from matplotlib->pynamics) (0.11.0)
Requirement already satisfied: kiwisolver>=1.0.1 in /usr/local/lib/python3.7/dist-packages (from matplotlib->pynamics) (1.4.0)
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: typing-extensions in /usr/local/lib/python3.7/dist-packages (from kiwisolver>=1.0.1->matplotlib->pynamics) (3.10.0.2)
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)

Here I have considered parameters as per thr result of conducted expriment.

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

#masses in kg
mB = Constant(.5,'mB',system)
mC = Constant(0.0028799999999999997,'mC',system)
mD = Constant(0.002216,'mD',system)
mE = Constant(0.0028799999999999997,'mE',system)
mF = Constant(0.002216,'mF',system)
mG = Constant(0.0028799999999999997,'mG',system)
mH = Constant(0.002216,'mH',system)
mI = Constant(0.0028799999999999997,'mI',system)
mJ = Constant(0.002216,'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(5.420568769248424e-07,'Izz_C',system)
Ixx_D = Constant(1e-2,'Ixx_D',system)
Iyy_D = Constant(1e-2,'Iyy_D',system)
Izz_D = Constant(9.437043491466668e-08,'Izz_D',system)
Ixx_E = Constant(1e-2,'Ixx_E',system)
Iyy_E = Constant(1e-2,'Iyy_E',system)
Izz_E = Constant(5.420568769248424e-07,'Izz_E',system)
Ixx_F = Constant(1e-2,'Ixx_F',system)
Iyy_F = Constant(1e-2,'Iyy_F',system)
Izz_F = Constant(9.437043491466668e-08,'Izz_F',system)
Ixx_G = Constant(1e-2,'Ixx_G',system)
Iyy_G = Constant(1e-2,'Iyy_G',system)
Izz_G = Constant(5.420568769248424e-07,'Izz_G',system)
Ixx_H = Constant(1e-2,'Ixx_H',system)
Iyy_H = Constant(1e-2,'Iyy_H',system)
Izz_H = Constant(9.437043491466668e-08,'Izz_H',system)
Ixx_I = Constant(1e-2,'Ixx_I',system)
Iyy_I = Constant(1e-2,'Iyy_I',system)
Izz_I = Constant(5.420568769248424e-07,'Izz_I',system)
Ixx_J = Constant(1e-2,'Ixx_J',system)
Iyy_J = Constant(1e-2,'Iyy_J',system)
Izz_J = Constant(9.437043491466668e-08,'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 attached to the 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 0x7f3ddfc6d9d0>

#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 0x7f3de0d3cd50>,
 <pynamics.spring.Spring at 0x7f3de0d3cdd0>)

#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 0x7f3de0e58210>

#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-04-13 06:21:45,830 - pynamics.system - INFO - getting dynamic equations

[-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-04-13 06:21:48,711 - pynamics.system - INFO - solving a = f/m and creating function
2022-04-13 06:21:51,185 - pynamics.system - INFO - substituting constrained in Ma-f.
2022-04-13 06:21:52,540 - pynamics.system - INFO - done solving a = f/m and creating function
2022-04-13 06:21:52,543 - 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-04-13 06:21:52,593 - pynamics.integration - INFO - beginning integration
2022-04-13 06:21:52,596 - pynamics.system - INFO - integration at time 0000.00
2022-04-13 06:21:58,918 - pynamics.system - INFO - integration at time 0001.71
2022-04-13 06:22:05,248 - pynamics.system - INFO - integration at time 0006.54
2022-04-13 06:22:11,567 - pynamics.integration - INFO - finished integration

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

<matplotlib.legend.Legend at 0x7f3de0ecaa50>

png

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-04-13 06:22:13,049 - pynamics.output - INFO - calculating outputs
2022-04-13 06:22:13,312 - pynamics.output - INFO - done calculating outputs

png

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-04-13 06:22:14,168 - pynamics.output - INFO - calculating outputs
2022-04-13 06:22:14,340 - pynamics.output - INFO - done calculating outputs

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

png

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 0x7f3ddf28a050>

png

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

##Discussion:

• Discuss the model you have selected for representing the observed behavior of your material/subsystem in your team’s full dynamic model, and the process used for fitting the data you collected.

I have divided the leg in 3 parts (Upper beam, Joint and lower beam of the leg) to consider and understand rotational intertie properties which fits our modeled systems.
I have considered upper beam in combination with the joint and the lower beam of the legs.
By doing this I can calculate system inertia accurately and these values are very useful in mass and damping optimization of the system.
System dynamics code shows the behavior of the system with defined inertia components.
Inertia which I have calculated is useful in optimization of the mass and damping of the system. We are going use nonlinear optimization technique because it is the most relevant optimization technique which we can apply to add damping to the system at given inertia values.

• What could you have done better in your experiment design and setup?

In my experiment I have used paper of 0.1 mm thickness and used vernior to take measurement. I feels that the paper we have used is the most sutable for our tessilation based prototype as it is providing require strenth and flexibility to the system. I have also use dxf file which I have generated as flat surface of legs with folds indication which is also beneficial in diving inertia of leg into 3 different parts which is a optimum way to calculate system inertia accuratelty. so for now as per my experience I have chooesn proper design setup.

• Discuss your rationale for the model you selected. Describe any assumptions or simplificaitons this model makes. Include external references used in selecting or understanding your model.

We selected this model as we found one reference paper in which they have considers four lagged rat model with one joint in each leg. We have considered paper made tessilation folding leg which has tessilation pattarn type joint which gives flexibility as well as stiffness to the leg when it moving. We have considerd a 0.1 mm thickness paper to make prototype simple and with less powering requriment.

• Justify the method you selected (least squares, nonlinear least squares, scipy.optimize.minimize(), Evolutionary algorithm, etc. ) for fitting experimental data to the model, as well as the specific algorithm used.

At this opint in my experiment I have not used any optimization method but in our project we are using this inertia data to optimoze mass and damping properties of the system. For that I will going to use least squares optimizaition method as It is sutitable for our model and we can impliment and ues that method accuretly.

• How well does your data fit the model you selected? Provide a numerical value as well as a qualitative analysis, using your figure to explain.

-In the case of calculating inertia I have collected data on bases of the model which we have already selected suits the best. By implimneting that data to system dynamics it is shown in above code that the graphs and the animation of system dynamics is matching the expected results.

• What are the limits of your model, within which you are confident of a good fit? Do you expect your system to operate outside of those limits?

From results of system dynamics, acceration constrains, mass and damping of the system shows that our model is good fit for the prototying. However We are considering the ware and tare in paper at joints. -As per expremental results I do not expect to operate system outside the limits.

Ref: [ ]Witte H, Biltzinger J, Hackert R, Schilling N, Schmidt M, Reich C, Fischer MS, “Torque patterns of the limbs of small therian mammals during locomotion on flat ground”, J Exp Biol, 2002 May, vol. 205, no. 9, pp. 1339-53