3DOF 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 3DOF 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 + \tau_3^2) dt\]

Where the actuator cost model is:

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

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

\[\frac{dq_1}{dt} = \dot{q}_1\]
\[\frac{dq_2}{dt} = \dot{q}_2\]
\[\frac{dq_3}{dt} = \dot{q}_3\]
\[\frac{d\dot{q}_1}{dt} = \frac{2I_2\sin(2q_2)\dot{q}_1\dot{q}_2 + 2I_3\sin(2q_2+2q_3)\dot{q}_1\dot{q}_2 + 2I_3\sin(2q_2+2q_3)\dot{q}_1\dot{q}_3 - 2l_2^2m_3\sin(2q_2)\dot{q}_1\dot{q}_2 - 4l_2l_{c3}m_3\sin(2q_2+q_3)\dot{q}_1\dot{q}_2 - 2l_2l_{c3}m_3\sin(2q_2+q_3)\dot{q}_1\dot{q}_3 - 2l_2l_{c3}m_3\sin(q_3)\dot{q}_1\dot{q}_3 - 2l_{c2}^2m_2\sin(2q_2)\dot{q}_1\dot{q}_2 - 2l_{c3}^2m_3\sin(2q_2+2q_3)\dot{q}_1\dot{q}_2 - 2l_{c3}^2m_3\sin(2q_2+2q_3)\dot{q}_1\dot{q}_3 - 2\tau_1}{-2I_1 + I_2\cos(2q_2) - I_2 + I_3\cos(2q_2+2q_3) - I_3 - l_2^2m_3\cos(2q_2) - l_2^2m_3 - 2l_2l_{c3}m_3\cos(2q_2+q_3) - 2l_2l_{c3}m_3\cos(q_3) - l_{c2}^2m_2\cos(2q_2) - l_{c2}^2m_2 - l_{c3}^2m_3\cos(2q_2+2q_3) - l_{c3}^2m_3}\]

\(\frac{d\dot{q}_2}{dt} = f_2(q_1, q_2, q_3, \dot{q}_1, \dot{q}_2, \dot{q}_3, \tau_2, \tau_3)\)

\(\frac{d\dot{q}_3}{dt} = f_3(q_1, q_2, q_3, \dot{q}_1, \dot{q}_2, \dot{q}_3, \tau_2, \tau_3)\)

The complete analytical expressions for \(f_2\) and \(f_3\) involving gravitational, centrifugal, Coriolis, and coupling terms are provided in the dynamics derivation output below.

Robot Configuration#

Joint Configuration:

  • Joint 1: Base rotation about Z-axis (azimuth)

  • Joint 2: Shoulder pitch about Y-axis (elevation)

  • Joint 3: Elbow pitch about Y-axis (relative to upper arm)

Forward Kinematics:

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

Design Parameters#

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

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

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

Boundary Conditions#

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

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

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

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

  • Joint angle bounds: \(q_1 \in [-\pi, \pi]\) rad, \(q_2 \in [-\pi/6, 5\pi/6]\) rad, \(q_3 \in [-2.5, 2.5]\) rad

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

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

Physical Parameters#

  • Link masses: \(m_1 = 3.0\) kg, \(m_2 = 2.5\) kg, \(m_3 = 1.5\) kg

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

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

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

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

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

State Variables#

  • \(q_1(t)\): Base joint angle (azimuth rotation) (rad)

  • \(q_2(t)\): Shoulder joint angle (elevation) (rad)

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

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

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

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

Control Variables#

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

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

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

Constraints#

  • Workspace constraint: End-effector height \(z_{ee} \geq 0.05\) m (ground clearance)

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

  • 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.

Dynamics Derivation#

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

examples/manipulator_3dof/manipulator_3dof_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, m3 = sm.symbols("m1 m2 m3")
 13
 14# Weight mass (kg)
 15m_box = sm.symbols("m_box")
 16
 17# Link lengths (m)
 18l1, l2, l3 = sm.symbols("l1 l2 l3")
 19
 20# Center of mass distances (m)
 21lc1, lc2, lc3 = sm.symbols("lc1 lc2 lc3")
 22
 23# Moments of inertia about COM (kg⋅m²)
 24I1, I2, I3 = sm.symbols("I1 I2 I3")
 25
 26# Gravity
 27g = sm.symbols("g")
 28
 29# Joint torques (control inputs)
 30tau1, tau2, tau3 = sm.symbols("tau1 tau2 tau3")
 31
 32# Joint coordinates and velocities
 33q1, q2, q3 = me.dynamicsymbols("q1 q2 q3")
 34q1d, q2d, q3d = me.dynamicsymbols("q1 q2 q3", 1)
 35
 36
 37# ============================================================================
 38# Reference Frames
 39# ============================================================================
 40
 41# N: Inertial frame
 42N = me.ReferenceFrame("N")
 43
 44# A: First link frame (rotation about Z - base rotation)
 45A = N.orientnew("A", "Axis", (q1, N.z))
 46
 47# B: Second link frame (rotation about Y - shoulder pitch)
 48B = A.orientnew("B", "Axis", (q2, A.y))
 49
 50# C: Third link frame (rotation about Y - elbow pitch)
 51C = B.orientnew("C", "Axis", (q3, B.y))
 52
 53
 54# ============================================================================
 55# Points and Velocities
 56# ============================================================================
 57
 58# Fixed base origin
 59O = me.Point("O")
 60O.set_vel(N, 0)
 61
 62# Joint 1 to Joint 2 (end of link 1)
 63P1 = O.locatenew("P1", l1 * A.z)
 64P1.v2pt_theory(O, N, A)
 65
 66# Joint 2 to Joint 3 (end of link 2)
 67P2 = P1.locatenew("P2", l2 * B.x)
 68P2.v2pt_theory(P1, N, B)
 69
 70# End effector (end of link 3)
 71P3 = P2.locatenew("P3", l3 * C.x)
 72P3.v2pt_theory(P2, N, C)
 73
 74# Centers of mass
 75G1 = O.locatenew("G1", lc1 * A.z)
 76G1.v2pt_theory(O, N, A)
 77
 78G2 = P1.locatenew("G2", lc2 * B.x)
 79G2.v2pt_theory(P1, N, B)
 80
 81G3 = P2.locatenew("G3", lc3 * C.x)
 82G3.v2pt_theory(P2, N, C)
 83
 84
 85# ============================================================================
 86# Rigid Bodies
 87# ============================================================================
 88
 89# Link 1: Vertical link (rotation about Z)
 90I1_dyadic = I1 * me.inertia(A, 0, 0, 1)
 91link1_body = me.RigidBody("link1", G1, A, m1, (I1_dyadic, G1))
 92
 93# Link 2: Horizontal link (rotation about Y)
 94I2_dyadic = I2 * me.inertia(B, 1, 0, 0)
 95link2_body = me.RigidBody("link2", G2, B, m2, (I2_dyadic, G2))
 96
 97# Link 3: Horizontal link (rotation about Y)
 98I3_dyadic = I3 * me.inertia(C, 1, 0, 0)
 99link3_body = me.RigidBody("link3", G3, C, m3, (I3_dyadic, G3))
100
101
102# ============================================================================
103# Forces (Passive Only)
104# ============================================================================
105
106# Gravitational forces on each center of mass
107loads = [
108    (G1, -m1 * g * N.z),  # Gravity on link 1
109    (G2, -m2 * g * N.z),  # Gravity on link 2
110    (G3, -m3 * g * N.z),  # Gravity on link 3
111    (P3, -m_box * g * N.z),  # Gravity on box at end effector
112]
113
114
115# ============================================================================
116# Lagrangian Mechanics
117# ============================================================================
118
119L = me.Lagrangian(N, link1_body, link2_body, link3_body)
120LM = me.LagrangesMethod(L, [q1, q2, q3], forcelist=loads, frame=N)
121
122
123# ============================================================================
124# Control Forces
125# ============================================================================
126
127# Joint torques acting on each generalized coordinate
128control_forces = sm.Matrix([tau1, tau2, tau3])
129
130
131# ============================================================================
132# Convert to MAPTOR Format
133# ============================================================================
134
135lagrangian_to_maptor_dynamics(LM, [q1, q2, q3], control_forces, "3dof_dynamics.txt")

This symbolic derivation produces the complex coupled dynamics equations accounting for gravitational forces from all links and payload, centrifugal forces from base rotation, Coriolis coupling between joints, and inertial effects. The systematic approach ensures mathematical correctness for the multi-body system while handling the coupling between base rotation and the vertical plane motion of the upper arm and forearm.

Running This Example#

cd examples/manipulator_3dof
python manipulator_3dof.py
python manipulator_3dof_animate.py

Code Implementation#

examples/manipulator_3dof/manipulator_3dof.py#
  1import casadi as ca
  2import numpy as np
  3
  4import maptor as mtor
  5
  6
  7# ============================================================================
  8# Physical Parameters
  9# ============================================================================
 10
 11m1 = 3.0
 12m2 = 2.5
 13m3 = 1.5
 14m_box = 5.0
 15
 16l1 = 0.3
 17l2 = 0.4
 18l3 = 0.4
 19
 20lc1 = 0.15
 21lc2 = 0.20
 22lc3 = 0.20
 23
 24I1 = m1 * l1**2 / 12
 25I2 = m2 * l2**2 / 12
 26I3 = m3 * l3**2 / 12
 27
 28g = 9.81
 29
 30
 31# ============================================================================
 32# End-Effector Position Specification
 33# ============================================================================
 34
 35x_ee_initial = 0.0
 36y_ee_initial = 0.5
 37z_ee_initial = 0.1
 38
 39x_ee_final = 0.0
 40y_ee_final = -0.5
 41z_ee_final = 0.1
 42
 43
 44# ============================================================================
 45# Inverse Kinematics
 46# ============================================================================
 47
 48
 49def calculate_inverse_kinematics(x_target, y_target, z_target, l1, l2, l3):
 50    q1 = np.arctan2(y_target, x_target)
 51
 52    r_horizontal = np.sqrt(x_target**2 + y_target**2)
 53    z_relative = z_target - l1
 54    r_total = np.sqrt(r_horizontal**2 + z_relative**2)
 55
 56    max_reach = l2 + l3
 57    min_reach = abs(l2 - l3)
 58    if r_total > max_reach:
 59        raise ValueError(f"Target unreachable: distance {r_total:.3f} > max reach {max_reach:.3f}")
 60    if r_total < min_reach:
 61        raise ValueError(f"Target too close: distance {r_total:.3f} < min reach {min_reach:.3f}")
 62
 63    cos_q3 = (r_total**2 - l2**2 - l3**2) / (2 * l2 * l3)
 64    q3 = -np.arccos(np.clip(cos_q3, -1, 1))
 65
 66    alpha = np.arctan2(z_relative, r_horizontal)
 67    beta = np.arctan2(l3 * np.sin(-q3), l2 + l3 * np.cos(-q3))
 68    q2 = alpha + beta
 69
 70    return q1, q2, q3
 71
 72
 73def verify_forward_kinematics(q1, q2, q3, l1, l2, l3):
 74    x_ee = (l2 * np.cos(q2) + l3 * np.cos(q2 + q3)) * np.cos(q1)
 75    y_ee = (l2 * np.cos(q2) + l3 * np.cos(q2 + q3)) * np.sin(q1)
 76    z_ee = l1 + l2 * np.sin(q2) + l3 * np.sin(q2 + q3)
 77    return x_ee, y_ee, z_ee
 78
 79
 80q1_initial, q2_initial, q3_initial = calculate_inverse_kinematics(
 81    x_ee_initial, y_ee_initial, z_ee_initial, l1, l2, l3
 82)
 83q1_final, q2_final, q3_final = calculate_inverse_kinematics(
 84    x_ee_final, y_ee_final, z_ee_final, l1, l2, l3
 85)
 86
 87x_check, y_check, z_check = verify_forward_kinematics(q1_final, q2_final, q3_final, l1, l2, l3)
 88position_error = np.sqrt(
 89    (x_check - x_ee_final) ** 2 + (y_check - y_ee_final) ** 2 + (z_check - z_ee_final) ** 2
 90)
 91
 92print("=== INVERSE KINEMATICS VERIFICATION ===")
 93print(f"Target position: ({x_ee_final:.3f}, {y_ee_final:.3f}, {z_ee_final:.3f}) m")
 94print(
 95    f"Calculated joint angles: q1={np.degrees(q1_final):.1f}°, q2={np.degrees(q2_final):.1f}°, q3={np.degrees(q3_final):.1f}°"
 96)
 97print(f"Forward kinematics check: ({x_check:.3f}, {y_check:.3f}, {z_check:.3f}) m")
 98print(f"Position error: {position_error:.6f} m")
 99print()
100
101
102# ============================================================================
103# Problem Setup
104# ============================================================================
105
106problem = mtor.Problem("KUKA-Scale 3DOF Manipulator")
107phase = problem.set_phase(1)
108
109# ============================================================================
110# Design Parameters
111# ============================================================================
112
113# Actuator sizing optimization - motor torque ratings
114max_tau1 = problem.parameter("base_motor_torque", boundary=(5.0, 50.0))
115max_tau2 = problem.parameter("shoulder_motor_torque", boundary=(5.0, 50.0))
116max_tau3 = problem.parameter("elbow_motor_torque", boundary=(5.0, 50.0))
117
118# ============================================================================
119# Variables
120# ============================================================================
121
122t = phase.time(initial=0.0)
123
124q1 = phase.state("q1", initial=q1_initial, final=q1_final, boundary=(-np.pi, np.pi))
125q2 = phase.state("q2", initial=q2_initial, final=q2_final, boundary=(-np.pi / 6, 5 * np.pi / 6))
126q3 = phase.state("q3", initial=q3_initial, final=q3_final, boundary=(-2.5, 2.5))
127
128q1_dot = phase.state("q1_dot", initial=0.0, final=0.0, boundary=(-1.5, 1.5))
129q2_dot = phase.state("q2_dot", initial=0.0, final=0.0, boundary=(-1.2, 1.2))
130q3_dot = phase.state("q3_dot", initial=0.0, final=0.0, boundary=(-2.0, 2.0))
131
132tau1 = phase.control("tau1")
133tau2 = phase.control("tau2")
134tau3 = phase.control("tau3")
135
136
137# ============================================================================
138# Dynamics
139# ============================================================================
140
141phase.dynamics(
142    {
143        q1: q1_dot,
144        q2: q2_dot,
145        q3: q3_dot,
146        q1_dot: (
147            2 * I2 * ca.sin(2 * q2) * q1_dot * q2_dot
148            + 2 * I3 * ca.sin(2 * q2 + 2 * q3) * q1_dot * q2_dot
149            + 2 * I3 * ca.sin(2 * q2 + 2 * q3) * q1_dot * q3_dot
150            - 2 * l2**2 * m3 * ca.sin(2 * q2) * q1_dot * q2_dot
151            - 4 * l2 * lc3 * m3 * ca.sin(2 * q2 + q3) * q1_dot * q2_dot
152            - 2 * l2 * lc3 * m3 * ca.sin(2 * q2 + q3) * q1_dot * q3_dot
153            - 2 * l2 * lc3 * m3 * ca.sin(q3) * q1_dot * q3_dot
154            - 2 * lc2**2 * m2 * ca.sin(2 * q2) * q1_dot * q2_dot
155            - 2 * lc3**2 * m3 * ca.sin(2 * q2 + 2 * q3) * q1_dot * q2_dot
156            - 2 * lc3**2 * m3 * ca.sin(2 * q2 + 2 * q3) * q1_dot * q3_dot
157            - 2 * tau1
158        )
159        / (
160            -2 * I1
161            + I2 * ca.cos(2 * q2)
162            - I2
163            + I3 * ca.cos(2 * q2 + 2 * q3)
164            - I3
165            - l2**2 * m3 * ca.cos(2 * q2)
166            - l2**2 * m3
167            - 2 * l2 * lc3 * m3 * ca.cos(2 * q2 + q3)
168            - 2 * l2 * lc3 * m3 * ca.cos(q3)
169            - lc2**2 * m2 * ca.cos(2 * q2)
170            - lc2**2 * m2
171            - lc3**2 * m3 * ca.cos(2 * q2 + 2 * q3)
172            - lc3**2 * m3
173        ),
174        q2_dot: (
175            lc3
176            * (
177                I2 * ca.sin(2 * q2) * q1_dot**2 / 2
178                + I3 * ca.sin(2 * q2 + 2 * q3) * q1_dot**2 / 2
179                + g * l2 * m3 * ca.cos(q2)
180                + g * l2 * m_box * ca.cos(q2)
181                + g * l3 * m_box * ca.cos(q2 + q3)
182                + g * lc2 * m2 * ca.cos(q2)
183                + g * lc3 * m3 * ca.cos(q2 + q3)
184                + l2 * lc3 * m3 * (2 * q2_dot + q3_dot) * ca.sin(q3) * q3_dot
185                - lc2**2 * m2 * ca.sin(2 * q2) * q1_dot**2 / 2
186                - m3
187                * (
188                    l2**2 * ca.sin(2 * q2) / 2
189                    + l2 * lc3 * ca.sin(2 * q2 + q3)
190                    + lc3**2 * ca.sin(2 * q2 + 2 * q3) / 2
191                )
192                * q1_dot**2
193                + tau2
194            )
195            + (l2 * ca.cos(q3) + lc3)
196            * (
197                -I3 * ca.sin(2 * q2 + 2 * q3) * q1_dot**2 / 2
198                - g * l3 * m_box * ca.cos(q2 + q3)
199                - g * lc3 * m3 * ca.cos(q2 + q3)
200                + l2 * lc3 * m3 * ca.sin(2 * q2 + q3) * q1_dot**2 / 2
201                + l2 * lc3 * m3 * ca.sin(q3) * q1_dot**2 / 2
202                + l2 * lc3 * m3 * ca.sin(q3) * q2_dot**2
203                + lc3**2 * m3 * ca.sin(2 * q2 + 2 * q3) * q1_dot**2 / 2
204                - tau3
205            )
206        )
207        / (lc3 * (l2**2 * m3 * ca.sin(q3) ** 2 + lc2**2 * m2)),
208        q3_dot: (
209            -lc3
210            * m3
211            * (l2 * ca.cos(q3) + lc3)
212            * (
213                I2 * ca.sin(2 * q2) * q1_dot**2 / 2
214                + I3 * ca.sin(2 * q2 + 2 * q3) * q1_dot**2 / 2
215                + g * l2 * m3 * ca.cos(q2)
216                + g * l2 * m_box * ca.cos(q2)
217                + g * l3 * m_box * ca.cos(q2 + q3)
218                + g * lc2 * m2 * ca.cos(q2)
219                + g * lc3 * m3 * ca.cos(q2 + q3)
220                + l2 * lc3 * m3 * (2 * q2_dot + q3_dot) * ca.sin(q3) * q3_dot
221                - lc2**2 * m2 * ca.sin(2 * q2) * q1_dot**2 / 2
222                - m3
223                * (
224                    l2**2 * ca.sin(2 * q2) / 2
225                    + l2 * lc3 * ca.sin(2 * q2 + q3)
226                    + lc3**2 * ca.sin(2 * q2 + 2 * q3) / 2
227                )
228                * q1_dot**2
229                + tau2
230            )
231            - (l2**2 * m3 + 2 * l2 * lc3 * m3 * ca.cos(q3) + lc2**2 * m2 + lc3**2 * m3)
232            * (
233                -I3 * ca.sin(2 * q2 + 2 * q3) * q1_dot**2 / 2
234                - g * l3 * m_box * ca.cos(q2 + q3)
235                - g * lc3 * m3 * ca.cos(q2 + q3)
236                + l2 * lc3 * m3 * ca.sin(2 * q2 + q3) * q1_dot**2 / 2
237                + l2 * lc3 * m3 * ca.sin(q3) * q1_dot**2 / 2
238                + l2 * lc3 * m3 * ca.sin(q3) * q2_dot**2
239                + lc3**2 * m3 * ca.sin(2 * q2 + 2 * q3) * q1_dot**2 / 2
240                - tau3
241            )
242        )
243        / (lc3**2 * m3 * (l2**2 * m3 * ca.sin(q3) ** 2 + lc2**2 * m2)),
244    }
245)
246
247
248# ============================================================================
249# Constraints
250# ============================================================================
251
252z_ee = l1 + l2 * ca.sin(q2) + l3 * ca.sin(q2 + q3)
253phase.path_constraints(z_ee >= 0.05)
254
255# Actuator capability constraints
256phase.path_constraints(
257    tau1 >= -max_tau1,
258    tau1 <= max_tau1,
259    tau2 >= -max_tau2,
260    tau2 <= max_tau2,
261    tau3 >= -max_tau3,
262    tau3 <= max_tau3,
263)
264
265
266# ============================================================================
267# Objective
268# ============================================================================
269
270# Actuator cost model (realistic motor pricing)
271actuator_cost = max_tau1 * 0.05 + max_tau2 * 0.08 + max_tau3 * 0.03
272
273# Energy consumption (motion smoothness)
274energy = phase.add_integral(tau1**2 + tau2**2 + tau3**2)
275
276# Multi-objective: minimize time + actuator investment + energy consumption
277problem.minimize(t.final + 0.1 * actuator_cost + 0.01 * energy)
278
279
280# ============================================================================
281# Mesh Configuration and Initial Guess
282# ============================================================================
283
284num_interval = 12
285degree = [4]
286final_mesh = degree * num_interval
287phase.mesh(final_mesh, np.linspace(-1.0, 1.0, num_interval + 1))
288
289states_guess = []
290controls_guess = []
291
292for N in final_mesh:
293    tau = np.linspace(-1, 1, N + 1)
294    t_norm = (tau + 1) / 2
295
296    q1_vals = q1_initial + (q1_final - q1_initial) * t_norm
297    q2_vals = q2_initial + (q2_final - q2_initial) * t_norm
298    q3_vals = q3_initial + (q3_final - q3_initial) * t_norm
299
300    q1_dot_vals = (q1_final - q1_initial) * np.sin(np.pi * t_norm) * 0.5
301    q2_dot_vals = (q2_final - q2_initial) * np.sin(np.pi * t_norm) * 0.5
302    q3_dot_vals = (q3_final - q3_initial) * np.sin(np.pi * t_norm) * 0.5
303
304    states_guess.append(
305        np.vstack([q1_vals, q2_vals, q3_vals, q1_dot_vals, q2_dot_vals, q3_dot_vals])
306    )
307
308    tau1_vals = np.ones(N) * 5.0
309    tau2_vals = np.ones(N) * 10.0
310    tau3_vals = np.ones(N) * 5.0
311    controls_guess.append(np.vstack([tau1_vals, tau2_vals, tau3_vals]))
312
313phase.guess(
314    states=states_guess,
315    controls=controls_guess,
316    terminal_time=6.0,
317)
318
319
320# ============================================================================
321# Solve
322# ============================================================================
323
324solution = mtor.solve_adaptive(
325    problem,
326    error_tolerance=1e-3,
327    max_iterations=15,
328    min_polynomial_degree=3,
329    max_polynomial_degree=8,
330    nlp_options={
331        "ipopt.print_level": 0,
332        "ipopt.max_iter": 1000,
333        "ipopt.tol": 1e-6,
334        "ipopt.constr_viol_tol": 1e-4,
335    },
336)
337
338
339# ============================================================================
340# Results
341# ============================================================================
342
343# Results
344if solution.status["success"]:
345    # Extract optimized parameters
346    params = solution.parameters
347    optimal_tau1 = params["values"][0]
348    optimal_tau2 = params["values"][1]
349    optimal_tau3 = params["values"][2]
350
351    # Calculate utilization rates
352    tau1_max = max(np.abs(solution["tau1"]))
353    tau2_max = max(np.abs(solution["tau2"]))
354    tau3_max = max(np.abs(solution["tau3"]))
355
356    utilization1 = (tau1_max / optimal_tau1) * 100
357    utilization2 = (tau2_max / optimal_tau2) * 100
358    utilization3 = (tau3_max / optimal_tau3) * 100
359
360    # Calculate costs
361    actuator_cost = optimal_tau1 * 0.05 + optimal_tau2 * 0.08 + optimal_tau3 * 0.03
362
363    # Position verification
364    q1_solved = solution["q1"][-1]
365    q2_solved = solution["q2"][-1]
366    q3_solved = solution["q3"][-1]
367    x_ee_achieved = (l2 * np.cos(q2_solved) + l3 * np.cos(q2_solved + q3_solved)) * np.cos(
368        q1_solved
369    )
370    y_ee_achieved = (l2 * np.cos(q2_solved) + l3 * np.cos(q2_solved + q3_solved)) * np.sin(
371        q1_solved
372    )
373    z_ee_achieved = l1 + l2 * np.sin(q2_solved) + l3 * np.sin(q2_solved + q3_solved)
374    position_error = np.sqrt(
375        (x_ee_achieved - x_ee_final) ** 2
376        + (y_ee_achieved - y_ee_final) ** 2
377        + (z_ee_achieved - z_ee_final) ** 2
378    )
379
380    print("=== OPTIMAL ACTUATOR DESIGN ===")
381    print(f"Base motor torque: {optimal_tau1:.1f} N⋅m (utilization: {utilization1:.1f}%)")
382    print(f"Shoulder motor torque: {optimal_tau2:.1f} N⋅m (utilization: {utilization2:.1f}%)")
383    print(f"Elbow motor torque: {optimal_tau3:.1f} N⋅m (utilization: {utilization3:.1f}%)")
384    print(
385        f"Total actuator cost: ${actuator_cost:.2f}. (This is just a made-up cost, not the actual price)"
386    )
387    print()
388    print("=== SYSTEM PERFORMANCE ===")
389    print(f"Mission time: {solution.status['total_mission_time']:.2f} seconds")
390    print(f"Position accuracy: {position_error * 1000:.2f} mm error")
391    print("5kg payload successfully transported")
392
393else:
394    print(f"Optimization failed: {solution.status['message']}")
395
396"""
397OUTPUT
398=== OPTIMAL ACTUATOR DESIGN ===
399Base motor torque: 12.8 N⋅m (utilization: 100.0%)
400Shoulder motor torque: 34.3 N⋅m (utilization: 100.0%)
401Elbow motor torque: 24.1 N⋅m (utilization: 100.0%)
402Total actuator cost: $4.11. (This is just a made-up cost, not the actual price)
403
404=== SYSTEM PERFORMANCE ===
405Mission time: 2.14 seconds
406Position accuracy: 0.00 mm error
4075kg payload successfully transported
408"""