2DOF Manipulator Design and Trajectory Optimization#

Mathematical Formulation#

Problem Statement#

Find the optimal motor sizing and joint torques that minimize actuator investment, mission time, and energy consumption for a 2DOF planar manipulator with 5kg payload performing point-to-point motion:

\[J = t_f + 0.1 \cdot C_{\text{actuator}} + 0.01 \int_0^{t_f} (\tau_1^2 + \tau_2^2) dt\]

Where the actuator cost model is:

\[C_{\text{actuator}} = 0.08 \cdot \tau_{\max,1} + 0.05 \cdot \tau_{\max,2}\]

Subject to the coupled 2DOF manipulator dynamics and actuator capability constraints:

\[\frac{dq_1}{dt} = \dot{q}_1\]
\[\frac{dq_2}{dt} = \dot{q}_2\]
\[\frac{d\dot{q}_1}{dt} = \frac{(I_2 + l_{c2}^2 m_2)(-g l_1 m_2 \cos(q_1) - g l_1 m_{payload} \cos(q_1) - g l_2 m_{payload} \cos(q_1 + q_2) - g l_{c1} m_1 \cos(q_1) - g l_{c2} m_2 \cos(q_1 + q_2) + l_1 l_{c2} m_2 (2\dot{q}_1 + \dot{q}_2) \sin(q_2) \dot{q}_2 + \tau_1) - (I_2 + l_1 l_{c2} m_2 \cos(q_2) + l_{c2}^2 m_2)(-g l_2 m_{payload} \cos(q_1 + q_2) - g l_{c2} m_2 \cos(q_1 + q_2) - l_1 l_{c2} m_2 \sin(q_2) \dot{q}_1^2 + \tau_2)}{I_1 I_2 + I_1 l_{c2}^2 m_2 + I_2 l_1^2 m_2 + I_2 l_{c1}^2 m_1 + l_1^2 l_{c2}^2 m_2^2 \sin^2(q_2) + l_{c1}^2 l_{c2}^2 m_1 m_2}\]
\[\frac{d\dot{q}_2}{dt} = \frac{-(I_2 + l_1 l_{c2} m_2 \cos(q_2) + l_{c2}^2 m_2)(-g l_1 m_2 \cos(q_1) - g l_1 m_{payload} \cos(q_1) - g l_2 m_{payload} \cos(q_1 + q_2) - g l_{c1} m_1 \cos(q_1) - g l_{c2} m_2 \cos(q_1 + q_2) + l_1 l_{c2} m_2 (2\dot{q}_1 + \dot{q}_2) \sin(q_2) \dot{q}_2 + \tau_1) + (-g l_2 m_{payload} \cos(q_1 + q_2) - g l_{c2} m_2 \cos(q_1 + q_2) - l_1 l_{c2} m_2 \sin(q_2) \dot{q}_1^2 + \tau_2)(I_1 + I_2 + l_1^2 m_2 + 2 l_1 l_{c2} m_2 \cos(q_2) + l_{c1}^2 m_1 + l_{c2}^2 m_2)}{I_1 I_2 + I_1 l_{c2}^2 m_2 + I_2 l_1^2 m_2 + I_2 l_{c1}^2 m_1 + l_1^2 l_{c2}^2 m_2^2 \sin^2(q_2) + l_{c1}^2 l_{c2}^2 m_1 m_2}\]

Robot Configuration#

Joint Configuration:

  • Joint 1: Base rotation (shoulder joint)

  • Joint 2: Elbow rotation (relative to upper arm)

Forward Kinematics:

\[x_{ee} = l_1\cos(q_1) + l_2\cos(q_1+q_2)\]
\[y_{ee} = l_1\sin(q_1) + l_2\sin(q_1+q_2)\]

Design Parameters#

  • Shoulder motor torque rating: \(\tau_{\max,1} \in [5, 100]\) N⋅m

  • Elbow motor torque rating: \(\tau_{\max,2} \in [5, 100]\) N⋅m

Boundary Conditions#

  • Initial end-effector position: \((0.3, 0.1)\) m

  • Final end-effector position: \((-0.6, 0.1)\) m

  • Initial joint velocities: \(\dot{q}_1(0) = \dot{q}_2(0) = 0\) rad/s

  • Final joint velocities: \(\dot{q}_1(t_f) = \dot{q}_2(t_f) = 0\) rad/s

  • Joint angle bounds: \(q_1 \in [0, \pi]\) rad, \(q_2 \in [-\pi, \pi]\) rad

  • Joint velocity bounds: \(\dot{q}_1, \dot{q}_2 \in [-2.0, 2.0]\) rad/s

  • Actuator capability constraints: \(-\tau_{\max,i} \leq \tau_i \leq \tau_{\max,i}\) for \(i = 1,2\)

Physical Parameters#

  • Link masses: \(m_1 = 2.0\) kg, \(m_2 = 1.5\) kg

  • Payload mass: \(m_{payload} = 5.0\) kg

  • Link lengths: \(l_1 = 0.4\) m, \(l_2 = 0.4\) m

  • Center of mass distances: \(l_{c1} = 0.20\) m, \(l_{c2} = 0.20\) m

  • Moments of inertia: \(I_1 = 0.0267\) kg⋅m², \(I_2 = 0.02\) kg⋅m²

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

State Variables#

  • \(q_1(t)\): Shoulder joint angle (rad)

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

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

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

Control Variables#

  • \(\tau_1(t)\): Shoulder joint torque (N⋅m)

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

Constraints#

  • Ground clearance constraint: End-effector height \(y_{ee} \geq 0.05\) m

  • Actuator capability: \(|\tau_i(t)| \leq \tau_{\max,i}\) for all \(t\) and \(i = 1,2\)

  • Inverse kinematics: Joint angles computed to achieve desired end-effector positions

  • Reachability verification: Target positions must satisfy \(d_{min} \leq \|r_{target}\| \leq d_{max}\)

Notes#

This problem demonstrates simultaneous design and trajectory optimization for robotics applications. Unlike pure trajectory planning, MAPTOR optimizes both the motor sizing (design parameters) and the motion trajectory simultaneously. The optimization determines the minimum motor torque ratings required while minimizing mission time and energy consumption for a planar 2DOF manipulator transporting a 5kg payload.

The planar configuration simplifies the 3D case while maintaining the essential coupling between joint dynamics, payload effects, and actuator sizing trade-offs.

Dynamics Derivation#

The 2DOF manipulator dynamics were derived using Lagrangian mechanics with SymPy, systematically constructing the equations of motion for the two-joint planar system with payload:

examples/manipulator_2dof/manipulator_2dof_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
 11m1, m2 = sm.symbols("m1 m2")
 12m_payload = sm.symbols("m_payload")
 13l1, l2 = sm.symbols("l1 l2")
 14lc1, lc2 = sm.symbols("lc1 lc2")
 15I1, I2 = sm.symbols("I1 I2")
 16g = sm.symbols("g")
 17tau1, tau2 = sm.symbols("tau1 tau2")
 18
 19q1, q2 = me.dynamicsymbols("q1 q2")
 20q1d, q2d = me.dynamicsymbols("q1 q2", 1)
 21
 22
 23# ============================================================================
 24# Reference Frames
 25# ============================================================================
 26
 27N = me.ReferenceFrame("N")
 28A = N.orientnew("A", "Axis", (q1, N.z))
 29B = A.orientnew("B", "Axis", (q2, A.z))
 30
 31
 32# ============================================================================
 33# Points and Velocities
 34# ============================================================================
 35
 36O = me.Point("O")
 37O.set_vel(N, 0)
 38
 39P1 = O.locatenew("P1", l1 * A.x)
 40P1.v2pt_theory(O, N, A)
 41
 42G1 = O.locatenew("G1", lc1 * A.x)
 43G1.v2pt_theory(O, N, A)
 44
 45G2 = P1.locatenew("G2", lc2 * B.x)
 46G2.v2pt_theory(P1, N, B)
 47
 48P2 = P1.locatenew("P2", l2 * B.x)
 49P2.v2pt_theory(P1, N, B)
 50
 51
 52# ============================================================================
 53# Rigid Bodies
 54# ============================================================================
 55
 56I1_dyadic = I1 * me.inertia(A, 0, 0, 1)
 57link1_body = me.RigidBody("link1", G1, A, m1, (I1_dyadic, G1))
 58
 59I2_dyadic = I2 * me.inertia(B, 0, 0, 1)
 60link2_body = me.RigidBody("link2", G2, B, m2, (I2_dyadic, G2))
 61
 62
 63# ============================================================================
 64# Forces (Passive Only)
 65# ============================================================================
 66
 67loads = [
 68    (G1, -m1 * g * N.y),  # Gravity on link 1 COM
 69    (G2, -m2 * g * N.y),  # Gravity on link 2 COM
 70    (P2, -m_payload * g * N.y),  # Gravity on payload at end effector
 71]
 72
 73
 74# ============================================================================
 75# Lagrangian Mechanics
 76# ============================================================================
 77
 78L = me.Lagrangian(N, link1_body, link2_body)
 79LM = me.LagrangesMethod(L, [q1, q2], forcelist=loads, frame=N)
 80
 81
 82# ============================================================================
 83# Control Forces
 84# ============================================================================
 85
 86# Joint torques acting on each generalized coordinate
 87control_forces = sm.Matrix([tau1, tau2])
 88
 89
 90# ============================================================================
 91# Convert to MAPTOR Format
 92# ============================================================================
 93
 94lagrangian_to_maptor_dynamics(LM, [q1, q2], control_forces, "2dof_dynamics.txt")
 95
 96"""
 97Output ready to be copy-pasted:
 98
 99State variables:
100q1 = phase.state('q1')
101q2 = phase.state('q2')
102q1_dot = phase.state('q1_dot')
103q2_dot = phase.state('q2_dot')
104
105Control variables:
106tau1 = phase.control('tau1')
107tau2 = phase.control('tau2')
108
109MAPTOR dynamics dictionary:
110phase.dynamics(
111    {
112        q1: q1_dot,
113        q2: q2_dot,
114        q1_dot: (
115            (I2 + lc2**2 * m2)
116            * (
117                -g * l1 * m2 * ca.cos(q1)
118                - g * l1 * m_payload * ca.cos(q1)
119                - g * l2 * m_payload * ca.cos(q1 + q2)
120                - g * lc1 * m1 * ca.cos(q1)
121                - g * lc2 * m2 * ca.cos(q1 + q2)
122                + l1 * lc2 * m2 * (2 * q1_dot + q2_dot) * ca.sin(q2) * q2_dot
123                + tau1
124            )
125            - (I2 + l1 * lc2 * m2 * ca.cos(q2) + lc2**2 * m2)
126            * (
127                -g * l2 * m_payload * ca.cos(q1 + q2)
128                - g * lc2 * m2 * ca.cos(q1 + q2)
129                - l1 * lc2 * m2 * ca.sin(q2) * q1_dot**2
130                + tau2
131            )
132        )
133        / (
134            I1 * I2
135            + I1 * lc2**2 * m2
136            + I2 * l1**2 * m2
137            + I2 * lc1**2 * m1
138            + l1**2 * lc2**2 * m2**2 * ca.sin(q2) ** 2
139            + lc1**2 * lc2**2 * m1 * m2
140        ),
141        q2_dot: (
142            -(I2 + l1 * lc2 * m2 * ca.cos(q2) + lc2**2 * m2)
143            * (
144                -g * l1 * m2 * ca.cos(q1)
145                - g * l1 * m_payload * ca.cos(q1)
146                - g * l2 * m_payload * ca.cos(q1 + q2)
147                - g * lc1 * m1 * ca.cos(q1)
148                - g * lc2 * m2 * ca.cos(q1 + q2)
149                + l1 * lc2 * m2 * (2 * q1_dot + q2_dot) * ca.sin(q2) * q2_dot
150                + tau1
151            )
152            + (
153                -g * l2 * m_payload * ca.cos(q1 + q2)
154                - g * lc2 * m2 * ca.cos(q1 + q2)
155                - l1 * lc2 * m2 * ca.sin(q2) * q1_dot**2
156                + tau2
157            )
158            * (I1 + I2 + l1**2 * m2 + 2 * l1 * lc2 * m2 * ca.cos(q2) + lc1**2 * m1 + lc2**2 * m2)
159        )
160        / (
161            I1 * I2
162            + I1 * lc2**2 * m2
163            + I2 * l1**2 * m2
164            + I2 * lc1**2 * m1
165            + l1**2 * lc2**2 * m2**2 * ca.sin(q2) ** 2
166            + lc1**2 * lc2**2 * m1 * m2
167        ),
168    }
169)
170"""

This symbolic derivation produces the coupled dynamics equations accounting for gravitational forces from both links and payload, centrifugal forces, Coriolis coupling between joints, and inertial effects. The systematic approach ensures mathematical correctness for the planar multi-body system.

Running This Example#

cd examples/manipulator_2dof
python manipulator_2dof.py
python manipulator_2dof_animate.py

Code Implementation#

examples/manipulator_2dof/manipulator_2dof.py#
  1import casadi as ca
  2import numpy as np
  3
  4import maptor as mtor
  5
  6
  7# ============================================================================
  8# Physical Parameters
  9# ============================================================================
 10
 11m1 = 2.0
 12m2 = 1.5
 13m_payload = 5.0
 14
 15l1 = 0.4
 16l2 = 0.4
 17
 18lc1 = 0.20
 19lc2 = 0.20
 20
 21I1 = m1 * l1**2 / 12
 22I2 = m2 * l2**2 / 12
 23
 24g = 9.81
 25
 26
 27# ============================================================================
 28# End-Effector Position Specification
 29# ============================================================================
 30
 31x_ee_initial = 0.3
 32y_ee_initial = 0.1
 33
 34x_ee_final = -0.6
 35y_ee_final = 0.1
 36
 37
 38# ============================================================================
 39# Forward/Inverse Kinematics
 40# ============================================================================
 41
 42
 43def calculate_inverse_kinematics(x_target, y_target, l1, l2):
 44    r_total = np.sqrt(x_target**2 + y_target**2)
 45
 46    max_reach = l1 + l2
 47    min_reach = abs(l1 - l2)
 48
 49    if r_total > max_reach:
 50        raise ValueError(f"Target unreachable: distance {r_total:.3f} > max reach {max_reach:.3f}")
 51    if r_total < min_reach:
 52        raise ValueError(f"Target too close: distance {r_total:.3f} < min reach {min_reach:.3f}")
 53
 54    cos_q2 = (r_total**2 - l1**2 - l2**2) / (2 * l1 * l2)
 55    cos_q2 = np.clip(cos_q2, -1, 1)
 56
 57    alpha = np.arctan2(y_target, x_target)
 58
 59    # Try elbow up configuration first
 60    q2_up = np.arccos(cos_q2)
 61    beta_up = np.arctan2(l2 * np.sin(q2_up), l1 + l2 * np.cos(q2_up))
 62    q1_up = alpha - beta_up
 63
 64    # Try elbow down configuration
 65    q2_down = -np.arccos(cos_q2)
 66    beta_down = np.arctan2(l2 * np.sin(q2_down), l1 + l2 * np.cos(q2_down))
 67    q1_down = alpha - beta_down
 68
 69    # Select configuration that keeps q1 >= 0 (above ground)
 70    if q1_up >= 0:
 71        return q1_up, q2_up
 72    elif q1_down >= 0:
 73        return q1_down, q2_down
 74    else:
 75        raise ValueError(
 76            f"Target ({x_target:.3f}, {y_target:.3f}) requires base link below ground. Both solutions: q1_up={np.degrees(q1_up):.1f}°, q1_down={np.degrees(q1_down):.1f}°"
 77        )
 78
 79
 80def verify_forward_kinematics(q1, q2, l1, l2):
 81    x_ee = l1 * np.cos(q1) + l2 * np.cos(q1 + q2)
 82    y_ee = l1 * np.sin(q1) + l2 * np.sin(q1 + q2)
 83    return x_ee, y_ee
 84
 85
 86q1_initial, q2_initial = calculate_inverse_kinematics(x_ee_initial, y_ee_initial, l1, l2)
 87q1_final, q2_final = calculate_inverse_kinematics(x_ee_final, y_ee_final, l1, l2)
 88
 89x_check, y_check = verify_forward_kinematics(q1_final, q2_final, l1, l2)
 90position_error = np.sqrt((x_check - x_ee_final) ** 2 + (y_check - y_ee_final) ** 2)
 91
 92print("=== INVERSE KINEMATICS VERIFICATION ===")
 93print(f"Target position: ({x_ee_final:.3f}, {y_ee_final:.3f}) m")
 94print(f"Calculated joint angles: q1={np.degrees(q1_final):.1f}°, q2={np.degrees(q2_final):.1f}°")
 95print(f"Forward kinematics check: ({x_check:.3f}, {y_check:.3f}) m")
 96print(f"Position error: {position_error:.6f} m")
 97print()
 98
 99
100# ============================================================================
101# Problem Setup
102# ============================================================================
103
104problem = mtor.Problem("Enhanced 2DOF Manipulator")
105phase = problem.set_phase(1)
106
107
108# ============================================================================
109# Design Parameters
110# ============================================================================
111
112max_tau1 = problem.parameter("joint_1_torque", boundary=(5.0, 100.0))
113max_tau2 = problem.parameter("joint_2_torque", boundary=(5.0, 100.0))
114
115
116# ============================================================================
117# Variables
118# ============================================================================
119
120t = phase.time(initial=0.0)
121
122q1 = phase.state("q1", initial=q1_initial, final=q1_final, boundary=(0, np.pi))
123q2 = phase.state("q2", initial=q2_initial, final=q2_final, boundary=(-np.pi, np.pi))
124
125q1_dot = phase.state("q1_dot", initial=0.0, final=0.0, boundary=(-2.0, 2.0))
126q2_dot = phase.state("q2_dot", initial=0.0, final=0.0, boundary=(-2.0, 2.0))
127
128tau1 = phase.control("tau1")
129tau2 = phase.control("tau2")
130
131
132# ============================================================================
133# Dynamics
134# ============================================================================
135
136phase.dynamics(
137    {
138        q1: q1_dot,
139        q2: q2_dot,
140        q1_dot: (
141            (I2 + lc2**2 * m2)
142            * (
143                -g * l1 * m2 * ca.cos(q1)
144                - g * l1 * m_payload * ca.cos(q1)
145                - g * l2 * m_payload * ca.cos(q1 + q2)
146                - g * lc1 * m1 * ca.cos(q1)
147                - g * lc2 * m2 * ca.cos(q1 + q2)
148                + l1 * lc2 * m2 * (2 * q1_dot + q2_dot) * ca.sin(q2) * q2_dot
149                + tau1
150            )
151            - (I2 + l1 * lc2 * m2 * ca.cos(q2) + lc2**2 * m2)
152            * (
153                -g * l2 * m_payload * ca.cos(q1 + q2)
154                - g * lc2 * m2 * ca.cos(q1 + q2)
155                - l1 * lc2 * m2 * ca.sin(q2) * q1_dot**2
156                + tau2
157            )
158        )
159        / (
160            I1 * I2
161            + I1 * lc2**2 * m2
162            + I2 * l1**2 * m2
163            + I2 * lc1**2 * m1
164            + l1**2 * lc2**2 * m2**2 * ca.sin(q2) ** 2
165            + lc1**2 * lc2**2 * m1 * m2
166        ),
167        q2_dot: (
168            -(I2 + l1 * lc2 * m2 * ca.cos(q2) + lc2**2 * m2)
169            * (
170                -g * l1 * m2 * ca.cos(q1)
171                - g * l1 * m_payload * ca.cos(q1)
172                - g * l2 * m_payload * ca.cos(q1 + q2)
173                - g * lc1 * m1 * ca.cos(q1)
174                - g * lc2 * m2 * ca.cos(q1 + q2)
175                + l1 * lc2 * m2 * (2 * q1_dot + q2_dot) * ca.sin(q2) * q2_dot
176                + tau1
177            )
178            + (
179                -g * l2 * m_payload * ca.cos(q1 + q2)
180                - g * lc2 * m2 * ca.cos(q1 + q2)
181                - l1 * lc2 * m2 * ca.sin(q2) * q1_dot**2
182                + tau2
183            )
184            * (I1 + I2 + l1**2 * m2 + 2 * l1 * lc2 * m2 * ca.cos(q2) + lc1**2 * m1 + lc2**2 * m2)
185        )
186        / (
187            I1 * I2
188            + I1 * lc2**2 * m2
189            + I2 * l1**2 * m2
190            + I2 * lc1**2 * m1
191            + l1**2 * lc2**2 * m2**2 * ca.sin(q2) ** 2
192            + lc1**2 * lc2**2 * m1 * m2
193        ),
194    }
195)
196
197
198# ============================================================================
199# Constraints
200# ============================================================================
201
202y_ee = l1 * ca.sin(q1) + l2 * ca.sin(q1 + q2)
203phase.path_constraints(y_ee >= 0.05)
204
205phase.path_constraints(
206    tau1 >= -max_tau1,
207    tau1 <= max_tau1,
208    tau2 >= -max_tau2,
209    tau2 <= max_tau2,
210)
211
212
213# ============================================================================
214# Objective
215# ============================================================================
216
217actuator_cost = max_tau1 * 0.08 + max_tau2 * 0.05
218energy = phase.add_integral(tau1**2 + tau2**2)
219problem.minimize(t.final + 0.1 * actuator_cost + 0.01 * energy)
220
221
222# ============================================================================
223# Mesh Configuration and Initial Guess
224# ============================================================================
225
226num_interval = 10
227degree = [4]
228final_mesh = degree * num_interval
229phase.mesh(final_mesh, np.linspace(-1.0, 1.0, num_interval + 1))
230
231states_guess = []
232controls_guess = []
233
234for N in final_mesh:
235    tau = np.linspace(-1, 1, N + 1)
236    t_norm = (tau + 1) / 2
237
238    q1_vals = q1_initial + (q1_final - q1_initial) * t_norm
239    q2_vals = q2_initial + (q2_final - q2_initial) * t_norm
240
241    q1_dot_vals = (q1_final - q1_initial) * np.sin(np.pi * t_norm) * 0.5
242    q2_dot_vals = (q2_final - q2_initial) * np.sin(np.pi * t_norm) * 0.5
243
244    states_guess.append(np.vstack([q1_vals, q2_vals, q1_dot_vals, q2_dot_vals]))
245
246    tau1_vals = np.ones(N) * 2.0
247    tau2_vals = np.ones(N) * 2.0
248    controls_guess.append(np.vstack([tau1_vals, tau2_vals]))
249
250phase.guess(
251    states=states_guess,
252    controls=controls_guess,
253    terminal_time=4.0,
254)
255
256
257# ============================================================================
258# Solve
259# ============================================================================
260
261solution = mtor.solve_adaptive(
262    problem,
263    error_tolerance=1e-3,
264    max_iterations=15,
265    min_polynomial_degree=3,
266    max_polynomial_degree=8,
267    nlp_options={
268        "ipopt.print_level": 0,
269        "ipopt.max_iter": 1000,
270        "ipopt.tol": 1e-6,
271        "ipopt.constr_viol_tol": 1e-4,
272    },
273)
274
275
276# ============================================================================
277# Results
278# ============================================================================
279
280if solution.status["success"]:
281    params = solution.parameters
282    optimal_tau1 = params["values"][0]
283    optimal_tau2 = params["values"][1]
284
285    tau1_max = max(np.abs(solution["tau1"]))
286    tau2_max = max(np.abs(solution["tau2"]))
287
288    utilization1 = (tau1_max / optimal_tau1) * 100
289    utilization2 = (tau2_max / optimal_tau2) * 100
290
291    actuator_cost = optimal_tau1 * 0.08 + optimal_tau2 * 0.05
292
293    q1_solved = solution["q1"][-1]
294    q2_solved = solution["q2"][-1]
295    x_ee_achieved = l1 * np.cos(q1_solved) + l2 * np.cos(q1_solved + q2_solved)
296    y_ee_achieved = l1 * np.sin(q1_solved) + l2 * np.sin(q1_solved + q2_solved)
297    position_error = np.sqrt((x_ee_achieved - x_ee_final) ** 2 + (y_ee_achieved - y_ee_final) ** 2)
298
299    print("=== OPTIMAL ACTUATOR DESIGN ===")
300    print(f"Joint 1 motor torque: {optimal_tau1:.1f} N⋅m (utilization: {utilization1:.1f}%)")
301    print(f"Joint 2 motor torque: {optimal_tau2:.1f} N⋅m (utilization: {utilization2:.1f}%)")
302    print(
303        f"Total actuator cost: ${actuator_cost:.2f}. (This is just a made-up cost, not the actual price)"
304    )
305    print()
306    print("=== SYSTEM PERFORMANCE ===")
307    print(f"Mission time: {solution.status['total_mission_time']:.2f} seconds")
308    print(f"Position accuracy: {position_error * 1000:.2f} mm error")
309    print("5kg payload successfully transported")
310
311    solution.plot()
312
313else:
314    print(f"Optimization failed: {solution.status['message']}")
315
316"""
317OUTPUT
318=== OPTIMAL ACTUATOR DESIGN ===
319Joint 1 motor torque: 60.7 N⋅m (utilization: 100.0%)
320Joint 2 motor torque: 26.9 N⋅m (utilization: 100.0%)
321Total actuator cost: $6.21. (This is just a made-up cost, not the actual price)
322
323=== SYSTEM PERFORMANCE ===
324Mission time: 1.95 seconds
325Position accuracy: 0.00 mm error
3265kg payload successfully transported
327"""