Using GPOPS-II to optimize sum of squared torques of a double pendulum as a prosthesis leg Abstract Milad Zarei MCE 593 Prosthesis Design & Control A two-dimensional, two links pendulum is developed to investigate the dynamic characteristics of normal and transfemoral amputee locomotion during swing phase of the walking gait. The equations derived using Langrange method and the stance foot-ground contact was not simulated in this project. GPOPS-II is used to optimize the torques of the hip and the during the swing phase. 1. Introduction The normal human gait uses minimal energy cost and a steady kinematical model. For amputees, achieving an acceptable quality of life, there is a need for a proper designed prosthetic leg that provides reliable stability during stance phase and sufficient control of motion during the swing phase of the gait cycle (Rietman et al., 2002). The energy cost of walking is 30%-50% higher in unilateral transfemoral amputees than in able-bodied controls (Gitter et al., 1991) and at least half of this is due to the loss of knee function (Waters et al., 1976). In this project I am going to investigate what movement and torques will be optimal for the swing phase during walking, and how similar are these to what we see in actual human movement. A double pendulum is considered as a leg which moves in swing phase (Figure 1). The initial state and final state will be defined based on actual data. GPOPS-II is used to find the optimal controls and the optimal movement for the swing phase. Figure 1 2D model
The swing phase of gait starts at toe off, when the foot leaves the ground. Following toe off, the leg is moved forward, and the swing phase continues until the foot returns to the ground; the leg then begins to support the body, and the stance phase begins. The sequence of swing and stance for a leg constitutes one complete gait cycle. For each leg, the swing phase makes approximately 40% of the gait cycle, with remaining 60% of the gait being in stance (Mena et al., 1980) (Figure 2). Figure 2 Gait Cycle During gait, there are three main parts in which energy is consumed. The three main parts are controlling onward movement during deceleration toward the end of swing phase, shock absorption at heelstrike, and propulsion during push off, when the center of gravity moved up and forward. A double pendulum is a combination of two pendulums that one is attached to the end of the other. There are many kinds of double pendulums which may be of equal or unequal masses or lengths. In this project, the pendulums are assumed to have the same length and mass, and the motion is confined to two dimensions. Figure 3 Double Pendulum
Human walking is performed with a strategy called the double pendulum. Human motion is controlled by neuro-muscular system. But biped walking is known as a passive mechanical process. Anthropomorphic legged mechanism with two moving links can represent stable, human-liked walking with no actuation and control. During walking, the leg that leaves the ground swings forward from the hip. This sweep is the first pendulum and then the leg strikes the ground with the heel and rolls through to the toe in a motion described as an inverted pendulum (Figure 3). The motion of the two legs is coordinated so that one foot or the other is always in contact with the ground. 2. Method A double pendulum is going to be used as a controlled prosthesis leg which is not on the ground. 2.1. Mathematical Model The top link has length d1 and the mass 1 is attached to the end of the link, also the same size link is attached to the mass 1. The second link contains the same symmetry and mass. The angle which link 1 makes with vertical axis is called θ 1 and the other angle of the link 2 and vertical axis is represented byθ 2 (Figure 4). Figure 4 Schematic Double Pendulum
Length of the first pendulum = L 1 Length of the second pendulum = L 2 Mass at the end of the first link = m 1 Mass at the end of the second link = m 2 The angle between pendulum 1 and the vertical line = θ 1 The angle between pendulum 2 and the vertical line = θ 2 The x-position of the m 1 = x 1 The y-position of the m 1 = y 1 The x-position of the m 2 = x 2 The y-position of the m 2 = y 2 The Lagrangian method is used to obtain the equations. The mathematical model of the double pendulum is shown below. The state space method is used to transform the second order equation into first order. For the purpose of coding in GPOPS2, the dynamic equations should be first order. x 1 = v 1 x 2 = v 2 v 1 = u 1 + m 2l 1 v 2 2 sin( ) cos( ) (m 2 l 2 v 2 2 ) sin( ) (m 1 + m 2 )gsin(x 1 ) l 1 (m 1 + m 2 ) m 2 l 1 cos 2 ( ) v 2 = u 2 + m 2l 2 v 2 2 sin( ) cos( ) + gsin(x 1 ) cos( ) (m 1 + m 2 ) + l 1 v 2 2 sin( ) (m 1 + m 2 ) gsin(x 2 )(m 1 + m 2 ) l 2 (m 1 + m 2 ) (m 2 l 2 cos 2 ( )) = x 1 x 2 2.2. Using GPOPS GPOPS helps to optimize the squared torques that are required for walking during swing phase. The optimal (open loop) controls and the optimal motion for the swing phase will be found during this project. The variables are angle and torque which should be bounded to a specific range. Moreover, the initial state, final state, initial velocity and final velocity of the prosthetic leg are task constraints that can be defined. Since a single pendulum was solved in GPOPS2, it s known that it is possible to use random initial guess for double pendulum. However, the double pendulum is going to be a model of prosthesis leg, a legitimate initial guess, which is based on experimental data, will be acceptable. The initial state would be toe off (from gait data) and final state would be heel strike. The code consists of three main parts that are main file, continuous file and endpoint file. Bounds for the variables, initial guess and setup settings are defined in main file. The bounds are assumed
to be big enough to have a good range of possible cases and answers. Based on the actual data, the hip angle at swing phase starts at -7 and finishes at 19, also the starting knee angle is 1.69 and the ending angle is 21.7 (Winter, DA (1991)). SNOPT is used as Matlab solver for the nonlinear part and MUMPS is used for solving linear section. In the continuous file, the evolution of dynamics and the integrands that are required to compute any integrals are specified. Also, the objective function which is sum of squared torques is defined here, but it shows up in the endpoint file. The dynamics trajectory shows that how the equations should be defined (The Lagrangian equations are coded in this part). The endpoint function defines how the start and terminus and the also defines the cost to be minimized. Results I have used Winter s data (Winter DA, 1991) as initial guess for the problem. The first two graphs are the position of the pendulums during entire walking gait cycle. Although, the results do not fit the actual data but they are promising because they follow the same pattern. Figure 5 Position of the 1 st link (hip) during full cycle
In the curves reported in figure 5 and figure 6, we have documented hip and knee angle during the entire cycle. Actually, the initial point, final point and cycle duration are given through the actual data (Winter s data) and GPOPS solve the double pendulum using those assumptions. As you can see, the graphs follow the Winter s results (Figure 7). Figure 6 Position of the 2 nd link (knee) during full cycle
Figure 7 Hip and knee angle (Winter s data 1991) Figure 8 Position of the 1 st link (hip) during swing phase
As it shows, the hip starts the motion from -7 to -19 during the swing phase. As you can see, the 1 st pendulum easily goes to the target point. In comparison to the Figure 7, hip angle during the swing phase looks elegant. Figure 9 Position of the 2 nd pendulum (knee) during swing phase Based on the figure 7 and 9, it can be said that knee angle tracks the actual data. The starting point and the end point are different from the real data but the general form is the same. In the following graphs, the angular velocity of both pendulums are shown. As it expected, they start from the zero at the begging and go back to the zero at the end of the gait.
Figure 10 Angular velocity of the 1 st pendulum (hip) during swing phase Figure 11 Angular velocity of the 2 nd pendulum (knee) during swing phase
Figure 12 Torques of the 1 st pendulum (hip) during swing phase Figure 13 Torques of the 2 nd pendulum (knee) during swing phase
Figure 14 Hip moment (Winter s data 1991) Figure 15 Knee moment (Winter s data 1991) According to the figures 12, 13, you can realize that the torques of the double pendulum follow the pattern in the swing phase (at the last 40% of the gait). Future Work Although a simple model is used during in this project but the results are encouraging because of the good tracking of the actual data. I believe that if ground force reaction could be added to the model, the results would be more decent. Moreover, adding contact between ground and the leg makes the problem nonlinear which will be more interesting problem. A spring-damper model can be simulated in this model for ground force reaction. Furthermore, using muscle dynamics in this model probably will improve the results. References [1] Rietman J.S., Potema K., Geertzen J.H.B., Gait analysis in prosthetics: opinions, ideas and conclusions, 2002
[2] Gitter A., Murno C., Czerniecki J.M., Joint moment and muscle power output, 1991 [3] Waters R.L., Mulroy S., Review: The energy expenditure of normal and pathologic gait, Gait and Posture, 1999 [4] Winter D.A., The biomechanics and motor control of human gait, 1991 [5] Rick Rarick, Hanz Richter, Antonie van den Bogert, Dan Simon, Holly Warner, Taylor Barto, Optimal Design of Transfemoral Prosthesis with Energy Storage and Regeneration, 2014 [6] D. Mena, J. M. Mansour, S. R. Simon, Analysis and Synthesis of Human Swing Leg Motion during Gait and its Clinical Applications, 1980 [7] Shandiz M.A., Farahmand F., Osman N. A. A., Zohoor H., A robotic model of transfemoral amputee locomotion for design optimization of knee controllers, 2012 [8] Michael A. Patterson, Anil V. Rao, A general-purpose Matlab software for solving multiplephase optimal control problems, 2014 Attached is the code. They are Main file, continuous file and endpoint file respectively. %--------------------------------------------------------------------% %---------------------- Degree-Pendulum Problem ---------------------% %--------------------------------------------------------------------% clear all clc tic % Parameters: %----------------------------------------------------------------% %--------------------- Data Required by Problem -----------------% %----------------------------------------------------------------% auxdata.d1 = 0.5; auxdata.d2 = 0.5; auxdata.m1 = 1; auxdata.m2 = 1; auxdata.i = 1; auxdata.g = 9.81; % length of pendulum 1 (m) % length of pendulum 2(m) % mass of pendulum (kg) % mass of pendulum (kg) % moment of inertia relative to pivot (kg m^2) % gravity (m s^-2) % t represents time (s) % x represents angle (rad) % v represents angular velocity (rad/s) % u represents torque (N*m) load('gaitdata');
%------------------------------ Bounds ------------------------------% t0 = 0; tf = 1; x1min = -5*pi; x1max = 5*pi; x2min = -5*pi; x2max = 5*pi; v1min = -10; v1max = 10; v2min = -10; v2max = 10; u1min = -100; u1max = 100; u2min = -100; u2max = 100; N = 50; bounds.phase.initialtime.lower = t0; bounds.phase.initialtime.upper = t0; bounds.phase.finaltime.lower = tf; bounds.phase.finaltime.upper = tf; bounds.phase.initialstate.lower = [gaitdata.angles(30,1)*pi/180 0 gaitdata.angles(30,2)*pi/180 0]; bounds.phase.initialstate.upper = [gaitdata.angles(30,1)*pi/180 0 gaitdata.angles(30,2)*pi/180 0]; bounds.phase.state.lower = [x1min, v1min, x2min, v2min]; bounds.phase.state.upper = [x1max, v1max, x2max, v2max]; bounds.phase.finalstate.lower = [gaitdata.angles(end,1)*pi/180 0 gaitdata.angles(end,2)*pi/180 0]; bounds.phase.finalstate.upper = [gaitdata.angles(end,1)*pi/180 0 gaitdata.angles(end,2)*pi/180 0]; bounds.phase.control.lower = [u1min, u2min]; bounds.phase.control.upper = [u1max, u2max]; bounds.phase.integral.lower = 0; bounds.phase.integral.upper = 50; guess.phase.time = [t0; tf]; guess.phase.state = zeros(2,4); guess.phase.control = zeros(2,2); guess.phase.integral = 0; %------------------------- Problem Setup -----------------------------% setup.name = 'pend-problem'; setup.functions.continuous = @doublependcontinuous; setup.functions.endpoint = @doublependendpoint; setup.auxdata = auxdata; setup.bounds = bounds; setup.guess = guess; setup.nlp.solver = 'snopt'; setup.derivatives.supplier = 'sparsecd'; setup.derivatives.derivativelevel = 'first'; setup.scales.method = 'none'; setup.mesh.method = 'hp-pattersonrao'; setup.derivatives.dependencies = 'sparsenan'; setup.mesh.tolerance = 1e-5; setup.method = 'RPM-Differentiation'; setup.nlp.ipoptoptions.linear_solver = 'ma57'; output = gpops2(setup);
function phaseout = doublependcontinuous(input) m1 = input.auxdata.m1; m2 = input.auxdata.m2; d1 = input.auxdata.d1; d2 = input.auxdata.d2; g = input.auxdata.g; x = input.phase.state; v = input.phase.state; u = input.phase.control; x1 = x(:,1); x2 = x(:,3); v1 = v(:,2); v2 = v(:,4); u1 = u(:,1); u2 = u(:,2); delta = x1-x2; x1dot = v1; x2dot = v2; v1dot = u1 + ((1/((d1*(m1+m2))-(m2*d1*cos(delta).^2)))'.*((((- m2*d1*v2.^2)'*sin(delta).*cos(delta)))+(g*m2*sin(x2).*cos(delta))- (m2*d2*(v2.^2).*sin(delta))-((m1+m2)*g.*sin(x1)))); v2dot = u2 + ((m2*d2*v2.^2.*sin(delta).*cos(delta))+(g.*sin(x1).*cos(delta)*(m1+m2) )+((d1*v2.^2).*sin(delta)*(m1+m2))- (g.*sin(x2)*(m1+m2))./((d2*(m1+m2))-(m2*d2.*cos(delta).^2))); phaseout.dynamics = [x1dot v1dot x2dot v2dot]; phaseout.integrand = u1.^2 + u2.^2; if (toc > 3) doublependplot(input); tic end function output = doublependendpoint(input) q = input.phase.integral; output.objective = q;