LUT Mechanical Engineering
Erik Makkonen
SIMULATED MUSCLE DRIVEN LEG
22.11.2021
Examiners: Professor Aki Mikkola
Academy Research Fellow Marko Matikainen Supervisors: Academy Research Fellow Marko Matikainen
Associate Professor Adam Kłodowski
LUT University
LUT School of Energy Systems LUT Mechanical Engineering Erik Makkonen
Simulated Muscle Driven Leg
Master’s Thesis 2021
47 pages, 21 figures, 6 tables and 3 appendices Examiners: Professor Aki Mikkola
Academy Research Fellow Marko Matikainen
Keywords: multibody systems, multibody dynamics, biomechanics, ankle joint, lower limb, pid controller, hill type muscle model, simulation
In this work multibody system of human lower limb is made. System consist of three bodies that are thigh, shin and foot. Objective is to have wanted movement in the ankle joint and also to see how well PID controller behaves. Hill type muscle model and its control is needed for that. Two hill type muscles are made to move ankle joint and the muscles are soleus for plantar flexion and tibialis anterior for dorsiflexion movement. Soleus part of muscle group that includes Achilles tendon which is important point of the thesis.
Equations for multibody dynamics are done to equation of motion of the system, which is being solved with MATLAB program. System is integrated with ode15s solver which is variable-step, variable-order solver using orders 1 to 5 of numerical differentiation formulas.
For muscles ready to use code is applied to system which generates the movement. PID controller is implemented to control wanted movement for three different scenarios, first is to keep foot stable at the starting position, then move foot up and last is to move foot down.
In results can be seen oscillation when movements are larger. Foot can be kept still at the starting position after little drop because muscles need to be activated and when having small movements it works well but having larger wanted movements muscle activation goes rapidly up and down. Difficulties are to make PID controller that works well in every scenario and finding correct parameters turned out to be difficult. Having larger ankle angles passive muscles starts producing force when it is lengthening which makes controlling active muscle harder.
LUT-Yliopisto
LUT School of Energy Systems LUT Kone
Erik Makkonen
Simuloitu Lihasohjattu Jalka
Diplomity¨o 2021
47 sivua, 21 kuvaa, 6 taulukkoa ja 3 liitett¨a Tarkastajat: Professori Aki Mikkola
Akatemiatutkija Marko Matikainen
Hakusanat: monikappalej¨arjestelm¨at, monikappaledynamiikka, biomekaniikka, nilkkanivel, alaraaja, pid-ohjain, hill-tyypin lihasmalli, simulaatio
T¨ass¨a ty¨oss¨a tehd¨a¨an monikappalej¨arjestelm¨a ihmisen alaraajasta. J¨arjestelm¨ass¨a on kolme kappaletta, reisi, s¨a¨ari ja jalkater¨a. Tavoitteena on saada haluttua liikett¨a nilkkaniveless¨a ja n¨ahd¨a PID-ohjaimen toimivuus, jota varten k¨aytet¨a¨an Hill-tyypin lihasmallia ja sen s¨a¨at¨o¨a.
J¨arjestelm¨a¨an tehd¨a¨an kaksi lihasta ja n¨am¨a lihakset ovat soleus plantaarifleksioon ja tibialis anterior dorsaaliflexioon. Soleus on osa kokonaisuutta, johon akillesj¨anne kuuluu ja on hyvin t¨arke¨a t¨at¨a ty¨ot¨a tehdess¨a.
Liikeyht¨al¨ot muodostetaan hy¨odynt¨aen monikappaledynamiikkaan pohjautuvaa l¨ahestymistapaa ja ne liikeyht¨al¨ot ohjelmoidaan MATLAB:lla ja ratkaistaan k¨aytt¨aen MATLAB ode15s-aikaintegraattoria. Ode15s on moniaskelmenetelm¨a 1-5 asteen numeerisen erottelun kaavoille. Lihaksille valmista koodia on sovellettu j¨arjestelm¨a¨an liikett¨a luomaan. PID-ohjain on toteutettu ohjaamaan haluttuja liikkeit¨a, joita ovat jalkater¨an paikallaan pit¨aminen sek¨a sen liikuttaminen yl¨os ett¨a alas.
Tuloksista n¨ahd¨a¨an v¨ar¨ahtely¨a, kun liikkeet ovat suurempia. Jalkater¨a pysyy hyvin paikallaan alun pienen putoamisen j¨alkeen, kun lihakset saavat tarvittavan aktivoinnin tason ja pienet liikkeet onnistuvan hyvin, mutta kun menn¨a¨an suurempiin liikkeisiin lihasten aktivointi menee yl¨os ja alas rajusti, mik¨a aiheuttaa heilahtelua. Hankaluuksia aiheuttavat PID-ohjaimen s¨a¨at¨o, mink¨a pit¨aisi toimia kaikissa tilanteissa ja sopivien parametrien l¨oyt¨aminen osoittautui haasteelliseksi. Suurilla nilkkakulmilla passiivinen lihas alkaa tuottaa venyess¨a¨an voimaa vastustamaan liikett¨a, joka hankaloittaa aktiivisen lihaksen hallintaa.
Suuri kiitos ohjaajalleni Markolle k¨arsiv¨allisyydest¨a ja hyv¨ast¨a ohjauksesta. Vaikka aikaa kului suunniteltua enemm¨an, ty¨ost¨a saatiin valmis paketti. Big thanks also to Adam and Leonid for helping me with coding part of the thesis.
Iso kiitos my¨os perheelleni jatkuvasta tuesta ja kannustuksesta, sek¨a yst¨aville jotka jaksoivat uskoa ja antaa positiivista virtaa. Nyt yht¨a kokemusta rikkaampana on aika siirty¨a uusiin haasteisiin.
Erik Makkonen
Lappeenranta 17.11.2021
TABLE OF CONTENTS
ABSTRACT TIIVISTELM ¨A
ACKNOWLEDGEMENTS TABLE OF CONTENTS
SYMBOLS AND ABBREVIATIONS
1 INTRODUCTION . . . 9
1.1 Lower limb anatomy . . . 9
1.1.1 Bones . . . 10
1.1.2 Muscles . . . 12
1.1.3 Joints and movements . . . 14
1.2 Restrictions and simplifications . . . 17
2 METHODS . . . 18
2.1 Multibody Systems . . . 20
2.1.1 Kinematics and Rotations . . . 20
2.1.2 Mass matrices . . . 22
2.1.3 Constraints . . . 23
2.1.4 Force definitions . . . 24
2.1.5 Equation of Motion . . . 25
2.2 Muscle model . . . 25
2.2.1 Contractile element . . . 26
2.2.2 Parallel elastic element . . . 27
2.2.3 Serial damping element . . . 27
2.2.4 Serial elastic element . . . 28
2.2.5 Contraction dynamics . . . 28
2.3 PID controller . . . 29
2.4 Model parameters . . . 30
2.4.1 Bodies . . . 30
2.4.2 Muscle . . . 32
3 RESULTS . . . 35
4 DISCUSSION . . . 41
4.1 Analysis of results . . . 41
4.2 Muscles . . . 42
4.3 PID Controller . . . 43
5 CONCLUSIONS . . . 45
REFERENCES. . . 46 APPENDICES
Appendix I: Code for main program
Appendix II: Code for generating symbolic MBS equations Appendix III: Code for Equations of Motion
SYMBOLS AND ABBREVIATIONS
Latin letters
A Rotation matrix
Arel Normalized Hill parameter Brel Normalized Hill parameter DSE,max Maximum value in damping
DSE Dimensionless factor to scaleDSE,max
Fi Muscle element force Fmax Maximum force for muscle
FPEE Force of PEE whenlCE is longer than optimal I¯θθ Inertial tensor
Ii Inertial tensor Kd Derivative gain Ki Integral gain Kp Proportional gain
l˙CE Contraction velocity of CE l˙M T C Contraction velocity of MTC lCE,opt Optimal CE length
lCE CE length
LP EE,0 Rest length of PEE normalized tolCE,opt
lP EE,0 Rest length of PEE lSEE,0 Rest length of SEE
M Mass matrix
mi Mass of a body
˙
q Body coordinate velocity vector Qc Constraint forces vector
Qe External forces vector Qv Quadratic velocity vector q Body coordinate vector
¨
q Body coordinate acceleration vector
R Global position vector reference coordinate system
r Global position vector to arbitrary point RSE Damping when force equals 0
¯
u Local position vector v Axis of rotation unit vector vP EE Exponent ofFP EE
Greek letters
∆FSEE,0 Force at the transition and force increase in liner part of SEE
∆USEE,l Relative addition stretch in linear part of SEE
∆USEE,nll Relative stretch at non-linear linear transition
∆Wlimb Width of normalized bell curve for force λ Lagrange’s multipliers
θ Euler parameters
ω Angular velocity vector Abbreviations
CE Contractile element EOM Equations of motion
MBS Multibody system
MTC Muscle tendon complex PEE Parallel elastic element SDE Serial damping element SEE Serial elastic element SOL Soleus (muscle)
TA Tibialis anterior (muscle)
1 INTRODUCTION
Biomechanics combines mechanical physics with biological material properties.
Biomechanics makes it possible to understand living system, its functions and structure.
With biomechanical knowledge changes are possible to predict and artificial intervention methods and materials found. (Fung 1993, p. 11; Kreighbaum and Barthels 1996, p. 1–3.) Examples of biomechanical problems are determining the mechanical properties, environment boundaries and solving the boundary value problems analytically or numerically with help of differential equations. When comparing biomechanical materials to most manufactured materials biomechanical can be history dependent and usually they have large deformations and their stress–strain relationships are nonlinear. (Fung 1993, p. 1.)
In this work mechanics of muscles are applied to multibody system (MBS) based computational model of a human leg. Specific movement is ankle joint movement up and down directions which needs at least two muscles. Achilles tendon is the most interesting part for this thesis.
The objective is to develop a multibody based model of a human lower limb with muscles that are controlled with PID controller. With the controlled ankle is wanted to keep still, move upwards and move downwards. Controller controls muscle activation level and uses target point location as input parameter. With this work it will be possible to see what kind of forces are in the muscles and how they change when moving foot up and down. The goal is to simulate ankle joint movement and see how well controller behaves, what kind of activation levels are in the muscles and what forces are in the muscles.
1.1 Lower limb anatomy
To understand what is needed for the developing a simple model leg, lower limb anatomy is needed. In this chapter all bones, muscles and joints are introduced briefly to give a basic concept how studied musculoskeletal system functions and to explain simplifications are made from that.
1.1.1 Bones
Lower limb can be divided into three parts: thigh, shin and foot. In thigh there is femur bone and patella (kneecap). Femur is the main bone and kneecap is for knee movement. They are shown in figure 1 below.
Figure 1. Thigh bones, femur and patella (Betts et al. 2013).
In shin there are also two bones, tibia and fibula. Tibia is larger and has contact with femur and ankle bones, fibula attaches only to ankle bones upper attachment point being in tibia.
Shin bones are in figure 2.
Figure 2. Shin bones, tibia and fibula from front and back (Betts et al. 2013).
There are 26 bones in each foot and most of them are for toes. When looking at ankle the most important are talus for joint contact and calcaneus for calf muscles. Tarsals and metatarsals are needed for other muscles. These bones are in figure 3 below.
Figure 3. Bones in foot (Betts et al. 2013).
1.1.2 Muscles
Muscle contract to produce force or relax allowing tension to be reduced and tendon limited movement to the resting position. Muscle are grouped in antagonistic pairs that are used for flexion and extension of a joint. Most muscles turn into tendons before attaching to the bone. Active muscles are contracting and creating movement while passive muscles are on the other side of the joint resisting movement. (Hervonen 1987.)
Lower limb of human have many muscles which are shown in figure 4. Muscles go on top of each other and some are diagonally attached to bones which makes force calculations very challenging if using many muscles.
Figure 4. Overview of lower limb muscles (How to relief 2017).
In this work whole complex is not needed, but it is important to know when doing simplifications for the model. The used model have muscles tibialis anterior and soleus that are highlighted in figure 5.
Figure 5.Tibialis anterior (left) and soleus (right) muscles highlighted in red (ExRx.net LLC 1999).
Tibialis anterior origins from tibia bone below knee and attachs to medial cuneiform and first metatarsal (see figure 3). Soleus origins from tibia and fibula below knee on the back side of shin and attaches to calcaneus as Achilles tendon. (Hervonen 1987, p. 251; ExRx.net LLC 1999.)
1.1.3 Joints and movements
Articulatio talocruralis, the upper ankle joint moves foot up (dorsiflexion) and down (plantarflexion) as seen in figure 6. The upper ankle joint is the most important joint in this work.
Figure 6. Ankle joint movements (Betts et al. 2013).
Muscles that generate the movement when activated are listed in table 1. Muscles that will be used in simulation work are in bold. Triceps surae is listed as one muscle but in consist of soleus and gastrocnemius that both insert into Achilles tendon.
Table 1. Ankle joint movement and muscles used (Hervonen 1987, p. 239).
Movement Muscles
Plantar flexion Triceps surae Flexor digitorum longus
Flexor hallucis longus Tibialis posterior
Peroneus longus Peroneus brevis Dorsalflexion Tibialis anterior
Extensor hallucis longus Extensor digitorum longus
Ankle has second joint, articulatio talocalneonavicularis, also known as subtalar joint.
Movements inversion and eversion can be seen in figure 7 and muscles used in table 2.
Figure 7. Subtalar joint movements (Betts et al. 2013).
Table 2. Subtalar joint movement and muscles used (Hervonen 1987, p. 239).
Movement Muscles
Eversion Peroneus longus Peroneus brevis Extensor digitorum longus Inversion Triceps surae
Flexor digitorum longus Extensor hallucis longus
Tibialis posterior Tibialis anterior Extensor hallucis longus
When comparing tables 1 and 2 same muscles are present which makes simulating one of the joints incredibly difficult. Subtalar joint will not be used in this work but it is important to know how close these two joints in ankle are.
1.2 Restrictions and simplifications
As tables 1 and 2 show, there are many muscles that are creating movement in ankle area.
In this work focus in only in ankle joint and subtalar joint will be totally rigid. Ankle joint muscles are reduced to only two, tibialis anterior (TA from now on) to get dorsalflexion movement and soleus (SOL) part of triceps surae muscle for plantarflexion movement.
As for bones, three different bodies will be made. Foot will be a single body, tibia and fibula are combined into one and finally femur and kneecap will be one body. Even though femur part is not directly used in this work it is modeled because if adding later second part of triceps surae muscle, gastrocnemius, its upper part is attached to femur. Because bones are very complicated, they are made as points with mass in simulation. This will make them rigid bodies.
2 METHODS
Multibody-based model of a leg consists of three bodies that are attached together with revolute joints. Two muscles added to ankle joint, tibialis anterior to move foot up and soleus to move foot down. PID controller is implemented to model to make foot follow input data. Following sections goes through equations used to make MBS and equations to make muscle forces with Hill type muscle model. Used parameters and values are at the end of the chapter. Whole process in done with MATLAB version R2018b (MATLAB 2018) and time integrator used is ode15s which is variable-step, variable-order solver using orders 1 to 5 of numerical differentiation formulas. There will be three different simulations, first foot is kept still at the starting position, then foot is moved up twice so control point moves up 10 mm each time and final simulation foot is moved down 30 mm at once.
On the next page is figure 8 that shows simulation process flowchart. Simulation consist of three parts, initial values and symbolic system matrices, then is integration process and finally post-processing of wanted figures. During integration new muscle forces are calculated for needed movement which PID controller is responsible for calculating suitable muscle activation level. Initial body velocities are 0 and from solving equations of motion (EOM) body accelerations are calculated which are integrated to velocities and velocities are integrated to new body locations. Main codes can be found as appendices at the end of the thesis where are main program (appendix I), generating symbolic system matrices (appendix II) and also equations of motion (appendix III) of the integration part.
Start
Load constant parameters
Make symbolic system matrices Initial body locations
and muscle lengths Start integration process Compare target foot position
and current foot position PID controller calculates new muscle activity level Calculate produced muscle forces
Solve equations of motion Go to the next timestep Integrate solved acceleration values to
get new body velocities and locations
Last timestep?
End integration Plot post-process figures
End Yes
No
Figure 8. Flow chart of the simulation process.
2.1 Multibody Systems
Following section of multibody dynamics are from lecture notes of Gerstmayr (2017) which uses material of well-established books by Shabana (2005) and Nikravesh (2008). This section goes through equations from single point to equation of motion of the system.
2.1.1 Kinematics and Rotations
Single arbitrary point in body can be described with equation
ri =Ri+Aiu¯i (1) where ri is vector to global position of arbitrary point, Ri is global position of reference coordinate system of the body,Ai is rotation matrix of the body andu¯i is position vector to local position of the point in body.
Ai can be formulated using different methods but in this work Euler parameters are used.
Euler parameters are set of rotational coordinates of the body reference
θi =
θ0
θ1
θ2
θ3
i
=
cosθ
2 v1sinθ
2 v2sinθ 2 v3sinθ 2
i
(2)
wherev1, v2 andv3 are components of unit vectorv that is axis of rotation andθ, the angle of rotation.
Using Euler parameters the transformation matrixAi can be written as
Ai =
2θ20+ 2θ12−1 2θ1θ2−2θ0θ3 2θ0θ2+ 2θ1θ3
2θ0θ3+ 2θ1θ2 2θ02+ 2θ22−1 2θ2θ3−2θ0θ1
2θ1θ3−2θ0θ2 2θ0θ1+ 2θ2θ3 2θ20+ 2θ23−1
i
(3)
Another way to from the rotation matrix is using two matrices that depends linearly on the
Euler parameters as
Ai =EiE¯iT (4) whereEiandE¯iT are
Ei =
−θ1 θ0 −θ3 θ2
−θ2 θ3 θ0 −θ1
−θ3 −θ2 θ1 θ0
i
E¯i =
−θ1 θ0 θ3 −θ2
−θ2 −θ3 θ0 θ1
−θ3 θ2 −θ1 θ0
i (5)
Differentiating equation 1 with respect to time gives point velocities. This equation can be written as
˙
ri =R˙i+A˙iu¯i (6) Another way to writeA˙iu¯iis using angular velocity vector
A˙iu¯i =ωi×u =Ai(¯ωi×u¯i) (7)
Angular velocity vectorωandω¯ is in terms of time derivative of Euler parameters ωi =Giθ˙i
¯
ωi =G¯iθ˙i
(8)
whereGandG¯ are 3 x 4 matrices depending on Euler parameters and are given by Gi = 2Ei
G¯i = 2E¯i
(9)
Using skew symmetric matricesω¯i×u¯i gets form of
¯
ωi ×u¯i = ˜¯ωiu¯i =−u˜¯iω¯i (10)
whereω˜¯iandu˜¯iare defined as
ω˜¯i =
0 −ω¯3i ω¯2i
¯
ω3i 0 −ω¯1i
−ω¯2i ω¯1i 0
˜¯ ui =
0 −u¯i3 u¯i2
¯
ui3 0 −u¯i1
−u¯i2 u¯i1 0
(11)
Point velocity vector can be written now as
˙
ri =R˙i−Aiu˜¯iG¯iθ˙i (12)
2.1.2 Mass matrices
Mi =
miRR miRθ symmetric miθθ
(13)
Mass matrix is 7 x 7 matrix that consist of 4 parts as seen in equation 13. First part of the equation,mRRthat is related to translation of the body reference can be written as
miRR =
mi 0 0
0 mi 0
0 0 mi
(14)
wheremi is total mass of the body.
Second part of mass matrix is matrix miRθ is related to inertia coupling between the translation that can be defined as
miRθ =− Z
V
ρAu˜¯G¯ dV (15)
From miRθ definition can be seen that it has component u˜¯ that means if the origin of the body axes is at the center of massmiRθ becomes a null matrix.
Last part of the mass matrix is
miθθ=G¯iTI¯θθi G¯i (16) whereI¯θθi is inertia tensor andG¯iwas based on Euler parameters.I¯θθi is defined as diagonal matrix
I¯θθi =
I¯X 0 0 0 I¯Y 0 0 0 I¯Z
(17)
whereI¯X,I¯Y andI¯Z are inertias in local coordinate system of the body.
2.1.3 Constraints
Spherical joint constraint to ground can be made with equation
C(q)=Ri +Aiu¯i = 0 (18) and creating spherical joint between two bodies is
C(q)=Ri+Aiu¯i =Rj +Aju¯j (19) To get revolute joint from spherical joint two additional constraints are needed to limit angle of rotation to only one axis. These are made with creating vectors that are in the same direction as local coordinate system that are needed to be constrained. First vector is skew vector and second is multiplied by it. From the resulting vector constraining the axes where no movement can happen revolute joint is left to this one axis that is not constrained.
Constraints are transformed to velocity level with Jacobian matrix taking constraint matrix C with respect of body coordinate vectorq. This following velocity level constraint matrix Cq can be transformed to acceleration level by taking body coordinate velocity vector with Jacobian matrix with respect ofq.
Cqq¨=Qc (20)
Qcis force from constraints that can be though as external forces reducing motion.
Baumgarten stabilisation is applied for constraints to reduce drift-off by applying velocity and position level constraints in addition to velocity level constraints. This means adding a spring-damper system to the constraints.
Cqq¨=Qc−2αC˙ −β2C (21) whereαandβ are free parameters. In this simulation values are forα is 1000 and forβ is
√2/0.001, the same values were used in the simulation that was guiding this work. This is because timestep is 0.001.
2.1.4 Force definitions
External forces comes from vectorial forces and moments affecting the body. External force vectorQe can be divided into force partQR and moment partQθ. QR is 3 x 1 matrix that includes forces affecting body in global coordinate system andQθis 4 x 1 matrix when using Euler parameters that is defined as
Qθ =GTM (22)
whereM is moment vector applied to the body.
Quadratic velocity vectorQv is vector that describes Coriolis and centrifugal effects when body has rotational motion. Qv can be written as
Qiv =
03
−2 ˙¯GiTI¯θθω¯i
(23)
where03 is three-dimensional null vector meaningQv does not affectQRwhen addingQe
andQv together later.
2.1.5 Equation of Motion
Finally masses, constraints and forces can be put into equations of motion. EOM for the constrained MBS is written based on the Lagrange’s multipliers method
M CqT Cq 0
¨ q λ
=
Qe+Qv Qc
(24)
whereλis Lagrange’s multipliers andq¨is body coordinate acceleration vector, which in this case is the vector that will be solved during the simulation.
2.2 Muscle model
Common muscle model is three element Hill-type muscle model (Hill 1938). G¨unther et al.
(2007) introduced damping element making it four element model and Haeufle et al. (2014) provided ready to use Matlab code for the muscle model that is used in this work.
Four element muscle tendon complex (MTC) is shown below in figure 9. These elements are parallel elastic element (PEE), contractile element (CE), serial elastic element (SEE) and serial damping element (SDE).
PEE SEE
CE SDE
lMTC
lCE lSEE
Figure 9. Structure of MTC model (mod. Haeufle et al. (2014)).
2.2.1 Contractile element
The contractile element represents muscle that contracts when activated and force is generated. Force has two relations, force-length relation and force-velocity relation.
Force-length relation shows how CE force depends on the length of muscle fiber and is modeled as (Haeufle et al. 2014)
Fisom(lCE) = exp
−
lCE/lCE,opt−1
∆Wlimb
vCE,limb
(25) where lCE,opt is the optimal fibre length for the muscle that reaches maximum isometric force, ∆Wlimb is parameter for width of normalized bell curve in force-length relation and vCE,limb its exponent. ∆Wlimb and vCE,limb can have different values for descending and ascending branches.
Force-velocity relation is modeled as
FCE( ˙lCE ≤0) =Fmax
qFisom+Arel
1− l˙CE
BrellCE,opt
−Arel
(26)
in concentric contractions, where Fmax is the maximum isometric force,Arel andBrel are normalized Hill parameters from Hill’s work in the 1938.Areldepends on length on CE as
Arel=
Arel,0 ·14(1 + 3q), lCE < lCE,opt
FisomArel,0· 14(1 + 3q), lCE ≥lCE,opt
(27)
andBrelis
Brel=Brel,0· 1
7(3 + 4q) (28)
Arel,0 andBrel,0 are maximum values forArelandBrel.
2.2.2 Parallel elastic element
Force for PEE is a function of lCE. CE and PEE are parallel to each other so their lengths are the same. The rest length of PEE,lP EE,0, is calculated as (Haeufle et al. 2014)
lP EE,0 =LP EE,0lCE,opt (29)
whereLP EE,0 is rest length of PEE normalized to optimal length of CE.
ThislP EE,0value is used in force equation to see if PEE generates force or not, PEE generates force when its length is long enough.
FP EE(lCE) =
0, lCE < lP EE,0
KP EE(lCE −lP EE,0)vP EE, lCE ≥lP EE,0
(30)
KP EE, factor of non-linearity inFP EE being
KP EE =FPEE Fmax
(lCE,opt(∆Wlimb,des+ 1−LP EE,0))vP EE (31) whereFPEEis force of PEE when it is longer than slack length.
2.2.3 Serial damping element
Force for SDE is viscous damper-like force, modeled as (Haeufle et al. 2014)
FSDE(lCE,l˙CE,l˙M T C, q)
=DSDE,max· (1−RSDE)· FCE(lCE,l˙CE, q) +FP EE(lCE) Fmax
+RSDE
!
·( ˙lM T C−l˙CE) (32) whereRSDE is damping when force is 0, and value forRSDE is maximum of 1. DSDE,max
is maximum damping coefficient when force is maximum that is calculated as (M¨orl et al.
2012)
DSDE,max =DSE· FmaxArel,0
lCE,optBrel,0
(33)
DSE being dimensionless factor to scaleDSDE,max.
2.2.4 Serial elastic element
SEE force has non-linear start which transitions into linear at given stretch. Force is described as (Haeufle et al. 2014)
FP EE(lCE) =
0, lSE < lSEE,0
KSEE,nl(lSE−lSEE,0)vSEE, lSE < lSEE,nll
∆FSEE,0+KSEE,l(lSE−lSEE,nll), lSE ≥lSEE,nll
(34)
where parameters are derived from following:
lSEE,nll = (1 + ∆USEE,nll)lSEE,0 (35)
vSEE = ∆USEE,nll
∆USEE,l
(36) KSEE,nl = ∆FSEE,0
(∆USEE,nlllSEE,0)vSEE (37)
KSEE,l = ∆FSEE,0
∆USEE,llSEE,0
(38)
where∆USEE,nll is relative stretch at transition from non-linear to linear and lSEE,0 is rest length of SEE.∆USEE,lrelative additional stretch in the linear part providing a force increase of∆FSEE,0 which is force at the transition and force increase in linear part.
2.2.5 Contraction dynamics
When muscle is activated, CE is contracted and this contracting speed is calculated with contraction dynamics (G¨unther et al. 2007; M¨orl et al. 2012)
0 = C2·l˙CE2 +C1·l˙CE+C0 (39) where coefficientsC2,C1 andC0are:
D0 =lCE,opt·Brel·DSDE,max·
RSE+ (1−RSE)·
qFisom(lCE) + FP EE(lCE) Fmax
(40)
C2 =DSDE,max·
RSE−
Arel− FP EE(lCE) Fmax
·(1−RSE)
(41) C1 =−(C2·l˙M T C+D0+FSEE(lM T C, lCE)−FP EE(lCE) +FmaxArel) (42) C0 =D0l˙M T C+lCE,optBrel(FSEE(lM T C, lCE)−FP EE(lCE)−FmaxqFisom(lCE)) (43) Putting these coefficients into equation 39 contraction velocity can be calculated. Work of Haeufle et al. (2014) also shows how eccentric also know as lengthening velocity behaves when passive muscle’s CE lengthens.
2.3 PID controller
Muscles are controlled with PID controller. Controller uses control point and compares it to target height that is given in. This error between simulated point location and target location is used to give muscle activation values. Every timestep has corresponding value for target height.
Activation level is determined from error of target position and simulated position with equation
q =min[1, Kpe+Kie˙+Kd
Z
e] (44)
whereeis error,Kpis proportional gain for error,Kiis integral gain for past error andKdin derivative gain for future error.
PID controller values were determined by trial and error. These final values are in table 3.
Proportional gain was reduced near target value to help with overshoot problems.
Table 3. PID controller gain values.
Gain Value Gain Value
Kp,SOL 40 Kp,T A 30
Ki,SOL 5 Ki,T A 20
Kd,SOL 2.5 Kd,T A 10
P controller is not enough because muscles need some activation value all the time. I and D components are added to get wanted behaviour. This effect shows at the beginning when foot should maintain still position.
2.4 Model parameters
Model parameters can be divided into body parameters and muscle parameters. Body parameters include masses, lengths and inertia tensors, and muscle parameters muscle attachment points and values for different elements of the muscle model.
Little simplification was made to opensim model so that masses and muscle attachment points are in 2D plane. TA muscle is a little bit in the inner edge of foot in reality to help with subtalar joint movement, depending on the right or left foot this TA’s lower attachment point would move the most but in this simplified model there is one symmetrical leg. For this simulation ankle joint is the main focus and muscle forces are in 2D only.
2.4.1 Bodies
Figure 10 shows how body mass points and joints are in the system. At the beginning of the simulation all local body coordinates are alignment with global coordinate system. Distances (Simbios 2018) between mass points and joints are also shown.
Body 1 - Thigh
170 mm
225.8 mm
Body 2 - Shin
186.7 mm
243.3 mm
Body 3 - Foot Hip joint
Knee joint
Ankle joint Control
point Y
Z
Figure 10. Bodies and joints in the system.
In figure 10 Y direction length from ankle joint to foot mass center is 63.6 mm and Z direction length is 14.8 mm. Control point is 100 mm from the mass center. In table 4 is the rest of the body parameters needed in the model.
Table 4. Body parameters for masses and inertial tensors (Simbios 2018).
Parameter Value Body 1 m1 9.3014 kg
Ix1 0.1412 kgm2 Iy1 0.0351 kgm2 Iz1 0.1339 kgm2 Body 2 m2 3.7075 kg
Ix2 0.0511 kgm2 Iy2 0.0051 kgm2 Iz2 0.0504 kgm2 Body 3 m3 1.5666 kg
Ix3 0.0052 kgm2 Iy3 0.0025 kgm2 Iz3 0.0051 kgm2
Masses of the first and second body had to be multiplied by thousand because there were no muscles around hip and knee joints. When creating force in ankle joint whole leg moved before increasing mass. Another way could have been changing rotational joints into fixed joints but if in the future more muscles are added mass can be changed easily back to normal.
2.4.2 Muscle
Muscles are attached to bodies as points and force vector is made between bodies. How muscles are in the model is shown in figure 11 and muscle starting and ending points in body coordinate system is in table 5 which distances are from Simbios (2018) but simplified to 2D plane.
Body 2 - Shin
Body 3 - Foot SOL muscle force
TA muscle force
Part of TA where length does not change
Y Z
Figure 11. SOL and TA muscle forces in lower leg.
Soleus starts from heel in the foot and ends just above and behind shin mass center. Tibialis anterior is made from three points, where force and its direction comes from foot to lower part of shin and third point is around shin mass center where the muscle ends. This third point is needed to calculate length of muscle and it would give wrong force direction if it went from foot straight to upper part of shin. Used values for lengths from mass centers to attachment points are in table below. In figure 11 grey parts are only for visual effect to see leg and muscle attachments better.
Table 5. Muscle attachment points in body coordinate system.
Muscle Body x y z
SOL Foot 0 -0.1126 0.0038
Shin 0 -0.002 0.0337
TA Foot 0 0.0044 -0.0092
Shin 0 0.033 -0.2083
Shin 0 0.018 0.0247
Two muscles that are in the model have many parameters to generate forces in simulation as equations before show. These parameters for different elements in muscle can be seen in table below.
Table 6. Muscle parameters (Haeufle et al. 2014; Simbios 2018; Garc´ıa-Vallejo et al. 2016).
Parameter TA SOL
CE Fmax 1528 N 3883 N
lCE,opt 0.082 m 0.055 m
∆Wlimb,des 0.35 0.35
∆Wlimb,asc 0.35 0.35
vCE,limb,des 1.5 1.5
vCE,limb,asc 3.0 3.0
Arel,0 0.25 0.25
Brel,0 2.25 2.25
Se 2 2
Fe 1.5 1.5
PEE LP EE,0 0.9 0.9
vP EE 2.5 2.5
FPEE 2.0 2.0
SDE DSE 0.3 0.3
RSE 0.01 0.01
SEE lSEE,0 0.223 m 0.245 m
∆USEE,nll 0.0425 0.0425
∆USEE,l 0.017 0.017
∆FSEE,0 568 N 568 N
As table 6 shows many of the muscle parameters are the same between muscles, but most important ones for the simulation now is the force and lengths. The rest can be changed later if muscle behavior needs to change. For now PID controller is the one that has bigger effect on than rest of the parameters and muscle behavior comes first from tuning that.
3 RESULTS
There were three different simulations done that all lasted for 2.5 seconds. First simulation is with stable input to keep foot at starting position. Input and simulated data can be seen in figure 12 of how the point that is controlled behaves. How controller controls muscle activation is shown in figure 13 and figure 14 shows what forces muscles produce in simulation.
0 0.5 1 1.5 2 2.5
−0.844
−0.844
−0.843
−0.843
−0.842
−0.842
−0.841
−0.841
Time [s]
Controlpointzcoordinate[m]
Target position Control point position
Figure 12. Stable input simulation.
As figure 12 shows wanted input to foot is to keep it still. At the beginning of the simulation foot drops down before muscles are activated to correct level. Seeing the scale of the figure this drop is very small. Figure 13 on the next page shows muscle activation and SOL muscle is fully passive because gravitational force is pulling foot down and TA muscle is needed to keep foot still. At the beginning there are couple short high level actions before correct level is found. Small oscillation can be seen in TA forces on figure 14.
0 0.5 1 1.5 2 2.5 0
0.2 0.4 0.6 0.8 1
Time [s]
Muscleactivationlevel
TA SOL
Figure 13. Stable input muscle activation.
0 0.5 1 1.5 2 2.5
0 5 10 15 20 25 30
Time [s]
Muscleforces[N]
TA SOL
Figure 14. Stable input muscle forces.
The second simulation is upwards movement. Movement is done with two steps of 10 mm each that takes 0.5 seconds. Stable parts are also 0.5 seconds long at the beginning, middle and end.
0 0.5 1 1.5 2 2.5
−0.84
−0.84
−0.83
−0.83
−0.82
−0.82
Time [s]
Controlpointzcoordinate[m]
Target trajectory Control point position
Figure 15. Upwards input simulation.
There is same small drop at the beginning as in stable case and when moving foot up first movement is smooth but second movement creates oscillation. TA muscle activation (figure 16) is around 20 % when moving foot up but it goes close to 0 % after last movement which can be part of oscillation problem.
0 0.5 1 1.5 2 2.5 0
0.2 0.4 0.6 0.8 1
Time [s]
Muscleactivationlevel
TA SOL
Figure 16. Upwards input muscle activation.
0 0.5 1 1.5 2 2.5
0 5 10 15 20 25 30
Time [s]
Muscleforces[N]
TA SOL
Figure 17. Upwards input muscle forces.
The third simulation input is first second stable, 0.5 second step down for 30 mm and last second again stable at new position.
0 0.5 1 1.5 2 2.5
−0.87
−0.86
−0.86
−0.85
−0.85
−0.84
−0.84
Time [s]
Controlpointzcoordinate[m]
Target trajectory Control point position
Figure 18. Downwards input simulation.
Third simulation has the largest oscillations and the end of the simulation. Figure 19 shows how during the movement SOL muscle is activated at around 40 % but after that it goes close to 0 % and then jumps up and down for the rest of the simulation. TA muscle also has some activity after 2 seconds even though it should be passive muscle while SOL is moving the foot down. Oscillation can be seen in muscle forces (figure 20) after 1.5 s forces start behaving not so well compared to the two other simulations.
0 0.5 1 1.5 2 2.5 0
0.2 0.4 0.6 0.8 1
Time [s]
Muscleactivationlevel
TA SOL
Figure 19. Downwards input muscle activation.
0 0.5 1 1.5 2 2.5
0 10 20 30 40 50 60 70 80 90
Time [s]
Muscleforces[N]
TA SOL
Figure 20. Downwards input muscle forces.
4 DISCUSSION
The work in thesis attempts to make working MBS where ankle joint is activated with two muscles. After simulation three different scenarios for ankle joint movement results are analysed and problems that emerged during simulation work are gone through. Areas that worked well are shown and those that did not bring solutions are researched and brought up.
4.1 Analysis of results
Before adding any external forces, MBS was tested as a triple pendulum and kinetic and potential energies were calculated. This overall energy was constant which means equations used to make MBS were correct.
In every simulation at first foot drops a little bit before controller gets right activation values to keep it up. This takes 0.7 seconds. First simulation shows that foot can be kept very still even though there are some oscillation with muscle forces (figure 14).
With second simulation after first step foot becomes stable faster than after second step, where some oscillation can be seen. As figure 15 during time from 1 to 1.5 s control point is closer to target value than at time from 2 to 2.5 s.
With the third simulation step is larger and changes quicker which induces large oscillation before settling for smaller oscillation. Large movements are more difficult for the controller than smaller ones when comparing simulations two and three. TA muscle have also small activation after 2 s even though it should be not needed because movement is downwards where SOL muscle in contracting and TA is relaxed.
In muscle activation figures (13, 16 and 19) activation goes from zero to very high and back to zero in short time. This is problem and partly causes oscillation in the foot. It happens with TA at the beginning of every simulation and with SOL at the downwards movement simulation (figure 19).
Forces were between 20 and 30 N in TA muscle in first two simulations and SOL force only around 5 N. At the third simulation when moving foot more down TA muscles passive force increased to 80 N and active SOL muscle had over 50 N force. Foot is able to move down with smaller force because SOL is heel of the foot and TA near mass center. With better force angle to generate movement SOL is lower value but still moving foot down.
4.2 Muscles
Now the case was the simplest one, two muscles total, one on each side of the joint. As the introduction shows there are many muscles in lower limb and there are two different joints in ankle that uses same muscles. Simplifying ankle joint to only two muscle system will provide enough forces to see movement in foot. Starting with two muscles also provides less things that can go wrong when simulating and adding new muscles later will be easier than trying to get all working at the beginning.
Some dampening elements needed to be reduced in muscle because there were numerical errors when calculating contraction velocityl˙M T C. Sometimesl˙M T Cvalue would be positive when it should be negative and vice versa so it was kept 0.
Using ready made matlab model for Hill type muscle force might have been too much for starting point. If some simpler model was used, results could have been more stable. Simbios (2018) uses Thelen (2003) muscle model that is shown in figure 21.
Figure 21. A Hill-type muscle model (Thelen 2003).
Gastrocnemius muscle would be important to add to see how Achilles tendon forces really add up because both gastrocnemius and soleus are connected to it. Now Achilles tendon only gets part of the potential force. GastrocnemiusFmaxvalue is 1639 N and soleus 3883 N (Garc´ıa-Vallejo et al. 2016) so soleus is more important for Achilles tendon butFmax is not everything.
Muscles shorten faster when loads are lighter (Seow 2013) and in simulation work only load was foot’s mass. This have an effect on how muscle activation is going up and down very fast because enough movement is generated. With higher mass for example whole body weight that would be lifted could mean activation levels are not so rapid.
4.3 PID Controller
Effect on having force element, muscle, that needs activation at all times can be seen at the start of the simulation. Unlike with hydraulic systems where valve can be closed and system stays still, when wanting to keep foot at starting position, error at is 0. This makes controller give muscle minimal activation and foot starts dropping. This takes 0.2 seconds
until controller gives high enough values to lift foot back to starting position and now I and D elements of the controller can keep it stable. This shows how control forces are limited and system response time is a factor in keeping it stable. If some sort of damping and friction element would be added to the model keeping the foot stable with relatively limited muscle could be possible.
When lifting foot up controller can follow ramp input well but for the foot to be stable at new position it cannot be too fast. Making TA length 1 mm smaller in 0.5 seconds causes unbalance and in 1 second it is stable.
One of the biggest difficulties with the controller is to get it work with all scenarios. When tuning PID controller to move foot up well, it did not work so good in other direction. Final values were compromise where all three scenarios worked at some level, even though there is still much left for improvement. One solution to this could be using adaptive controller that changes PID values based on detecting the movement.
During tuning there was also forth scenario, moving foot up and down in single simulation.
This fourth scenario was unstable all the time and it was dropped because suitable PID controller values were not found. It worked in one direction but when going to other end problems became too significant to make it work.
Adding more muscles to the model would bring on following problems with the controller:
How to control many active muscles? In which cases optimization is needed? How would adding subtalar joint affect muscles that are used in both ankle and subtalar joints?
When adding muscles to knee joint and hip joint PID controllers can be added to control each joint. Similar control points can be used to give accurate location input to shin and thigh.
Having multiple muscles controlling same movement will be more difficult.
5 CONCLUSIONS
Multibody system of lower limb was made with two Hill type muscles to generate movement in ankle joint, soleus and tibialis anterior. Subtalar joint was ignored and all forces were in 2D plane. Muscles were controlled with PID controller that had input data for single point in foot and this point’s height location was put into target position.
Results varied from good at keeping foot still to big oscillation when moving down. Moving foot up worked also good but having bigger movement induced small oscillation. Having both movements in same simulation made system unstable and more work is needed.
Biggest problems were that muscle activation goes very fast between 0 and maximum which can be stabilized when keeping foot still or making small movements only, with bigger movements oscillation occurs. Before adding more muscles to system oscillation problems are needed to dealt with.
REFERENCES
Betts, J. G., Young, K. A., Wise, J. A., Johnson, E., Poe, B., Kruse, D. H., Korol, O., Johnson, J. E., Womble, M. and Desaix, P. 2013. Anatomy and Physiology. Openstax Texas.
Available: https://openstax.org/books/anatomy-and-physiology/pages/1-introduction ExRx.net LLC 1999. Muscle directory. Available: https://exrx.net/Lists/Muscle
Fung, Y. C. 1993. Biomechanics: mechanical properties of living tissues. 2nd edn, New York: Springer. 568 p.
Garc´ıa-Vallejo, D., Font-Llagunes, J. M. and Schiehlen, W. 2016. Dynamical analysis and design of active orthoses for spinal cord injured subjects by aesthetic and energetic optimization. Nonlinear dynamics 84: 2, pp. 559–581.
Gerstmayr, J. 2017. Introduction to multibody system dynamics. Lecture notes.
G¨unther, M., Schmitt, S. and Wank, V. 2007. High-frequency oscillations as a consequence of neglected serial damping in hill-type muscle models. Biological Cybernetics 97: 1, pp. 63–
79.
Haeufle, D., G¨unther, M., Bayer, A. and Schmitt, S. 2014. Hill-type muscle model with serial damping and eccentric force–velocity relation. Journal of Biomechanics 47: 6, pp. 1531–
1536.
Hervonen, A. 1987. Tuki- ja liikuntaelimist¨on anatomia. 3rd edn, Tampere: L¨a¨aketieteellinen oppimateriaalikustantamo. 384 p.
Hill, A. V. 1938. The heat of shortening and the dynamic constants of muscle. Proc. R. Soc.
Lond. 126: 843, pp. 136–195.
How to relief 2017. Lower limb: Bones, muscles, joints nerves. Available:
http://www.howtorelief.com/lower-limb-bones-muscles-joints-nerves/
Kreighbaum, E. and Barthels, K. M. 1996. Biomechanics: a qualitative approach for studying human movement. 4th edn, Boston: Allyn and Bacon. 619 p.
MATLAB 2018. R2018b. Available: https://se.mathworks.com/products/matlab.html
M¨orl, F., Siebert, T., Schmitt, S., Blickhan, R. and Gunther, M. 2012. Electro-mechanical delay in hill-type muscle models. Journal of Mechanics in Medicine and Biology 12: 5, 18 p.
Nikravesh, P. E. 2008. Planar multibody dynamics: formulation, programming, and applications. CRC Press, Boca Raton (FL). 446 p.
Seow, C. Y. 2013. Hill’s equation of muscle performance and its hidden insight on molecular mechanisms. Journal of General Physiology’s 142: 6, pp. 561–573.
Shabana, A. A. 2005. Dynamics of multibody systems. 3rd edn, Cambridge University Press, Cambridge. 374 p.
Simbios 2018. Opensim 4.0. Available: http://simtk.org/projects/opensim/
Thelen, D. G. 2003. Adjustment of muscle mechanics model parameters to simulate dynamic contractions in older adults. ASME Journal of Biomechanical Engineering 125: 1, pp. 70–
77.
Code for main program
% inpuit is controlled in eomLeg.m file
% Parameters
g = 9.81; % m/s2
% Parameters are from OpenSim gait 2354 model
% Body 1 - femur
m1 = 9.3014e4; % kg
L11 = 0.17; % m, distance to hip joint L12 = 0.2258; % m, distance to knee joint
% Inertia tensors Ix1_ = 0.1412;
Iy1_ = 0.0351;
Iz1_ = 0.1339;
% Body 2 - tibia and fibula m2 = 3.7075e4; % kg
L21 = 0.1867; % m, distance to knee joint L22 = 0.2433; % m, distance to ankle joint
% Inertia tensors Ix2_ = 0.0511;
Iy2_ = 0.0051;
Iz2_ = 0.0504;
% Body 3 - foot
% gait 2354 has 3 bodies, talus, calcaneum and toes
% that are combined into one body in this model m3 = 1.5666; % kg
L31 = 0.0636; % m, length distance to ankle joint H3 = 0.0148; % m, height distance to ankle joint
% Inertia tensors Ix3_ = 0.0052;
Iy3_ = 0.0025;
Iz3_ = 0.0051;
% muscle parameters
muscle_sol = muscle_param_sol();
muscle_ta = muscle_param_ta();
% SOL attachment points
% location in local coordinate system
u_sol1 = [0; -0.1126; 0.0038]; % heel attachment, body 3 u_sol2 = [0; -0.002; 0.0337]; % tibia attachment, body 2
% TA attachment points
u_ta1 = [0; 0.018; 0.0247]; % tibia attachment, body 2 u_ta2 = [0; 0.033; -0.2083]; % tibia lower point, body 2 u_ta3 = [0; 0.0044; -0.0092]; % foot, body 3
parameters = [g, m1, L11, L12, Ix1_, Iy1_, Iz1_, m2, L21, L22, ...
Ix2_, Iy2_, Iz2_, m3, L31, H3, Ix3_, Iy3_, Iz3_, u_sol1.’, u_sol2 .’,...
u_ta1.’, u_ta2.’, u_ta3.’];
%%
% generate symbolic MBS equations if (exist(’body’)==0)
body = generateLeg();
end
%%
% Initial conditions
y0 = zeros(76,1); % 42 coordinates 3+5+5+5 constraints,
% 8 muscle parameters
% 8 for wanted results
y0(3) = -L11; % body1 z coordinate at start
y0(4) = 1; % e01
y0(10) = y0(3)-L12-L21; % body2 z loc
y0(11) = 1; % e02
y0(16) = L31; % body3 y loc
y0(17) = y0(10)-L22-H3; % body3 z loc
y0(18) = 1; % e03
% muscles
% sol initial mtc length
% distance between 2 muscle attachment points
y0(61) = l_mtc_2points(y0(15:21), y0(8:14), u_sol1, u_sol2);
% CE length
q_sol = 0.001; % initial activation level [0 < q =< 1]
% 0.001 is minimal activation, no neural stimulation
fhandle = @(l_CE_sol)init_muscle_force_equilib(l_CE_sol, y0(61),...
q_sol, muscle_sol);
l_CE_sol = fzero(fhandle, [0 y0(61)]);
clear fhandle y0(62) = l_CE_sol;
% y63 is dot_l_mtc, y64 is dot_l_CE_sol, 0 at start
% TA
q_ta = 0.001;
y0(65) = l_mtc_3points(y0(15:21), y0(8:14), u_ta3, u_ta2, u_ta1);
fhandle = @(l_CE_ta)init_muscle_force_equilib(l_CE_ta, y0(65),...
q_ta, muscle_ta);
l_CE_ta = fzero(fhandle, [0 y0(65)]);
clear fhandle y0(66) = l_CE_ta;
% y0 69 and 70 sol length error and error dot, 0 at start
% y0 71 and 72 ta length errors
%%
% Simulation settings dt = 1e-3;
t_end = 2.5;
tspan = 0:dt:t_end;
options = odeset(’Stats’,’on’,’RelTol’,1e-6);
parameters(35) = dt;
parameters(36) = t_end;
% Integration tic
[t,y] = ode15s(@eomLeg, tspan, y0, options, parameters);
toc
% gradients from integration,output values that do not need intergration q_ta_vec = gradient(y(:,73))/dt;
q_sol_vec = gradient(y(:,74))/dt;
F_MTC_ta = gradient(y(:,75))/dt;
F_MTC_sol = gradient(y(:,76))/dt;
%%
% Post-process
figure(2); % Animation
% Af1 = matlabFunction(body(1).A);
% Af2 = matlabFunction(body(2).A);
% Af3 = matlabFunction(body(3).A);
for ii = 1:floor((1/dt)/200):length(t) clf
mass_cm1 = [y(ii,1); y(ii,2); y(ii,3)];
mass_cm2 = [y(ii,8); y(ii,9); y(ii,10)];
mass_cm3 = [y(ii,15); y(ii,16); y(ii,17)];
A1 = Af1(y(ii,4), y(ii,5), y(ii,6),y(ii,7));
A2 = Af2(y(ii,11), y(ii,12), y(ii,13),y(ii,14));
A3 = Af3(y(ii,18), y(ii,19), y(ii,20),y(ii,21));
p1 = [0; 0; L11];
p2 = [0; 0; -L12];
p3 = [0; 0; L21];
p4 = [0; 0; -L22];
p5 = [0; -L31; 0];
p6 = [0; L31; 0];
p1v = mass_cm1 + A1*p1;
p2v = mass_cm1 + A1*p2;
p3v = mass_cm2 + A2*p3;
p4v = mass_cm2 + A2*p4;
p5v = mass_cm3 + A3*p5;
p6v = mass_cm3 + A3*p6;
pta3 = mass_cm3 + A3*u_ta3;
pta2 = mass_cm2 + A2*u_ta2;
psol3 = mass_cm3 + A3*u_sol1;
psol2 = mass_cm2 + A2*u_sol2;
plot3([p1v(1) p2v(1)],[p1v(2) p2v(2)],[p1v(3) p2v(3)],’r-’) view(90,0);
grid on
axis([-2 2 -1 1 -1 0])
hold on
plot3([p3v(1) p4v(1)],[p3v(2) p4v(2)],[p3v(3) p4v(3)],’g-’) plot3([p5v(1) p6v(1)],[p5v(2) p6v(2)],[p5v(3) p6v(3)],’r-’) plot3(pta3(1),pta3(2),pta3(3), ’b*’)
plot3(pta2(1),pta2(2),pta2(3), ’b*’) plot3(psol3(1),psol3(2),psol3(3), ’k*’) plot3(psol2(1),psol2(2),psol2(3), ’k*’) hold off
pause(0.005) end
Code for generating symbolic MBS equations
function body = generateLeg()
% Generating system matrices using Euler parameters
% symbolic variables
syms R11 R21 R31 e01 e11 e21 e31
syms dR11 dR21 dR31 de01 de11 de21 de31 syms R12 R22 R32 e02 e12 e22 e32
syms dR12 dR22 dR32 de02 de12 de22 de32 syms R13 R23 R33 e03 e13 e23 e33
syms dR13 dR23 dR33 de03 de13 de23 de33 syms g m1 m2 m3 L11 L12 L21 L22 L31 H_3 syms Fx_sol Fy_sol Fz_sol
syms Fx_ta2 Fy_ta2 Fz_ta2 syms Fx_ta3 Fy_ta3 Fz_ta3 syms Ix1_ Iy1_ Iz1_
syms Ix2_ Iy2_ Iz2_
syms Ix3_ Iy3_ Iz3_
nBodies = 3;
% properites body(1).m = m1;
body(1).L = L11+L12;
body(2).m = m2;
body(2).L = L21+L22;
body(3).m = m3;
body(3).L = 2*L31;
body(3).H = H_3;
body(1).Izz_ = Iz1_;
body(1).Ixx_ = Ix1_;
body(1).Iyy_ = Iy1_;
body(2).Izz_ = Iz2_;
body(2).Ixx_ = Ix2_;
body(2).Iyy_ = Iy2_;
body(3).Izz_ = Iz3_;
body(3).Ixx_ = Ix3_;
body(3).Iyy_ = Iy3_;
% coordinate vectors
body(1).q = [R11, R21, R31, e01 e11 e21 e31].’;
body(2).q = [R12, R22, R32, e02 e12 e22 e32].’;
body(3).q = [R13, R23, R33, e03 e13 e23 e33].’;
body(1).dq = [dR11, dR21, dR31, de01 de11 de21 de31].’;
body(2).dq = [dR12, dR22, dR32, de02 de12 de22 de32].’;
body(3).dq = [dR13, dR23, dR33, de03 de13 de23 de33].’;
body(1).e = [e11; e21; e31];
body(1).de = [de11; de21; de31];
body(1).e0 = e01;
body(1).de0 = de01;
body(2).e = [e12; e22; e32];
body(2).de = [de12; de22; de32];
body(2).e0 = e02;
body(2).de0 = de02;
body(3).e = [e13; e23; e33];
body(3).de = [de13; de23; de33];
body(3).e0 = e03;
body(3).de0 = de03;
q = [body(1).q; body(2).q; body(3).q];
dq = [body(1).dq; body(2).dq; body(3).dq];
% Rotation matrices for ii = 1:nBodies
body(ii).p = [body(ii).e0; body(ii).e];
body(ii).dp = [body(ii).de0; body(ii).de];
body(ii).A = (2*body(ii).e0ˆ2 - 1)*eye(3) + ...
2*(body(ii).e*body(ii).e.’ + body(ii).e0*skew(body(ii).e));
body(ii).E = [-body(ii).e, skew(body(ii).e) + body(ii).e0*eye(3)];
body(ii).E_ = [-body(ii).e, -skew(body(ii).e) + body(ii).e0*eye(3)];
body(ii).G = 2*body(ii).E;
body(ii).G_ = 2*body(ii).E_;
end
% Angular velocity of the system for ii = 1:nBodies
body(ii).w_ = body(ii).G_*body(ii).dp;
end
% Mass matrix for ii = 1:nBodies
body(ii).Itt_ = [
body(ii).Ixx_, 0, 0;
0, body(ii).Iyy_, 0;
0, 0, body(ii).Izz_];
body(ii).Mrr = body(ii).m * eye(3);
body(ii).Mtt = body(ii).G_.’ * body(ii).Itt_ * body(ii).G_;
body(ii).M(1:3,1:3) = body(ii).Mrr;
body(ii).M(4:7,4:7) = body(ii).Mtt;
end
% Constraints
% Euler parameter constraints for ii = 1:nBodies
