Robot Arm Control#

Mathematical Formulation#

Problem Statement#

Find the optimal control torques \(u_1(t)\), \(u_2(t)\), \(u_3(t)\) that minimize the maneuver time:

\[J = t_f\]

Subject to the robot arm dynamic constraints:

\[\frac{dy_1}{dt} = y_2\]
\[\frac{dy_2}{dt} = \frac{u_1}{L}\]
\[\frac{dy_3}{dt} = y_4\]
\[\frac{dy_4}{dt} = \frac{u_2}{I_\theta}\]
\[\frac{dy_5}{dt} = y_6\]
\[\frac{dy_6}{dt} = \frac{u_3}{I_\phi}\]

Where the inertias are:

\[I_\phi = \frac{1}{3}\left[(L - y_1)^3 + y_1^3\right]\]
\[I_\theta = I_\phi \sin^2(y_5)\]

Boundary Conditions#

  • Initial conditions: \(y_1(0) = 4.5\), \(y_2(0) = 0\), \(y_3(0) = 0\), \(y_4(0) = 0\), \(y_5(0) = \frac{\pi}{4}\), \(y_6(0) = 0\)

  • Final conditions: \(y_1(t_f) = 4.5\), \(y_2(t_f) = 0\), \(y_3(t_f) = \frac{2\pi}{3}\), \(y_4(t_f) = 0\), \(y_5(t_f) = \frac{\pi}{4}\), \(y_6(t_f) = 0\)

  • Control bounds: \(-1 \leq u_i \leq 1\) for \(i = 1,2,3\)

Physical Parameters#

  • Robot arm length: \(L = 5\) m

State Variables#

  • \(y_1(t)\): Radial position coordinate (m)

  • \(y_2(t)\): Radial velocity (m/s)

  • \(y_3(t)\): Azimuthal angle (rad)

  • \(y_4(t)\): Azimuthal angular velocity (rad/s)

  • \(y_5(t)\): Elevation angle (rad)

  • \(y_6(t)\): Elevation angular velocity (rad/s)

Control Variables#

  • \(u_1(t)\): Radial control torque

  • \(u_2(t)\): Azimuthal control torque

  • \(u_3(t)\): Elevation control torque

Notes#

This problem involves controlling a robot arm with three degrees of freedom to perform a minimum-time maneuver with bounded control torques.

Running This Example#

cd examples/robot_arm
python robot_arm.py

Code Implementation#

examples/robot_arm/robot_arm.py#
  1import casadi as ca
  2import numpy as np
  3
  4import maptor as mtor
  5
  6
  7# Problem parameters
  8L = 5
  9
 10# Problem setup
 11problem = mtor.Problem("Robot Arm Control")
 12phase = problem.set_phase(1)
 13
 14# Variables (free final time - minimum time problem)
 15t = phase.time(initial=0.0)
 16y1 = phase.state("y1", initial=9.0 / 2.0, final=9.0 / 2.0)
 17y2 = phase.state("y2", initial=0.0, final=0.0)
 18y3 = phase.state("y3", initial=0.0, final=2.0 * np.pi / 3.0)
 19y4 = phase.state("y4", initial=0.0, final=0.0)
 20y5 = phase.state("y5", initial=np.pi / 4.0, final=np.pi / 4.0)
 21y6 = phase.state("y6", initial=0.0, final=0.0)
 22
 23u1 = phase.control("u1", boundary=(-1.0, 1.0))
 24u2 = phase.control("u2", boundary=(-1.0, 1.0))
 25u3 = phase.control("u3", boundary=(-1.0, 1.0))
 26
 27# Inertia calculations (equations 10.832, 10.833)
 28I_phi = (1.0 / 3.0) * ((L - y1) ** 3 + y1**3)
 29I_theta = I_phi * (ca.sin(y5)) ** 2
 30
 31# Dynamics (equations 10.826-10.831)
 32phase.dynamics({y1: y2, y2: u1 / L, y3: y4, y4: u2 / I_theta, y5: y6, y6: u3 / I_phi})
 33
 34# Objective: minimize final time (minimum time problem)
 35problem.minimize(t.final)
 36
 37# Mesh and guess
 38phase.mesh([8, 8, 8], [-1.0, -1 / 3, 1 / 3, 1.0])
 39
 40states_guess = []
 41controls_guess = []
 42for N in [8, 8, 8]:
 43    tau = np.linspace(-1, 1, N + 1)
 44    t_norm = (tau + 1) / 2
 45
 46    # Linear interpolation between initial and final conditions
 47    y1_vals = 9.0 / 2.0 + (9.0 / 2.0 - 9.0 / 2.0) * t_norm  # Constant
 48    y2_vals = 0.0 + (0.0 - 0.0) * t_norm  # Constant at zero
 49    y3_vals = 0.0 + (2.0 * np.pi / 3.0 - 0.0) * t_norm  # Linear increase
 50    y4_vals = 0.0 + (0.0 - 0.0) * t_norm  # Constant at zero
 51    y5_vals = np.pi / 4.0 + (np.pi / 4.0 - np.pi / 4.0) * t_norm  # Constant
 52    y6_vals = 0.0 + (0.0 - 0.0) * t_norm  # Constant at zero
 53
 54    states_guess.append(np.vstack([y1_vals, y2_vals, y3_vals, y4_vals, y5_vals, y6_vals]))
 55
 56    # Control guess (small values for smooth motion)
 57    u1_vals = np.zeros(N)
 58    u2_vals = 0.1 * np.sin(np.pi * np.linspace(0, 1, N))  # Smooth motion for y3
 59    u3_vals = np.zeros(N)
 60    controls_guess.append(np.vstack([u1_vals, u2_vals, u3_vals]))
 61
 62phase.guess(
 63    states=states_guess,
 64    controls=controls_guess,
 65    terminal_time=9.0,
 66)
 67
 68# Solve
 69solution = mtor.solve_adaptive(
 70    problem,
 71    min_polynomial_degree=3,
 72    max_polynomial_degree=12,
 73    nlp_options={
 74        "ipopt.print_level": 0,
 75        "ipopt.max_iter": 3000,
 76        "ipopt.tol": 1e-8,
 77        "ipopt.constr_viol_tol": 1e-7,
 78    },
 79)
 80
 81# Results
 82if solution.status["success"]:
 83    final_time = solution.phases[1]["times"]["final"]
 84    print(f"Minimum time: {final_time:.8f}")
 85    print(f"Reference: 9.14093620 (Error: {abs(final_time - 9.14093620) / 9.14093620 * 100:.3f}%)")
 86
 87    # Final state verification
 88    y1_final = solution[(1, "y1")][-1]
 89    y2_final = solution[(1, "y2")][-1]
 90    y3_final = solution[(1, "y3")][-1]
 91    y4_final = solution[(1, "y4")][-1]
 92    y5_final = solution[(1, "y5")][-1]
 93    y6_final = solution[(1, "y6")][-1]
 94
 95    print("Final states:")
 96    print(f"  y1: {y1_final:.6f} (target: {9.0 / 2.0:.6f})")
 97    print(f"  y2: {y2_final:.6f} (target: 0.0)")
 98    print(f"  y3: {y3_final:.6f} (target: {2.0 * np.pi / 3.0:.6f})")
 99    print(f"  y4: {y4_final:.6f} (target: 0.0)")
100    print(f"  y5: {y5_final:.6f} (target: {np.pi / 4.0:.6f})")
101    print(f"  y6: {y6_final:.6f} (target: 0.0)")
102
103    solution.plot()
104else:
105    print(f"Failed: {solution.status['message']}")