Acrobot Swing Up with MATLAB

A crob ot Swing Up with MA TLAB Y u Xiao Decem b er 20, 2024 Boston Univ ersity , Boston, MA 1 The A crob ot The acrob ot usually refers to a 2-link underactuated robot with one end fixed at some p oin t in the co ordinate frame. It lo oks similar to a 2-link rob ot arm, but with only one actuator at the elb o w. This feature makes it an underactuated rob ot. Underactuated robot is a family of rob ots which dra ws particular inter- ests among engineers. They are hard to con trol since they are usually highly non-linear. The acrob ot is one of the simplest examples in this family , as its equations of motion is obtainable b y manual deriv ation. In this note, w e fo cus on the classical swing up task for acrob ot. The task aims at bring the acrobot from the b ottom position to the up right position. W e presen t solutions for this task using an optimization based metho d with man- ually derived system equations. W e studied tw o kind of acrob ot: the classical 2-link acrob ot and an extended v ersion with three links. The computer program- ming en vironment we used in the exp erimen ts is MA TLAB, with a op en-sourced MA TLAB library called OptimT ra j. 2 Theoretical Preliminaries 2.1 State Space Mo del of Dynamical Systems P eople ha ve b een using differential equations to describ e nature since the estab- lishmen t of morden science. These mathematical ob jects equating v ariables and their deriv atives ha ve n umerous application in man y different fileds. Some of the most celerbrated examples including heat equation describing heat diffusion in ro ds, equations describing the growth of population and our fav orite equations describing the motion of rigid b odies. The state space mo del of dynamical system is a set of first-order ordinary differen tial equation(ODE for short) describing the behaviour of a dynamical system. Using a set of generalized co ordinates, w e can reduce high-order differ- en tial equations into first-order. 1 ˙ x = f ( x ( t ) , u ( t ) , t ) , is a t ypical state space mo del. x denotes the state space v ariables, ˙ x denotes their first-order deriv ativ es, u is the con trol input. Con trol input isn’t a necessity for ev ery system. A passive system is a system with no con trol input. A sp ecial form of ODE w e will use in is note is ODEs with a mass matrix. M ˙ x = f ( x ( t ) , u ( t ) , t ) The mass matrix M raise when the function f no longer directly describ e the deriv ativ es of state space v ariables, but a weigh ted sum of the them. This form of ODEs are common results of motion of rigid b odies, where we directly acquire the mass matrix. W e are going to use this form in the numerical solution of passiv e systems. How ever, we need to reco ver the standard form b y calculating the in verse of mass matrix. ˙ x = M − 1 f ( x ( t ) , u ( t ) , t ) The state space mo del of a dynamical is mostly non-linear, which means that the function f ( x, u ) is usually non-linear. The classical to ols analyzing the b eha viour of a non-linear systems is Lyapuno v theory . In this note, w e w ant to emphasize the tra jectory optimization metho d for control, instead of the theoretical prop erties of the ob jective rob ot. So w e will lea ve theoretical analysis of the system to study in the future. 2.2 The General Optimal tra jectory Design Problem T ra jectory optimization is a class of optimal con trol metho ds. W e are going to give up sloving optimal feedback controller, and instead using optimization to ol to find a go od solution for a single initial condition. The dynamics of the system is ˙ x = f ( x ( t ) , u ( t ) , t ) , and initial and final conditions can b e defined within some prescrib ed low er and upp er b ounds ψ i,l ≤ ψ i ( x ( t i ) , u ( t i ) , t i ) ≤ ψ i,u , ψ f ,l ≤ ψ f ( x ( t f ) , u ( t f ) , t f ) ≤ ψ f ,u . In addition, w e can imp ose simple b ounds on the state v ariables x l ≤ x ( t ) ≤ x u and on con trol v ariables u l ≤ u ( t ) ≤ u u . The basic problem is to determine the control v ector u ( t ) to minimize the p erformance index 2 J = φ ( x ( t f ) , t f ) , where φ is the pre-defined cost function. T o solv e the optimal tra jectory design problem, w e form it in to a non-linear programming(NLP) problem. The general NLP problem requires finding the n v ectors to solve min x F ( x ) , sub ject to m constraints c L ≤ c ( x ) ≤ c U , and b ounds x L ≤ x ≤ x U , where F ( x ) is a scalar function, and c ( x ) are non-linear constraints. 2.3 Direct Collo cation T o solv e the general optimal tra jectory design problem with NLP solver, we need to form a scalar cost function J o ver tra jectories, and conv ert dynamic constrain t into inequalities. A ccording to metho d taken in this step, tra jectory optimization metho ds are often divided into tw o classes: indirect metho d and direct method. In the direct setting, we will first discretize the state v ariable x and control input u , then form ulate an optimization o ver the tra jector. In this note, w e use the direct metho d. The idea of direct transcriping is to discritize time into interv als(usuallt equal in terv als) as t i , t 2 , t 3 , . . . , t f . The states and con trols can be discretized o ver the tra jectory b y defining x k = x ( t k ) and u k = u ( t k ) . The discretized states and controls are used as decision v ariables of NLP problem, which is y = [ x 1 , u 1 , . . . , x N , u N ] . F or the dynamic constraints(the equations of motion), w e can form an equal- it y constraint on every interv al x k +1 − x k − hf k = 0 , where h is the step size of discretization(usually fixed), and f k = f ( x k , u k , t k ) . W e can use the technique of constructing a defect v ariable for each of these equalit y constraint, as ζ k = x k +1 − x k − hf k . As a result of the transcription, the optimal control constrain ts can b e re- placed b y a set of inequalities of decision v ariables 3 c L ≤ c ( y ) ≤ c U where c ( y ) = [ ζ 1 , ζ 2 , . . . , ζ N − 1 , ψ 1 , ψ N , ] , and c L = [0 , 0 , . . . , 0 , ψ i,l , ψ f ,l ] , c U = [0 , 0 , . . . , 0 , ψ i,u , ψ f ,u ] . Collo cation is an imp ortan t metho ds used to transcrib e differential dynamic constrain ts into a set of algebraic constraints, in order to form an NLP problem. The basic idea is to choose a p olynomial up to a certain degree with a num b er of p oin ts with b oundary v alues matched control and state v alue at knots, and to enforce the polynomials to satisfy the equations of motion ar the collo cation p oin ts(in termediate p oin ts b et ween knots). 3 Mathematical Notation There will b e plent y of equations in this note, so it’s go od to clarify mathematical notation b efore jumping into the equations. Our acrob ot lives in a physical world with a gravitational acceleration germ g . It has t wo massless link, and tw o point mass. A figure depicting our acrob ot lo ok as the follow. In this figure, the t wo line segmen ts in blue represent the massless links. W e will use l 1 to to represen t the length of the first link(the upp er one in this figure), and use l 2 for the length of the second link(the lo wer one in this figure). The three small circles in different colors(red, green and blue) are the ends of links. The red p oint is the fixed end of the first link; the green p oin t is a revolut joint connecting tw o links with a mass m 1 ; the blue p oin t is a blue p oint is the end of the second link with a mass m 2 . The angle of the first link is represent by q 1 , and the second angle in represent b y q 2 . Since there are plen ty of trigonometry 4 functions in the equations of motion, we use a short hand for sinesoid functions. W e use c θ to denote function cos ( θ ) , and use s θ to denote function sin ( θ ) . W e use ˙ q to denote the first-order time deriv ative of q , and use ¨ q to denote the second time deriv ative of q . τ denotes torques, from gravit y or control input. 4 A crob ot Swing Up 4.1 Equations of Motion The acrob ot is a tw o-link p endulum, while only the second join t has an actuator. Th us, it is an underactuated rob ot. W e will first derive the dynamics model of the double p endulum, and sim- ulate a passiv e one in MA TLAB. W e are going to use the method of Lagrange to deriv e the equations of motion. d dt ∂ L ∂ ˙ q i − ∂ L ∂ q i = Q i , where L denotes the Lagrangian, q i denotes the generalized coordinates and Q i denotes the force applied. L = T − U T = 1 2 ( m 1 + m 2 ) l 2 1 ( ˙ q 1 ) 2 + 1 2 m 2 l 2 2 ( ˙ q 1 + ˙ q 2 ) 2 + m 2 l 1 l 2 ˙ q 1 ( ˙ q 1 + ˙ q 2 ) c 2 U = − ( m 1 + m 2 ) g l 1 c 1 − m 2 g l 2 c 1+2 Plugging the Lagrangian in to the Lagrange Equations, w e obtain the equa- tions of motion. Then w e can reform the equations of motion in to a characteristic form called the Manipulator Equations, whic h takes the form: M ( q ) ¨ q + C ( q , ˙ q ) ˙ q = τ g ( q ) + B u , where q is the state vector, I is the inertia matrix, C captures Coriolis forces, and τ g is the gra vity vector. The matrix B maps con trol inputs u in to generalized force. In our first task to sim ulate the passiv e double p endulum, we don’t hav e to w orry ab out B or u. The other matrix take the following form: M ( q ) =  ( m 1 + m 2 ) l 2 1 + m 2 l 2 2 + 2 m 2 l 1 l 2 c 2 m 2 l 2 2 + m 2 l 1 l 2 c 2 m 2 l 2 2 + m 2 l 1 l 2 c 2 m 2 l 2 2  C ( q , ˙ q ) =  0 − m 2 l 1 l 2 (2 ˙ q 1 + ˙ q 2 ) s 2 m 2 l 1 l 2 ˙ q 1 s 2 0  τ g ( q ) = − g  ( m 1 + m 2 ) l 1 s 1 + m 2 l 2 s 1+2 m 2 l 2 s 1+2  5 4.2 Dynamics Mo del of 2-link Acrobot Alrigh t, w e now ha ve a horrific lo oking second-order differential equation. The next necessary step is to reform the Manipulator Equations into first-order dif- feren tial equations, so that we can form a state space mo del. The state space v ariable we are going to use is q =  q 1 q 2 ˙ q 1 ˙ q 2  . The first-order differen- tial equations w e wish to hav e takes the form: M ass ( q ) ˙ q = f ( q ) W e use the matrix I and C to construct the Mass matrix: M ass ( q ) =  I 2 × 2 0 2 × 2 C ( q ) M ( q )  , whic h is a 4-by-4 matrix. Then we can reformulate the Manipulator Equation in to a set of 4 differential equations:  I 2 × 2 0 2 × 2 C ( q ) M ( q )      ˙ q 1 ˙ q 2 ¨ q 1 ¨ q 2     =     ˙ q 1 ˙ q 2 − g ( m 1 + m 2 ) l 1 s 1 + m 2 l 2 s 1+2 m 2 l 2 s 1+2     Finally , we hav e a set of first-order differential equations, which we can thro w in to some numerical in tegration solv er. Here we will use the built-in solver from MA TLAB. The following t wo figures are frames from the simulation of a passive acrob ot swing from right to left. 4.3 T ra jectory Optimization Alrigh t we hav e a passive double p endulum swinging bac k and forth, but that is not actually very e xciting. The exciting part of this note is to demonstrate ho w to swing up the acrob ot from the b ottom using a control technique called tra jectory optimization. The softw are library we are going to use is an op en- sourced tra jectory optimization library called OptimT ra j. No w, we hav e to reformulate the equations of motion in to the cannonical form, so our plan is: ˙ q = M ass ( q ) − 1 f ( q, u ) , 6 whic h is a non-linear dynamical system. Then, we can construct an op- timization problem using this mo del, which we can thro w into our tra jectory optimization library . The follo wing fugure plots the tra jectories of the t wo join ts(red and green joints). The v alues of these t wo actuated join ts start from [0 , 0] and end at [ π , 0] . Then, let’s animate the swing up sequence of the 2-link acrob ot. Here are three frames from the animation. 5 A 3-link A crob ot 5.1 Equations of Motion Let’s derive the equations of motion for a 3-link acrob ot in 2-dimensional space. Here w e are again using the metho d of Lagrange. Let’s get start with the Lagrangian, whic h comes from the kinetic and p oten tial energy of the system. T = 1 2 ( m 1 + m 2 + m 3 ) l 2 1 ˙ q 2 1 + 1 2 ( m 2 + m 3 ) l 2 2 ( ˙ q 1 + ˙ q 2 ) 2 + 1 2 m 3 l 2 3 ( ˙ q 1 + ˙ q 2 + ˙ q 3 ) 2 + ( m 2 + m 3 ) l 1 l 2 ˙ q 1 ( ˙ q 1 + ˙ q 2 ) c 2 + m 3 l 1 l 3 ˙ q 1 ( ˙ q 1 + ˙ q 2 + ˙ q 3 ) c 2+3 + m 3 l 2 l 3 ( ˙ q 1 + ˙ q 2 )( ˙ q 1 + ˙ q 2 + ˙ q 3 ) c 3 U = − ( m 1 + m 2 + m 3 ) l 1 g c 1 − ( m 2 + m 3 ) l 2 g c 1+2 − m 3 l 3 g c 1+2+3 L = T − U Next, let’s recall our fa vorite Lagrange Equations. 7 d dt ∂ L ∂ ˙ q i − ∂ L ∂ q i = τ i Plugging the Lagrangian into these equations yields the equations of motion. τ 1 = ( m 1 + m 2 + m 3 ) l 2 1 ¨ q 1 + ( m 2 + m 3 ) l 2 2 ( ¨ q 1 + ¨ q 2 ) + m 3 l 2 3 ( ¨ q 1 + ¨ q 2 + ¨ q 3 ) + ( m 2 + m 3 ) l 1 l 2 (2 ¨ q 1 + ¨ q 2 ) c 2 − ( m 2 + m 3 ) l 1 l 2 (2 ˙ q 1 + ˙ q 2 ) ˙ q 2 s 2 + m 3 l 1 l 3 (2 ¨ q 1 + ¨ q 2 + ¨ q 3 ) c 2+3 − m 3 l 1 l 3 (2 ˙ q 1 + ˙ q 2 + ˙ q 3 )( ˙ q 2 + ˙ q 3 ) s 2+3 + m 3 l 2 l 3 (2 ¨ q 1 + 2 ¨ q 2 + ¨ q 3 ) c 3 − m 3 l 2 l 3 (2 ˙ q 1 + ˙ q 2 + ˙ q 3 ) ˙ q 3 s 3 + ( m 1 + m 2 + m 3 ) l 1 g s 1 + ( m 2 + m 3 ) l 2 g s 1+2 + m 3 l 3 g s 1+2+3 τ 2 = ( m 2 + m 3 ) l 2 2 ( ¨ q 1 + ¨ q 2 ) + m 3 l 2 3 ( ¨ q 1 + ¨ q 2 + ¨ q 3 ) + ( m 2 + m 3 ) l 1 l 2 ¨ q 1 c 2 − ( m 2 + m 3 ) l 1 l 2 ˙ q 1 ˙ q 2 s 2 + m 3 l 1 l 3 ¨ q 1 c 2+3 − m 3 l 1 l 3 ˙ q 1 ( ˙ q 2 + ˙ q 3 ) s 2+3 + m 3 l 2 l 3 (2 ¨ q 1 + 2 ¨ q 2 + ¨ q 3 ) c 3 − m 3 l 2 l 3 (2 ˙ q 1 + 2 ˙ q 2 + ˙ q 3 ) ˙ q 3 s 3 + ( m 2 + m 3 ) l 1 l 2 ˙ q 1 ( ˙ q 1 + ˙ q 2 ) s 2 + m 3 l 1 l 3 ( ˙ q 1 + ˙ q 2 )( ˙ q 1 + ˙ q 2 + ˙ q 3 ) s 2 + ( m 2 + m 3 ) l 2 g s 1+2 + m 3 l 3 g s 1+2+3 τ 3 = m 3 l 2 3 ( ¨ q 1 + ¨ q 2 + ¨ q 3 ) + m 3 l 1 l 3 ¨ q 1 c 2+3 − m 3 l 1 l 3 ˙ q 1 ( ˙ q 2 + ˙ q 3 ) s 2+3 + m 3 l 2 l 3 ( ¨ q 1 + ¨ q 2 ) c 3 − m 3 l 2 l 3 ( ˙ q 1 + ˙ q 2 ) ˙ q 3 s 3 + m 3 l 1 l 3 ˙ q 1 ( ˙ q 1 + ˙ q 2 + ˙ q 3 ) s 2+3 + m 3 l 2 l 3 ( ˙ q 1 + ˙ q 2 )( ˙ q 1 + ˙ q 2 + ˙ q 3 ) s 3 + m 3 l 3 g s 1+2+3 5.2 Dynamics Mo del of 2-link Acrobot These equations may lo ok horrific for normal p eople(include me). Ho wev er, we can put them into a nicer lo oking matrix form, whic h is what we hav e to tell the computer. The Manipulator Equations of our 3-link acrob ot tak es the form: M ( q ) ¨ q + C ( q , ˙ q ) ˙ q = τ g ( q ) + B u W e hav e to again reform ulate the problem into a first-order differential equa- tion by constructing the mass matrix. This time, the mass matrix is a 6 × 6 matrix, since our state v ariable has b ecame 6-dimensional. M ass ( q ) =  I 3 × 3 0 3 × 3 C ( q ) 3 × 3 M ( q ) 3 × 3  As expected, the mass matrix is going to b e a little bit complicated, so w e decide to present the matrix elemen t-by-elemen t. M ij denotes elements from matrix M ( q ) , while C ij denotes elemen ts from matrix C ( q , ˙ q ) . M 11 = ( m 1 + m 2 + m 3 ) l 2 1 + ( m 2 + m 3 ) l 2 2 + m 3 l 2 3 + 2( m 2 + m 3 ) l 1 l 2 c 2 + 2 m 3 l 1 l 3 c 2+3 + 2 m 3 l 2 l 3 c 3 M 12 = ( m 2 + m 3 ) l 2 2 + m 3 l 2 3 + ( m 2 + m 3 ) l 1 l 2 c 2 + m 2 l 1 l 3 c 2+3 + 2 m 3 l 2 l 3 c 3 M 13 = m 3 l 2 3 + m 3 l 1 l 3 c 2+3 + m 3 l 2 l 3 c 3 M 21 = ( m 2 + m 3 ) l 2 2 + m 3 l 2 3 + ( m 2 + m 3 ) l 1 l 2 c 2 + m 3 l 1 l 3 c 2+3 + 2 m 3 l 2 l 3 c 3 M 22 = ( m 2 + m 3 ) l 2 2 + m 3 l 2 3 + 2 m 3 l 2 l 3 c 3 M 23 = m 3 l 2 3 + m 3 l 2 l 3 c 3 M 31 = m 3 l 2 3 + m 3 l 1 l 3 c 2+3 + m 3 l 2 l 3 c 3 8 M 32 = m 3 l 2 3 + m 3 l 2 l 3 c 3 M 33 = m 3 l 2 3 C 11 = 0 C 12 = − ( m 2 + m 3 ) l 1 l 2 (2 ˙ q 1 + ˙ q 2 ) s 2 − m 3 l 1 l 3 (2 ˙ q 1 + ˙ q 2 + ˙ q 3 ) s 2+3 C 13 = − m 3 l 1 l 3 (2 ˙ q 1 + ˙ q 2 + ˙ q 3 ) s 2+3 − m 3 l 2 l 3 (2 ˙ q 1 + ˙ q 2 + ˙ q 3 ) s 3 C 21 = − ( m 2 + m 3 ) l 1 l 2 ˙ q 2 s 2 − m 3 l 1 l 3 ( ˙ q 2 + ˙ q 3 ) s 2+3 + ( m 2 + m 3 ) l 1 l 2 ( ˙ q 1 + ˙ q 2 ) s 2 + m 3 l 1 l 3 ( ˙ q 1 + ˙ q 2 + ˙ q 3 ) s 3 C 22 = m 3 l 1 l 3 ( ˙ q 1 + ˙ q 2 + ˙ q 3 ) s 3 C 23 = − m 3 l 2 l 3 (2 ˙ q 1 + 2 ˙ q 2 + ˙ q 3 ) s 3 C 31 = − m 3 l 1 l 3 ( ˙ q 2 + ˙ q 3 ) s 2+3 + m 3 l 1 l 3 ( ˙ q 1 + ˙ q 2 + ˙ q 3 ) s 2+3 + m 3 l 2 l 3 ( ˙ q 1 + ˙ q 2 + ˙ q 3 ) s 3 C 32 = m 3 l 2 l 3 ( ˙ q 1 + ˙ q 2 + ˙ q 3 ) s 3 C 33 = − m 3 l 2 l 3 ( ˙ q 1 + ˙ q 2 ) s 3 V ery co ol! No w we are ready to put ev erything into the computer. In MA TLAB, again we construct the following first-order differential equations:  I 3 × 3 0 3 × 3 C ( q ) 3 × 3 M ( q ) 3 × 3          q 1 q 2 q 3 ˙ q 1 ˙ q 2 ˙ q 3         =         ˙ q 1 ˙ q 2 ˙ q 3 τ g 1 + u 1 τ g 1 + u 2 τ g 3 + u 3         , where τ g i are the gra vity terms, while u i are the con trol input. Here we ha ve three input, whic h means the system is fully actuated. Since the acrobot is underactuated, we will take aw ay u 1 when we solve the acrob ot problem. The gra vity terms are: τ g 1 = − g [( m 1 + m 2 + m 3 ) l 1 s 1 + ( m 2 + m 3 ) l 2 s 1+2 + m 3 l 3 s 1+2+3 ] τ g 2 = − g [( m 2 + m 3 ) l 2 s 1+2 + m 3 l 3 s 1+2+3 ] τ g 3 = − g m 3 l 3 s 1+2+3 Exciting news! All the mathematic is done and no w let’s talk ab out how to do sim ulation on a computer. The first thing we wan t to work out on the computer is to c heck whether we ha ve the correct dynamics mo del. T o chec k whether our mo del is correct, an in tuitive w ay is sim ulate the passiv e system. W e can give the system an initial state, such as [ π 3 0 0 0 0 0 ] , and watc h it ev olves ov er time. If there 9 are any bugs in our equations, chances are the system will lo ok ed unreal. I hav e to clarify that this metho d do esn’t gauruntee the mo del is correct, but I choose to b eliev e m y eyeballs and commonsense(because I’m an engineer, t ypically lazy p erson). Since our mo del is barely a set of differen tial equations, w e can use n umerical integration to ols to solve it approximatly . Our plan here is using the built-in solver from MA TLAB. The dynamics mo del of acrob ot fits right in one of the standard form supp ort b y MA TLAB, whic h called “ODE with time and state dep endent mass matrix”. In order to tell MA TLAB what we intended to do, w e hav e to sp ecify options for the o de45 solver. options = odeset ( 0 M ass 0 , @ mass ) , [ t, y ] = ode 45(@ f , tspan, q 0 , options ) , where the name/v alue pair ( 0 M ass 0 , @ mass ) tells o de45 that w e hav e a mass matrix in our equations and we ha ve a function called ’mass’ calculating it. In the second line of co de, ’tspan’ stands for the time span we wish to integrate; ’q0’ is the initial state of the system; ’@f ’ link o de45 to a function called ’f ’, whic h calculates the time deriv ative of the state v ariables. The o de45 solver will giv e us the numerical solution of our system ov er time, giv en our man ually pic ked initial state. Then w e can plot the solution and con vince ourselves w e hav e the correct mo del, if we are lucky to b e typo-free. In the follo wing images, the acrob ot swing from right to left passively . 5.3 T ra jectory Optimization Finally we are here, after all the ugly mathematics and b oring sim ulation. W e ha ve all we need to wirte a piece of code to solve the 3-link acrob ot swing- up problem. W e hav e a dynamics mo del of the system, which we’v e convined ourselv es is correct, and a library to perform tra jectory optimization. No w, we will construct an tra jectory optim ization using the dynamics mo del and thro w it to OptimT ra j. T o use the OptimT ra j library , we need to first construct a function whic h computes the dynamics. Here we ha ve to put the inv erse of the mass matrix on the righ t hand side, like ˙ q = M ass ( q ) − 1 f ( q, u ) , where the inv erse matrix is prov able to exist. The second thing is to supply a cost function, which we will tak e the dot pro duct of the control effort. This will leads to the minimization of con trol effort. Then w e also need to sp ecify some constraint for the optimization problem, including initial state, final state, 10 upp er and low er b ounds of state v ariables and initial guesses. After sp ecifying all these stuff, w e finally can leav e the hard work to computer. The following t wo figures plot the tra jectories of t wo actuated joints(green and blue joints). The v alues of these t wo actuated join ts start from [0 , 0] and end at [ π , 0] . Then, let’s animate the swing up sequence of the 3-link acrob ot. Here are three frames from the animation. 6 Remarks This note present an exp erimen tal study of acrob ot, whic h is a easygoing mem- b er in the family of underactuated rob ots. T o mak e the result mentioned here concrete, stabilit y and controlabilit y analysis are exp ected in the future. References [1] Russ T edrake. Underactuated Rob otics: Algorithms for W alking, Running, Swimming, Flying, and Manipulation (Course Notes for MIT 6.832). Down- loaded on [date] from h ttp://underactuated.mit.edu/ [2] Matthew Peter Kelly . OptimT ra j: T ra jectory Optimization for MA TLAB. h ttps://github.com/MatthewP eterKelly/OptimT ra j 11 [3] F.T opputo, C. Zhang. Survey of Direct T ranscription for Lo w-Thrust Space T ra jectory Optimization with Applications. 12