Notebook

Trajectory Optimization using Direct Collocation¶

CS/ECE/ISyE 524 — Introduction to Optimization — Spring 2018¶

Alex Dawson-Elli
Patrick Dills
Kieran Nichols

Table of Contents¶

Introduction
Mathematical modeling
Exploration of Modeling Assumptions
Experimentation with Full Dynamics
Conclusion
Appendix

Introduction ¶

As Mechanical Engineers, we were inspired to gain new insight into a canonical controls problem though optimization techniques learned in 524. In this report, we develop optimal trajectories for a cart-pole system. The system being modeled consists of a cart and a point mass pendulum. The cart of mass ($m_1$) lays on a frictionless track and can be moved horizontally by control force ($u$). The pendulum with a massless rod (length, $l$) and a bob (mass, $m_2$) is attached to the cart by a frictionless bearing. Because of it's inertia, the force acting on the cart will also cause the pendulum to swing.

[1]

This class of problem, know as trajectory optimization, has its roots in the Brachistochrone problem posed by Johann Bernoulli in 1697.[2] Brachistochrone problem is to find the path of a ball down a slope which reaches the end in the shortest time. The form of the problem is particularly difficult because it requires optimizing over the set of functions as opposed to optimizing over a variable. To solve in closed form, this required the development of calculus of variation, and is impractical for all but the simplest problems. Modern computers have made the solution of more complex problems trajectory optimization problems possible with applications ranging from computing the paths of rockets to walking robots.[3]

The control problem to be solved may be stated as follows: generate a control signal ($u$) that when applied to the system brings it from an initial configuration (or state) $x_0$ to a desired configuration $x_f$, in the prescribed amount of time. There are infinitely many ways to accomplish this task, but not every trajectory is desirable. For reasons such as reducing component wear, reducing power consumption and increasing smoothness of the path, the heuristics such as minimizing total energy and minimizing sum of the squared force have been used [citation]. In this investigation, we will minimize the square of control force ($u^2$)

In this investigation, we first present a mathematical derivation of the cart-pole dynamics, and standard form of our models. In the Exploration of Modeling Assumptions section we develop 3 models of the same system each of which makes different simplifying assumptions. The assumptions make the problem easier to solve - in the first case this is even enough to change the problem type - at the expense of model fidelity. In the results and discussion section we use three test problems to explore where assumptions are valid and where they break down.

In the final section we investigate the ability of the full dynamics model to swing up the pendulum (from non- inverted to inverted) while avoiding obstacles using either explicit (hard) constraints or regularization (soft constraints). We then develop a model of a double pendulum (cart, two rods, and two bobs) and solve swing up for that system.

citations:

[1]Kelly, Matthew. "An Introduction to Trajectory Optimization: How to Do Your Own Direct Collocation." SIAM Review 59.4 (2017): 849-904.

[2]300 Years of Optimal Control: From The Brachystochrone to the Maximum Principle, Hector J. Sussmann and Jan C. Willems. IEEE Control Systems, 1997.

[3]Posa, Michael, and Russ Tedrake. "Direct trajectory optimization of rigid body dynamical systems through contact." Algorithmic Foundations of Robotics X. Springer, Berlin, Heidelberg, 2013. 527-542

Mathematical modeling ¶

Cartesian coordinates of the cart and pole will be \begin{equation} \begin{bmatrix} q_1 \\ 0 \end{bmatrix} \qquad \text{and} \qquad \begin{bmatrix} q_1 + l \sin(q_2) \\ l \cos(q_2)] \end{bmatrix} \end{equation} where ($q_1$) is the horizontal position of the cart and ($q_2$) is the angle of the pendulum.

The state, $x$ and state derivative, $\dot{x}$ will give the dynamics of the system in first-order form and represents the minimal generalized coordinates and its derivatives \begin{equation} x = \begin{bmatrix} q_1 \\ q_2 \\ \dot{q_1} \\ \dot{q_2} \end{bmatrix} \qquad \text{and} \qquad \dot{x} = f ( x , u ) = \begin{bmatrix} \dot{q_1} \\ \dot{q_2} \\ \ddot{q_1} \\ \ddot{q_2} \end{bmatrix} \end{equation}

Using Lagrangian method, $ L = T - V $ Where T is the kinetic energy and V the potential energy of the system. L summarizes the system dynamics.

\begin{align} & T = T_{cart} + T_{pole} \\ &T_{cart} = \frac{1}{2} m_1\dot{ q_1}^2 \\ &T_{pole} = \frac{1}{2} m_2 [\dot{(q_1 + l \sin(q_2)}^2 +\dot{(l\cos(q_2)}^2] \\\\ & V = V_{cart} + V_{pole} \\ &V = 0 + m_2 g l \cos(q_2) \\ \end{align}

The potential energy of the cart is zero since it is not moving vertically and it has a height of 0.

Thus $L = \frac{1}{2} m_1 q_1^2 + \frac{1}{2} m_2 [\dot{(x + l \sin(q_2)}^2 +\dot{(l\cos(q_2)}^2] - m_2 g l \cos(q_2)$

The generalized coordinates could be defined as \begin{equation} X = \begin{bmatrix} q_1 \\ q_2 \end{bmatrix} \end{equation} To yield the equations of motion for the cart/pendulum system, the Lagrangian equations will: \begin{align} &\frac{d}{dt}(\frac{\partial L}{\partial{\dot{q_1}}}) - \frac{\partial L}{\partial{\dot{q_1}}} = u \\ &\frac{d}{dt}(\frac{\partial L}{\partial{\dot{q_2}}}) - \frac{\partial L}{\partial{\dot{q_2}}} = 0 \end{align}

Solving for the partial and time derivatives will yield \begin{align} &(m_1 + m_2) \ddot{q_1} + m_1 l \ddot{q_2} \cos(q_2) - m_2 l \dot{q_2}^2 \sin(q_2) = u \\ &m_2 l \ddot{q_1} \cos(q_2) + m_2 l^2 \ddot{q_2} - m_2 g l \sin(q_2) = 0 \end{align} The detailed derivations of the derivatives can be found on this <a href="http://www.aoengr.com/Dynamics/PendulumOnCart.pdf%5C%22%3Ewebisite.

Rearranging the above equations and solving for the $\ddot{q_1}$ and $\ddot{q_2} $ terms respectively, \begin{align} &\ddot{q_1} = \frac{ l m_2 \sin(q_2) \dot{q_2}^2 + u + m_{2} g \cos(q_{2}) sin(q_2)} {m_1 + m_2\sin^2(q_2)} \\ &\ddot{q_2} = \frac{ l m_2 \cos(q_2) \sin(q_2^2) \dot{q_2}^2 + u \cos(q_2) + (m_1 + m_2) g \sin(q_2)} {l (m_1 + m_2\sin^2(q_2))} \end{align}

The objective function will be the continuous integral of the actuator effort (control force) squared: \begin{equation} \min_{u(t)} \int_{t_{initial}}^{t_{final}} u^2 ( t ) d t \end{equation}

The above objective function is a problem with vector functions as decision variables subject to differential equations as constraints. A Transcription process will be used in this project to create a constrained parameter optimization with real numbers as the decision variables that will contain algebraic equations. The continuous time and differential equations will be converted to discrete sets of real numbers by direct collocation where time will be broken into a number of grid points (size N).

\begin{align} % & k \in \{1,\dots,N\}\\ &t_k = {t_0, \dots, t_N}\\ & x_k = x(t_k)\\ & \dot{x_k} = \dot{x}(t_k)\\ & u_k = u(t_k)\\ & h_k = t_{n+1} - t_n \end{align}

To estimate future state and state derivatives ($x_k$ and $\dot{x_k}$), a trapezoidal method will be used where

\begin{align} &\int_{t_{initial}}^{t_{final}} x ( t ) dt \approx \sum_{k = 0}^{N-1} \frac{h_k}{2} (\dot{x_k} + \dot{x_{k+1}}) \end{align}

Additionally, state path constraints were included to ensure that the states were close to the expected trajectory. \begin{align} & {x}_{min} <= {x}_{k} <= {x}_{max}\\ \end{align}

With the direct collocation, the objective function will be the sum of the control force from initial to final step. \begin{equation} \min_{u(t)} \int_{t_{initial}}^{t_{final}} u^2 ( t ) dt \approx \min_{u(k)} \sum_{k = 0}^{N-1} u_k^2 \end{equation}

Restating this optimization problem in discrete standard form will yield \begin{align} \underset{u_k \in {R^2}}{\text{minimize}}\qquad& \sum_{k=1}^{N} u_k^2 \\ \text{subject to:}\qquad & {x_{1}} - {x}_{initial} = 0 && \\ & {\dot{x_{k}}} - {\dot{x}}_{final} = 0 && \\ & {x}_{k} - {x}_{max} <= 0 && \forall k \in \{1,\dots,N\}\\ & -{x}_{k} + {x}_{min} <= 0 && \forall k \in \{1,\dots,N\}\\ & {x}_{k+1} - {x}_{k} - 0.5 h ({\dot x}_{k} +{\dot x}_{k+1}) = 0 && \forall k \in \{1,\dots,N-1\}\\ % & \mathbf{\dot{x}}_{k+1} - \mathbf{\dot{x}}_{k} - % = 0 &&\forall k \in \{1,\dots,k-1\}\\ \end{align}

Exploration of Modeling Assumptions ¶

Small Angle Assumption and quazi-static assumptions ¶

This first section is the simplist of the models with linear dynamic constraints to solve the swing up problem. The assumptions include the small angle approximation and quazi-static conditions.

The Small Angle Assumption: $$ cos(q_2) \approx 1 $$ $$ sin(q_2) \approx q_2 $$

This simplification removes the trigonometric functions from the dynamic constraints and is usually considered valid for angles of less than 10 degrees about the angle of linearization.

The quazi-static term $$ \dot q_2^2 \approx 0 $$

The pendulum is assumed to be moving with a small velocity so when that small velocity is sqared, the resulting value is approximately $0$.

The simplified and linear dynamic constraints for the acceleration terms of the cart and pendulum for the optimization are:

\begin{align} & \ddot q_1 = \frac{lm_2sin(q_2)\dot q_2^2 + u + m_2gcos(q_2)sin(q_2)}{m_1 + m_2(1-cos^2(q_2))} \approx \frac{u + m_2gq_2}{m_1} \\ & \ddot q_2 = - \frac{lm_2cos(q_2)sin(q_2)\dot q_2^2 + ucos(q_2) + (m_1+m_2)gsin(q_2)}{lm_1 + lm_2(1-cos^2(q_2))} \approx - \frac{u + (m_1+m_2)gq_2}{lm_1} \end{align}

Standard Form¶

\begin{aligned} \underset{\mathbf{x},\mathbf{\dot x},\mathbf{u}_i \in \mathbb{R^2}}{\text{minimize}}\qquad& \sum_{i=1}^{n}u_i^2\\ \text{subject to:}\qquad & {x_{1}} - {x}_{initial} = 0 \\ & {\dot{x_{k}}} - {\dot{x}}_{final} = 0 && \\ & {x}_{k} - {x}_{max} <= 0 && \forall k \in \{1,\dots,N\}\\ & -{x}_{k} + {x}_{min} <= 0 && \forall k \in \{1,\dots,N\}\\ & \ddot q_{1k} - \frac{u_k + m_2gq_{2k}}{m_1} = 0 && \forall k \in \{1,\dots,N\}\\ & \ddot q_{2k} + \frac{u_k + (m_1+m_2)gq_{2k}}{lm_1} = 0 && \forall k \in \{1,\dots,N\}\\ & {x}_{k+1} - {x}_{k} - 0.5 h ({\dot x}_{k} +{\dot x}_{k+1}) = 0 && \forall k \in \{1,\dots,N-1\}\\ & \text{where:}\\ & \begin{equation} x = \begin{bmatrix} q_1 \\ q_2 \\ \dot{q_1} \\ \dot{q_2} \end{bmatrix} \qquad \text{and} \qquad \dot{x} = f ( x , u ) = \begin{bmatrix} \dot{q_1} \\ \dot{q_2} \\ \ddot{q_1} \\ \ddot{q_2} \end{bmatrix} \end{equation} \end{aligned}

All of the constraints are linear. However, the quadratic solver Gurobi will be used since the objective function is quadratic.

In [191]:

using JuMP, Gurobi
"""
linearize the dynamics of the cart pull mechanism through the small angle assumption -> first term
in taylor series expansion for sin(q) and cos(q) and quazi-static assumption

inputs:
    x0 = [q₁ q₂ q₁dot q₂dot]'      #initial system state
    xf = [q₁ q₂ q₁dot q₂dot]'      #final system state
    tf = [sec]                     #time to get to final state
    h =  [sec]                     #time between collocation points

outputs:
    path = [4xk]                   #state matrix that specifies path 
"""
function SolvLinearizedCartPole(x0, xf, tf, h , q2const = false, itype = "Trap")
    
    #define model
    m = Model(solver = GurobiSolver(OutputFlag=0))
    T = 0:h:tf                   #time vector
    K = length(T)                 #number of collocation points
    @variable(m,    x[1:4,1:K])   #system state at discrete times
    @variable(m, xdot[1:4,1:K])   #derivative of system state at discrete times
    @variable(m,    u[1:1,1:K])   #control input to system along q1

    #boundry constraints - initial and final state
    @constraint(m, x[:,1] .== x0)
    @constraint(m, x[:,K] .== xf)
    
    #path constraints
    #q₂ limit
    if q2const
        for k in 1:K
            @constraint(m, x[2,k] >= xf[2,1] - pi/4)
            @constraint(m, x[2,k] <= xf[2,1] + pi/4)
        end
    end
    
    
    #q₁ limits
#     for k in 1:K
#         @constraint(m, x[1,k] >= -10)
#         @constraint(m, x[1,k] <=  10)
#     end
        
    #dynamics (path) constraints, integral form
    #model constants
    l = 1         #length of arm
    m1 = 3        #mass of cart
    m2 = 1        #mass of pendulum point mass
    g = 9.81
    linAngle = 0 #the q2 angle we linearize the system about 
    
    
    #setup the dynamics constraints linearized about q1 = 0
    if linAngle == 0
        for k in 1:K
            #notes - linearized system with first term in taylor series, and neglected coreolis acceleration
            @constraint(m,xdot[1,k] == x[3,k])
            @constraint(m,xdot[2,k] == x[4,k])
            @constraint(m,xdot[3,k] == u[1,k] + m2*g*1*x[2,k] / m1) 
            @constraint(m,xdot[4,k] == -u[1,k]  -(m1 + m2)*g*x[2,k] / m1) 

        end
    end
    
    if linAngle == pi
        for k in 1:K
            #notes - linearized system with first term in taylor series, and neglected coreolis acceleration
            @constraint(m,xdot[1,k] == x[3,k])
            @constraint(m,xdot[2,k] == x[4,k])
            @constraint(m,xdot[3,k] == u[1,k]  + m2*g*1*x[2,k] / m1) 
            @constraint(m,xdot[4,k] == u[1,k]  + (m1 + m2)*g*x[2,k] / m1) 

        end
    end

    if itype == "forwardEuler" 
        #add forward euler dynamics constraint
        for k in 1:K-1
            @constraint(m, x[:,k+1] .== x[:,k] + h*xdot[:,k])
        end
        
    elseif itype == "Trap"
        for k in 1:K-1
             #add backwards euler dynamics constraint
            @constraint(m, x[:,k+1] .== x[:,k] + .5*h*(xdot[:,k] + xdot[:,k+1]))
        end     
    end
    
    
    # minimize 2-norm (THIS IS LEAST-SQUARES)
    @objective(m, Min, sum(u.^2) ) #+ sum(x[2,:]).^2))
    solve(m)
    #print(m)
    
    control = getvalue(u)
    xopt = getvalue(x)
    xdotopt = getvalue(xdot)
    
    return (control, xopt,xdotopt)
end

Out[191]:

SolvLinearizedCartPole

Small Angle Assumption ¶

In this section the complexity of dynamic constraints are increased in an attempt to obtain a more accurate solution to the swing up problem. The small angle assumption is still made to simplify the model. However, the effect of centripetal acceleration on the optimization is included.

Small Angle Assumption: $$ cos(q_2) \approx 1 $$ $$ sin(q_2) \approx q_2 $$

Centripetal Acceleration: $$ \alpha = lm_2\dot q_2^2 $$

This simplification removes the trigonometric functions from the dynamic constraints and is usually considered valid for angles of less than 10 degrees about the angle of linearization.

The simplified dynamic constraints for the optimization are: $$ \ddot q_1 = \frac{lm_2sin(q_2)\dot q_2^2 + u_k + m_2gcos(q_2)sin(q_2)}{m_1 + m_2(1-cos^2(q_2))} \approx \frac{lm_2q_2\dot q_2^2 + u + m_2gq_2}{m_1} $$ $$ \ddot q_2 = - \frac{lm_2cos(q_2)sin(q_2)\dot q_2^2 + u_kcos(q_2) + (m_1+m_2)gsin(q_2)}{lm_1 + lm_2(1-cos^2(q_2))} \approx - \frac{lm_2q_2\dot q_2^2 + u + (m_1+m_2)gq_2}{lm_1} $$

Both sets of constraints are non-linear and are not able to be expressed in a quadratic or second order cone form because of centripetal acceleration terms, $lm_2q_2\dot q_2^2$, in each of the equations. This forces us to approach the optimization problem with a nonlinear solver like "Ipopt".

Standard Form¶

\begin{aligned} \underset{\mathbf{x},\mathbf{\dot x},\mathbf{u}_i \in \mathbb{R^2}}{\text{minimize}}\qquad& \sum_{i=1}^{n}u_i^2\\ \text{subject to:}\qquad & {x_{1}} - {x}_{initial} = 0 \\ & {\dot{x_{k}}} - {\dot{x}}_{final} = 0 && \\ & {x}_{k} - {x}_{max} <= 0 && \forall k \in \{1,\dots,N\}\\ & -{x}_{k} + {x}_{min} <= 0 && \forall k \in \{1,\dots,N\}\\ & \ddot q_{1k} - \frac{lm_2q_{2k}\dot q_{2k}^2 + u + m_2gq_{2k}}{m_1} = 0 && \forall k \in \{1,\dots,N\}\\ & \ddot q_{2k} + \frac{lm_2q_{2k}\dot q_{2k}^2 + u + (m_1+m_2)gq_{2k}}{lm_1} = 0 && \forall k \in \{1,\dots,N\}\\ & {x}_{k+1} - {x}_{k} - 0.5 h ({\dot x}_{k} +{\dot x}_{k+1}) = 0 && \forall k \in \{1,\dots,N-1\}\\ & \text{where:}\\ & \begin{equation} x = \begin{bmatrix} q_1 \\ q_2 \\ \dot{q_1} \\ \dot{q_2} \end{bmatrix} \qquad \text{and} \qquad \dot{x} = f ( x , u ) = \begin{bmatrix} \dot{q_1} \\ \dot{q_2} \\ \ddot{q_1} \\ \ddot{q_2} \end{bmatrix} \end{equation} \end{aligned}

In [188]:

using JuMP, Ipopt

"""
simplify the dynamics of the cart pull mechanism through the small angle assumption -> first term
in taylor series expansion for sin(q) and cos(q)

inputs:
    x0 = [q₁ q₂ q₁dot q₂dot]'      #initial system state
    xf = [q₁ q₂ q₁dot q₂dot]'      #final system state
    tf = [sec]                     #time to get to final state
    h =  [sec]                     #time between collocation points

outputs:
    path = [4xk]                   #state matrix that specifies path 
"""
function SolvNonStaticCartPole(x0, xf, tf, h ; q2const = false,  itype = "Trap")
    
    #define model
    #m = Model(solver = GurobiSolver(OutputFlag=0))
    m = Model(solver = IpoptSolver(print_level=0))
    T = 0:h:tf                    #time vector
    K = length(T)                 #number of collocation points
    @variable(m,    x[1:4,1:K])   #system state at discrete times
    @variable(m, xdot[1:4,1:K])   #derivative of system state at discrete times
    @variable(m,    u[1:1,1:K])   #control input to system along q1

    #boundry constraints - initial and final state
    @constraint(m, x[:,1] .== x0)
    @constraint(m, x[:,K] .== xf)
    
    
    #path constraints
    #q₂ limit
    if q2const
        for k in 1:K
            @constraint(m, x[2,k] >= xf[2,1] - pi/4)
            @constraint(m, x[2,k] <= xf[2,1] + pi/4)
        end
    end
    
    #q₁ limits
    for k in 1:K
        @constraint(m, x[1,k] >= -4)
        @constraint(m, x[1,k] <=  4)
    end
        
    
    #dynamics (path) constraints, integral form
    #model constants
    l = 1         #length of arm
    m1 = 3        #mass of cart
    m2 = 1        #mass of pendulum point mass
    g = 9.81
    linAngle = pi
    
    #setup the dynamics constraints linearized about q1 = 0
    if linAngle == 0
        for k in 1:K
            #notes - linearized system with first term in taylor series
            @constraint(m,xdot[1,k] == x[3,k])
            @constraint(m,xdot[2,k] == x[4,k])
            
            q1 = x[1,k] ; q2 = x[2,k] ; q1dot = x[3,k] ; q2dot = x[4,k]
            @NLconstraint(m,xdot[3,k] == (l*m2*q2*q2dot^2 + u[1,k] + m2*g*1*q2) / m1)
            @NLconstraint(m,xdot[4,k] == -(l*m2*1*q2*q2dot^2 + u[1,k]*1 + (m1 + m2)*g*q2) / l*m1) 
        end
    end
    
    if linAngle == pi
        for k in 1:K
            #notes - linearized system with first term in taylor series
            @constraint(m,xdot[1,k] == x[3,k])
            @constraint(m,xdot[2,k] == x[4,k])
            
            q1 = x[1,k] ; q2 = x[2,k] ; q1dot = x[3,k] ; q2dot = x[4,k]
            @NLconstraint(m,xdot[3,k] == (l*m2*q2*q2dot^2 + u[1,k] + m2*g*1*q2) / m1)
            @NLconstraint(m,xdot[4,k] == -(l*m2*1*q2*q2dot^2 - u[1,k]*1 -(m1 + m2)*g*q2) / l*m1) 

        end
    end
    
    if itype == "forwardEuler" 
        #add forward euler dynamics constraint
        for k in 1:K-1
            @constraint(m, x[:,k+1] .== x[:,k] + h*xdot[:,k])
        end
        
    elseif itype == "Trap"
        #add backwards euler dynamics constraint
        for k in 1:K-1
            @constraint(m, x[:,k+1] .== x[:,k] + .5*h*(xdot[:,k] + xdot[:,k+1]))
        end     
    end
    
    # minimize 2-norm (THIS IS LEAST-SQUARES)
    #@objective(m, Min, .5*h*sum(u[1:end-1].^2 + u[2:end].^2))
    @objective(m, Min, sum(u.^2)) 
    solve(m)
    
    control = getvalue(u)
    xopt = getvalue(x)
    xdotopt = getvalue(xdot)
    
    return (control, xopt, xdotopt)
end

Out[188]:

SolvNonStaticCartPole

Full Dynamics ¶

The full dynamic model for a single pendulum is used in this section. There are no simplifying assumptions made to simplify the constraints and a nonlinear solver with nonlinear constraints are necessary.

Standard Form¶

\begin{aligned} \underset{\mathbf{x},\mathbf{\dot x},\mathbf{u}_i \in \mathbb{R^2}}{\text{minimize}}\qquad& \sum_{i=1}^{n}u_i^2\\ \text{subject to:}\qquad & {x_{1}} - {x}_{initial} = 0 \\ & {\dot{x_{k}}} - {\dot{x}}_{final} = 0 && \\ & {x}_{k} - {x}_{max} <= 0 && \forall k \in \{1,\dots,N\}\\ & -{x}_{k} + {x}_{min} <= 0 && \forall k \in \{1,\dots,N\}\\ & \ddot q_{1k} - \frac{lm_2sin(q_{2k})\dot q_{k2}^2 + u_k + m_2gcos(q_{2k})sin(q_{2k})}{m_1 + m_2(1-cos^2(q_{2k}))} = 0 && \forall k \in \{1,\dots,N\}\\ & \ddot q_{2k} + \frac{lm_2cos(q_{2k})sin(q_{2k})\dot q_{2k}^2 + u_kcos(q_{2k}) + (m_1+m_2)gsin(q_{2k})}{lm_1 + lm_2(1-cos^2(q_{2k}))} = 0 && \forall k \in \{1,\dots,N\}\\ & {x}_{k+1} - {x}_{k} - 0.5 h ({\dot x}_{k} +{\dot x}_{k+1}) = 0 && \forall k \in \{1,\dots,N-1\}\\ & \text{where:}\\ & \begin{equation} x = \begin{bmatrix} q_1 \\ q_2 \\ \dot{q_1} \\ \dot{q_2} \end{bmatrix} \qquad \text{and} \qquad \dot{x} = f ( x , u ) = \begin{bmatrix} \dot{q_1} \\ \dot{q_2} \\ \ddot{q_1} \\ \ddot{q_2} \end{bmatrix} \end{equation} \end{aligned}

In [179]:

using JuMP, PyPlot, Ipopt

#define initial and final states, x
x0 = [0 0 0 0]' 
xf = [3 pi 0 0]'   #wind up at the same angle, but 

"""
solve the cart pole problem with and full dynamics

inputs:
    x0 = [q₁ q₂ q₁dot q₂dot]'      #initial system state
    xf = [q₁ q₂ q₁dot q₂dot]'      #final system state
    tf = [sec]                     #time to get to final state
    h =  [sec]                     #time between collocation points

outputs:
    path = [4xk]                   #state matrix that specifies path 
"""
function SolvFullCartPole(x0, xf, tf, h; q2const = false, xStart="None" , itype="Trap")
    
    #define model
    m = Model(solver = IpoptSolver(print_level=0))
    T = 0:h:tf                   #time vector
    K = length(T)                 #number of collocation points
    @variable(m, x[1:4,1:K])   #system state at discrete times
    @variable(m, xdot[1:4,1:K])   #derivative of system state at discrete times
    @variable(m,    u[1:1,1:K])   #control input to system along q1
    if xStart != "None"  setvalue(x[:,:] , xStart) end

    #boundry constraints - initial and final state
    @constraint(m, x[:,1] .== x0)
    @constraint(m, x[:,K] .== xf)

    #path constraints
    if q2const
        for k in 1:K
            @constraint(m, x[2,k] >= xf[2,1] - pi/4)
            @constraint(m, x[2,k] <= xf[2,1] + pi/4)
        end
   end
    
      for k in 1:K
        @constraint(m, x[1,k] >= -10)
        @constraint(m, x[1,k] <=  10)
    end
    
    #model constants
    l = 1         #length of arm
    m1 = 3        #mass of cart
    m2 = 1        #mass of pendulum point mass
    g = 9.81
    
    
    #setup the full dynamics constraints
    for k in 1:K
        @constraint(m,xdot[1,k] == x[3,k])                      
        @constraint(m,xdot[2,k] == x[4,k])                         
        q1 = x[1,k] ; q2 = x[2,k] ; q1dot = x[3,k] ; q2dot = x[4,k]
        @NLconstraint(m,xdot[3,k] ==  (l*m2*sin(q2)*q2dot^2 + u[1,k] + m2*g*cos(q2)*sin(q2)) /
                                    (m1 + m2*(1-cos(q2)^2)))
        @NLconstraint(m,xdot[4,k] == -(l*m2*cos(q2)*sin(q2)*q2dot^2 + u[1,k]*cos(q2) + (m1 + m2)*g*sin(q2))/
                                    (l*m1 + l*m2*(1-cos(q2)^2)))
    end
    

    if itype == "forwardEuler" 
        #add forward euler dynamics constraint
        for k in 1:K-1
            @constraint(m, x[:,k+1] .== x[:,k] + h*xdot[:,k])
        end
        
    elseif itype == "Trap"
        for k in 1:K-1
             #add backwards euler dynamics constraint
            @constraint(m, x[:,k+1] .== x[:,k] + .5*h*(xdot[:,k] + xdot[:,k+1]))
        end     
    end

    # minimize 2-norm (THIS IS LEAST-SQUARES)
    #@objective(m, Min, .5*h*sum(u[1:end-1].^2 + u[2:end].^2))
    @objective(m, Min, sum(u.^2) ) # + 10000*sum(x[2,:] - pi).^2)
    solve(m)
    #print(m)
    
    control = getvalue(u)
    xopt = getvalue(x)
    xdotopt = getvalue(xdot)
    
    return (control, xopt, xdotopt)
end

Out[179]:

SolvFullCartPole

In [104]:

using PyPlot

# Plotting the output of the Optimization in q coords and effort
function plotq2(x,h,tf)
    #figure(figsize=(12,4))
    title("q_2 trajectory")
    xlabel("time[sec]")
    ylabel("Angle [rad]")
    t = 0:h:tf
    plot( t, x[2,:], markersize = 2)
end

function plotq1(x,h,tf)
    title("q_1 trajectory")
    xlabel("time[sec]")
    ylabel("Distance [m]")
    t = 0:h:tf
    plot( t, x[1,:], markersize = 2)
end

function plotu(u,h,tf)
    title("Optimization Output Effort")
    xlabel("time[sec]")
    ylabel("Force [N]")
    plot(u[1,:])
end

# plotting the cart and pendulum position at each collocation point
function plotTipTraj(x)
    #Transform to x and y coordinates of the mass location
    l = 1
    K = size(x)[2]
    xCordM = zeros(1,K)
    yCordM = zeros(1,K)
    
    xCordM = x[1,:] + l*sin.(x[2,:])
    yCordM = -l*cos.(x[2,:])
    
    title("trajectory of tip in xy plane ")
    xlabel("Meters")
    ylabel("Meters")
    plot( xCordM, yCordM,".", markersize = 2)
end

# Plotting a stem plot of the cart and pendulum position at each colocation point
function plotXY(x)
    l = 1
    K = size(x)[2]
    
    # Transform to x and y coordinates of the tip location
    xCordM = zeros(1,K)
    yCordM = zeros(1,K)
    xCordM = x[1,:] + l*sin.(x[2,:])
    yCordM = -l*cos.(x[2,:])
    
    # Transform to x and y coordinates of the Cart
    xCordC = zeros(1,K)
    yCordC = zeros(1,K)
    xCordC = x[1,:]'
    
    #figure(figsize=(12,4))
    title("XY Transform of Colocation Results ")
    xlabel("Meters")
    ylabel("Meters")
    plot( xCordM, yCordM,".", markersize = 20);
    plot( xCordC, yCordC,"rs", markersize = 15);
    #legend(["Mass Location","Cart Location"])
    for i =1:length(xCordM)
     plot( [xCordM[i],xCordC[i]] , [yCordM[i], yCordC[i]],"k");
    end
end

function plotContour(xoff,yoff)
    xv = linspace(-6,4,1000)
    yv = linspace(-1.1,1.1,1000)
    function meshgrid{T}(vx::AbstractVector{T}, vy::AbstractVector{T})
        m, n = length(vy), length(vx)
        vx = reshape(vx, 1, n)
        vy = reshape(vy, m, 1)
        (repmat(vx, m, 1), repmat(vy, 1, n))
    end
    (X,Y) = meshgrid(xv,yv);

    z = [1/(sqrt(2*pi*sig^2))*exp(-((x-xoff)^2 + (y-yoff)^2)/(2*sig^2)) for y in yv, x in xv]
    contour(X, Y, z, zdir="z", 60, offset=0, linewidths=.5 );
end

# Plotting the Ellipse Constraining the motion of the pendulum
function plotEllipse(a,b,c,xoff,yoff) 
    angle = linspace(0,2*3.14595,360);
    r = zeros(1,length(angle));
    xE = zeros(1,length(angle));
    yE = zeros(1,length(angle));
    for i = 1:length(angle);
        r[i] = a*b/sqrt(b^2*cos(angle[i])^2 + a^2*sin(angle[i])^2);
        xE[i] = r[i]*cos(angle[i]) + xoff;
        yE[i] = r[i]*sin(angle[i]) + yoff;
    end
    plot(xE,yE,"g.--");
end

Out[104]:

plotEllipse (generic function with 1 method)

Test Case 1) $x_0 = [0, 0, 0, 0]'$ ; $x_f = [3, 0, 0, 0]'$¶

visualization of test case 1 performed on full dynamics model

In this test case, the system is moving about a stable equilibrium point at $q_2 = 0$, meaning that any displacement in $q_2$ results in a restoring torque due to gravity. Because the total time execution is long for the given distance (3meters), average velocity of the cart is kept low, and angular deviation of $q_2$ is small. Also, angular velocity of q2 is kept at a minimum. This leads us to believe that the linearized model, which makes both small angle and quazi-static assumptions, and the intermediate model, which makes small angle assumptions, will produce accurate results for this test case.

In [185]:

#define initial and final states
x0 = [0 0 0 0]' 
xf = [3 0 0 0]' 
tf = 3 ; h = .01

#evaluate each model
u1,x1,xdot1 =  SolvLinearizedCartPole(x0, xf, tf, h ; q2const = false , itype = "Trap")
u2,x2,xdot2 =  SolvNonStaticCartPole(x0, xf, tf, h ; q2const = false,  itype = "Trap")
u3,x3,xdot3 =  SolvFullCartPole(x0, xf, tf, h;  q2const = false, xStart="None" , itype= "Trap");

Academic license - for non-commercial use only

In [186]:

#plot the resulting tip trajectory
figure(figsize=(12,4))
plotTipTraj(x1)
plotTipTraj(x2)
plotTipTraj(x3)
legend(["small angle + quazi-static","small angle", "Full Dynamics" ])

Out[186]:

PyObject <matplotlib.legend.Legend object at 0x00000000A9F4BE10>

In [253]:

#plot q1 vs. time
figure(figsize=(12,4)) 
plotq1(x1,h,tf)
plotq1(x2,h,tf)
plotq1(x3,h,tf)
legend(["small angle + quazi-static","small angle", "Full Dynamics" ])

Out[253]:

PyObject <matplotlib.legend.Legend object at 0x0000000053BC4DD8>

In [254]:

figure(figsize=(12,4)) 
plotq2(x1,h,tf)
plotq2(x2,h,tf)
plotq2(x3,h,tf)
legend(["small angle + quazi-static","small angle", "Full Dynamics" ])

Out[254]:

PyObject <matplotlib.legend.Legend object at 0x00000000548853C8>

In [255]:

figure(figsize=(12,4)) 
plotu(u1)
plotu(u2)
plotu(u3)
legend(["small angle + quazi-static","small angle", "Full Dynamics" ])

Out[255]:

PyObject <matplotlib.legend.Legend object at 0x0000000056681F98>

Test Case 2) $x_0 = [0, \pi, 0, 0]'$ ; $x_f = [3, \pi, 0, 0]'$¶

visualization of test case 2 performed on full dynamics model

In this test case, the system is moving about a unstable equilibrium point at $q_2 = \pi$, meaning that any displacement in $q_2$ results in a restoring torque due to gravity. Because the total time execution is long for the given distance (3meters), average velocity of the cart kept low, and angular deviation of $q_2$ is small. In addition, the angular velocity of q2 is kept at a minimum. This leads to the prediction that the linearized model, which makes both small angle and quazi-static assumptions, and the intermediate model, with small angle assumptions, should produce accurate results for this test case.

We tested a constrained and unconstrained version of this problem. All three cases were solved in the unconstrained case. However only the full dynamics were able to be solved in the constrained case shown above.

Unconstrained¶

In [206]:

#define initial and final states
x0 = [0 pi 0 0]' 
xf = [3 pi 0 0]' 
tf = 3 ; h = .01

#evaluate each model
u1,x1,xdot1 =  SolvLinearizedCartPole(x0, xf, tf, h ; q2const = false , itype = "Trap")
u2,x2,xdot2 =  SolvNonStaticCartPole(x0, xf, tf, h ; q2const = false,  itype = "Trap")
u3,x3,xdot3 =  SolvFullCartPole(x0, xf, tf, h;  q2const = false, xStart="None" , itype= "Trap");

Academic license - for non-commercial use only

In [216]:

#plot q1 vs. time
figure(figsize=(12,4)) 
plotq1(x1,h,tf)
plotq1(x2,h,tf)
plotq1(x3,h,tf)
legend(["small angle + quazi-static","small angle", "Full Dynamics" ])
figure(figsize=(12,4)) 
plotq2(x1,h,tf)
plotq2(x2,h,tf)
plotq2(x3,h,tf)
legend(["small angle + quazi-static","small angle", "Full Dynamics" ])

Out[216]:

PyObject <matplotlib.legend.Legend object at 0x00000000A62ED080>

In [215]:

figure(figsize=(12,4))
plotXY(x1)
legend(["Linearized and Quasistatic Model"])
figure(figsize=(12,4))
plotXY(x2)
legend(["Linearized Model"])
figure(figsize=(12,4))
plotXY(x3)
legend(["Full Dynamics"])

Out[215]:

PyObject <matplotlib.legend.Legend object at 0x00000000AFD74A90>

Constrained¶

In [217]:

#define initial and final states
x0 = [0 pi 0 0]' 
xf = [3 pi 0 0]' 
tf = 3 ; h = .01

#Evaluate the Constrained Inverted Pendulem
u3,x3,xdot3 =  SolvFullCartPole(x0, xf, tf, h;  q2const = true, xStart="None" , itype= "Trap");

In [219]:

#plot q1 vs. time
figure(figsize=(12,4)) 
plotq1(x3,h,tf)
legend(["Full Dynamics" ])
figure(figsize=(12,4)) 
plotq2(x3,h,tf)
legend(["Full Dynamics" ])
figure(figsize=(12,4))
plotXY(x3)
legend(["Full Dynamics"])

Out[219]:

PyObject <matplotlib.legend.Legend object at 0x00000000AEC22DD8>

Test Case 3) $x_0 = [0, 0, 0, 0]'$ ; $x_f = [3, \pi, 0, 0]'$¶

visualization of test case 3 performed on full dynamics model

In the final test case, the system is asked to perform swing-up. Swing-up requires both large angular deviations, and fast angular velocities of $ q_2 $. This violates both the small angle and quazi-static assumptions of the simplified models.

In [220]:

#define initial and final states
x0 = [0 0 0 0]' 
xf = [3 pi 0 0]' 
tf = 3 ; h = .01

#evaluate each model
u1,x1,xdot1 =  SolvLinearizedCartPole(x0, xf, tf, h ; q2const = false , itype = "Trap")
u2,x2,xdot2 =  SolvNonStaticCartPole(x0, xf, tf, h ; q2const = false,  itype = "Trap")
u3,x3,xdot3 =  SolvFullCartPole(x0, xf, tf, h;  q2const = false, xStart="None" , itype= "Trap");

Academic license - for non-commercial use only

In [221]:

#plot the resulting tip trajectory
figure(figsize=(12,4))
plotTipTraj(x1)
plotTipTraj(x2)
plotTipTraj(x3)
legend(["small angle + quazi-static","small angle", "Full Dynamics" ]);

In [227]:

#plot q1 vs. time
figure(figsize=(12,4)) 
plotq1(x1,h,tf)
plotq1(x2,h,tf)
plotq1(x3,h,tf)
legend(["small angle + quazi-static","small angle", "Full Dynamics" ]);

Out[227]:

PyObject <matplotlib.legend.Legend object at 0x000000004945BFD0>

In [228]:

figure(figsize=(12,4)) 
plotq2(x1,h,tf)
plotq2(x2,h,tf)
plotq2(x3,h,tf)
legend(["small angle + quazi-static","small angle", "Full Dynamics" ])

Out[228]:

PyObject <matplotlib.legend.Legend object at 0x00000000494E6E10>

In [229]:

figure(figsize=(12,4)) 
plotu(u1)
plotu(u2)
plotu(u3)
legend(["small angle + quazi-static","small angle", "Full Dynamics" ])

Out[229]:

PyObject <matplotlib.legend.Legend object at 0x000000004A24E9E8>

Results and Discussion ¶

The main limitations of our models come from the simplifying assumptions made in each function. The small angle assumption produces poor and inaccurate results in the "inverted pendulum" and "swing up cases". The variation in the angle $q_2$ deviates too far from the angle of linearization. As a result, trajectories become chaotic. The Quasi-static assumption produces poor results in the unconstrained inverted pendulum and swing up case, because the velocity of the pendulum is large and has a significant effect on the dynamics of the system. Other modeling simplifications include neglecting friction, numerical integration, and rigid body or point mass assumptions. However, without physical testing it is impossible to discriminate the impact of these model simplifications.

Despite the limitations of our model we were able to obtain reasonably consistent results from all the models for "Test Case 1". In this case all of the models produced a trajectory utilizing a similar magnitude control signal. The linear and quasi-static model produced a nearly identical mass trajectory as the full dynamic model with a much shorter solution time. This result shows the value of the linearized and quasi-static solution as long as it is utilized where its assumptions are valid. Very similar results can be obtained without the penalty in solution time generated with the use of a nonlinear solver like Ipopt.

"Test Case 2" and "Test Case 3" demonstrate the importance of the full dynamic system. As expected, neither of the simplified models are able to produce convincing results when modeling assumptions are violated. The unconstrained inverted pendulum plots and the swing-up plots show this nicely. Furthermore, including centripetal acceleration may produce the worst model. The solutions do not seem to be as accurate as the full dynamic model and a nonlinear solver must be used to solve the optimization problem. With that said there are degrees to the non-linearity of a problem. The nonlinear but simplified model solved faster than the full dynamic model.

Our full dynamic model produced the most believable results. We recognize that even our most complex model may not fully characterize the actual cart pendulum system due to our previously listed limitations. We propose that future studies implement the optimized control force found in our results into an actual system to investigate the model's ability to estimate future system states and highlight the potential negative effects of the limitations.

Experimentation with Full Dynamics ¶

The full dynamic model is the most accurate model in the widest range of circumstances at the cost of solution time. It is interesting to see how it behaves in different circumstances. For example if the pendulum had to avoid an obstacle during swing up.

Obstacle Avoidance With Hard Constraints ¶

One approach to obstacle avoidance is to use explicit constraints. The approach constrains the solution to a space outside an ellipse which is placed around the object. The quadratic constraint is implemented for each of the colocation points constraining the mass of the pendulum from entering the designated area at any point in time.

The form of the constraint implemented at each colocation point is:

Standard Form¶

\begin{aligned} \underset{\mathbf{x},\mathbf{\dot x},\mathbf{u}_i \in \mathbb{R^2}}{\text{minimize}}\qquad& \sum_{i=1}^{n}u_i^2\\ \text{subject to:}\qquad & {x_{1}} - {x}_{initial} = 0 \\ & {\dot{x_{k}}} - {\dot{x}}_{final} = 0 && \\ & {x}_{k} - {x}_{max} <= 0 && \forall k \in \{1,\dots,N\}\\ & -{x}_{k} + {x}_{min} <= 0 && \forall k \in \{1,\dots,N\}\\ & \ddot q_1 - \frac{lm_2sin(q_2)\dot q_2^2 + u + m_2gcos(q_2)sin(q_2)}{m_1 + m_2(1-cos^2(q_2))} = 0 && \forall k \in \{1,\dots,N\}\\ & \ddot q_2 + \frac{lm_2cos(q_2)sin(q_2)\dot q_2^2 + ucos(q_2) + (m_1+m_2)gsin(q_2)}{lm_1 + lm_2(1-cos^2(q_2))} = 0 && \forall k \in \{1,\dots,N\}\\ & \frac{((q_1 + lsin(q_2))-x_off) + x_off)^2}{a^2} + \frac{((-lcos(q_2))) + y_off)^2}{b^2} - 1 >= 0 && \forall k \in \{1,\dots,N\}\\ & {x}_{k+1} - {x}_{k} - 0.5 h ({\dot x}_{k} +{\dot x}_{k+1}) = 0 && \forall k \in \{1,\dots,N-1\}\\ & \text{where:}\\ & \begin{equation} x = \begin{bmatrix} q_1 \\ q_2 \\ \dot{q_1} \\ \dot{q_2} \end{bmatrix} \qquad \text{and} \qquad \dot{x} = f ( x , u ) = \begin{bmatrix} \dot{q_1} \\ \dot{q_2} \\ \ddot{q_1} \\ \ddot{q_2} \end{bmatrix} \end{equation} \end{aligned}

In [93]:

using JuMP, PyPlot, Ipopt

#define initial and final states, x
x0 = [0 0 0 0]' 
xf = [3 pi 0 0]'   #wind up at the same angle, but 

"""
solve the cart pull problem using direct collocation and full dynamics

inputs:
    x0 = [q₁ q₂ q₁dot q₂dot]'      #initial system state
    xf = [q₁ q₂ q₁dot q₂dot]'      #final system state
    tf = [sec]                     #time to get to final state
    h =  [sec]                     #time between collocation points

outputs:
    path = [4xk]                   #state matrix that specifies path 
"""
function SolvFullCartPullObstical(x0, xf, tf, h, a, b, c, xoff, yoff, itype = "backwardEuler")
    
    #define model
    m = Model(solver = IpoptSolver(print_level=0))
    T = 0:h:tf                   #time vector
    K = length(T)                 #number of collocation points
    @variable(m,    x[1:4,1:K])   #system state at discrete times
    @variable(m, xdot[1:4,1:K])   #derivative of system state at discrete times
    @variable(m,    u[1:1,1:K])   #control input to system along q1

    #boundry constraints - initial and final state
    @constraint(m, x[:,1] .== x0)
    @constraint(m, x[:,K] .== xf)
    
    #constraints on the total cart range
    @constraint(m, x[1,:] .<= 4)
    @constraint(m,-6 .<= x[1,:])

    
    #model constants
    l = 1         #length of arm
    m1 = 3        #mass of cart
    m2 = 1        #mass of pendulum point mass
    g = 9.81
    
    
        # Elypse constraint on the mass location
    for i = 1:K
        @NLconstraint(m, ((x[1,i] + l*sin(x[2,i])) - xoff)^2/a^2 + ((-l*cos(x[2,i])) - yoff)^2/b^2 >= c )
    end
    
    #setup the full dynamics constraints
    for k in 1:K
        @constraint(m,xdot[1,k] == x[3,k])                      #this step could be wrong!!! I may need to integrate here
        @constraint(m,xdot[2,k] == x[4,k])                      #look here for an issue
         #calculate xdot    
        q1 = x[1,k] ; q2 = x[2,k] ; q1dot = x[3,k] ; q2dot = x[4,k]
        @NLconstraint(m,xdot[3,k] ==  (l*m2*sin(q2)*q2dot^2 + u[1,k] + m2*g*cos(q2)*sin(q2)) /
                                    (m1 + m2*(1-cos(q2)^2)))
        @NLconstraint(m,xdot[4,k] == -(l*m2*cos(q2)*sin(q2)*q2dot^2 + u[1,k]*cos(q2) + (m1 + m2)*g*sin(q2))/
                                    (l*m1 + l*m2*(1-cos(q2)^2)))

    end
    

    if itype == "forwardEuler" 
        #add forward euler dynamics constraint
        for k in 1:K-1
            @constraint(m, x[:,k+1] .== x[:,k] + h*xdot[:,k])
        end
        
    elseif itype == "backwardEuler"
        for k in 1:K-1
             #add backwards euler dynamics constraint
            @constraint(m, x[:,k+1] .== x[:,k] + .5*h*(xdot[:,k] + xdot[:,k+1]))
        end     
    end

    # minimize 2-norm (THIS IS LEAST-SQUARES)
    #@objective(m, Min, .5*h*sum(u[1:end-1].^2 + u[2:end].^2))
    @objective(m, Min, sum(u.^2) ) #+ sum(x[2,:]).^2))
    solve(m)
    #print(m)
    
    control = getvalue(u)
    xopt = getvalue(x)
    xdotopt = getvalue(xdot)
    
    return ( m, control, xopt, xdotopt)
end

Out[93]:

SolvFullCartPullObstical

In [105]:

# Elipse Constarint Paramaters
a = 1
b = 0.5
c = 1
xoff = 1.5
yoff = -1
tf = 3 ; h = .03

(m, c, x, xdot) = SolvFullCartPullObstical(x0,xf,tf,h, a, b, c, xoff, yoff)
figure(figsize=(12,4))
plotq1(x,h,tf)
plotq2(x,h,tf)
figure(figsize=(12,4))
plotu(u,h,tf)
figure(figsize=(12,4))
plotEllipse(a,b,c,xoff,yoff) 
plotXY(x)

The Q space optimization plot shows the carts position and the angle of the pendulum at each colocation point. As expected the cart and the pendulum ends up in the desired position. However, the optimal trajectory is achieved by reversing first allowing the cart more room to avoid the obstacle. The effort plot shows the maximum cart force is needed slow the pendulum as it aproaches its vertical position. This result is consistent with the full dynamics used in this simulation. Finally the XY transform of colocation results shows how the pendulum is able to avoid the obstacle plotted in green.

This method of object avoidance is useful because it is fairly generic, multiple constraints can be used, and the generated trajectories closely match the real life dynamics of the system. However, nonlinear constraints add complexity to the model and increase solution times. For this solution to implement properly an additional nonlinear constraint must be added for each colocation point, in this case 100 additional constraints per object.

Obstacle Avoidance With Regularization ¶

Another, approach to obstacle avoidance is to include a "soft constraint" by including regularization in the objective function. The following function was used to represent the area around an obstacle.

$$ z = \frac{1}{2\pi\sigma^2}e^-[\frac{(x-x_off)^2 + (y-y_off)^2}{2\sigma^2}] $$

A plot visualizing the function used in this section is shown below:

In [38]:

xv = linspace(-4,6,1000)
yv = linspace(-5,5,1000)
xoff = 1.5
yoff = -2
sig = 1

# swap x and y to obtain standard coordinates
z = [1/(sqrt(2*pi*sig^2))*exp(-((x-xoff)^2 + (y-yoff)^2)/(2*sig^2)) for y in yv, x in xv]

function meshgrid{T}(vx::AbstractVector{T}, vy::AbstractVector{T})
    m, n = length(vy), length(vx)
    vx = reshape(vx, 1, n)
    vy = reshape(vy, m, 1)
    (repmat(vx, m, 1), repmat(vy, 1, n))
end
(X,Y) = meshgrid(xv,yv);

pygui(false)
figure(figsize=(12,12))
surf(X,Y, z+0.5, rstride=8, cstride=8,cmap="rainbow",edgecolor="black", linewidths=.05)
contour(X, Y, z, zdir="z", 60, offset=0, origin="lower", linewidths=.5 )
xlabel("X Coord"); ylabel("Y Coord"); zlabel("Penalty");
tight_layout()

The function penalizes the objective function centered at the point $ (x_off,y_off) $ and promotes a solutions further away from the object. The objective function, with regularization included and in terms of $q_1$ and $q_2$, takes the form of the following equation.

$$ \sum(u^2) + \lambda \sum \frac{1}{2\pi\sigma^2}e^-[\frac{(x-x_off)^2 + (y-y_off)^2}{2\sigma^2}] $$

Standard Form¶

\begin{aligned} \underset{\mathbf{x},\mathbf{\dot x},\mathbf{u}_i \in \mathbb{R^2}}{\text{minimize}}\qquad& \sum_{i=1}^{n}u_i^2 + \lambda \sum_{i=1}^{n} \frac{1}{2\pi\sigma^2}e^-[\frac{((q_1 + lsin.(q_2))-x_off)^2 + ((-lcos.(q_2))-y_off)^2}{2\sigma^2}]\\ \text{subject to:}\qquad & {x_{1}} - {x}_{initial} = 0 \\ & {\dot{x_{k}}} - {\dot{x}}_{final} = 0 && \\ & {x}_{k} - {x}_{max} <= 0 && \forall k \in \{1,\dots,N\}\\ & -{x}_{k} + {x}_{min} <= 0 && \forall k \in \{1,\dots,N\}\\ & \ddot q_1 - \frac{lm_2sin(q_2)\dot q_2^2 + u + m_2gcos(q_2)sin(q_2)}{m_1 + m_2(1-cos^2(q_2))} = 0 && \forall k \in \{1,\dots,N\}\\ & \ddot q_2 + \frac{lm_2cos(q_2)sin(q_2)\dot q_2^2 + ucos(q_2) + (m_1+m_2)gsin(q_2)}{lm_1 + lm_2(1-cos^2(q_2))} = 0 && \forall k \in \{1,\dots,N\}\\ & {x}_{k+1} - {x}_{k} - 0.5 h ({\dot x}_{k} +{\dot x}_{k+1}) = 0 && \forall k \in \{1,\dots,N-1\}\\ & \text{where:}\\ & \begin{equation} x = \begin{bmatrix} q_1 \\ q_2 \\ \dot{q_1} \\ \dot{q_2} \end{bmatrix} \qquad \text{and} \qquad \dot{x} = f ( x , u ) = \begin{bmatrix} \dot{q_1} \\ \dot{q_2} \\ \ddot{q_1} \\ \ddot{q_2} \end{bmatrix} \end{equation} \end{aligned}

In [70]:

using JuMP, PyPlot, Ipopt

#define initial and final states, x
x0 = [0 0 0 0]' 
xf = [3 pi 0 0]'   #wind up at the same angle, but 

"""
solve the cart pull problem using direct collocation and full dynamics

inputs:
    x0 = [q₁ q₂ q₁dot q₂dot]'      #initial system state
    xf = [q₁ q₂ q₁dot q₂dot]'      #final system state
    tf = [sec]                     #time to get to final state
    h =  [sec]                     #time between collocation points

outputs:
    path = [4xk]                   #state matrix that specifies path 
"""
function SolveRegularizedCartPull(x0, xf, tf, h, xoff, yoff, y, itype = "backwardEuler")
    
    #define model
    m = Model(solver = IpoptSolver(print_level=0))
    T = 0:h:tf                   #time vector
    K = length(T)                 #number of collocation points
    @variable(m,    x[1:4,1:K])   #system state at discrete times
    @variable(m, xdot[1:4,1:K])   #derivative of system state at discrete times
    @variable(m,    u[1:1,1:K])   #control input to system along q1

    #boundry constraints - initial and final state
    @constraint(m, x[:,1] .== x0)
    @constraint(m, x[:,K] .== xf)
    
    #constraints on the total cart range
    @constraint(m, x[1,:] .<= 4)
    @constraint(m,-6 .<= x[1,:])
    
    #model constants
    l = 1         #length of arm
    m1 = 3        #mass of cart
    m2 = 1        #mass of pendulum point mass
    g = 9.81
    
    #setup the full dynamics constraints
    for k in 1:K
        @constraint(m,xdot[1,k] == x[3,k])                      #this step could be wrong!!! I may need to integrate here
        @constraint(m,xdot[2,k] == x[4,k])                      #look here for an issue
         #calculate xdot    
        q1 = x[1,k] ; q2 = x[2,k] ; q1dot = x[3,k] ; q2dot = x[4,k]
        @NLconstraint(m,xdot[3,k] ==  (l*m2*sin(q2)*q2dot^2 + u[1,k] + m2*g*cos(q2)*sin(q2)) /
                                    (m1 + m2*(1-cos(q2)^2)))
        @NLconstraint(m,xdot[4,k] == -(l*m2*cos(q2)*sin(q2)*q2dot^2 + u[1,k]*cos(q2) + (m1 + m2)*g*sin(q2))/
                                    (l*m1 + l*m2*(1-cos(q2)^2)))

    end
    

    if itype == "forwardEuler" 
        #add forward euler dynamics constraint
        for k in 1:K-1
            @constraint(m, x[:,k+1] .== x[:,k] + h*xdot[:,k])
        end
        
    elseif itype == "backwardEuler"
        for k in 1:K-1
             #add backwards euler dynamics constraint
            @constraint(m, x[:,k+1] .== x[:,k] + .5*h*(xdot[:,k] + xdot[:,k+1]))
        end     
    end

    #xCordM = x[1,:] + l*sin.(x[2,:])
    #yCordM = -l*cos.(x[2,:])
    
    # minimize 2-norm (THIS IS LEAST-SQUARES)

    @NLexpression(m, reg[1,i=1:K],  1/(sqrt(2*pi*sig^2))*exp(-((((x[1,i] + l*sin(x[2,i]))-xoff)^2 + (-l*cos(x[2,i])-yoff)^2)/(2*sig^2))) ) 
    @NLobjective(m, Min, sum(u[j]^2 + y*reg[1,j] for j=1:K) ) 
    solve(m)
    #print(m)
    
    control = getvalue(u)
    xopt = getvalue(x)
    xdotopt = getvalue(xdot)
    
    return (m, control, xopt, xdotopt)
end

Out[70]:

SolveRegularizedCartPull

Solution: Avoiding Obsticals with Soft Constraints¶

In [106]:

y = 1000000
xoff = 1.5
yoff = -1
tf = 3 ; h = .03

(m, u, x, xdot) = SolveRegularizedCartPull( x0, xf, tf, h, xoff, yoff, y)
figure(figsize=(12,4))
plotq1(x,h,tf)
plotq2(x,h,tf)
legend(["q1 Cart Position","q2 Cart Pendulum Angle"])
figure(figsize=(12,4))
plotu(u,h,tf)
legend(["Control Effort"])
figure(figsize=(12,4))
plotXY(x)
plotContour(xoff,yoff)

Out[106]:

PyObject <matplotlib.contour.QuadContourSet object at 0x00000000A8903E48>

Adjusting lambda allows the optimizer to find a solution that avoids the object placed at $(x_off,y_off) $. A different lambda is needed at different locations to force the optimizer to miss the obstacle. This results in a different pareto front for each possible location of an obstacle. This could be considered a drawback of the object avoidance regularization technique, because lambda must be re-adjusted each time the object changes location or the trajectory changes. A different and possibly more effective solution for obstacle avoidance using regularization might be obtained by using a different cost function like an inverse function.

Double Pendulum Swing Up ¶

A derivation of the dynamics of a double pendulum is beyond the scope of the report, but follows a very similar procedure to that detailed in mathematical modeling which utilized lagrangian methods. for a detailed discussion of the derivation, consult:

Xin X., Liu Y. (2014) Double Pendulum on Cart. In: Control Design and Analysis for Underactuated Robotic Systems. Springer, London

In [200]:

using JuMP, PyPlot, Ipopt
"""
solve the cart pole double pendulum problem using direct collocation and full dynamics

inputs:
    x0 = [q₁ q₂ q₁dot q₂dot]'      #initial system state
    xf = [q₁ q₂ q₁dot q₂dot]'      #final system state
    tf = [sec]                     #time to get to final state
    h =  [sec]                     #time between collocation points

outputs:
    path = [4xk]                   #state matrix that specifies path 
"""
function SolvFullCartPoleDoublePendulum(x0, xf, tf, h ; xStart = "None" , itype = "backwardEuler")
    
    #define model
    m = Model(solver = IpoptSolver(print_level=0))
    T = 0:h:tf                   #time vector
    K = length(T)                 #number of collocation points
    @variable(m, x[1:6,1:K])   #system state at discrete times
    @variable(m, xdot[1:6,1:K])   #derivative of system state at discrete times
    @variable(m,    u[1:1,1:K])   #control input to system along q1
    if xStart != "None"  setvalue(x[:,:] , xStart) end


    #boundry constraints - initial and final state
    @constraint(m, x[:,1] .== x0)
    @constraint(m, x[:,K] .== xf)

    
    #model constants
    l1 = 1        #length of arm 1
    l2 = 1        #length of arm 2
    m1 = 3        #mass of cart
    m2 = 1        #mass of pendulum 1 point mass
    m3 = 1        #mass of pendulum 2 point mass 
    g = 9.81      #gravity
    
    #path Constraints
    for k in 1:K
        @constraint(m, x[1,k] <= 10)
        @constraint(m, x[1,k] >= -10)
    end
    
    
    #setup the full dynamics constraints
    for k in 1:K
        @constraint(m,xdot[1:3,k] .== x[4:6,k])
        
        #matrix variables (unsupported)        
#         M = [m1+m2+m3              l1*(m1 + m2)*cos(q2)   m2*l2*cos(q3);
#              l1*(m1+m2)*cos(q2)    l1^2*(m1+m2)           l1*l2*m2*cos(q2-q3);
#              l2*m2*cos(q3)         l1*l2*m2*cos(q2-q3)    l2^2*m2             ]
        
#         C = [l1*(m1+m2)*q2dot^2*sin(q2) + m2*l2*q3dot^2*sin(q3);
#              -l1*l2*m2*q3dot^2*sin(q2-q3) + g*(m1+m2)*l1*sin(q2);
#              l1*l2*m2*q2dot^2*sin(q2-q3) + g*l2*m2*sin(q3)         ] 
        
                
        #solve using scalar methods (can't use matrix formulations)
        
        #extract q's from matrix variables
        q1 = x[1,k] ; q2 = x[2,k] ; q3 = x[3,k] ; q1dot = x[4,k] ; q2dot = x[5,k] ; q3dot = x[6,k]
        q1ddot = xdot[4,k] ; q2ddot = xdot[5,k] ; q3ddot = xdot[6,k]
        
        #calculate qddot's
        @NLconstraint(m,(m1+m2+m3)*q1ddot + l1*(m1 + m2)*cos(q2)*q2ddot + m2*l2*cos(q3)*q3ddot
                        -(l1*(m1+m2)*q2dot*q2dot*sin(q2) + m2*l2*q3dot*q3dot*sin(q3) + u[1,k]) == 0)
        
        @NLconstraint(m, l1*(m1+m2)*cos(q2)*q1ddot + l1*l2*(m1+m2)*q2ddot + l1*l2*m2*cos(q2-q3)*q3ddot
                        -(-l1*l2*m2*q3dot*q3dot*sin(q2-q3) + g*(m1+m2)*l1*sin(q2)) == 0)
        
         @NLconstraint(m, l2*m2*cos(q3)*q1ddot + l1*l2*m2*cos(q2-q3)*q2ddot + l2*l2*m2*q3ddot
                         -(l1*l2*m2*q2dot*q2dot*sin(q2-q3) + g*l2*m2*sin(q3)) == 0 )     
    end
        

    if itype == "forwardEuler" 
        #add forward euler dynamics constraint
        for k in 1:K-1
            @constraint(m, x[:,k+1] .== x[:,k] + h*xdot[:,k])
        end
        
    elseif itype == "backwardEuler"
        for k in 1:K-1
             #add backwards euler dynamics constraint
            @constraint(m, x[:,k+1] .== x[:,k] + .5*h*(xdot[:,k] + xdot[:,k+1]))
        end     
    end

    # minimize 2-norm (THIS IS LEAST-SQUARES)
    #@objective(m, Min, .5*h*sum(u[1:end-1].^2 + u[2:end].^2))
    @objective(m, Min, sum(u.^2))
    solve(m)
    #print(m)
    
    control = getvalue(u)
    xopt = getvalue(x)
    xdotopt = getvalue(xdot)
    #return m
    return (m,control, xopt,xdotopt)
end

Out[200]:

SolvFullCartPoleDoublePendulum

In [203]:

x0 = [0 pi pi 0 0 0]' 
xf = [3 0 0  0 0 0]' 

m,c,x,xdot = SolvFullCartPoleDoublePendulum(x0, xf, 3, .1)

using PyPlot
figure(figsize=(12,4))
#plot(c[1,:])
plot( x[1,:], "b.-", markersize=4)
plot(x[2,:],"r")
plot(x[3,:],"r")
legend(["Angle 1","Angle 2","Cart Position"])

xx = resample(x,.1,1/45)
process2CSVPen2(xx)

Results¶

The behavior of the double pendulum exemplified some of the undesirable features of NLPs. NLPs cannot guarantee globally optimal solutions, and as such the resulting behavior of the program is highly dependent on starting configuration. This was the case with the double pendulum and why seeding attempts were made with the results of other models, or with a linear interpolation between the starting and ending configuration. NLPs scale very poorly, and this placed a computational limit on the number of collocation points that could be used for a particular simulation. The behavior of this conditions seems natural, but other conditions were more jerky. The behavior of a double pendulum is chaotic, such that small perturbations in input system state result in large differences in output state. This could be helped by adding a degree of damping to the model, and also using either regularization or explicit constraints on kinematic variables such as $q$ and $\dot q$.

Conclusion ¶

Here we presented the results of trajectory optimization via direct collocation for a variety of models representing a cart-pole system. We also examined using explicit constraints and regularizers to steer system trajectories around obstacles. These analyses were fully open loop, and incorporate no feedback from positional sensors. The resulting trajectories are extremely sensitive to disturbances. In fact, if the problem is formulated as an IVP and forward integrated (solve the ODE) the resulting kinematics are different due to numerical error and because the dynamics were only satisfied at the collocation points. As such, real world problems which are subject to long time horizons, disturbances, or uncertainty in model parameters incorporate sensor feedback into their control algorithms.

Our group is very interested in control, and in particular the control of human locomotion. Within the neuromuscular control literature, optimization techniques have been used to model control for the human body in simulation. In control of complex behavior (like walking) over long time horizons, optimization techniques are used to tune high bandwidth PD feedback controls. By contrast, Open loop methods such like shooting methods have been used to generate short time horizon behavior like a standing long jump[2]. Both of these approaches have limitations, in that the first is not practical for complex behavior as humans have low bandwidth[3], and the second is difficult to solve for complex models, and may diverge over long time horizons.

It’s possible that a solution to these problems is a low bandwidth feedback controller that has high performance due to accurate feed forward models. To explore this idea, we would need to Increase model complexity to model of the human lower body, most likely within a simulation engine such as opensim. Cycling is a good example control problem as it avoids the discontinuities of foot contact.

[1] Optimizing Locomotion Controllers Using Biologically-Based Actuators and Objectives Jack M. Wang, Samuel R. Hamner, Scott L. Delp, and Vladlen Koltun ACM Transactions on Graphics 31(4), 2012

[2]Ong CF, Hicks JL, Delp SL. Simulation-based Design for Wearable Robotic Systems: An Optimization Framework for Enhancing a Standing Long Jump. IEEE Transactions on Biomedical Engineering, in press.

[3]Berthoz, Alain. The brain's sense of movement. Vol. 10. Harvard University Press, 2000.

Appendix ¶

this appendex contains the code that was used to export data to unity game engine to generate the 3D animations of system trajectories.

In [193]:

"""takes a 3x3 rotation matrix and converts it to a 4x1 array of euler parameters"""
function A2P(A::Array) 
  e0 = sqrt((trace(A) + 1)/4)
  if e0 != 0
    e1 = (A[3,2] - A[2,3])/(4*e0)
    e2 = (A[1,3] - A[3,1])/(4*e0)
    e3 = (A[2,1] - A[1,2])/(4*e0)
  end

  if e0 == 0  #implies Χ = π
    #figure out which e terms are non-zero
    e1flag = false; e2flag = false; e3flag = false;
    if A[1,1] + 1 != 0  e1flag = true end
    if A[2,2] + 1 != 0  e2flag = true end
    if A[3,3] + 1 != 0  e3flag = true end

    if e1flag
      e1 = sqrt((A[1,1] + 1)/2)
      e2 = (A[2,1] + A[1,2])/(4*e1)
      e3 = (A[3,1] + A[1,3])/(4*e1)
    elseif e2flag
      e2 = sqrt((A[2,2] + 1)/2)
      e1 = (A[2,1] + A[1,2])/(4*e2)
      e3 = (A[3,2] + A[2,3])/(4*e2)
    elseif e3flag
      e3 = sqrt((A[3,3] + 1)/2)
      e1 = (A[3,1] + A[1,3])/(4*e3)
      e2 = (A[3,2] + A[2,3])/(4*e3)
    else
      warn("something is wrong with A value  $A ")
    end
  end

  p = [e0 e1 e2 e3]'
end

#principle rotations
Rx(Θ) = [1 0 0 ; 0 cos(Θ) -sin(Θ) ; 0 sin(Θ) cos(Θ)]
Ry(Θ) = [ cos(Θ) 0 sin(Θ) ; 0 1 0 ; -sin(Θ) 0 cos(Θ)]
Rz(Θ) = [ cos(Θ) -sin(Θ) 0 ; sin(Θ) cos(Θ) 0  ; 0 0 1]



"""
treat the cart pull problem as an initial value problem and numerically integrate forward in time 
with the full dynamics to get the best estimate of the "real" trajectory

the approach will be a simple forward euler integrator

inputs:
    x0 =   [q₁ q₂ q₁dot q₂dot]'      #initial system state
    u[k] = [u₁ u₂ ... uₙ]            #discrete sampled control value
    tf =   [sec]                     #time between collocation points
    h =    [sec]                     #time between collocation points
    method = str                     #level of assumptions being made 

outputs:
    x[k] = [4 x n]                   
    

"""
function pullCartForwardDynamics(x0,u,tf,h,method = 1)
    #setup
    tspan = 0:h:tf
    x =    zeros(4,length(tspan)+1)
    xdot = zeros(4,length(tspan)+1)
    x[:,1] = x0
    u = resample(u,tf/(length(u)-1),h) #make u the right size to be sampled
    
    
    
    #forward euler integration
    for (k,t) in enumerate(tspan)
        xdot[:,k] = calcxdot(x[:,k],u[k],h,method) 
        x[:,k+1]  = x[:,k] + h*xdot[:,k]
    end
    
    return x
end

"""
calcuate xdot from the specified system dynamics using forward euler
"""
function calcxdot(x,u,h,method)
    #model constants
    l = 1         #length of arm
    m1 = 1        #mass of cart
    m2 = 1        #mass of pendulum 
    g = 9.81
    
    
    xdot = zeros(4,1)
    
    #calculate xdot    
    q1 = x[1,1] ; q2 = x[2,1] ; q1dot = x[3,1] ; q2dot = x[4,1]
    xdot[3,1] =  (l*m2*sin(q2)*q2dot^2 + u + m2*g*cos(q2)*sin(q2)) / (m1 + m2*(1-cos(q2)^2))
    xdot[4,1] = -(l*m2*cos(q2)*sin(q2)*q2dot^2 + u*cos(q2) + (m1 + m2)*g*sin(q2))/ (l*m1 + l*m2*(1-cos(q2)^2))
    
    #forward euler step
    xdot[1:2,1] = x[3:4,1] + h*xdot[3:4,1]
    
    return xdot
    
end


using DataFrames
"""
convert state vector x for the cart pull problem from a minimal set of generalized coordinates to a
maximal set of cartesian coordinates using euler parameters to describe rotation. additionally, 
rotations will be made so that things look right in unity

inputs:
    x - state vector resulting from from our control optimization
    fln - string of file location
outputs:
    csv - write a csv to the proper location
    
assumptions:
-This code assumes the original simulation is resampled at 30Hz so the visualization timing is correct
- left handed coordinates as defined in unity:
    * y is up
    * x is left
    * z is out of the page

movement of the cart occures along the x, and rotation of the pendulum occures about z axis
"""
function process2CSV(x,path = "./pullCartVis3D/Assets/Data/data.csv")
    
    #extract state, we only need q1 and q2
    x = x[1:2,:]
    
    #build out fill state matrix  xFull = [r₁;r₂;p₁;p₂]
    nb = 2;
    xFull = zeros(7*nb, size(x)[2])  
    
    #place location parameters appropriately
    xFull[1,:] = x[1,:]
    xFull[4,:] = x[1,:]
    xFull[6,:] = .15*ones(size(x)[2],1)                 #unity compensation
    
    #place orientation parameters appropriately
    for i in range(1,size(x)[2])  
        xFull[7:10,i]   = A2P(Rx(pi/2)*eye(3))          #unity compensation
        xFull[11:14,i] = A2P(Rx(pi/2)*Ry(x[2,i]))       #unity compensation
    end
        
    #print to CSV
    xFull = convert(DataFrame, xFull)
    writetable(path,xFull)
end


function process2CSVPen2(x,path = "./pullCartVis3D/Assets/Data/data.csv")
    
    #extract state, we only need q1 - q3
    x = x[1:3,:]
    
    #build out full state matrix  xFull = [r₁;r₂;r₃;p₁;p₂;p₃]
    nb = 3;
    xFull = zeros(7*nb, size(x)[2])  
    
    #place r locations
    xFull[1,:] =  -x[1,:]
    xFull[4,:] =  -x[1,:]
    xFull[7,:] =  -x[1,:] - .85*sin.(x[2,:])                #forward kinematics from unity origin
    xFull[8,:] =  .85*cos.(x[2,:])                         #forward kinematics from unity origin
    
    xFull[6,:] = .15*ones(size(x)[2],1)                   #unity compensation
    xFull[9,:] = .15*ones(size(x)[2],1)                   #unity compensation
    
    #place orientation parameters appropriately
    for i in range(1,size(x)[2])  
        xFull[10:13,i]   = A2P(Rx(pi/2)*eye(3))           #unity compensation
        xFull[14:17,i]   = A2P(Rx(pi/2)*Ry(x[2,i]))       #unity compensation
        xFull[18:21,i]   = A2P(Rx(pi/2)*Ry(pi)*Ry(x[3,i]))       #unity compensation
    end
        
    #print to CSV
    xFull = convert(DataFrame, xFull)
    writetable(path,xFull)
end


"""
linearly interpolate between points on a grid u[k]
"""
function interpolate(u::Array,h::Float64,t::Float64)
    if ndims(u) == 1 u = reshape(u,1,length(u)) end   #robust to 1d arrays
    k = Int(floor(t/h) + 1)
    if k == size(u)[2] return u[k] end              #you sampled at the exact last point
    deltat = t - (k-1)*h;                            #correcting for 1 index
    u_t = u[k] + deltat*(u[k+1]-u[k])/h
    return u_t
end
    
"""
resample an x array at the requested sample rate
"""
function resample(x,h0,h1)
    if ndims(x) == 1 x = reshape(x,1,length(x)) end   #robust to 1d arrays
    tf = h0*(size(x)[2] - 1)
    colsNew = Int(floor(tf/h1) + 1)
    xnew = zeros(size(x)[1],colsNew)
    for row in 1:size(x)[1]
        for col in 1:colsNew
                xnew[row,col] = interpolate(x[row:row,:],h0,(col-1)*h1)
        end
    end
    return xnew
end

"""
seed the starting state of the double pendulum with linear change between starting and ending state
"""
function seedDoublePendulum(x0, xf,tf,h)
    T = 0:h:tf                   
    K = length(T)
    x = zeros(6,K)
    x[1,:] = linspace(x0[1,1],xf[1,1],K)
    x[2,:] = linspace(x0[2,1],xf[2,1],K)
    x[3,:] = linspace(x0[3,1],xf[3,1],K)
end

Out[193]:

seedDoublePendulum