Augmented solution for one-bar linkage
Overview: embedding technique
The augmented technique is a generalized method to solve for the dynamic response in a system of moving parts. The system of differential algebraic equations (DAE) are solve for all generalized coordinates and constraint forces in a system of equations,
$$\left[\begin{array} ~\mathbf{M} & \mathbf{C_q}^T \\ \mathbf{C_q} & \mathbf{0} \end{array}\right] \left[\begin{array} ~\mathbf{\ddot{q}} \\ \mathbf{\lambda}\end{array}\right]= \left[\begin{array} ~\mathbf{Q_e} \\ \mathbf{Q_d}\end{array}\right]$$
where
$$\mathbf{M}$$
is a diagonal mass matrix for each body,
$$\mathbf{C_q}$$
is the Jacobian of the constraint equations,
$$\ddot{q}$$
is the second time derivative of each generalized coordinate,
$$\mathbf{\lambda}$$
is a vector of Lagrange multiplier that decribes the reaction forces at each constraint,
$$\mathbf{Q_e}$$
is a vector of external generalized forces (\(F\delta R~and~M\delta \theta\)), and
$$\mathbf{Q_d} = -[\mathbf{(C_q\dot{q})_q\dot{q}+2C_{qt}\dot{q}+C_{tt}}]$$
are the constraints on acceleration.
The resulting linear algebra equation creates \(n+n_c\) linearly independent equations. This is much larger than the embedding technique with \(n-n_c\) equations, but the left hand side matrix is very sparse. As an example, consider a link pinned to the ground
System: link pinned to ground
In the diagram above, a link is pinned to the point \(r_{pin} = x\hat{i}\). In this notebook, you will
build the system of differential algebraic equations
solve the DAE's
plot and animate the results (coordinates and reaction forces)
The system of equations depends upon the variables and equations below. You can expand look at the code cells to see the function definitions using sympy.lambdify:
$$\mathbf{q}$$
$$\dot{\mathbf{q}}$$
$$\mathbf{C}(\mathbf{q},~t)$$
:
C_link = lambdify ...$$\mathbf{C_q}$$
:
Cq_link = lambdify ...$$\mathbf{Q_d} = -[\mathbf{(C_q\dot{q})_q\dot{q}+2C_{qt}\dot{q}+C_{tt}}]$$
:
Qd_link = lambdify ...
1. Define generalized coordinates \(\mathbf{q}\) and \(\dot{\mathbf{q}}\)
The generalized coordinates \(\mathbf{q} = [R_x^1,~R_y^1,~\theta^1]\), are defined as follows,
using Symbolics, Plots, NonlinearSolve, ModelingToolkit, DifferentialEquationsusing ModelingToolkit: t_nounits as t, D_nounits as Dimport Latexifybegin
q = @variables x₁(t), y₁(t), θ₁(t)
qt = @variables dx₁(t), dy₁(t), dθ₁(t)
qp = @variables q₁, q₂, q₃
L = 0.5 # m
m = 0.3 # kg
θ₀ = 0 # rad initial angle
g = 9.81 # m/s/s gravitational constant
hinge_x = 0
hinge_y = 0
end0
begin
C(q, t) = [q[1] - L/2*cos(q[3]) - hinge_x;
q[2] - L/2*sin(q[3]) - hinge_y]
C_fun = Symbolics.build_function(C(q,t), q, t; expression = Val(false))[1]
C(q, t)
end$$\begin{equation} \left[ \begin{array}{c} \mathtt{x_1}\left( t \right) - 0.25 ~ \cos\left( \mathtt{\theta_1}\left( t \right) \right) \\ \mathtt{y_1}\left( t \right) - 0.25 ~ \sin\left( \mathtt{\theta_1}\left( t \right) \right) \\ \end{array} \right] \end{equation}$$
2. \(\mathbf{C}(\mathbf{q},~t)\):
The constraints are defined as \(\mathbf{C}(\mathbf{q},~t)=\)
and \(\frac{\partial C}{\partial t} = C_t =\)
3. \(\mathbf{C_q}\):
Then, the Jacobian of constraints, \(\mathbf{C_q}=\)
begin
Cq(q, t) = Symbolics.jacobian(C(q, t), q)
Cq(q, t)
end$$\begin{equation} \left[ \begin{array}{ccc} 1 & 0 & 0.25 ~ \sin\left( \mathtt{\theta_1}\left( t \right) \right) \\ 0 & 1 & - 0.25 ~ \cos\left( \mathtt{\theta_1}\left( t \right) \right) \\ \end{array} \right] \end{equation}$$
begin
Ct(q, t) = Symbolics.derivative(C(qp,t), t)
Ctt(q, t) = Symbolics.derivative(Ct(qp, t), t)
Ctt(q, t)
end$$\begin{equation} \left[ \begin{array}{c} 0 \\ 0 \\ \end{array} \right] \end{equation}$$
begin
Qd(q, dq, t) = -Symbolics.jacobian(Cq(q, t)*dq, q)*dq - 2*Symbolics.derivative(Cq(qp,t), t)*dq - Ctt(q, t)
Symbolics.simplify.(Qd(q, qt, t))
end$$\begin{equation} \left[ \begin{array}{c} - 0.25 ~ \left( \mathtt{d\theta_1}\left( t \right) \right)^{2} ~ \cos\left( \mathtt{\theta_1}\left( t \right) \right) \\ - 0.25 ~ \left( \mathtt{d\theta_1}\left( t \right) \right)^{2} ~ \sin\left( \mathtt{\theta_1}\left( t \right) \right) \\ \end{array} \right] \end{equation}$$
M = [m 0 0;
0 m 0;
0 0 m*L^2/12]3×3 Matrix{Float64}:
0.3 0.0 0.0
0.0 0.3 0.0
0.0 0.0 0.00625
begin
Cq_fun = Symbolics.build_function(Cq(q,t), q, t; expression = Val(false))[1]
LHS(q,t) = [M Cq_fun(q,t)';
Cq_fun(q,t) zeros(2,2)]
# LHS_fun = Symbolics.build_function(LHS, q, t; expression = Val(false))[1]
LHS([0.0, 0.0, 0], 0.0)
end5×5 Matrix{Float64}:
0.3 0.0 0.0 1.0 0.0
0.0 0.3 0.0 0.0 1.0
0.0 0.0 0.00625 0.0 -0.25
1.0 0.0 0.0 0.0 0.0
0.0 1.0 -0.25 0.0 0.0
begin
RHS(q, dq, t) = [0;
-m*g;
0;
Qd(q, qt, t)]
RHS(q, qt, t)
RHS_fun = Symbolics.build_function(RHS(q, qt, t), q, qt, t; expression = Val(false))[1]
RHS_fun([0, 0, 0], [0,0,0], 0)
#RHS(q, qt, t)
end5-element Vector{Float64}:
0.0
-2.943
0.0
-0.0
-0.0
Baumgarte stabilization
The Augmented solution only constrains the accelerations in the system because
$$\mathbf{\ddot{C}} = \mathbf{C_q\ddot{q} - Q_d} = \mathbf{0}$$
Numerical solutions use an approximation of differential equations to solve nonlinear problems. The approximation can lead to compounding error at each time step.
The Baumgarte stbailization adds the first and second derivatives to the acceleration constraints as such
$$\mathbf{\ddot{C}} +2\alpha \mathbf{\dot{C}} + \beta^2\mathbf{C} = \mathbf{0}$$
where \(\alpha\) and \(\beta\) are numerical constants that are used to constrain compounding error over time. Now, the \(\mathbf{Q_d}\) has two terms added
$$\mathbf{C_q \ddot{q}} = \mathbf{Q_d} - 2\alpha(\mathbf{C_q \dot{q}}+\mathbf{C_t}) - \beta^2\mathbf{C}$$
Solve and plot results
Here, you compare the numerical solution to the analytical solution. You will release the link from rest at \(\theta=0^o\). The augmented method uses a state, y, that includes all of the generalized coordinates and derivatives, y\(=[\mathbf{q},~\dot{\mathbf{q}}]\). Before integrating over time, solve for the initial state,
$$\mathbf{C}(\mathbf{q},~t=0) = 0$$
$$\mathbf{C_q\dot{q}} = -\mathbf{C}_t$$
You find the solution using NonlinearProblem.
begin
setup_q(q, p) = [C_fun(q, 0); q[p[1]] - p[2]]
q_prob = NonlinearProblem(setup_q, [0, 0, 0], (3, 0))
q0 = solve(q_prob)
q0
endretcode: Success
u: 3-element Vector{Float64}:
0.25
0.0
0.0
function pendulum!(dy, y, params, time)
q = y[1:3]
dq = y[4:6]
α, β = params
rhs = RHS_fun(q, dq, time)
rhs[4:5] += -2*α*Cq_fun(q,time)*dq - β^2*C_fun(q, time)
sols = LHS(q, time)\rhs
dy[1:3] = dq
dy[4:6] = sols[1:3]
lambda = sols[4:5]
return nothing
endpendulum! (generic function with 1 method)
begin
time_span = (0.0, 3)
# params = (b, k, m, L, g)
initial_state = [q0; 0.0; 0.0; 0.0]
problem = ODEProblem(pendulum!, initial_state, time_span, [0, 0])
solution = solve(problem, Tsit5())
t_steps = LinRange(time_span[1], time_span[2], 200)
plot(solution(t_steps).t,
solution(t_steps)[3,:]*180/pi,
linewidth = 4)
end
begin
R = zeros(length(t_steps), 2)
for i in range(1, length(t_steps))
qi = solution(t_steps[i])[1:3]
dqi = solution(t_steps[i])[4:6]
R[i,:] = (LHS(qi, t_steps[i])\RHS_fun(qi, dqi, t_steps[i]))[4:5]
end
endplot(t_steps, R)
Visualize motion and forces
In the next block, we use the integrated solutions to the Augmented DAE's to plot the positions, paths, and forces acting on the compound pendulum. The solution supplies us with the generalized coordinates,
$$q(t) = [x(t),~y(t),~\theta(t)]$$
and its derivatives
$$\dot{q}(t) = [\dot{x}(t),~\dot{y}(t),~\dot{\theta}(t)]$$
and the reaction forces are all contained in the values of R from the \(\lambda(t)\) solutions,
$$R_x(t) = -\lambda_1(t)$$
$$R_y(t) = -\lambda_2(t)$$
begin
q_vals = solution(t_steps)[1:3,:]
dq_vals = solution(t_steps)[4:6,:]
@gif for i in range(1, length(t_steps))
scatter(0, 0, m = :circle, label = "ground support")
plot!(q_vals[1, 1:i], q_vals[2, 1:i], label = "path")
plot_rectangle(q_vals[:, i], [L, 0.02], label = "")
force_scale = -0.1
quiver!(
[0.0], [0.0],
quiver = (
[force_scale * R[i, 1]],
[force_scale * R[i, 2]]
),
color = :red,
linewidth = 2,
label = ""
)
plot!(aspect_ratio = 1,
xlims = [-1, 1],
ylims = [-1, 1]
)
end
end
Wrapping up
In this notebook, you
used the augmented technique to set up and solve a link pinned to the ground as a dynamic system.
added the Baumgarte stabilization to enforce position and velocity constraints at each time step
plotted and animated the results
Built with Julia 1.12.5 and
DifferentialEquations 7.17.0Latexify 0.16.10
ModelingToolkit 11.10.0
NonlinearSolve 4.17.1
Plots 1.41.6
Symbolics 7.18.0
To run this tutorial locally, download this file and open it with Pluto.jl.