Acrobot Swing-Up#

Mathematical Formulation#

Problem Statement#

Find the optimal elbow torque \(\tau(t)\) that swings up the acrobot from the hanging equilibrium to the inverted equilibrium while minimizing time and control effort:

\[J = t_f + 0.01 \int_0^{t_f} \tau^2 dt\]

Subject to the underactuated acrobot dynamics:

\[\frac{d\theta_1}{dt} = \dot{\theta}_1\]
\[\frac{d\theta_2}{dt} = \dot{\theta}_2\]
\[\frac{d\dot{\theta}_1}{dt} = \frac{-I_2 \left( g l_1 m_2 \sin(\theta_1) + g l_{c1} m_1 \sin(\theta_1) + g l_{c2} m_2 \sin(\theta_1 + \theta_2) - l_1 l_{c2} m_2 (2\dot{\theta}_1 + \dot{\theta}_2) \sin(\theta_2) \dot{\theta}_2 \right) + (I_2 + l_1 l_{c2} m_2 \cos(\theta_2)) \left( g l_{c2} m_2 \sin(\theta_1 + \theta_2) + l_1 l_{c2} m_2 \sin(\theta_2) \dot{\theta}_1^2 - \tau \right)}{I_1 I_2 + I_2 l_1^2 m_2 - l_1^2 l_{c2}^2 m_2^2 \cos^2(\theta_2)}\]
\[\frac{d\dot{\theta}_2}{dt} = \frac{(I_2 + l_1 l_{c2} m_2 \cos(\theta_2)) \left( g l_1 m_2 \sin(\theta_1) + g l_{c1} m_1 \sin(\theta_1) + g l_{c2} m_2 \sin(\theta_1 + \theta_2) - l_1 l_{c2} m_2 (2\dot{\theta}_1 + \dot{\theta}_2) \sin(\theta_2) \dot{\theta}_2 \right) - \left( g l_{c2} m_2 \sin(\theta_1 + \theta_2) + l_1 l_{c2} m_2 \sin(\theta_2) \dot{\theta}_1^2 - \tau \right) (I_1 + I_2 + l_1^2 m_2 + 2 l_1 l_{c2} m_2 \cos(\theta_2))}{I_1 I_2 + I_2 l_1^2 m_2 - l_1^2 l_{c2}^2 m_2^2 \cos^2(\theta_2)}\]

Boundary Conditions#

  • Initial conditions: \(\theta_1(0) = \frac{\pi}{6}\) rad, \(\theta_2(0) = 0\) rad, \(\dot{\theta}_1(0) = 0\) rad/s, \(\dot{\theta}_2(0) = 0\) rad/s

  • Final conditions: \(\theta_1(t_f) = \pi\) rad, \(\theta_2(t_f) = 0\) rad, \(\dot{\theta}_1(t_f) = 0\) rad/s, \(\dot{\theta}_2(t_f) = 0\) rad/s

  • Control bounds: \(-20.0 \leq \tau \leq 20.0\) N⋅m

Physical Parameters#

  • Upper arm mass: \(m_1 = 1.0\) kg

  • Forearm mass: \(m_2 = 1.0\) kg

  • Upper arm length: \(l_1 = 1.0\) m

  • Forearm length: \(l_2 = 1.0\) m

  • Upper arm COM distance: \(l_{c1} = 0.5\) m

  • Forearm COM distance: \(l_{c2} = 0.5\) m

  • Upper arm inertia about shoulder: \(I_1 = \frac{m_1 l_1^2}{3} = 0.333\) kg⋅m²

  • Forearm inertia about elbow: \(I_2 = \frac{m_2 l_2^2}{3} = 0.333\) kg⋅m²

  • Gravity: \(g = 9.81\) m/s²

State Variables#

  • \(\theta_1(t)\): Shoulder joint angle from downward vertical (rad)

  • \(\theta_2(t)\): Elbow joint angle relative to upper arm (rad)

  • \(\dot{\theta}_1(t)\): Shoulder joint angular velocity (rad/s)

  • \(\dot{\theta}_2(t)\): Elbow joint angular velocity (rad/s)

Control Variable#

  • \(\tau(t)\): Elbow joint torque (N⋅m)

Notes#

The acrobot is a classic underactuated robotics problem where only the elbow joint can be controlled while the shoulder joint is passive. This creates a challenging swing-up task requiring the system to use nonlinear coupling between the joints to achieve the inverted configuration. The problem demonstrates how underactuated systems can achieve full controllability through dynamic coupling, despite having fewer actuators than degrees of freedom.

Dynamics Derivation#

The acrobot dynamics were derived using Lagrangian mechanics with SymPy, including comprehensive term-by-term verification against established literature. The derivation systematically constructs the mass matrix, gravity vector, and Coriolis terms:

examples/acrobot/acrobot_dynamics.py#
  1import sympy as sm
  2import sympy.physics.mechanics as me
  3
  4from maptor.mechanics import lagrangian_to_maptor_dynamics
  5
  6
  7# ============================================================================
  8# Physical Parameters
  9# ============================================================================
 10
 11# Link masses (kg)
 12m1, m2 = sm.symbols("m1 m2")
 13
 14# Link lengths (m)
 15l1, l2 = sm.symbols("l1 l2")
 16
 17# Center of mass distances (m)
 18lc1, lc2 = sm.symbols("lc1 lc2")
 19
 20# Moments of inertia about pivots (kg⋅m²)
 21I1, I2 = sm.symbols("I1 I2")
 22
 23# Gravity
 24g = sm.symbols("g")
 25
 26# Control torque (only on second joint)
 27tau = sm.symbols("tau")
 28
 29# Joint coordinates: theta1 (shoulder from vertical), theta2 (elbow relative)
 30theta1, theta2 = me.dynamicsymbols("theta1 theta2")
 31theta1d, theta2d = me.dynamicsymbols("theta1 theta2", 1)
 32
 33
 34# ============================================================================
 35# Reference Frames
 36# ============================================================================
 37
 38# N: Inertial frame (Y-axis points up, X-axis points right)
 39N = me.ReferenceFrame("N")
 40
 41# A: Link 1 frame (rotated by theta1 from vertical)
 42# Positive theta1 rotates counterclockwise from downward vertical (-Y direction)
 43A = N.orientnew("A", "Axis", (theta1, N.z))
 44
 45# B: Link 2 frame (rotated by theta2 relative to link 1)
 46# Positive theta2 rotates counterclockwise relative to link 1
 47B = A.orientnew("B", "Axis", (theta2, A.z))
 48
 49
 50# ============================================================================
 51# Points and Velocities
 52# ============================================================================
 53
 54# Fixed shoulder joint (origin)
 55O = me.Point("O")
 56O.set_vel(N, 0)
 57
 58# Elbow joint (end of link 1)
 59# Link 1 extends in -A.y direction (downward in link 1 frame)
 60P1 = O.locatenew("P1", l1 * (-A.y))
 61P1.v2pt_theory(O, N, A)
 62
 63# End effector (end of link 2)
 64# Link 2 extends in -B.y direction (downward in link 2 frame)
 65P2 = P1.locatenew("P2", l2 * (-B.y))
 66P2.v2pt_theory(P1, N, B)
 67
 68# Centers of mass
 69# Link 1 COM: distance lc1 from shoulder along link 1
 70G1 = O.locatenew("G1", lc1 * (-A.y))
 71G1.v2pt_theory(O, N, A)
 72
 73# Link 2 COM: distance lc2 from elbow along link 2
 74G2 = P1.locatenew("G2", lc2 * (-B.y))
 75G2.v2pt_theory(P1, N, B)
 76
 77
 78# ============================================================================
 79# Rigid Bodies
 80# ============================================================================
 81
 82# Link 1: Inertia about shoulder pivot
 83I1_dyadic = I1 * me.inertia(A, 0, 0, 1)
 84link1_body = me.RigidBody("link1", G1, A, m1, (I1_dyadic, O))  # Inertia about pivot O
 85
 86# Link 2: Inertia about elbow pivot
 87I2_dyadic = I2 * me.inertia(B, 0, 0, 1)
 88link2_body = me.RigidBody("link2", G2, B, m2, (I2_dyadic, P1))  # Inertia about pivot P1
 89
 90
 91# ============================================================================
 92# Forces (Passive Only)
 93# ============================================================================
 94
 95# Gravitational forces on centers of mass (gravity acts in -N.y direction)
 96loads = [
 97    (G1, -m1 * g * N.y),  # Gravity on link 1 COM
 98    (G2, -m2 * g * N.y),  # Gravity on link 2 COM
 99]
100
101
102# ============================================================================
103# Lagrangian Mechanics
104# ============================================================================
105
106L = me.Lagrangian(N, link1_body, link2_body)
107LM = me.LagrangesMethod(L, [theta1, theta2], forcelist=loads, frame=N)
108
109
110# ============================================================================
111# Control Forces
112# ============================================================================
113
114# Acrobot: only second joint (elbow) is actuated
115# No torque on theta1 (shoulder), torque tau on theta2 (elbow)
116control_forces = sm.Matrix([0, tau])
117
118
119# ============================================================================
120# Term-by-Term Verification
121# ============================================================================
122
123print("=== ACROBOT TERM-BY-TERM VERIFICATION ===")
124print("Literature reference: https://underactuated.csail.mit.edu/acrobot.html#section1")
125print()
126
127# Form the equations to access components
128LM.form_lagranges_equations()
129
130# Extract mass matrix M(q)
131M = LM.mass_matrix
132print("MASS MATRIX M(q):")
133print("Literature equation (8):")
134print("M11 = I1 + I2 + m2*l1² + 2*m2*l1*lc2*cos(θ2)")
135print("M12 = I2 + m2*l1*lc2*cos(θ2)")
136print("M21 = I2 + m2*l1*lc2*cos(θ2)")
137print("M22 = I2")
138print()
139print("SymPy generated:")
140print(f"M11 = {sm.simplify(M[0, 0])}")
141print(f"M12 = {sm.simplify(M[0, 1])}")
142print(f"M21 = {sm.simplify(M[1, 0])}")
143print(f"M22 = {sm.simplify(M[1, 1])}")
144print()
145
146# Extract gravity vector by setting velocities to zero
147gravity_forcing = LM.forcing.subs([(theta1d, 0), (theta2d, 0)])
148print("GRAVITY VECTOR τg(q):")
149print("Literature equation (10):")
150print("τg1 = -m1*g*lc1*sin(θ1) - m2*g*(l1*sin(θ1) + lc2*sin(θ1+θ2))")
151print("τg2 = -m2*g*lc2*sin(θ1+θ2)")
152print()
153print("SymPy generated:")
154print(f"τg1 = {sm.simplify(gravity_forcing[0])}")
155print(f"τg2 = {sm.simplify(gravity_forcing[1])}")
156print()
157
158# Extract Coriolis terms by subtracting gravity from total forcing
159total_forcing = LM.forcing
160coriolis_forcing = sm.simplify(total_forcing - gravity_forcing)
161print("CORIOLIS TERMS (velocity-dependent):")
162print("Literature: C(q,q̇)*q̇ where")
163print("C11 = -2*m2*l1*lc2*sin(θ2)*θ̇2")
164print("C12 = -m2*l1*lc2*sin(θ2)*θ̇2")
165print("C21 = m2*l1*lc2*sin(θ2)*θ̇1")
166print("C22 = 0")
167print()
168print("SymPy generated Coriolis forcing:")
169print(f"Coriolis1 = {sm.simplify(coriolis_forcing[0])}")
170print(f"Coriolis2 = {sm.simplify(coriolis_forcing[1])}")
171print()
172
173# Verify equation structure
174print("COMPLETE EQUATION VERIFICATION:")
175print("Literature form: M(q)*q̈ + C(q,q̇)*q̇ = τg(q) + B*u")
176print("Where B = [0, 1]ᵀ and u = τ (elbow torque)")
177print()
178
179
180# ============================================================================
181# Convert to MAPTOR Format
182# ============================================================================
183
184print("=== MAPTOR DYNAMICS GENERATION ===")
185lagrangian_to_maptor_dynamics(LM, [theta1, theta2], control_forces, "acrobot_dynamics.txt")
186
187"""
188=== ACROBOT TERM-BY-TERM VERIFICATION ===
189Literature reference: https://underactuated.csail.mit.edu/acrobot.html#section1
190
191MASS MATRIX M(q):
192Literature equation (8):
193M11 = I1 + I2 + m2*l1² + 2*m2*l1*lc2*cos(θ2)
194M12 = I2 + m2*l1*lc2*cos(θ2)
195M21 = I2 + m2*l1*lc2*cos(θ2)
196M22 = I2
197
198SymPy generated:
199M11 = I1 + I2 + l1**2*m2 + 2*l1*lc2*m2*cos(theta2(t))
200M12 = I2 + l1*lc2*m2*cos(theta2(t))
201M21 = I2 + l1*lc2*m2*cos(theta2(t))
202M22 = I2
203
204GRAVITY VECTOR τg(q):
205Literature equation (10):
206τg1 = -m1*g*lc1*sin(θ1) - m2*g*(l1*sin(θ1) + lc2*sin(θ1+θ2))
207τg2 = -m2*g*lc2*sin(θ1+θ2)
208
209SymPy generated:
210τg1 = -g*(l1*m2*sin(theta1(t)) + lc1*m1*sin(theta1(t)) + lc2*m2*sin(theta1(t) + theta2(t)))
211τg2 = -g*lc2*m2*sin(theta1(t) + theta2(t))
212
213CORIOLIS TERMS (velocity-dependent):
214Literature: C(q,q̇)*q̇ where
215C11 = -2*m2*l1*lc2*sin(θ2)*θ̇2
216C12 = -m2*l1*lc2*sin(θ2)*θ̇2
217C21 = m2*l1*lc2*sin(θ2)*θ̇1
218C22 = 0
219
220SymPy generated Coriolis forcing:
221Coriolis1 = l1*lc2*m2*(2*Derivative(theta1(t), t) + Derivative(theta2(t), t))*sin(theta2(t))*Derivative(theta2(t), t)
222Coriolis2 = -l1*lc2*m2*sin(theta2(t))*Derivative(theta1(t), t)**2
223
224COMPLETE EQUATION VERIFICATION:
225Literature form: M(q)*q̈ + C(q,q̇)*q̇ = τg(q) + B*u
226Where B = [0, 1]ᵀ and u = τ (elbow torque)
227
228=== MAPTOR DYNAMICS GENERATION ===
229CasADi MAPTOR Dynamics:
230============================================================
231
232State variables:
233theta1 = phase.state('theta1')
234theta2 = phase.state('theta2')
235theta1_dot = phase.state('theta1_dot')
236theta2_dot = phase.state('theta2_dot')
237
238Control variables:
239tau = phase.control('tau')
240
241MAPTOR dynamics dictionary:
242phase.dynamics(
243    {
244        theta1: theta1_dot,
245        theta2: theta2_dot,
246        theta1_dot: (
247            -I2
248            * (
249                g * l1 * m2 * ca.sin(theta1)
250                + g * lc1 * m1 * ca.sin(theta1)
251                + g * lc2 * m2 * ca.sin(theta1 + theta2)
252                - l1 * lc2 * m2 * (2 * theta1_dot + theta2_dot) * ca.sin(theta2) * theta2_dot
253            )
254            + (I2 + l1 * lc2 * m2 * ca.cos(theta2))
255            * (
256                g * lc2 * m2 * ca.sin(theta1 + theta2)
257                + l1 * lc2 * m2 * ca.sin(theta2) * theta1_dot**2
258                - tau
259            )
260        )
261        / (I1 * I2 + I2 * l1**2 * m2 - l1**2 * lc2**2 * m2**2 * ca.cos(theta2) ** 2),
262        theta2_dot: (
263            (I2 + l1 * lc2 * m2 * ca.cos(theta2))
264            * (
265                g * l1 * m2 * ca.sin(theta1)
266                + g * lc1 * m1 * ca.sin(theta1)
267                + g * lc2 * m2 * ca.sin(theta1 + theta2)
268                - l1 * lc2 * m2 * (2 * theta1_dot + theta2_dot) * ca.sin(theta2) * theta2_dot
269            )
270            - (
271                g * lc2 * m2 * ca.sin(theta1 + theta2)
272                + l1 * lc2 * m2 * ca.sin(theta2) * theta1_dot**2
273                - tau
274            )
275            * (I1 + I2 + l1**2 * m2 + 2 * l1 * lc2 * m2 * ca.cos(theta2))
276        )
277        / (I1 * I2 + I2 * l1**2 * m2 - l1**2 * lc2**2 * m2**2 * ca.cos(theta2) ** 2),
278    }
279)
280"""

This symbolic derivation produces the complex coupled dynamics equations used in the swing-up controller implementation, ensuring mathematical correctness through comparison with MIT’s Underactuated Robotics course materials and providing complete transparency in the underlying mechanics.

Running This Example#

cd examples/acrobot
python acrobot.py
python acrobot_animate.py

Code Implementation#

examples/acrobot/acrobot.py#
  1import casadi as ca
  2import numpy as np
  3
  4import maptor as mtor
  5
  6
  7# ============================================================================
  8# Physical Parameters
  9# ============================================================================
 10
 11# Link masses (kg)
 12m1 = 1.0  # Upper arm mass
 13m2 = 1.0  # Forearm mass
 14
 15# Link lengths (m)
 16l1 = 1.0  # Upper arm length
 17l2 = 1.0  # Forearm length
 18
 19# Center of mass distances (m)
 20lc1 = 0.5  # Upper arm COM distance from shoulder
 21lc2 = 0.5  # Forearm COM distance from elbow
 22
 23# Moments of inertia about pivots (kg⋅m²)
 24I1 = m1 * l1**2 / 3  # Upper arm about shoulder
 25I2 = m2 * l2**2 / 3  # Forearm about elbow
 26
 27# Gravity
 28g = 9.81  # m/s²
 29
 30
 31# ============================================================================
 32# Problem Setup
 33# ============================================================================
 34
 35problem = mtor.Problem("Acrobot Swing-Up")
 36phase = problem.set_phase(1)
 37
 38
 39# ============================================================================
 40# Variables
 41# ============================================================================
 42
 43# Time variable
 44t = phase.time(initial=0.0)
 45
 46# State variables
 47theta1 = phase.state("theta1", initial=np.pi / 6, final=np.pi)  # Shoulder: down to up
 48theta2 = phase.state("theta2", initial=0.0, final=0.0)  # Elbow: straight to straight
 49theta1_dot = phase.state("theta1_dot", initial=0.0, final=0.0)  # Start and end at rest
 50theta2_dot = phase.state("theta2_dot", initial=0.0, final=0.0)  # Start and end at rest
 51
 52# Control variable (only elbow joint is actuated)
 53tau = phase.control("tau", boundary=(-20.0, 20.0))  # Elbow torque (N⋅m)
 54
 55
 56# ============================================================================
 57# Dynamics (Generated from acrobot_dynamics.py)
 58# ============================================================================
 59
 60phase.dynamics(
 61    {
 62        theta1: theta1_dot,
 63        theta2: theta2_dot,
 64        theta1_dot: (
 65            -I2
 66            * (
 67                g * l1 * m2 * ca.sin(theta1)
 68                + g * lc1 * m1 * ca.sin(theta1)
 69                + g * lc2 * m2 * ca.sin(theta1 + theta2)
 70                - l1 * lc2 * m2 * (2 * theta1_dot + theta2_dot) * ca.sin(theta2) * theta2_dot
 71            )
 72            + (I2 + l1 * lc2 * m2 * ca.cos(theta2))
 73            * (
 74                g * lc2 * m2 * ca.sin(theta1 + theta2)
 75                + l1 * lc2 * m2 * ca.sin(theta2) * theta1_dot**2
 76                - tau
 77            )
 78        )
 79        / (I1 * I2 + I2 * l1**2 * m2 - l1**2 * lc2**2 * m2**2 * ca.cos(theta2) ** 2),
 80        theta2_dot: (
 81            (I2 + l1 * lc2 * m2 * ca.cos(theta2))
 82            * (
 83                g * l1 * m2 * ca.sin(theta1)
 84                + g * lc1 * m1 * ca.sin(theta1)
 85                + g * lc2 * m2 * ca.sin(theta1 + theta2)
 86                - l1 * lc2 * m2 * (2 * theta1_dot + theta2_dot) * ca.sin(theta2) * theta2_dot
 87            )
 88            - (
 89                g * lc2 * m2 * ca.sin(theta1 + theta2)
 90                + l1 * lc2 * m2 * ca.sin(theta2) * theta1_dot**2
 91                - tau
 92            )
 93            * (I1 + I2 + l1**2 * m2 + 2 * l1 * lc2 * m2 * ca.cos(theta2))
 94        )
 95        / (I1 * I2 + I2 * l1**2 * m2 - l1**2 * lc2**2 * m2**2 * ca.cos(theta2) ** 2),
 96    }
 97)
 98
 99
100# ============================================================================
101# Objective
102# ============================================================================
103
104control_effort = phase.add_integral(tau**2)
105problem.minimize(t.final + 0.01 * control_effort)
106
107
108# ============================================================================
109# Mesh Configuration and Initial Guess
110# ============================================================================
111
112num_interval = 16
113phase.mesh([3] * num_interval, np.linspace(-1.0, 1.0, num_interval + 1))
114
115phase.guess(
116    terminal_time=10.0,
117)
118
119
120# ============================================================================
121# Solve
122# ============================================================================
123
124solution = mtor.solve_adaptive(
125    problem,
126    error_tolerance=1e-3,
127    max_iterations=20,
128    min_polynomial_degree=3,
129    max_polynomial_degree=8,
130    nlp_options={
131        "ipopt.max_iter": 1000,
132        "ipopt.mumps_pivtol": 5e-7,
133        "ipopt.linear_solver": "mumps",
134        "ipopt.constr_viol_tol": 1e-7,
135        "ipopt.print_level": 0,
136        "ipopt.nlp_scaling_method": "gradient-based",
137        "ipopt.mu_strategy": "adaptive",
138        "ipopt.check_derivatives_for_naninf": "yes",
139        "ipopt.hessian_approximation": "exact",
140        "ipopt.tol": 1e-8,
141    },
142)
143
144
145# ============================================================================
146# Results
147# ============================================================================
148
149if solution.status["success"]:
150    print(f"Objective: {solution.status['objective']:.6f}")
151    print(f"Mission time: {solution.status['total_mission_time']:.3f} seconds")
152
153    # Final joint angles
154    theta1_final = solution["theta1"][-1]
155    theta2_final = solution["theta2"][-1]
156    print(f"Final shoulder angle: {theta1_final:.6f} rad ({np.degrees(theta1_final):.2f}°)")
157    print(f"Final elbow angle: {theta2_final:.6f} rad ({np.degrees(theta2_final):.2f}°)")
158
159    # End-effector position analysis
160    # Initial position (both links hanging down)
161    x_ee_initial = l1 * np.sin(0) + l2 * np.sin(0 + 0)
162    y_ee_initial = -l1 * np.cos(0) - l2 * np.cos(0 + 0)
163
164    # Final position (both links pointing up)
165    x_ee_final = l1 * np.sin(theta1_final) + l2 * np.sin(theta1_final + theta2_final)
166    y_ee_final = -l1 * np.cos(theta1_final) - l2 * np.cos(theta1_final + theta2_final)
167
168    print(
169        f"End-effector moved from ({x_ee_initial:.3f}, {y_ee_initial:.3f}) to ({x_ee_final:.3f}, {y_ee_final:.3f})"
170    )
171
172    # Control statistics
173    tau_max = max(np.abs(solution["tau"]))
174    print(f"Maximum elbow torque: {tau_max:.3f} N⋅m")
175
176    solution.plot()
177
178else:
179    print(f"Failed: {solution.status['message']}")