tessellationrobot.github.io
Angle Optimization
Our robot will be built of 4 identical legs. Below is a diagram of how we define the diffrent points, refernce 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 tessalation 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 tessalation 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.
##Coding the Model
#import libraries
!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
import time
Requirement already satisfied: pynamics in c:\anaconda3\lib\site-packages (0.2.0)
Requirement already satisfied: numpy in c:\anaconda3\lib\site-packages (from pynamics) (1.22.3)
Requirement already satisfied: sympy in c:\anaconda3\lib\site-packages (from pynamics) (1.9)
Requirement already satisfied: matplotlib in c:\anaconda3\lib\site-packages (from pynamics) (3.5.1)
Requirement already satisfied: scipy in c:\anaconda3\lib\site-packages (from pynamics) (1.7.3)
Requirement already satisfied: pyparsing>=2.2.1 in c:\anaconda3\lib\site-packages (from matplotlib->pynamics) (3.0.7)
Requirement already satisfied: kiwisolver>=1.0.1 in c:\anaconda3\lib\site-packages (from matplotlib->pynamics) (1.4.0)
Requirement already satisfied: cycler>=0.10 in c:\anaconda3\lib\site-packages (from matplotlib->pynamics) (0.11.0)
Requirement already satisfied: packaging>=20.0 in c:\anaconda3\lib\site-packages (from matplotlib->pynamics) (21.3)
Requirement already satisfied: fonttools>=4.22.0 in c:\anaconda3\lib\site-packages (from matplotlib->pynamics) (4.31.2)
Requirement already satisfied: python-dateutil>=2.7 in c:\anaconda3\lib\site-packages (from matplotlib->pynamics) (2.8.2)
Requirement already satisfied: pillow>=6.2.0 in c:\anaconda3\lib\site-packages (from matplotlib->pynamics) (9.0.1)
Requirement already satisfied: six>=1.5 in c:\anaconda3\lib\site-packages (from python-dateutil>=2.7->matplotlib->pynamics) (1.16.0)
Requirement already satisfied: mpmath>=0.19 in c:\anaconda3\lib\site-packages (from sympy->pynamics) (1.2.1)
WARNING: You are using pip version 21.3.1; however, version 22.0.4 is available.
You should consider upgrading via the 'c:\Anaconda3\python.exe -m pip install --upgrade pip' command.
#create system
system=System()
pynamics.set_system(__name__,system)
#known system physical constants/parameters
#lenghts of all beams in m
#found through physical mesurment
#note lenghts are length to that point from the point that preceds it
lB = Constant(.15,'lB',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 of beams in kg
#calculated using measured paper weight per area
mB = Constant(.1,'mB',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-3,'b',system) #joint damping (kg/sec)
k = Constant(1e-1,'k',system) #joint springiness (n/m)
#end stop parameters
kEndStop = Constant(1e2,'kEndStop',system)
bEndStop = Constant(1e-1,'bEndStop',system)
#joint spring neutral postions
#only 2 are needed as all legs are the same just in different phases
preload1 = Constant(-80*pi/180,'preload1',system)
preload2 = Constant(-5*pi/180,'preload2',system)
#end stop limit angle
limit = Constant(-(-0*pi/180),'limit',system)
#Inertial values that describe the inertia of the rigid bodies about x, y and z axes
#Interias about zz are from parameter id
#the other ineras do not affect this model as it is constraned to stay in a plane
#all values in kg*m^2
#body intertias (do not affect current model due to some modeling decisions)
Ixx_B = Constant(1e-3,'Ixx_B',system)
Iyy_B = Constant(1e-3,'Iyy_B',system)
Izz_B = Constant(1e-3,'Izz_B',system)
#leg beam inertias
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)
#Define state variables for leg beam roations:
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)
#define initial system postion and speed
initialvalues = {}
#inital body postion and speed
initialvalues[Aq]=0*pi/180
initialvalues[Aq_d]=0*pi/180
initialvalues[Ax]=0
initialvalues[Ax_d]=0
initialvalues[Ay]=0.17
initialvalues[Ay_d]=0
#initial leg postions and speeds
#leg 3
initialvalues[qG]=-(90+45)*pi/180
initialvalues[qG_d]=0*pi/180
initialvalues[qH]=-5*pi/180
initialvalues[qH_d]=0*pi/180
#leg 4
initialvalues[qI]=-(90-20)*pi/180
initialvalues[qI_d]=0*pi/180
initialvalues[qJ]=-5*pi/180
initialvalues[qJ_d]=0*pi/180
#collect inital conditons
statevariables = system.get_state_variables()
ini = [initialvalues[item] for item in statevariables]
#Defining reference frames of the system
N = Frame('N',system)#origin
A = Frame('A',system)#body
#leg 3
G = Frame('G',system)
H = Frame('H',system)
#leg 4
I = Frame('I',system)
J = Frame('J',system)
#define orign and vector body
system.set_newtonian(N)
A.rotate_fixed_axis(N,[0,0,1],Aq,system)
#legs attached to the body are rotated relative to the body
G.rotate_fixed_axis(A,[0,0,1],qG,system)#leg 3
I.rotate_fixed_axis(A,[0,0,1],qI,system)#leg 4
#legs end beams are attacheced to leg first beams and are rotated relative to the first beams they are attached to
H.rotate_fixed_axis(G,[0,0,1],qH,system)#leg 3
J.rotate_fixed_axis(I,[0,0,1],qJ,system)#leg 4
#define orign vector
pNO=0*N.x
#vectors to and in body
pNA = Ax*N.x + Ay*N.y #N to point A
pAB = pNA + lB*A.x #A to point B
#leg 3
pAI = pNA + lI*I.x
pIJ = pAI + lJ*J.x
#leg 4
pAG = pNA + lG*G.x
pGH = pAG + lH*H.x
#Centers of mass for all beams
#body
pAcm=pNA+lB/2*A.x
#legs
pIcm = pAI - lI/2*I.x
pJcm = pIJ - lJ/2*J.x
pGcm = pAG - lG/2*G.x
pHcm = pGH - lH/2*H.x
#Find Angular velocities
#body
wNA = N.get_w_to(A)
#leg first beams
wAI = A.get_w_to(I)
wAG = A.get_w_to(G)
#leg second beams
wIJ = I.get_w_to(J)
wGH = G.get_w_to(H)
#define rotaional ineratias of all system beams
#body
IA = Dyadic.build(A,Ixx_B,Iyy_B,Izz_B)
#legs
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)
#use masses and rotational inertias to define beam bodies
#body
BodyA = Body('BodyA',A,pAcm,mB,IA,system)
#legs
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)
# leg joint damping forces
system.addforce(-b*wIJ,wIJ)
system.addforce(-b*wGH,wGH)
<pynamics.force.Force at 0x1433d429e80>
#leg joint Spring forces
system.add_spring_force1(k,(qJ-preload2)*I.z,wIJ)
system.add_spring_force1(k,(qH-preload2)*G.z,wGH)
(<pynamics.force.Force at 0x1433d4e2040>,
<pynamics.spring.Spring at 0x1433d4e2f40>)
#Define end Stops
# The end stops are modeled as stiff springs with high damping constants that
#act at the joint between the leg beams only when the angle between the beams of
#the leg goes exceeds kEndStop.
#End stop springs
system.add_spring_force1(kEndStop,((abs(qJ+limit)+qJ+limit)/(2*(qJ+limit+1e-10)))*(qJ+limit)*I.z,wIJ)
system.add_spring_force1(kEndStop,((abs(qH+limit)+qH+limit)/(2*(qH+limit+1e-10)))*(qH+limit)*G.z,wGH)
#end Stop Damping forces
system.addforce(-bEndStop*((abs(qJ+limit)+qJ+limit)/(2*(qJ+limit+1e-10)))*wIJ,wIJ)
system.addforce(-bEndStop*((abs(qH+limit)+qH+limit)/(2*(qH+limit+1e-10)))*wGH,wGH)
#note
#((abs(qD+limit)+qD+limit)/(2*(qD+limit))) = [0] if qD is below limit and [1] if qD is above limit
#this statment thus acts like a switch that turns the spring and damper on when the angle between the beams exceds the limit
<pynamics.force.Force at 0x1433d4f9ac0>
#constraning AB not to fall
# Due to the ground contact forces making the simulation take an exceding long
#amount of to simulate the body of the robot is held up with constraints and
#only ground frictions are modeled. For more on this see the Ground friction
#forces section.
eq = []
eq.append(.17*N.y - pNA)#keep point A at .17 height
eq.append(.17*N.y - pAB)#keep point B at .17 height
#make eq vector equations scalars
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))
eq_dd_scalar.append(-Aq_dd)
#constrain system acclerations to meet the desired constraints
system.add_constraint(AccelerationConstraint(eq_dd_scalar))
#Ground Friction forces
We initaly tried to model ground contact forces both frictional and vertical
(normal) for the ends of each of the legs. However, this was very computaionaly
expensive. This was not helped by only modeling the contact for the front legs
and using constraints to hold the rear node at a fixed heigth while free to
move horizontally.
Thus, we decided to contrain the model to a fixed height and model only the
frictional forces of the front two legs with the ground. This eliminated the
need for conditional ground normal contact forces greatly reducing simulation
time. This elimination of verical movement is not expected to dramaticaly reduce the utility of our system model as storing energy in the robot’s height is not a major component of our robot’s locamotion strategy.
However, with the elimination of true vertical movement a new way to apply frictional forceswas need. We solved this by adding a damping force which is applied to which ever leg tip is the lowest and thus would be the leg in contact with the ground. This is a resonable model for friction in our system.
#Ground Friction forces
#ground force parameters
b_constraint = Constant(1e1,'b_constraint',system)
g_force = Constant(1e1,'g_force',system)
#find leg tip velocites
vJ = pIJ.time_derivative(N,system)
vH = pGH.time_derivative(N,system)
#detect which leg end is lower
Jlower = (sympy.tanh((+pGH.dot(N.y)-pIJ.dot(N.y))*500)+1)/2
Hlower = (sympy.tanh((-pGH.dot(N.y)+pIJ.dot(N.y))*500)+1)/2
#sympy.tanh((value))*1000)+1)/2 = 0 if value is negitve and 1 if value is positive
#apply damping forces to whichever leg tip is lower (and thus in contact wiht the ground)
system.addforce(-b_constraint*vJ*Jlower,vJ)
system.addforce(-b_constraint*vH*Hlower,vH)
#apply uppward ground force to the lower leg
system.addforce(g_force*N.y*Jlower,vJ)
system.addforce(g_force*N.y*Hlower,vH)
#below is how we could model ground contact for the ends of one the legs if it did not make the simulation take excessive time
#stretch = -pGH.dot(N.y)
#stretch_s = (stretch+abs(stretch))
#on = stretch_s/(2*stretch+1e-10)
#system.add_spring_force1(k_constraint,-stretch_s*N.y,vH)
#system.addforce(-b_constraint*vH*on,vH)
<pynamics.force.Force at 0x1433d50f280>
#Motor controls section
When the servo is commanded to move the servo is accelerated at a constant rate until it reaches its maximum velocity. The servo then travels at constant velocity until it decelerates at a constant rate reaching zero rotational velocity at the desired angle. This constant acceleration is added to the system using an acceleration constraint on the motor angle. The desired acceleration step function is generated using the tanh function twice. One acceleration period is applied at the start bring the motor up to its max speed and another at the end to bring the motor speed back down to zero. This can be seen in the below graphs.
#Motor control constants
#known Constants messured from the servo
wmax = Constant(10.76,'wmax',system) #max speed rad/sec
tmax = Constant(.14,'tmax',system) #max tourque nm
#inertia values for motor acceleration considerations
Ilow = Constant(7.263e-6,'Ilow',system) #kgm^2
Ihigh= Constant(6.047e-4,'Ihigh',system) #kgm^2
#assumed
afactor= Constant(100,'afactor',system) #used to scale down accleration
#derived
amax = tmax/Ilow #max possible accleration
#motor comand signals generatiors
def step(tstart): #aproximates a single sided step function using tanh
stepf = 30 #the larger this value the closer to a true step function but the more expensive the simulation is
return (sympy.tanh((tstart*stepf))+1)/2
def pulse(tstart,tstop,a):#returns a function with magnitude 'a' from tstart to tstop and 0 at all other times
return a*(step(system.t-tstart)-step(system.t-tstop))
def command(angles,tstarts,wmax,amax,afactor,toControl):
#takes in motor commands (move servo by amount angle at time tstart) and returns a sum of functions that commands
#that motor through thouse motions
#inputs:
#angles array of move motor by given angle commands in rad
#tstarts array of when to execute the angles commands
#wmax max motor speed
#amax max possible acceleration
#afactor reduce amax by afactor
#toControl angle to drive
out =0
for i in range(0,len(angles)):
if angles[i]>0:
a = amax/afactor
toff = wmax/a
tend = ((angles[i]-a*toff**2)/wmax+tstarts[i]+1*toff)
out+=pulse(tstarts[i],tstarts[i]+toff,a)+pulse(tend,tend+toff,-a)
if angles[i]<0:
a = amax/afactor
toff = wmax/a
tend = ((-angles[i]-a*toff**2)/wmax+tstarts[i]+1*toff)
out+= pulse(tstarts[i],tstarts[i]+toff,-a)+pulse(tend,tend+toff,a)
return out-toControl
#generate comands
commands = []
#commands.append(command([85*pi/180,-20*pi/180,-65*pi/180],[.5,1.5,3.5],wmax,amax,afactor,qC_dd)) #leg 1
#commands.append(command([-65*pi/180,85*pi/180,-20*pi/180],[1,2,4],wmax,amax,afactor,qE_dd)) #leg 2
commands.append(command([85*pi/180,-20*pi/180,-65*pi/180],[.3,.9,1.5],wmax,amax,afactor,qG_dd)) #leg 3
commands.append(command([-65*pi/180,85*pi/180,-20*pi/180],[.6,1.2,1.8],wmax,amax,afactor,qI_dd)) #leg 4
#this will simulate all six phases of our gait stratgey after which the
#robot's legs will be back to their starting positions
#apply motor commands
system.add_constraint(AccelerationConstraint(commands))
system.addforcegravity(-g*N.y) #add gravity
#Run simulation
f,ma = system.getdynamics()
2022-04-21 08:58:36,418 - pynamics.system - INFO - getting dynamic equations
f
[-b_constraint*lG**2*(Aq_d + qG_d)*(tanh(-500*lG*sin(Aq)*cos(qG) - 500*lG*sin(qG)*cos(Aq) - 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) - 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) + 500*lI*sin(Aq)*cos(qI) + 500*lI*sin(qI)*cos(Aq) + 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) + 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2) - b_constraint*lG*lH*(Aq_d + qG_d)*(tanh(-500*lG*sin(Aq)*cos(qG) - 500*lG*sin(qG)*cos(Aq) - 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) - 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) + 500*lI*sin(Aq)*cos(qI) + 500*lI*sin(qI)*cos(Aq) + 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) + 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*cos(qH) - b_constraint*lG*lH*(tanh(-500*lG*sin(Aq)*cos(qG) - 500*lG*sin(qG)*cos(Aq) - 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) - 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) + 500*lI*sin(Aq)*cos(qI) + 500*lI*sin(qI)*cos(Aq) + 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) + 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*(Aq_d + qG_d + qH_d)*cos(qH) - b_constraint*lH**2*(tanh(-500*lG*sin(Aq)*cos(qG) - 500*lG*sin(qG)*cos(Aq) - 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) - 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) + 500*lI*sin(Aq)*cos(qI) + 500*lI*sin(qI)*cos(Aq) + 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) + 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*(Aq_d + qG_d + qH_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 - g_force*lG*(tanh(-500*lG*sin(Aq)*cos(qG) - 500*lG*sin(qG)*cos(Aq) - 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) - 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) + 500*lI*sin(Aq)*cos(qI) + 500*lI*sin(qI)*cos(Aq) + 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) + 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*sin(Aq)*sin(qG) + g_force*lG*(tanh(-500*lG*sin(Aq)*cos(qG) - 500*lG*sin(qG)*cos(Aq) - 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) - 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) + 500*lI*sin(Aq)*cos(qI) + 500*lI*sin(qI)*cos(Aq) + 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) + 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*cos(Aq)*cos(qG) + g_force*lH*(-sin(qG)*sin(qH) + cos(qG)*cos(qH))*(tanh(-500*lG*sin(Aq)*cos(qG) - 500*lG*sin(qG)*cos(Aq) - 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) - 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) + 500*lI*sin(Aq)*cos(qI) + 500*lI*sin(qI)*cos(Aq) + 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) + 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*cos(Aq) + g_force*lH*(-sin(qG)*cos(qH) - sin(qH)*cos(qG))*(tanh(-500*lG*sin(Aq)*cos(qG) - 500*lG*sin(qG)*cos(Aq) - 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) - 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) + 500*lI*sin(Aq)*cos(qI) + 500*lI*sin(qI)*cos(Aq) + 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) + 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*sin(Aq) + lG*(b_constraint*Ax_d*(tanh(-500*lG*sin(Aq)*cos(qG) - 500*lG*sin(qG)*cos(Aq) - 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) - 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) + 500*lI*sin(Aq)*cos(qI) + 500*lI*sin(qI)*cos(Aq) + 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) + 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*sin(Aq) - b_constraint*Ay_d*(tanh(-500*lG*sin(Aq)*cos(qG) - 500*lG*sin(qG)*cos(Aq) - 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) - 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) + 500*lI*sin(Aq)*cos(qI) + 500*lI*sin(qI)*cos(Aq) + 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) + 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*cos(Aq))*cos(qG) - lG*(-b_constraint*Ax_d*(tanh(-500*lG*sin(Aq)*cos(qG) - 500*lG*sin(qG)*cos(Aq) - 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) - 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) + 500*lI*sin(Aq)*cos(qI) + 500*lI*sin(qI)*cos(Aq) + 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) + 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*cos(Aq) - b_constraint*Ay_d*(tanh(-500*lG*sin(Aq)*cos(qG) - 500*lG*sin(qG)*cos(Aq) - 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) - 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) + 500*lI*sin(Aq)*cos(qI) + 500*lI*sin(qI)*cos(Aq) + 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) + 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*sin(Aq))*sin(qG) + lH*(-sin(qG)*sin(qH) + cos(qG)*cos(qH))*(b_constraint*Ax_d*(tanh(-500*lG*sin(Aq)*cos(qG) - 500*lG*sin(qG)*cos(Aq) - 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) - 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) + 500*lI*sin(Aq)*cos(qI) + 500*lI*sin(qI)*cos(Aq) + 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) + 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*sin(Aq) - b_constraint*Ay_d*(tanh(-500*lG*sin(Aq)*cos(qG) - 500*lG*sin(qG)*cos(Aq) - 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) - 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) + 500*lI*sin(Aq)*cos(qI) + 500*lI*sin(qI)*cos(Aq) + 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) + 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*cos(Aq)) + lH*(-sin(qG)*cos(qH) - sin(qH)*cos(qG))*(-b_constraint*Ax_d*(tanh(-500*lG*sin(Aq)*cos(qG) - 500*lG*sin(qG)*cos(Aq) - 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) - 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) + 500*lI*sin(Aq)*cos(qI) + 500*lI*sin(qI)*cos(Aq) + 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) + 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*cos(Aq) - b_constraint*Ay_d*(tanh(-500*lG*sin(Aq)*cos(qG) - 500*lG*sin(qG)*cos(Aq) - 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) - 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) + 500*lI*sin(Aq)*cos(qI) + 500*lI*sin(qI)*cos(Aq) + 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) + 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*sin(Aq)), -b*qH_d - bEndStop*qH_d*(limit + qH + Abs(limit + qH))/(2*limit + 2*qH + 2.0e-10) - b_constraint*lG*lH*(Aq_d + qG_d)*(tanh(-500*lG*sin(Aq)*cos(qG) - 500*lG*sin(qG)*cos(Aq) - 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) - 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) + 500*lI*sin(Aq)*cos(qI) + 500*lI*sin(qI)*cos(Aq) + 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) + 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*cos(qH) - b_constraint*lH**2*(tanh(-500*lG*sin(Aq)*cos(qG) - 500*lG*sin(qG)*cos(Aq) - 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) - 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) + 500*lI*sin(Aq)*cos(qI) + 500*lI*sin(qI)*cos(Aq) + 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) + 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*(Aq_d + qG_d + qH_d) - 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_force*lH*(-sin(qG)*sin(qH) + cos(qG)*cos(qH))*(tanh(-500*lG*sin(Aq)*cos(qG) - 500*lG*sin(qG)*cos(Aq) - 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) - 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) + 500*lI*sin(Aq)*cos(qI) + 500*lI*sin(qI)*cos(Aq) + 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) + 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*cos(Aq) + g_force*lH*(-sin(qG)*cos(qH) - sin(qH)*cos(qG))*(tanh(-500*lG*sin(Aq)*cos(qG) - 500*lG*sin(qG)*cos(Aq) - 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) - 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) + 500*lI*sin(Aq)*cos(qI) + 500*lI*sin(qI)*cos(Aq) + 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) + 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*sin(Aq) - k*(-preload2 + qH) - kEndStop*(limit + qH)*(limit + qH + Abs(limit + qH))/(2*limit + 2*qH + 2.0e-10) + lH*(-sin(qG)*sin(qH) + cos(qG)*cos(qH))*(b_constraint*Ax_d*(tanh(-500*lG*sin(Aq)*cos(qG) - 500*lG*sin(qG)*cos(Aq) - 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) - 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) + 500*lI*sin(Aq)*cos(qI) + 500*lI*sin(qI)*cos(Aq) + 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) + 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*sin(Aq) - b_constraint*Ay_d*(tanh(-500*lG*sin(Aq)*cos(qG) - 500*lG*sin(qG)*cos(Aq) - 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) - 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) + 500*lI*sin(Aq)*cos(qI) + 500*lI*sin(qI)*cos(Aq) + 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) + 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*cos(Aq)) + lH*(-sin(qG)*cos(qH) - sin(qH)*cos(qG))*(-b_constraint*Ax_d*(tanh(-500*lG*sin(Aq)*cos(qG) - 500*lG*sin(qG)*cos(Aq) - 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) - 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) + 500*lI*sin(Aq)*cos(qI) + 500*lI*sin(qI)*cos(Aq) + 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) + 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*cos(Aq) - b_constraint*Ay_d*(tanh(-500*lG*sin(Aq)*cos(qG) - 500*lG*sin(qG)*cos(Aq) - 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) - 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) + 500*lI*sin(Aq)*cos(qI) + 500*lI*sin(qI)*cos(Aq) + 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) + 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*sin(Aq)), -b_constraint*lI**2*(Aq_d + qI_d)*(tanh(500*lG*sin(Aq)*cos(qG) + 500*lG*sin(qG)*cos(Aq) + 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) + 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) - 500*lI*sin(Aq)*cos(qI) - 500*lI*sin(qI)*cos(Aq) - 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) - 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2) - b_constraint*lI*lJ*(Aq_d + qI_d)*(tanh(500*lG*sin(Aq)*cos(qG) + 500*lG*sin(qG)*cos(Aq) + 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) + 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) - 500*lI*sin(Aq)*cos(qI) - 500*lI*sin(qI)*cos(Aq) - 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) - 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*cos(qJ) - b_constraint*lI*lJ*(tanh(500*lG*sin(Aq)*cos(qG) + 500*lG*sin(qG)*cos(Aq) + 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) + 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) - 500*lI*sin(Aq)*cos(qI) - 500*lI*sin(qI)*cos(Aq) - 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) - 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*(Aq_d + qI_d + qJ_d)*cos(qJ) - b_constraint*lJ**2*(tanh(500*lG*sin(Aq)*cos(qG) + 500*lG*sin(qG)*cos(Aq) + 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) + 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) - 500*lI*sin(Aq)*cos(qI) - 500*lI*sin(qI)*cos(Aq) - 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) - 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*(Aq_d + qI_d + qJ_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 - g_force*lI*(tanh(500*lG*sin(Aq)*cos(qG) + 500*lG*sin(qG)*cos(Aq) + 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) + 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) - 500*lI*sin(Aq)*cos(qI) - 500*lI*sin(qI)*cos(Aq) - 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) - 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*sin(Aq)*sin(qI) + g_force*lI*(tanh(500*lG*sin(Aq)*cos(qG) + 500*lG*sin(qG)*cos(Aq) + 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) + 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) - 500*lI*sin(Aq)*cos(qI) - 500*lI*sin(qI)*cos(Aq) - 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) - 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*cos(Aq)*cos(qI) + g_force*lJ*(-sin(qI)*sin(qJ) + cos(qI)*cos(qJ))*(tanh(500*lG*sin(Aq)*cos(qG) + 500*lG*sin(qG)*cos(Aq) + 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) + 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) - 500*lI*sin(Aq)*cos(qI) - 500*lI*sin(qI)*cos(Aq) - 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) - 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*cos(Aq) + g_force*lJ*(-sin(qI)*cos(qJ) - sin(qJ)*cos(qI))*(tanh(500*lG*sin(Aq)*cos(qG) + 500*lG*sin(qG)*cos(Aq) + 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) + 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) - 500*lI*sin(Aq)*cos(qI) - 500*lI*sin(qI)*cos(Aq) - 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) - 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*sin(Aq) + lI*(b_constraint*Ax_d*(tanh(500*lG*sin(Aq)*cos(qG) + 500*lG*sin(qG)*cos(Aq) + 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) + 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) - 500*lI*sin(Aq)*cos(qI) - 500*lI*sin(qI)*cos(Aq) - 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) - 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*sin(Aq) - b_constraint*Ay_d*(tanh(500*lG*sin(Aq)*cos(qG) + 500*lG*sin(qG)*cos(Aq) + 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) + 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) - 500*lI*sin(Aq)*cos(qI) - 500*lI*sin(qI)*cos(Aq) - 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) - 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*cos(Aq))*cos(qI) - lI*(-b_constraint*Ax_d*(tanh(500*lG*sin(Aq)*cos(qG) + 500*lG*sin(qG)*cos(Aq) + 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) + 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) - 500*lI*sin(Aq)*cos(qI) - 500*lI*sin(qI)*cos(Aq) - 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) - 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*cos(Aq) - b_constraint*Ay_d*(tanh(500*lG*sin(Aq)*cos(qG) + 500*lG*sin(qG)*cos(Aq) + 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) + 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) - 500*lI*sin(Aq)*cos(qI) - 500*lI*sin(qI)*cos(Aq) - 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) - 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*sin(Aq))*sin(qI) + lJ*(-sin(qI)*sin(qJ) + cos(qI)*cos(qJ))*(b_constraint*Ax_d*(tanh(500*lG*sin(Aq)*cos(qG) + 500*lG*sin(qG)*cos(Aq) + 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) + 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) - 500*lI*sin(Aq)*cos(qI) - 500*lI*sin(qI)*cos(Aq) - 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) - 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*sin(Aq) - b_constraint*Ay_d*(tanh(500*lG*sin(Aq)*cos(qG) + 500*lG*sin(qG)*cos(Aq) + 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) + 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) - 500*lI*sin(Aq)*cos(qI) - 500*lI*sin(qI)*cos(Aq) - 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) - 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*cos(Aq)) + lJ*(-sin(qI)*cos(qJ) - sin(qJ)*cos(qI))*(-b_constraint*Ax_d*(tanh(500*lG*sin(Aq)*cos(qG) + 500*lG*sin(qG)*cos(Aq) + 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) + 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) - 500*lI*sin(Aq)*cos(qI) - 500*lI*sin(qI)*cos(Aq) - 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) - 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*cos(Aq) - b_constraint*Ay_d*(tanh(500*lG*sin(Aq)*cos(qG) + 500*lG*sin(qG)*cos(Aq) + 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) + 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) - 500*lI*sin(Aq)*cos(qI) - 500*lI*sin(qI)*cos(Aq) - 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) - 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*sin(Aq)), -b*qJ_d - bEndStop*qJ_d*(limit + qJ + Abs(limit + qJ))/(2*limit + 2*qJ + 2.0e-10) - b_constraint*lI*lJ*(Aq_d + qI_d)*(tanh(500*lG*sin(Aq)*cos(qG) + 500*lG*sin(qG)*cos(Aq) + 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) + 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) - 500*lI*sin(Aq)*cos(qI) - 500*lI*sin(qI)*cos(Aq) - 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) - 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*cos(qJ) - b_constraint*lJ**2*(tanh(500*lG*sin(Aq)*cos(qG) + 500*lG*sin(qG)*cos(Aq) + 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) + 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) - 500*lI*sin(Aq)*cos(qI) - 500*lI*sin(qI)*cos(Aq) - 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) - 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*(Aq_d + qI_d + qJ_d) - 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 + g_force*lJ*(-sin(qI)*sin(qJ) + cos(qI)*cos(qJ))*(tanh(500*lG*sin(Aq)*cos(qG) + 500*lG*sin(qG)*cos(Aq) + 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) + 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) - 500*lI*sin(Aq)*cos(qI) - 500*lI*sin(qI)*cos(Aq) - 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) - 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*cos(Aq) + g_force*lJ*(-sin(qI)*cos(qJ) - sin(qJ)*cos(qI))*(tanh(500*lG*sin(Aq)*cos(qG) + 500*lG*sin(qG)*cos(Aq) + 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) + 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) - 500*lI*sin(Aq)*cos(qI) - 500*lI*sin(qI)*cos(Aq) - 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) - 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*sin(Aq) - k*(-preload2 + qJ) - kEndStop*(limit + qJ)*(limit + qJ + Abs(limit + qJ))/(2*limit + 2*qJ + 2.0e-10) + lJ*(-sin(qI)*sin(qJ) + cos(qI)*cos(qJ))*(b_constraint*Ax_d*(tanh(500*lG*sin(Aq)*cos(qG) + 500*lG*sin(qG)*cos(Aq) + 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) + 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) - 500*lI*sin(Aq)*cos(qI) - 500*lI*sin(qI)*cos(Aq) - 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) - 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*sin(Aq) - b_constraint*Ay_d*(tanh(500*lG*sin(Aq)*cos(qG) + 500*lG*sin(qG)*cos(Aq) + 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) + 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) - 500*lI*sin(Aq)*cos(qI) - 500*lI*sin(qI)*cos(Aq) - 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) - 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*cos(Aq)) + lJ*(-sin(qI)*cos(qJ) - sin(qJ)*cos(qI))*(-b_constraint*Ax_d*(tanh(500*lG*sin(Aq)*cos(qG) + 500*lG*sin(qG)*cos(Aq) + 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) + 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) - 500*lI*sin(Aq)*cos(qI) - 500*lI*sin(qI)*cos(Aq) - 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) - 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*cos(Aq) - b_constraint*Ay_d*(tanh(500*lG*sin(Aq)*cos(qG) + 500*lG*sin(qG)*cos(Aq) + 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) + 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) - 500*lI*sin(Aq)*cos(qI) - 500*lI*sin(qI)*cos(Aq) - 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) - 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*sin(Aq)), b_constraint*lG*(Aq_d + qG_d)*(tanh(-500*lG*sin(Aq)*cos(qG) - 500*lG*sin(qG)*cos(Aq) - 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) - 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) + 500*lI*sin(Aq)*cos(qI) + 500*lI*sin(qI)*cos(Aq) + 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) + 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*sin(Aq)*cos(qG) + b_constraint*lG*(Aq_d + qG_d)*(tanh(-500*lG*sin(Aq)*cos(qG) - 500*lG*sin(qG)*cos(Aq) - 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) - 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) + 500*lI*sin(Aq)*cos(qI) + 500*lI*sin(qI)*cos(Aq) + 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) + 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*sin(qG)*cos(Aq) + b_constraint*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*(tanh(-500*lG*sin(Aq)*cos(qG) - 500*lG*sin(qG)*cos(Aq) - 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) - 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) + 500*lI*sin(Aq)*cos(qI) + 500*lI*sin(qI)*cos(Aq) + 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) + 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*(Aq_d + qG_d + qH_d)*sin(qH) - b_constraint*lH*(-sin(Aq)*cos(qG) - sin(qG)*cos(Aq))*(tanh(-500*lG*sin(Aq)*cos(qG) - 500*lG*sin(qG)*cos(Aq) - 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) - 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) + 500*lI*sin(Aq)*cos(qI) + 500*lI*sin(qI)*cos(Aq) + 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) + 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*(Aq_d + qG_d + qH_d)*cos(qH) + b_constraint*lI*(Aq_d + qI_d)*(tanh(500*lG*sin(Aq)*cos(qG) + 500*lG*sin(qG)*cos(Aq) + 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) + 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) - 500*lI*sin(Aq)*cos(qI) - 500*lI*sin(qI)*cos(Aq) - 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) - 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*sin(Aq)*cos(qI) + b_constraint*lI*(Aq_d + qI_d)*(tanh(500*lG*sin(Aq)*cos(qG) + 500*lG*sin(qG)*cos(Aq) + 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) + 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) - 500*lI*sin(Aq)*cos(qI) - 500*lI*sin(qI)*cos(Aq) - 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) - 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*sin(qI)*cos(Aq) + b_constraint*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*(tanh(500*lG*sin(Aq)*cos(qG) + 500*lG*sin(qG)*cos(Aq) + 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) + 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) - 500*lI*sin(Aq)*cos(qI) - 500*lI*sin(qI)*cos(Aq) - 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) - 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*(Aq_d + qI_d + qJ_d)*sin(qJ) - b_constraint*lJ*(-sin(Aq)*cos(qI) - sin(qI)*cos(Aq))*(tanh(500*lG*sin(Aq)*cos(qG) + 500*lG*sin(qG)*cos(Aq) + 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) + 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) - 500*lI*sin(Aq)*cos(qI) - 500*lI*sin(qI)*cos(Aq) - 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) - 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*(Aq_d + qI_d + qJ_d)*cos(qJ) - b_constraint*Ax_d*(tanh(-500*lG*sin(Aq)*cos(qG) - 500*lG*sin(qG)*cos(Aq) - 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) - 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) + 500*lI*sin(Aq)*cos(qI) + 500*lI*sin(qI)*cos(Aq) + 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) + 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2) - b_constraint*Ax_d*(tanh(500*lG*sin(Aq)*cos(qG) + 500*lG*sin(qG)*cos(Aq) + 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) + 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) - 500*lI*sin(Aq)*cos(qI) - 500*lI*sin(qI)*cos(Aq) - 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) - 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2), b_constraint*lG*(Aq_d + qG_d)*(tanh(-500*lG*sin(Aq)*cos(qG) - 500*lG*sin(qG)*cos(Aq) - 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) - 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) + 500*lI*sin(Aq)*cos(qI) + 500*lI*sin(qI)*cos(Aq) + 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) + 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*sin(Aq)*sin(qG) - b_constraint*lG*(Aq_d + qG_d)*(tanh(-500*lG*sin(Aq)*cos(qG) - 500*lG*sin(qG)*cos(Aq) - 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) - 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) + 500*lI*sin(Aq)*cos(qI) + 500*lI*sin(qI)*cos(Aq) + 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) + 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*cos(Aq)*cos(qG) - b_constraint*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*(tanh(-500*lG*sin(Aq)*cos(qG) - 500*lG*sin(qG)*cos(Aq) - 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) - 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) + 500*lI*sin(Aq)*cos(qI) + 500*lI*sin(qI)*cos(Aq) + 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) + 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*(Aq_d + qG_d + qH_d)*cos(qH) + b_constraint*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*(tanh(-500*lG*sin(Aq)*cos(qG) - 500*lG*sin(qG)*cos(Aq) - 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) - 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) + 500*lI*sin(Aq)*cos(qI) + 500*lI*sin(qI)*cos(Aq) + 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) + 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*(Aq_d + qG_d + qH_d)*sin(qH) + b_constraint*lI*(Aq_d + qI_d)*(tanh(500*lG*sin(Aq)*cos(qG) + 500*lG*sin(qG)*cos(Aq) + 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) + 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) - 500*lI*sin(Aq)*cos(qI) - 500*lI*sin(qI)*cos(Aq) - 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) - 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*sin(Aq)*sin(qI) - b_constraint*lI*(Aq_d + qI_d)*(tanh(500*lG*sin(Aq)*cos(qG) + 500*lG*sin(qG)*cos(Aq) + 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) + 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) - 500*lI*sin(Aq)*cos(qI) - 500*lI*sin(qI)*cos(Aq) - 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) - 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*cos(Aq)*cos(qI) - b_constraint*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*(tanh(500*lG*sin(Aq)*cos(qG) + 500*lG*sin(qG)*cos(Aq) + 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) + 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) - 500*lI*sin(Aq)*cos(qI) - 500*lI*sin(qI)*cos(Aq) - 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) - 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*(Aq_d + qI_d + qJ_d)*cos(qJ) + b_constraint*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*(tanh(500*lG*sin(Aq)*cos(qG) + 500*lG*sin(qG)*cos(Aq) + 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) + 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) - 500*lI*sin(Aq)*cos(qI) - 500*lI*sin(qI)*cos(Aq) - 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) - 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*(Aq_d + qI_d + qJ_d)*sin(qJ) - b_constraint*Ay_d*(tanh(-500*lG*sin(Aq)*cos(qG) - 500*lG*sin(qG)*cos(Aq) - 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) - 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) + 500*lI*sin(Aq)*cos(qI) + 500*lI*sin(qI)*cos(Aq) + 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) + 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2) - b_constraint*Ay_d*(tanh(500*lG*sin(Aq)*cos(qG) + 500*lG*sin(qG)*cos(Aq) + 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) + 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) - 500*lI*sin(Aq)*cos(qI) - 500*lI*sin(qI)*cos(Aq) - 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) - 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2) - g*mB - g*mG - g*mH - g*mI - g*mJ + g_force*(tanh(-500*lG*sin(Aq)*cos(qG) - 500*lG*sin(qG)*cos(Aq) - 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) - 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) + 500*lI*sin(Aq)*cos(qI) + 500*lI*sin(qI)*cos(Aq) + 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) + 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2) + g_force*(tanh(500*lG*sin(Aq)*cos(qG) + 500*lG*sin(qG)*cos(Aq) + 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) + 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) - 500*lI*sin(Aq)*cos(qI) - 500*lI*sin(qI)*cos(Aq) - 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) - 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2), -b_constraint*lG**2*(Aq_d + qG_d)*(tanh(-500*lG*sin(Aq)*cos(qG) - 500*lG*sin(qG)*cos(Aq) - 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) - 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) + 500*lI*sin(Aq)*cos(qI) + 500*lI*sin(qI)*cos(Aq) + 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) + 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2) - b_constraint*lG*lH*(Aq_d + qG_d)*(tanh(-500*lG*sin(Aq)*cos(qG) - 500*lG*sin(qG)*cos(Aq) - 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) - 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) + 500*lI*sin(Aq)*cos(qI) + 500*lI*sin(qI)*cos(Aq) + 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) + 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*cos(qH) - b_constraint*lG*lH*(tanh(-500*lG*sin(Aq)*cos(qG) - 500*lG*sin(qG)*cos(Aq) - 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) - 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) + 500*lI*sin(Aq)*cos(qI) + 500*lI*sin(qI)*cos(Aq) + 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) + 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*(Aq_d + qG_d + qH_d)*cos(qH) - b_constraint*lH**2*(tanh(-500*lG*sin(Aq)*cos(qG) - 500*lG*sin(qG)*cos(Aq) - 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) - 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) + 500*lI*sin(Aq)*cos(qI) + 500*lI*sin(qI)*cos(Aq) + 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) + 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*(Aq_d + qG_d + qH_d) - b_constraint*lI**2*(Aq_d + qI_d)*(tanh(500*lG*sin(Aq)*cos(qG) + 500*lG*sin(qG)*cos(Aq) + 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) + 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) - 500*lI*sin(Aq)*cos(qI) - 500*lI*sin(qI)*cos(Aq) - 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) - 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2) - b_constraint*lI*lJ*(Aq_d + qI_d)*(tanh(500*lG*sin(Aq)*cos(qG) + 500*lG*sin(qG)*cos(Aq) + 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) + 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) - 500*lI*sin(Aq)*cos(qI) - 500*lI*sin(qI)*cos(Aq) - 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) - 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*cos(qJ) - b_constraint*lI*lJ*(tanh(500*lG*sin(Aq)*cos(qG) + 500*lG*sin(qG)*cos(Aq) + 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) + 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) - 500*lI*sin(Aq)*cos(qI) - 500*lI*sin(qI)*cos(Aq) - 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) - 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*(Aq_d + qI_d + qJ_d)*cos(qJ) - b_constraint*lJ**2*(tanh(500*lG*sin(Aq)*cos(qG) + 500*lG*sin(qG)*cos(Aq) + 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) + 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) - 500*lI*sin(Aq)*cos(qI) - 500*lI*sin(qI)*cos(Aq) - 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) - 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*(Aq_d + qI_d + qJ_d) - g*lB*mB*cos(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 - g_force*lG*(tanh(-500*lG*sin(Aq)*cos(qG) - 500*lG*sin(qG)*cos(Aq) - 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) - 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) + 500*lI*sin(Aq)*cos(qI) + 500*lI*sin(qI)*cos(Aq) + 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) + 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*sin(Aq)*sin(qG) + g_force*lG*(tanh(-500*lG*sin(Aq)*cos(qG) - 500*lG*sin(qG)*cos(Aq) - 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) - 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) + 500*lI*sin(Aq)*cos(qI) + 500*lI*sin(qI)*cos(Aq) + 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) + 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*cos(Aq)*cos(qG) + g_force*lH*(-sin(qG)*sin(qH) + cos(qG)*cos(qH))*(tanh(-500*lG*sin(Aq)*cos(qG) - 500*lG*sin(qG)*cos(Aq) - 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) - 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) + 500*lI*sin(Aq)*cos(qI) + 500*lI*sin(qI)*cos(Aq) + 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) + 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*cos(Aq) + g_force*lH*(-sin(qG)*cos(qH) - sin(qH)*cos(qG))*(tanh(-500*lG*sin(Aq)*cos(qG) - 500*lG*sin(qG)*cos(Aq) - 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) - 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) + 500*lI*sin(Aq)*cos(qI) + 500*lI*sin(qI)*cos(Aq) + 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) + 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*sin(Aq) - g_force*lI*(tanh(500*lG*sin(Aq)*cos(qG) + 500*lG*sin(qG)*cos(Aq) + 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) + 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) - 500*lI*sin(Aq)*cos(qI) - 500*lI*sin(qI)*cos(Aq) - 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) - 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*sin(Aq)*sin(qI) + g_force*lI*(tanh(500*lG*sin(Aq)*cos(qG) + 500*lG*sin(qG)*cos(Aq) + 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) + 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) - 500*lI*sin(Aq)*cos(qI) - 500*lI*sin(qI)*cos(Aq) - 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) - 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*cos(Aq)*cos(qI) + g_force*lJ*(-sin(qI)*sin(qJ) + cos(qI)*cos(qJ))*(tanh(500*lG*sin(Aq)*cos(qG) + 500*lG*sin(qG)*cos(Aq) + 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) + 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) - 500*lI*sin(Aq)*cos(qI) - 500*lI*sin(qI)*cos(Aq) - 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) - 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*cos(Aq) + g_force*lJ*(-sin(qI)*cos(qJ) - sin(qJ)*cos(qI))*(tanh(500*lG*sin(Aq)*cos(qG) + 500*lG*sin(qG)*cos(Aq) + 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) + 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) - 500*lI*sin(Aq)*cos(qI) - 500*lI*sin(qI)*cos(Aq) - 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) - 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*sin(Aq) + lG*(b_constraint*Ax_d*(tanh(-500*lG*sin(Aq)*cos(qG) - 500*lG*sin(qG)*cos(Aq) - 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) - 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) + 500*lI*sin(Aq)*cos(qI) + 500*lI*sin(qI)*cos(Aq) + 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) + 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*sin(Aq) - b_constraint*Ay_d*(tanh(-500*lG*sin(Aq)*cos(qG) - 500*lG*sin(qG)*cos(Aq) - 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) - 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) + 500*lI*sin(Aq)*cos(qI) + 500*lI*sin(qI)*cos(Aq) + 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) + 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*cos(Aq))*cos(qG) - lG*(-b_constraint*Ax_d*(tanh(-500*lG*sin(Aq)*cos(qG) - 500*lG*sin(qG)*cos(Aq) - 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) - 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) + 500*lI*sin(Aq)*cos(qI) + 500*lI*sin(qI)*cos(Aq) + 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) + 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*cos(Aq) - b_constraint*Ay_d*(tanh(-500*lG*sin(Aq)*cos(qG) - 500*lG*sin(qG)*cos(Aq) - 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) - 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) + 500*lI*sin(Aq)*cos(qI) + 500*lI*sin(qI)*cos(Aq) + 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) + 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*sin(Aq))*sin(qG) + lH*(-sin(qG)*sin(qH) + cos(qG)*cos(qH))*(b_constraint*Ax_d*(tanh(-500*lG*sin(Aq)*cos(qG) - 500*lG*sin(qG)*cos(Aq) - 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) - 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) + 500*lI*sin(Aq)*cos(qI) + 500*lI*sin(qI)*cos(Aq) + 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) + 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*sin(Aq) - b_constraint*Ay_d*(tanh(-500*lG*sin(Aq)*cos(qG) - 500*lG*sin(qG)*cos(Aq) - 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) - 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) + 500*lI*sin(Aq)*cos(qI) + 500*lI*sin(qI)*cos(Aq) + 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) + 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*cos(Aq)) + lH*(-sin(qG)*cos(qH) - sin(qH)*cos(qG))*(-b_constraint*Ax_d*(tanh(-500*lG*sin(Aq)*cos(qG) - 500*lG*sin(qG)*cos(Aq) - 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) - 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) + 500*lI*sin(Aq)*cos(qI) + 500*lI*sin(qI)*cos(Aq) + 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) + 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*cos(Aq) - b_constraint*Ay_d*(tanh(-500*lG*sin(Aq)*cos(qG) - 500*lG*sin(qG)*cos(Aq) - 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) - 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) + 500*lI*sin(Aq)*cos(qI) + 500*lI*sin(qI)*cos(Aq) + 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) + 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*sin(Aq)) + lI*(b_constraint*Ax_d*(tanh(500*lG*sin(Aq)*cos(qG) + 500*lG*sin(qG)*cos(Aq) + 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) + 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) - 500*lI*sin(Aq)*cos(qI) - 500*lI*sin(qI)*cos(Aq) - 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) - 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*sin(Aq) - b_constraint*Ay_d*(tanh(500*lG*sin(Aq)*cos(qG) + 500*lG*sin(qG)*cos(Aq) + 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) + 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) - 500*lI*sin(Aq)*cos(qI) - 500*lI*sin(qI)*cos(Aq) - 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) - 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*cos(Aq))*cos(qI) - lI*(-b_constraint*Ax_d*(tanh(500*lG*sin(Aq)*cos(qG) + 500*lG*sin(qG)*cos(Aq) + 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) + 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) - 500*lI*sin(Aq)*cos(qI) - 500*lI*sin(qI)*cos(Aq) - 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) - 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*cos(Aq) - b_constraint*Ay_d*(tanh(500*lG*sin(Aq)*cos(qG) + 500*lG*sin(qG)*cos(Aq) + 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) + 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) - 500*lI*sin(Aq)*cos(qI) - 500*lI*sin(qI)*cos(Aq) - 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) - 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*sin(Aq))*sin(qI) + lJ*(-sin(qI)*sin(qJ) + cos(qI)*cos(qJ))*(b_constraint*Ax_d*(tanh(500*lG*sin(Aq)*cos(qG) + 500*lG*sin(qG)*cos(Aq) + 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) + 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) - 500*lI*sin(Aq)*cos(qI) - 500*lI*sin(qI)*cos(Aq) - 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) - 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*sin(Aq) - b_constraint*Ay_d*(tanh(500*lG*sin(Aq)*cos(qG) + 500*lG*sin(qG)*cos(Aq) + 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) + 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) - 500*lI*sin(Aq)*cos(qI) - 500*lI*sin(qI)*cos(Aq) - 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) - 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*cos(Aq)) + lJ*(-sin(qI)*cos(qJ) - sin(qJ)*cos(qI))*(-b_constraint*Ax_d*(tanh(500*lG*sin(Aq)*cos(qG) + 500*lG*sin(qG)*cos(Aq) + 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) + 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) - 500*lI*sin(Aq)*cos(qI) - 500*lI*sin(qI)*cos(Aq) - 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) - 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*cos(Aq) - b_constraint*Ay_d*(tanh(500*lG*sin(Aq)*cos(qG) + 500*lG*sin(qG)*cos(Aq) + 500*lH*(-sin(Aq)*sin(qG) + cos(Aq)*cos(qG))*sin(qH) + 500*lH*(sin(Aq)*cos(qG) + sin(qG)*cos(Aq))*cos(qH) - 500*lI*sin(Aq)*cos(qI) - 500*lI*sin(qI)*cos(Aq) - 500*lJ*(-sin(Aq)*sin(qI) + cos(Aq)*cos(qI))*sin(qJ) - 500*lJ*(sin(Aq)*cos(qI) + sin(qI)*cos(Aq))*cos(qJ))/2 + 1/2)*sin(Aq))]
ma
[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 + mB*Ax_dd + mG*Ax_dd + mH*Ax_dd + mI*Ax_dd + mJ*Ax_dd + (-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(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)) - (-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 + mB*Ay_dd + mG*Ay_dd + mH*Ay_dd + mI*Ay_dd + mJ*Ay_dd + (-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(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)) + (-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_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*mB*Ax_dd*sin(Aq)/2 + lB*mB*Ay_dd*cos(Aq)/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-21 08:58:53,152 - pynamics.system - INFO - solving a = f/m and creating function
2022-04-21 08:58:57,249 - pynamics.system - INFO - substituting constrained in Ma-f.
2022-04-21 08:58:59,769 - pynamics.system - INFO - done solving a = f/m and creating function
2022-04-21 08:58:59,771 - pynamics.system - INFO - calculating function for lambdas
#simulation parameters
tol = 1e-5
tinitial = 0
tfinal = 1.1
fps = 120
tstep = 1/fps
t = numpy.r_[tinitial:tfinal:tstep]
start = time.time()
states=pynamics.integration.integrate(func1,ini,t,rtol=tol,atol=tol, args=({'constants':system.constant_values},))
print("seconds to run simulation:")
print(time.time()-start)
2022-04-21 08:59:00,768 - pynamics.integration - INFO - beginning integration
2022-04-21 08:59:00,770 - pynamics.system - INFO - integration at time 0000.00
2022-04-21 08:59:12,384 - pynamics.system - INFO - integration at time 0000.04
2022-04-21 08:59:24,489 - pynamics.system - INFO - integration at time 0000.50
2022-04-21 08:59:35,817 - pynamics.system - INFO - integration at time 0000.67
2022-04-21 08:59:49,114 - pynamics.system - INFO - integration at time 0000.76
2022-04-21 08:59:59,514 - pynamics.system - INFO - integration at time 0000.93
2022-04-21 09:00:01,812 - pynamics.integration - INFO - finished integration
seconds to run simulation:
62.00254845619202
plt.figure()
artists = plt.plot(t,states[:,:4])
plt.legend(artists,['qC','qD','qE','qF'])
plt.xlabel("time in seconds")
plt.ylabel("angle in rad")
Text(0, 0.5, 'angle in rad')
This graph shows the angles of the joints that make up the first leg. qC is the angle driven by the servo model. qD is the joint in the middle of the leg and this graph shows how the end stop limits the ablity of qD to reach postive values even when the motor applies foces that would cause it to do so.
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()
plt.xlabel("time in seconds")
plt.ylabel("Energy in joules")
2022-04-21 09:00:02,459 - pynamics.output - INFO - calculating outputs
2022-04-21 09:00:02,506 - pynamics.output - INFO - done calculating outputs
Text(0, 0.5, 'Energy in joules')
As expected the system’s kinetic energy decays after the servos stop adding energy to the system.
finaltime = tfinal*fps
finaltime
states[int(finaltime)-1,4]
0.1683555628765135
points = [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-21 09:00:03,044 - pynamics.output - INFO - calculating outputs
2022-04-21 09:00:03,084 - pynamics.output - INFO - done calculating outputs
<AxesSubplot:>
points_output.animate(fps = fps,movie_name = 'fullspeed.mp4',lw=2,marker='o',color=(1,0,0,1),linestyle='-')
points_output.animate(fps = fps/4,movie_name = 'quarterspeednorm.mp4',lw=2,marker='o',color=(1,0,0,1),linestyle='-')
<AxesSubplot:>
from matplotlib import animation, rc
from IPython.display import HTML
HTML(points_output.anim.to_html5_video())
This type of motion is reasonable and close to what we expected. Note how the directional stiffness of the middle leg joints allows for forward motion without the need for a motor to lift the leg tip up before bringing it forward.