From 3b87de12783025c87ee24018e684dada0c2b6189 Mon Sep 17 00:00:00 2001
From: Peter Stahlecker <peter.stahlecker@gmail.com>
Date: Tue, 18 Feb 2025 17:29:12 +0100
Subject: [PATCH 1/7] method to generate an initial guess. Fixes #354

---
 opty/direct_collocation.py            | 197 ++++++++++++-
 opty/tests/test_direct_collocation.py | 402 ++++++++++++++++++++++++++
 2 files changed, 598 insertions(+), 1 deletion(-)

diff --git a/opty/direct_collocation.py b/opty/direct_collocation.py
index 2e1e6855..375d01c2 100644
--- a/opty/direct_collocation.py
+++ b/opty/direct_collocation.py
@@ -244,7 +244,6 @@ def solve(self, free, lagrange=[], zl=[], zu=[]):
                 gives the status of the algorithm as a message
 
         """
-
         return super().solve(free, lagrange=lagrange, zl=zl, zu=zu)
 
     def _generate_bound_arrays(self):
@@ -686,6 +685,202 @@ def parse_free(self, free):
 
         return parse_free(free, n, q, N, variable_duration)
 
+    def create_initial_guess(self):
+        """Creates an initial guess for the free variables in the optimization.
+
+        Returns:
+        --------
+        initial_guess : (n*N + q*N + r + s)-ndarray
+            The initial guess for the free variables in the optimization problem.
+
+        Strategy:
+        --------
+        - Instance constraints which contain instances of other state variables
+          are ignored.
+        - Between successsive instances of a state variable, the values are
+          interpolated linearly.
+        - If bounds exist for unknown input trajectories or unknown parameters,
+          the values are set to the midpoint of the interval of the bounds.
+        - If bounds exist for the variable time interval, the value is set to
+          the midpoint of its bounds.
+        - If one of the range limit of a bound is ``-np.inf`` or ``np.inf``,
+          the value is set to the other finite limit of bound.
+        - All else is set to zero.
+
+    """
+        if self.collocator.instance_constraints is None:
+            instance_matrix = sm.Matrix([])
+        else:
+            instance_matrix = sm.Matrix(self.collocator.instance_constraints)
+        par_map = self.collocator.known_parameter_map
+        instance_matrix = instance_matrix.subs({key: par_map[key] for key
+                            in par_map.keys()})
+
+        initial_guess = np.zeros(self.num_free)
+        num_nodes = self.collocator.num_collocation_nodes
+
+        # delete instance constraints which contain instances of other state
+        # variables
+        row_delete = []
+        M_new = instance_matrix.copy()
+        for i, exp1 in enumerate(M_new):
+            for a in sm.preorder_traversal(exp1):
+                zaehler = 0
+                for b in sm.preorder_traversal(a):
+                    if isinstance(b, sm.Function):
+                        zaehler += 1
+                        if zaehler > 1:
+                            row_delete.append(i)
+        if len(row_delete) > 0:
+            for row in sorted(row_delete, reverse=True):
+                M_new.row_del(row)
+        instance_matrix = M_new
+
+        if self.collocator.instance_constraints is not None:
+            if not isinstance(self.collocator.node_time_interval, sm.Symbol):
+        # fixed time interval
+                liste = []
+                liste1 = []
+                for exp1 in instance_matrix:
+                    for a in sm.preorder_traversal(exp1):
+                        liste.append(a)
+
+                for entry in liste:
+                    if isinstance(entry, sm.Function):
+                        idx = liste.index(entry)
+                        liste2 = [0, 0, 0, 0]
+                        liste2[0] = entry
+                        liste2[1] = liste[idx + 1]
+                        liste2[2] = liste[idx -1]
+                        liste2[3] = entry.name
+                        liste1.append(liste2)
+
+                name_rank = {name.name: i for i, name in enumerate(
+                        self.collocator.state_symbols)}
+                liste3 = sorted(liste1, key=lambda x: (name_rank[x[3]], x[1]))
+
+                duration = self.collocator.node_time_interval * (num_nodes-1)
+                for state in self.collocator.state_symbols:
+                    state_idx = self.collocator.state_symbols.index(state)
+                    times = []
+                    values = []
+                    for i in liste3:
+                        if state.name == i[3]:
+                            times.append(i[1])
+                            values.append(-i[2])
+                    if len(times) == 0:
+                        pass
+                    elif len(times) == 1:
+                        initial_guess[state_idx*num_nodes:(state_idx+1) \
+                            *num_nodes] = values[0]
+                    else:
+                        for i in range(len(times)-1):
+                            start = round(times[i]/duration*num_nodes)
+                            ende = round(times[i+1]/duration*num_nodes)
+                            werte = np.linspace(values[i], values[i+1],
+                                ende-start)
+                            initial_guess[state_idx*num_nodes+start:
+                                state_idx*num_nodes+ende] = werte
+
+        # Variable time interval
+            else:
+                liste = []
+                liste1 = []
+                instance_matrix = instance_matrix.subs({self.collocator.
+                                    node_time_interval: 1.0})
+                for exp1 in instance_matrix:
+                    for a in sm.preorder_traversal(exp1):
+                        liste.append(a)
+
+                for entry in liste:
+                    if isinstance(entry, sm.Function):
+                        idx = liste.index(entry)
+                        liste2 = [0, 0, 0, 0]
+                        liste2[0] = entry
+                        liste2[1] = liste[idx + 1]
+                        liste2[2] = liste[idx -1]
+                        liste2[3] = entry.name
+                        liste1.append(liste2)
+
+                name_rank = {name.name: i for i, name in enumerate(
+                        self.collocator.state_symbols)}
+                liste3 = sorted(liste1, key=lambda x: (name_rank[x[3]], x[1]))
+
+                duration = (num_nodes-1)
+                for state in self.collocator.state_symbols:
+                    state_idx = self.collocator.state_symbols.index(state)
+                    times = []
+                    values = []
+                    for i in liste3:
+                        if state.name == i[3]:
+                            times.append(i[1])
+                            values.append(-i[2])
+                    if len(times) == 0:
+                        pass
+                    elif len(times) == 1:
+                        initial_guess[state_idx*num_nodes:(state_idx+1) \
+                            *num_nodes] = values[0]
+                    else:
+                        for i in range(len(times)-1):
+                            start = int(times[i])
+                            ende = int(times[i+1])
+                            werte = np.linspace(values[i], values[i+1],
+                                ende-start)
+                            initial_guess[state_idx*num_nodes+start:
+                                state_idx*num_nodes+ende] = werte
+
+        # set the values of unknown input trajectories.
+        if self.bounds is not None and \
+            len(self.collocator.unknown_input_trajectories) > 0:
+            start_idx = len(self.collocator.state_symbols) * num_nodes
+            for symb in self.collocator.unknown_input_trajectories:
+                idx = self.collocator.unknown_input_trajectories.index(symb)
+                if symb in self.bounds.keys():
+                    if self.bounds[symb][0] == -np.inf:
+                        wert = self.bounds[symb][1]
+                    elif self.bounds[symb][1] == np.inf:
+                        wert = self.bounds[symb][0]
+                    else:
+                        wert = 0.5*(self.bounds[symb][0] + self.bounds[symb][1])
+                    initial_guess[start_idx + idx*num_nodes:start_idx+(idx+1)
+                            *num_nodes] = wert
+
+        # set the values of unknown parameters.
+        if self.bounds is not None and \
+            len(self.collocator.unknown_parameters) > 0:
+            start_idx = (len(self.collocator.state_symbols) +
+                     len(self.collocator.unknown_input_trajectories))* num_nodes
+            for symb in self.collocator.unknown_parameters:
+                idx = self.collocator.unknown_parameters.index(symb)
+                if symb in self.bounds.keys():
+                    if self.bounds[symb][0] == -np.inf:
+                        wert = self.bounds[symb][1]
+                    elif self.bounds[symb][1] == np.inf:
+                        wert = self.bounds[symb][0]
+                    else:
+                        wert = 0.5*(self.bounds[symb][0] + self.bounds[symb][1])
+                    initial_guess[start_idx + idx]  = wert
+
+        # set the value of the variable time interval.
+        if self.bounds is not None and \
+            isinstance(self.collocator.node_time_interval, sm.Symbol):
+                if self.collocator.node_time_interval in self.bounds.keys():
+                    if self.bounds[self.collocator.node_time_interval][0] \
+                        == -np.inf:
+                        wert = self.bounds[self.collocator.node_time_interval][1]
+                    elif self.bounds[self.collocator.node_time_interval][1] \
+                        == np.inf:
+                        wert = self.bounds[self.collocator.node_time_interval][0]
+                    else:
+                        wert = 0.5*(self.bounds[self.collocator.
+                            node_time_interval][0] +
+                            self.bounds[self.collocator.
+                            node_time_interval][1])
+                    initial_guess[-1] = wert
+
+        return initial_guess
+
+
 class ConstraintCollocator(object):
     """This class is responsible for generating the constraint function and the
     sparse Jacobian of the constraint function using direct collocation methods
diff --git a/opty/tests/test_direct_collocation.py b/opty/tests/test_direct_collocation.py
index f939e78b..31017e07 100644
--- a/opty/tests/test_direct_collocation.py
+++ b/opty/tests/test_direct_collocation.py
@@ -1907,3 +1907,405 @@ def test_attributes_read_only():
 
         with raises(AttributeError):
             setattr(test, XX, 5)
+
+def test_linear_initial_guess():
+    """Test to check if the initial guess is created correctly."""
+
+    x, y, ux, uy = mech.dynamicsymbols('x y ux uy')
+    u1, u2 = mech.dynamicsymbols('u1 u2')
+    a1, a2 = sym.symbols('a1 a2')
+    b1, b2 = sym.symbols('b1 b2')
+    t = mech.dynamicsymbols._t
+
+    # just random eoms, no physical meaning.
+    eom = sym.Matrix([
+        -x.diff(t) + a1,
+        -ux.diff(t) + u1 * b1,
+        -y.diff(t) + a1,
+        -uy.diff(t) + a2 * u2 + b2,
+    ])
+
+    state_symbols = (x, y, ux, uy)
+    par_map = {b1: 1.0,
+               b2: 2.0,
+               }
+    num_nodes = 61
+
+    # A: CONSTANT TIME INTERVAL
+    # A!: normal
+    t0, t1, t2, tf = 0.0, 2.0, 4.0, 6.0
+    interval_value = tf/(num_nodes - 1)
+
+    def obj(free):
+        Fx = free[0*num_nodes:3*num_nodes]
+        return interval_value*np.sum(Fx**2)
+
+    def obj_grad(free):
+        grad = np.zeros_like(free)
+        grad[0: 2*num_nodes] = 2.0*free[0:2*num_nodes]*interval_value
+        return grad
+
+    instance_constraints = (
+        x.func(t0) - 3.0,
+        y.func(t0) - 2.0,
+        ux.func(t0) - 5.0,
+
+        x.func(t1) - 1.5*b1,
+        y.func(t1) - 4.0,
+        ux.func(t1) - 1.0+b2,
+        uy.func(t1) - 4.5,
+
+        x.func(t2) + 2.0,
+        y.func(t2) - 3.0,
+
+        x.func(tf) + 1.0,
+        ux.func(tf) + 5.0,
+    )
+
+    bounds= {
+        a1: (-1.0, 10.0),
+        a2: (-1.0, 12.0),
+
+        u1: (-10.0, 1.0),
+        u2: (-1.0, 10.0),
+
+        x: (-5.0, 5.0 ),
+        ux: (-1.0, 1.0),
+        y: (-4.0, 4.0),
+        uy: (-1.0, 1.0),
+    }
+
+    prob = Problem(
+        obj,
+        obj_grad,
+        eom,
+        state_symbols,
+        num_nodes,
+        interval_value,
+        known_parameter_map=par_map,
+        instance_constraints=instance_constraints,
+        bounds=bounds,
+        time_symbol=t,
+    )
+
+    # Set the expected initial guess
+    expected_guess = np.zeros(prob.num_free)
+    duration = (num_nodes-1)*interval_value
+    # x - guess
+    start = round(t0/duration*num_nodes)
+    ende = round(t1/duration*num_nodes)
+    werte = np.linspace(3.0, 1.5*par_map[b1], ende-start)
+    expected_guess[0*num_nodes+start:0*num_nodes+ende] = werte
+    start = round(t1/duration*num_nodes)
+    ende = round(t2/duration*num_nodes)
+    werte = np.linspace(1.5*par_map[b1], -2.0, ende-start)
+    expected_guess[0*num_nodes+start:0*num_nodes+ende] = werte
+    start = round(t2/duration*num_nodes)
+    ende = round(tf/duration*num_nodes)
+    werte = np.linspace(-2.0, -1.0, ende-start)
+    expected_guess[0*num_nodes+start:0*num_nodes+ende] = werte
+     # y - guess
+    start = round(t0/duration*num_nodes)
+    ende = round(t1/duration*num_nodes)
+    werte = np.linspace(2.0, 4.0, ende-start)
+    expected_guess[1*num_nodes+start:1*num_nodes+ende] = werte
+    start = round(t1/duration*num_nodes)
+    ende = round(t2/duration*num_nodes)
+    werte = np.linspace(4.0, 3.0, ende-start)
+    expected_guess[1*num_nodes+start:1*num_nodes+ende] = werte
+    # ux - guess
+    start = round(t0/duration*num_nodes)
+    ende = round(t1/duration*num_nodes)
+    werte = np.linspace(5.0, 1.0-par_map[b2], ende-start)
+    expected_guess[2*num_nodes+start:2*num_nodes+ende] = werte
+    start = round(t1/duration*num_nodes)
+    ende = round(tf/duration*num_nodes)
+    werte = np.linspace(1.0-par_map[b2], -5.0, ende-start)
+    expected_guess[2*num_nodes+start:2*num_nodes+ende] = werte
+    # uy - guess
+    expected_guess[3*num_nodes:4*num_nodes] = 4.5
+    # u1 - guess
+    expected_guess[4*num_nodes:5*num_nodes] = -9.0/2.0
+    # u2 - guess
+    expected_guess[5*num_nodes:6*num_nodes] = 9.0/2.0
+    # a1 - guess
+    expected_guess[6*num_nodes] = 9/2
+    # a2 - guess
+    expected_guess[6*num_nodes+1] = 11.0/2.0
+
+
+    initial_guess = prob.create_initial_guess()
+    np.testing.assert_allclose(initial_guess, expected_guess)
+
+    # A2: np.inf, -np.inf in bounds
+    bounds[a1] = (-np.inf, 10.0)
+    bounds[a2] = (-10.0, np.inf)
+
+    prob = Problem(
+        obj,
+        obj_grad,
+        eom,
+        state_symbols,
+        num_nodes,
+        interval_value,
+        known_parameter_map=par_map,
+        instance_constraints=instance_constraints,
+        bounds=bounds,
+        time_symbol=t,
+    )
+
+    expected_guess[6*num_nodes] = 10.0
+    expected_guess[6*num_nodes+1] = -10.0
+
+    initial_guess = prob.create_initial_guess()
+    np.testing.assert_allclose(initial_guess, expected_guess)
+
+    # A3: no bounds
+    expected_guess[4*num_nodes: 5*num_nodes] = 0.0
+    expected_guess[5*num_nodes: 6*num_nodes] = 0.0
+    expected_guess[6*num_nodes] = 0.0
+    expected_guess[6*num_nodes+1] = 0.0
+
+    prob = Problem(
+        obj,
+        obj_grad,
+        eom,
+        state_symbols,
+        num_nodes,
+        interval_value,
+        known_parameter_map=par_map,
+        instance_constraints=instance_constraints,
+        time_symbol=t,
+    )
+
+    initial_guess = prob.create_initial_guess()
+    np.testing.assert_allclose(initial_guess, expected_guess)
+
+    # A4: state instances in instance_constraints
+    instance_constraints = (
+        x.func(t0) - 3.0 + ux.func(tf),
+        y.func(t0) - 2.0,
+        ux.func(t0) - 5.0,
+
+        x.func(t1) - 1.5*b1,
+        y.func(t1) - 4.0,
+        ux.func(t1) - 1.0+b2,
+        uy.func(t1) - 4.5,
+
+        x.func(t2) + 2.0,
+        y.func(t2) - 3.0,
+
+        x.func(tf) + 1.0,
+        ux.func(tf) + 5.0,
+    )
+
+    start = round(t0/duration*num_nodes)
+    ende = round(t1/duration*num_nodes)
+    expected_guess[0*num_nodes+start:0*num_nodes+ende] = 0.0
+
+    prob = Problem(
+        obj,
+        obj_grad,
+        eom,
+        state_symbols,
+        num_nodes,
+        interval_value,
+        known_parameter_map=par_map,
+        instance_constraints=instance_constraints,
+        time_symbol=t,
+    )
+
+    initial_guess = prob.create_initial_guess()
+    np.testing.assert_allclose(initial_guess, expected_guess)
+
+    # B: VARIABLE TIME INTERVAL
+    # B1: normal
+    h = sym.symbols('h')
+    t00, t10, t20, tf0 = 0.0, int(num_nodes/3)*h, int(2*num_nodes/3)*h, (num_nodes - 1)*h
+    interval_value = h
+
+    def obj(free):
+        Fx = free[0*num_nodes:3*num_nodes]
+        return free[-1]*np.sum(Fx**2)
+
+    def obj_grad(free):
+        grad = np.zeros_like(free)
+        grad[0: 2*num_nodes] = 2.0*free[0:2*num_nodes]*free[-1]
+        return grad
+
+    instance_constraints = (
+        x.func(t00) - 3.0,
+        y.func(t00) - 2.0,
+        ux.func(t00) - 5.0,
+
+        x.func(t10) - 1.5*b1,
+        y.func(t10) - 4.0,
+        ux.func(t10) - 1.0+b2,
+        uy.func(t10) - 4.5,
+
+        x.func(t20) + 2.0,
+        y.func(t20) - 3.0,
+
+        x.func(tf0) + 1.0,
+        ux.func(tf0) + 5.0,
+    )
+
+    bounds= {
+        a1: (-1.0, 10.0),
+        a2: (-1.0, 12.0),
+
+        u1: (-10.0, 1.0),
+        u2: (-1.0, 10.0),
+
+        x: (-5.0, 5.0 ),
+        ux: (-1.0, 1.0),
+        y: (-4.0, 4.0),
+        uy: (-1.0, 1.0),
+        h: (1.0, 2.0),
+    }
+
+    prob = Problem(
+        obj,
+        obj_grad,
+        eom,
+        state_symbols,
+        num_nodes,
+        interval_value,
+        known_parameter_map=par_map,
+        instance_constraints=instance_constraints,
+        bounds=bounds,
+        time_symbol=t,
+    )
+
+    # Set the expected initial guess
+    expected_guess = np.zeros(prob.num_free)
+    t0, t1, t2 = int(0.0), int(num_nodes/3), int(2*num_nodes/3)
+    tf = int(num_nodes - 1)
+
+    # x - guess
+    start = t0
+    ende = t1
+    werte = np.linspace(3.0, 1.5*par_map[b1], ende-start)
+    expected_guess[0*num_nodes+start:0*num_nodes+ende] = werte
+    start = t1
+    ende = t2
+    werte = np.linspace(1.5*par_map[b1], -2.0, ende-start)
+    expected_guess[0*num_nodes+start:0*num_nodes+ende] = werte
+    start = t2
+    ende = tf
+    werte = np.linspace(-2.0, -1.0, ende-start)
+    expected_guess[0*num_nodes+start:0*num_nodes+ende] = werte
+     # y - guess
+    start = t0
+    ende = t1
+    werte = np.linspace(2.0, 4.0, ende-start)
+    expected_guess[1*num_nodes+start:1*num_nodes+ende] = werte
+    start = t1
+    ende = t2
+    werte = np.linspace(4.0, 3.0, ende-start)
+    expected_guess[1*num_nodes+start:1*num_nodes+ende] = werte
+    # ux - guess
+    start = t0
+    ende = t1
+    werte = np.linspace(5.0, 1.0-par_map[b2], ende-start)
+    expected_guess[2*num_nodes+start:2*num_nodes+ende] = werte
+    start = t1
+    ende = tf
+    werte = np.linspace(1.0-par_map[b2], -5.0, ende-start)
+    expected_guess[2*num_nodes+start:2*num_nodes+ende] = werte
+    # uy - guess
+    expected_guess[3*num_nodes:4*num_nodes] = 4.5
+    # u1 - guess
+    expected_guess[4*num_nodes:5*num_nodes] = -9.0/2.0
+    # u2 - guess
+    expected_guess[5*num_nodes:6*num_nodes] = 9.0/2.0
+    # a1 - guess
+    expected_guess[6*num_nodes] = 9/2
+    # a2 - guess
+    expected_guess[6*num_nodes+1] = 11.0/2.0
+    # h - guess
+    expected_guess[-1] = 1.5
+
+    initial_guess = prob.create_initial_guess()
+    np.testing.assert_allclose(initial_guess, expected_guess)
+
+    # B2: np.inf, -np.inf in bounds
+    bounds[a1] = (-np.inf, 10.0)
+    bounds[a2] = (-10.0, np.inf)
+
+    prob = Problem(
+        obj,
+        obj_grad,
+        eom,
+        state_symbols,
+        num_nodes,
+        interval_value,
+        known_parameter_map=par_map,
+        instance_constraints=instance_constraints,
+        bounds=bounds,
+        time_symbol=t,
+    )
+
+    expected_guess[6*num_nodes] = 10.0
+    expected_guess[6*num_nodes+1] = -10.0
+
+    initial_guess = prob.create_initial_guess()
+    np.testing.assert_allclose(initial_guess, expected_guess)
+
+    # B3: no bounds
+    expected_guess[4*num_nodes: 5*num_nodes] = 0.0
+    expected_guess[5*num_nodes: 6*num_nodes] = 0.0
+    expected_guess[6*num_nodes] = 0.0
+    expected_guess[6*num_nodes+1] = 0.0
+    expected_guess[-1] = 0.0
+
+    prob = Problem(
+        obj,
+        obj_grad,
+        eom,
+        state_symbols,
+        num_nodes,
+        interval_value,
+        known_parameter_map=par_map,
+        instance_constraints=instance_constraints,
+        time_symbol=t,
+        )
+    initial_guess = prob.create_initial_guess()
+    np.testing.assert_allclose(initial_guess, expected_guess)
+
+    # B4: state instances in instance_constraints
+    instance_constraints = (
+        x.func(t00) - 3.0 + ux.func(tf0),
+        y.func(t00) - 2.0,
+        ux.func(t00) - 5.0,
+
+        x.func(t10) - 1.5*b1,
+        y.func(t10) - 4.0,
+        ux.func(t10) - 1.0+b2,
+        uy.func(t10) - 4.5,
+
+        x.func(t20) + 2.0,
+        y.func(t20) - 3.0,
+
+        x.func(tf0) + 1.0,
+        ux.func(tf0) + 5.0,
+    )
+
+    start = t0
+    ende = t1
+    expected_guess[0*num_nodes+start:0*num_nodes+ende] = 0.0
+
+    prob = Problem(
+        obj,
+        obj_grad,
+        eom,
+        state_symbols,
+        num_nodes,
+        interval_value,
+        known_parameter_map=par_map,
+        instance_constraints=instance_constraints,
+        time_symbol=t,
+    )
+
+    initial_guess = prob.create_initial_guess()
+    np.testing.assert_allclose(initial_guess, expected_guess)

From eaa52eda140d48b308e5d9ded04ab487303f791a Mon Sep 17 00:00:00 2001
From: Peter Stahlecker <peter.stahlecker@gmail.com>
Date: Tue, 18 Feb 2025 17:56:24 +0100
Subject: [PATCH 2/7] corrected length of underline

---
 opty/direct_collocation.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/opty/direct_collocation.py b/opty/direct_collocation.py
index 375d01c2..8818d2b4 100644
--- a/opty/direct_collocation.py
+++ b/opty/direct_collocation.py
@@ -694,7 +694,7 @@ def create_initial_guess(self):
             The initial guess for the free variables in the optimization problem.
 
         Strategy:
-        --------
+        ---------
         - Instance constraints which contain instances of other state variables
           are ignored.
         - Between successsive instances of a state variable, the values are

From 9d56ba300b8369a3fd0ada7e19f3e58b0df23960 Mon Sep 17 00:00:00 2001
From: Peter Stahlecker <peter.stahlecker@gmail.com>
Date: Tue, 18 Feb 2025 20:32:32 +0100
Subject: [PATCH 3/7] corrected as per suggestions

---
 opty/direct_collocation.py            | 15 ++++++++-------
 opty/tests/test_direct_collocation.py | 18 +++++++++---------
 2 files changed, 17 insertions(+), 16 deletions(-)

diff --git a/opty/direct_collocation.py b/opty/direct_collocation.py
index 8818d2b4..fa6a8ccf 100644
--- a/opty/direct_collocation.py
+++ b/opty/direct_collocation.py
@@ -685,16 +685,17 @@ def parse_free(self, free):
 
         return parse_free(free, n, q, N, variable_duration)
 
-    def create_initial_guess(self):
-        """Creates an initial guess for the free variables in the optimization.
+    def create_linear_initial_guess(self):
+        """Creates an initial guess that is the linear interpolation between
+        exact instance constraints. Please see the notes for more information.
 
-        Returns:
-        --------
+        Returns
+        -------
         initial_guess : (n*N + q*N + r + s)-ndarray
             The initial guess for the free variables in the optimization problem.
 
-        Strategy:
-        ---------
+        Notes
+        -----
         - Instance constraints which contain instances of other state variables
           are ignored.
         - Between successsive instances of a state variable, the values are
@@ -707,7 +708,7 @@ def create_initial_guess(self):
           the value is set to the other finite limit of bound.
         - All else is set to zero.
 
-    """
+        """
         if self.collocator.instance_constraints is None:
             instance_matrix = sm.Matrix([])
         else:
diff --git a/opty/tests/test_direct_collocation.py b/opty/tests/test_direct_collocation.py
index 31017e07..1f7e0bc3 100644
--- a/opty/tests/test_direct_collocation.py
+++ b/opty/tests/test_direct_collocation.py
@@ -1928,7 +1928,7 @@ def test_linear_initial_guess():
     state_symbols = (x, y, ux, uy)
     par_map = {b1: 1.0,
                b2: 2.0,
-               }
+    }
     num_nodes = 61
 
     # A: CONSTANT TIME INTERVAL
@@ -2034,7 +2034,7 @@ def obj_grad(free):
     expected_guess[6*num_nodes+1] = 11.0/2.0
 
 
-    initial_guess = prob.create_initial_guess()
+    initial_guess = prob.create_linear_initial_guess()
     np.testing.assert_allclose(initial_guess, expected_guess)
 
     # A2: np.inf, -np.inf in bounds
@@ -2057,7 +2057,7 @@ def obj_grad(free):
     expected_guess[6*num_nodes] = 10.0
     expected_guess[6*num_nodes+1] = -10.0
 
-    initial_guess = prob.create_initial_guess()
+    initial_guess = prob.create_linear_initial_guess()
     np.testing.assert_allclose(initial_guess, expected_guess)
 
     # A3: no bounds
@@ -2078,7 +2078,7 @@ def obj_grad(free):
         time_symbol=t,
     )
 
-    initial_guess = prob.create_initial_guess()
+    initial_guess = prob.create_linear_initial_guess()
     np.testing.assert_allclose(initial_guess, expected_guess)
 
     # A4: state instances in instance_constraints
@@ -2115,7 +2115,7 @@ def obj_grad(free):
         time_symbol=t,
     )
 
-    initial_guess = prob.create_initial_guess()
+    initial_guess = prob.create_linear_initial_guess()
     np.testing.assert_allclose(initial_guess, expected_guess)
 
     # B: VARIABLE TIME INTERVAL
@@ -2226,7 +2226,7 @@ def obj_grad(free):
     # h - guess
     expected_guess[-1] = 1.5
 
-    initial_guess = prob.create_initial_guess()
+    initial_guess = prob.create_linear_initial_guess()
     np.testing.assert_allclose(initial_guess, expected_guess)
 
     # B2: np.inf, -np.inf in bounds
@@ -2249,7 +2249,7 @@ def obj_grad(free):
     expected_guess[6*num_nodes] = 10.0
     expected_guess[6*num_nodes+1] = -10.0
 
-    initial_guess = prob.create_initial_guess()
+    initial_guess = prob.create_linear_initial_guess()
     np.testing.assert_allclose(initial_guess, expected_guess)
 
     # B3: no bounds
@@ -2270,7 +2270,7 @@ def obj_grad(free):
         instance_constraints=instance_constraints,
         time_symbol=t,
         )
-    initial_guess = prob.create_initial_guess()
+    initial_guess = prob.create_linear_initial_guess()
     np.testing.assert_allclose(initial_guess, expected_guess)
 
     # B4: state instances in instance_constraints
@@ -2307,5 +2307,5 @@ def obj_grad(free):
         time_symbol=t,
     )
 
-    initial_guess = prob.create_initial_guess()
+    initial_guess = prob.create_linear_initial_guess()
     np.testing.assert_allclose(initial_guess, expected_guess)

From 1a17afb66b5025b8244d8b93a463fc95d75dfdd6 Mon Sep 17 00:00:00 2001
From: "Jason K. Moore" <moorepants@gmail.com>
Date: Wed, 19 Feb 2025 05:16:39 +0100
Subject: [PATCH 4/7] Add plotting to initial guess test function.

---
 opty/tests/test_direct_collocation.py | 23 ++++++++++++++++++++++-
 1 file changed, 22 insertions(+), 1 deletion(-)

diff --git a/opty/tests/test_direct_collocation.py b/opty/tests/test_direct_collocation.py
index 1f7e0bc3..cd37bfbc 100644
--- a/opty/tests/test_direct_collocation.py
+++ b/opty/tests/test_direct_collocation.py
@@ -1908,7 +1908,7 @@ def test_attributes_read_only():
         with raises(AttributeError):
             setattr(test, XX, 5)
 
-def test_linear_initial_guess():
+def test_linear_initial_guess(plot=False):
     """Test to check if the initial guess is created correctly."""
 
     x, y, ux, uy = mech.dynamicsymbols('x y ux uy')
@@ -2037,6 +2037,9 @@ def obj_grad(free):
     initial_guess = prob.create_linear_initial_guess()
     np.testing.assert_allclose(initial_guess, expected_guess)
 
+    if plot:
+        prob.plot_trajectories(initial_guess)
+
     # A2: np.inf, -np.inf in bounds
     bounds[a1] = (-np.inf, 10.0)
     bounds[a2] = (-10.0, np.inf)
@@ -2081,6 +2084,9 @@ def obj_grad(free):
     initial_guess = prob.create_linear_initial_guess()
     np.testing.assert_allclose(initial_guess, expected_guess)
 
+    if plot:
+        prob.plot_trajectories(initial_guess)
+
     # A4: state instances in instance_constraints
     instance_constraints = (
         x.func(t0) - 3.0 + ux.func(tf),
@@ -2118,6 +2124,9 @@ def obj_grad(free):
     initial_guess = prob.create_linear_initial_guess()
     np.testing.assert_allclose(initial_guess, expected_guess)
 
+    if plot:
+        prob.plot_trajectories(initial_guess)
+
     # B: VARIABLE TIME INTERVAL
     # B1: normal
     h = sym.symbols('h')
@@ -2229,6 +2238,9 @@ def obj_grad(free):
     initial_guess = prob.create_linear_initial_guess()
     np.testing.assert_allclose(initial_guess, expected_guess)
 
+    if plot:
+        prob.plot_trajectories(initial_guess)
+
     # B2: np.inf, -np.inf in bounds
     bounds[a1] = (-np.inf, 10.0)
     bounds[a2] = (-10.0, np.inf)
@@ -2252,6 +2264,9 @@ def obj_grad(free):
     initial_guess = prob.create_linear_initial_guess()
     np.testing.assert_allclose(initial_guess, expected_guess)
 
+    if plot:
+        prob.plot_trajectories(initial_guess)
+
     # B3: no bounds
     expected_guess[4*num_nodes: 5*num_nodes] = 0.0
     expected_guess[5*num_nodes: 6*num_nodes] = 0.0
@@ -2273,6 +2288,9 @@ def obj_grad(free):
     initial_guess = prob.create_linear_initial_guess()
     np.testing.assert_allclose(initial_guess, expected_guess)
 
+    if plot:
+        prob.plot_trajectories(initial_guess)
+
     # B4: state instances in instance_constraints
     instance_constraints = (
         x.func(t00) - 3.0 + ux.func(tf0),
@@ -2309,3 +2327,6 @@ def obj_grad(free):
 
     initial_guess = prob.create_linear_initial_guess()
     np.testing.assert_allclose(initial_guess, expected_guess)
+
+    if plot:
+        prob.plot_trajectories(initial_guess)

From f2235d6f79cd3985d37fbc2bdff93fd3b495a000 Mon Sep 17 00:00:00 2001
From: Peter Stahlecker <peter.stahlecker@gmail.com>
Date: Wed, 19 Feb 2025 14:37:45 +0100
Subject: [PATCH 5/7] corrected error with instance = 0, added two examples

---
 .../plot_pendulum_swing_up_fixed_duration.py  |  6 +-
 .../beginner/plot_sliding_block.py            | 14 ++--
 opty/direct_collocation.py                    | 65 ++++++++++++++----
 opty/tests/test_direct_collocation.py         | 68 +++++++------------
 4 files changed, 85 insertions(+), 68 deletions(-)

diff --git a/examples-gallery/beginner/plot_pendulum_swing_up_fixed_duration.py b/examples-gallery/beginner/plot_pendulum_swing_up_fixed_duration.py
index e771df51..49a06636 100644
--- a/examples-gallery/beginner/plot_pendulum_swing_up_fixed_duration.py
+++ b/examples-gallery/beginner/plot_pendulum_swing_up_fixed_duration.py
@@ -78,9 +78,9 @@
                time_symbol=t)
 
 # %%
-# Use a random positive initial guess.
-initial_guess = np.random.randn(prob.num_free)
-
+# Use a linear guess.
+# %%
+initial_guess = prob.create_linear_initial_guess(plot=True)
 # %%
 # Find the optimal solution.
 solution, info = prob.solve(initial_guess)
diff --git a/examples-gallery/beginner/plot_sliding_block.py b/examples-gallery/beginner/plot_sliding_block.py
index b9a9ded7..d6078969 100644
--- a/examples-gallery/beginner/plot_sliding_block.py
+++ b/examples-gallery/beginner/plot_sliding_block.py
@@ -1,4 +1,3 @@
-# %%
 """
 Block Sliding Over a Hill
 =========================
@@ -174,6 +173,7 @@ def obj_grad(free):
         bounds=bounds,
     )
 
+    initial_guess = prob.create_linear_initial_guess()
     solution, info = prob.solve(initial_guess)
     solution_list.append(solution)
     info_list.append(info)
@@ -273,15 +273,15 @@ def update(frame):
 print(f'Optimal h value is: {solution_list[selection][-1]:.3f}')
 
 # %%
-prob_list[selection].plot_objective_value()
+_ = prob_list[selection].plot_objective_value()
 
 # %%
 # Plot errors in the solution.
-prob_list[selection].plot_constraint_violations(solution_list[selection])
+_ = prob_list[selection].plot_constraint_violations(solution_list[selection])
 
 # %%
 # Plot the trajectories of the block.
-prob_list[selection].plot_trajectories(solution_list[selection])
+_ = prob_list[selection].plot_trajectories(solution_list[selection])
 
 # %%
 # Animate the solution.
@@ -294,15 +294,15 @@ def update(frame):
 print('Message from optimizer:', info_list[selection]['status_msg'])
 
 # %%
-prob_list[selection].plot_objective_value()
+_ = prob_list[selection].plot_objective_value()
 
 # %%
 # Plot errors in the solution.
-prob_list[selection].plot_constraint_violations(solution_list[selection])
+_ =prob_list[selection].plot_constraint_violations(solution_list[selection])
 
 # %%
 # Plot the trajectories of the block.
-prob_list[selection].plot_trajectories(solution_list[selection])
+_ =prob_list[selection].plot_trajectories(solution_list[selection])
 
 # %%
 # Animate the solution.
diff --git a/opty/direct_collocation.py b/opty/direct_collocation.py
index fa6a8ccf..f58ad820 100644
--- a/opty/direct_collocation.py
+++ b/opty/direct_collocation.py
@@ -685,13 +685,18 @@ def parse_free(self, free):
 
         return parse_free(free, n, q, N, variable_duration)
 
-    def create_linear_initial_guess(self):
+    def create_linear_initial_guess(self, plot=False):
         """Creates an initial guess that is the linear interpolation between
         exact instance constraints. Please see the notes for more information.
 
+        Parameters
+        ----------
+        plot : bool, optional (default=False)
+            If True, the initial guess will be plotted.
+
         Returns
         -------
-        initial_guess : (n*N + q*N + r + s)-ndarray
+        initial_guess : ndarray shape(n*N + q*N + r + s)
             The initial guess for the free variables in the optimization problem.
 
         Notes
@@ -709,6 +714,7 @@ def create_linear_initial_guess(self):
         - All else is set to zero.
 
         """
+        hilfs = sm.symbols('hilfs')
         if self.collocator.instance_constraints is None:
             instance_matrix = sm.Matrix([])
         else:
@@ -716,6 +722,10 @@ def create_linear_initial_guess(self):
         par_map = self.collocator.known_parameter_map
         instance_matrix = instance_matrix.subs({key: par_map[key] for key
                             in par_map.keys()})
+        # setting the variable time interval to 1.0 makes getting the node
+        # numbers of the instance times easy.
+        instance_matrix = instance_matrix.subs({self.collocator.
+                            node_time_interval: 1.0})
 
         initial_guess = np.zeros(self.num_free)
         num_nodes = self.collocator.num_collocation_nodes
@@ -737,6 +747,16 @@ def create_linear_initial_guess(self):
                 M_new.row_del(row)
         instance_matrix = M_new
 
+        # If the instance constraint is of the from ``x(t) - 0``, or ``x(t)``
+        # a dummy ``hilfs`` is added so all constraints have the same form.
+        # This dummy will be set to zero later.
+        for i, expr in enumerate(instance_matrix):
+            zaehler = 0
+            for a in sm.preorder_traversal(expr):
+                zaehler += 1
+            if zaehler == 2:
+                instance_matrix[i, 0] = instance_matrix[i, 0] + hilfs
+
         if self.collocator.instance_constraints is not None:
             if not isinstance(self.collocator.node_time_interval, sm.Symbol):
         # fixed time interval
@@ -759,7 +779,6 @@ def create_linear_initial_guess(self):
                 name_rank = {name.name: i for i, name in enumerate(
                         self.collocator.state_symbols)}
                 liste3 = sorted(liste1, key=lambda x: (name_rank[x[3]], x[1]))
-
                 duration = self.collocator.node_time_interval * (num_nodes-1)
                 for state in self.collocator.state_symbols:
                     state_idx = self.collocator.state_symbols.index(state)
@@ -768,7 +787,10 @@ def create_linear_initial_guess(self):
                     for i in liste3:
                         if state.name == i[3]:
                             times.append(i[1])
-                            values.append(-i[2])
+                            if i[-2] != hilfs:
+                                values.append(-i[2])
+                            else:
+                                values.append(0)
                     if len(times) == 0:
                         pass
                     elif len(times) == 1:
@@ -787,12 +809,9 @@ def create_linear_initial_guess(self):
             else:
                 liste = []
                 liste1 = []
-                instance_matrix = instance_matrix.subs({self.collocator.
-                                    node_time_interval: 1.0})
                 for exp1 in instance_matrix:
                     for a in sm.preorder_traversal(exp1):
                         liste.append(a)
-
                 for entry in liste:
                     if isinstance(entry, sm.Function):
                         idx = liste.index(entry)
@@ -815,7 +834,10 @@ def create_linear_initial_guess(self):
                     for i in liste3:
                         if state.name == i[3]:
                             times.append(i[1])
-                            values.append(-i[2])
+                            if i[-2] != hilfs:
+                                values.append(-i[2])
+                            else:
+                                values.append(0)
                     if len(times) == 0:
                         pass
                     elif len(times) == 1:
@@ -863,22 +885,35 @@ def create_linear_initial_guess(self):
                     initial_guess[start_idx + idx]  = wert
 
         # set the value of the variable time interval.
-        if self.bounds is not None and \
-            isinstance(self.collocator.node_time_interval, sm.Symbol):
+        if isinstance(self.collocator.node_time_interval, sm.Symbol):
+            if self.bounds is not None:
                 if self.collocator.node_time_interval in self.bounds.keys():
                     if self.bounds[self.collocator.node_time_interval][0] \
                         == -np.inf:
-                        wert = self.bounds[self.collocator.node_time_interval][1]
+                        wert = self.bounds[self.collocator.
+                                               node_time_interval][1]
                     elif self.bounds[self.collocator.node_time_interval][1] \
                         == np.inf:
-                        wert = self.bounds[self.collocator.node_time_interval][0]
+                        wert = self.bounds[self.collocator.
+                                               node_time_interval][0]
                     else:
                         wert = 0.5*(self.bounds[self.collocator.
-                            node_time_interval][0] +
-                            self.bounds[self.collocator.
-                            node_time_interval][1])
+                                node_time_interval][0] +
+                                self.bounds[self.collocator.
+                                node_time_interval][1])
                     initial_guess[-1] = wert
 
+            if self.bounds is None:
+                initial_guess[-1] = 1.0 / (num_nodes-1)
+
+            elif self.collocator.node_time_interval not in self.bounds.keys():
+                initial_guess[-1] = 1.0 / (num_nodes-1)
+            else:
+                pass
+
+        if plot:
+            self.plot_trajectories(initial_guess)
+
         return initial_guess
 
 
diff --git a/opty/tests/test_direct_collocation.py b/opty/tests/test_direct_collocation.py
index cd37bfbc..2852b632 100644
--- a/opty/tests/test_direct_collocation.py
+++ b/opty/tests/test_direct_collocation.py
@@ -1928,7 +1928,7 @@ def test_linear_initial_guess(plot=False):
     state_symbols = (x, y, ux, uy)
     par_map = {b1: 1.0,
                b2: 2.0,
-    }
+               }
     num_nodes = 61
 
     # A: CONSTANT TIME INTERVAL
@@ -1946,7 +1946,7 @@ def obj_grad(free):
         return grad
 
     instance_constraints = (
-        x.func(t0) - 3.0,
+        x.func(t0),
         y.func(t0) - 2.0,
         ux.func(t0) - 5.0,
 
@@ -1956,10 +1956,10 @@ def obj_grad(free):
         uy.func(t1) - 4.5,
 
         x.func(t2) + 2.0,
-        y.func(t2) - 3.0,
+        y.func(t2),
 
-        x.func(tf) + 1.0,
-        ux.func(tf) + 5.0,
+        x.func(tf),
+        ux.func(tf),
     )
 
     bounds= {
@@ -1994,7 +1994,7 @@ def obj_grad(free):
     # x - guess
     start = round(t0/duration*num_nodes)
     ende = round(t1/duration*num_nodes)
-    werte = np.linspace(3.0, 1.5*par_map[b1], ende-start)
+    werte = np.linspace(0.0, 1.5*par_map[b1], ende-start)
     expected_guess[0*num_nodes+start:0*num_nodes+ende] = werte
     start = round(t1/duration*num_nodes)
     ende = round(t2/duration*num_nodes)
@@ -2002,7 +2002,7 @@ def obj_grad(free):
     expected_guess[0*num_nodes+start:0*num_nodes+ende] = werte
     start = round(t2/duration*num_nodes)
     ende = round(tf/duration*num_nodes)
-    werte = np.linspace(-2.0, -1.0, ende-start)
+    werte = np.linspace(-2.0, 0.0, ende-start)
     expected_guess[0*num_nodes+start:0*num_nodes+ende] = werte
      # y - guess
     start = round(t0/duration*num_nodes)
@@ -2011,7 +2011,7 @@ def obj_grad(free):
     expected_guess[1*num_nodes+start:1*num_nodes+ende] = werte
     start = round(t1/duration*num_nodes)
     ende = round(t2/duration*num_nodes)
-    werte = np.linspace(4.0, 3.0, ende-start)
+    werte = np.linspace(4.0, 0.0, ende-start)
     expected_guess[1*num_nodes+start:1*num_nodes+ende] = werte
     # ux - guess
     start = round(t0/duration*num_nodes)
@@ -2020,7 +2020,7 @@ def obj_grad(free):
     expected_guess[2*num_nodes+start:2*num_nodes+ende] = werte
     start = round(t1/duration*num_nodes)
     ende = round(tf/duration*num_nodes)
-    werte = np.linspace(1.0-par_map[b2], -5.0, ende-start)
+    werte = np.linspace(1.0-par_map[b2], 0.0, ende-start)
     expected_guess[2*num_nodes+start:2*num_nodes+ende] = werte
     # uy - guess
     expected_guess[3*num_nodes:4*num_nodes] = 4.5
@@ -2033,13 +2033,9 @@ def obj_grad(free):
     # a2 - guess
     expected_guess[6*num_nodes+1] = 11.0/2.0
 
-
     initial_guess = prob.create_linear_initial_guess()
     np.testing.assert_allclose(initial_guess, expected_guess)
 
-    if plot:
-        prob.plot_trajectories(initial_guess)
-
     # A2: np.inf, -np.inf in bounds
     bounds[a1] = (-np.inf, 10.0)
     bounds[a2] = (-10.0, np.inf)
@@ -2084,9 +2080,6 @@ def obj_grad(free):
     initial_guess = prob.create_linear_initial_guess()
     np.testing.assert_allclose(initial_guess, expected_guess)
 
-    if plot:
-        prob.plot_trajectories(initial_guess)
-
     # A4: state instances in instance_constraints
     instance_constraints = (
         x.func(t0) - 3.0 + ux.func(tf),
@@ -2099,10 +2092,10 @@ def obj_grad(free):
         uy.func(t1) - 4.5,
 
         x.func(t2) + 2.0,
-        y.func(t2) - 3.0,
+        y.func(t2),
 
-        x.func(tf) + 1.0,
-        ux.func(tf) + 5.0,
+        x.func(tf),
+        ux.func(tf),
     )
 
     start = round(t0/duration*num_nodes)
@@ -2124,9 +2117,7 @@ def obj_grad(free):
     initial_guess = prob.create_linear_initial_guess()
     np.testing.assert_allclose(initial_guess, expected_guess)
 
-    if plot:
-        prob.plot_trajectories(initial_guess)
-
+    # ========================================================================
     # B: VARIABLE TIME INTERVAL
     # B1: normal
     h = sym.symbols('h')
@@ -2143,8 +2134,8 @@ def obj_grad(free):
         return grad
 
     instance_constraints = (
-        x.func(t00) - 3.0,
-        y.func(t00) - 2.0,
+        x.func(t00),
+        y.func(t00),
         ux.func(t00) - 5.0,
 
         x.func(t10) - 1.5*b1,
@@ -2152,7 +2143,7 @@ def obj_grad(free):
         ux.func(t10) - 1.0+b2,
         uy.func(t10) - 4.5,
 
-        x.func(t20) + 2.0,
+        x.func(t20),
         y.func(t20) - 3.0,
 
         x.func(tf0) + 1.0,
@@ -2194,20 +2185,20 @@ def obj_grad(free):
     # x - guess
     start = t0
     ende = t1
-    werte = np.linspace(3.0, 1.5*par_map[b1], ende-start)
+    werte = np.linspace(0.0, 1.5*par_map[b1], ende-start)
     expected_guess[0*num_nodes+start:0*num_nodes+ende] = werte
     start = t1
     ende = t2
-    werte = np.linspace(1.5*par_map[b1], -2.0, ende-start)
+    werte = np.linspace(1.5*par_map[b1], 0.0, ende-start)
     expected_guess[0*num_nodes+start:0*num_nodes+ende] = werte
     start = t2
     ende = tf
-    werte = np.linspace(-2.0, -1.0, ende-start)
+    werte = np.linspace(0.0, -1.0, ende-start)
     expected_guess[0*num_nodes+start:0*num_nodes+ende] = werte
      # y - guess
     start = t0
     ende = t1
-    werte = np.linspace(2.0, 4.0, ende-start)
+    werte = np.linspace(0.0, 4.0, ende-start)
     expected_guess[1*num_nodes+start:1*num_nodes+ende] = werte
     start = t1
     ende = t2
@@ -2238,9 +2229,6 @@ def obj_grad(free):
     initial_guess = prob.create_linear_initial_guess()
     np.testing.assert_allclose(initial_guess, expected_guess)
 
-    if plot:
-        prob.plot_trajectories(initial_guess)
-
     # B2: np.inf, -np.inf in bounds
     bounds[a1] = (-np.inf, 10.0)
     bounds[a2] = (-10.0, np.inf)
@@ -2264,15 +2252,12 @@ def obj_grad(free):
     initial_guess = prob.create_linear_initial_guess()
     np.testing.assert_allclose(initial_guess, expected_guess)
 
-    if plot:
-        prob.plot_trajectories(initial_guess)
-
     # B3: no bounds
     expected_guess[4*num_nodes: 5*num_nodes] = 0.0
     expected_guess[5*num_nodes: 6*num_nodes] = 0.0
     expected_guess[6*num_nodes] = 0.0
     expected_guess[6*num_nodes+1] = 0.0
-    expected_guess[-1] = 0.0
+    expected_guess[-1] = 1.0 / (num_nodes-1)
 
     prob = Problem(
         obj,
@@ -2288,13 +2273,10 @@ def obj_grad(free):
     initial_guess = prob.create_linear_initial_guess()
     np.testing.assert_allclose(initial_guess, expected_guess)
 
-    if plot:
-        prob.plot_trajectories(initial_guess)
-
     # B4: state instances in instance_constraints
     instance_constraints = (
         x.func(t00) - 3.0 + ux.func(tf0),
-        y.func(t00) - 2.0,
+        y.func(t00),
         ux.func(t00) - 5.0,
 
         x.func(t10) - 1.5*b1,
@@ -2302,11 +2284,11 @@ def obj_grad(free):
         ux.func(t10) - 1.0+b2,
         uy.func(t10) - 4.5,
 
-        x.func(t20) + 2.0,
+        x.func(t20),
         y.func(t20) - 3.0,
 
-        x.func(tf0) + 1.0,
-        ux.func(tf0) + 5.0,
+        x.func(tf0)+ 1.0,
+        ux.func(tf0)+ 5.0,
     )
 
     start = t0

From d89338b47a0e14b2daec1b47744740b7d10b4645 Mon Sep 17 00:00:00 2001
From: Peter Stahlecker <peter.stahlecker@gmail.com>
Date: Fri, 28 Feb 2025 14:02:27 +0100
Subject: [PATCH 6/7] removed plot kwarg, added end_time kwarg, expanded the
 test

---
 opty/direct_collocation.py            |  14 +-
 opty/tests/test_direct_collocation.py | 212 ++++++++++++++++++++------
 2 files changed, 171 insertions(+), 55 deletions(-)

diff --git a/opty/direct_collocation.py b/opty/direct_collocation.py
index cc69bd63..1e2c1999 100644
--- a/opty/direct_collocation.py
+++ b/opty/direct_collocation.py
@@ -685,14 +685,15 @@ def parse_free(self, free):
 
         return parse_free(free, n, q, N, variable_duration)
 
-    def create_linear_initial_guess(self, plot=False):
+    def create_linear_initial_guess(self, end_time=1.0):
         """Creates an initial guess that is the linear interpolation between
         exact instance constraints. Please see the notes for more information.
 
         Parameters
         ----------
-        plot : bool, optional (default=False)
-            If True, the initial guess will be plotted.
+        end_time : float, optional (default=1.0)
+            In case of a variable time interval, this is the assumed duration
+            of the simulation.
 
         Returns
         -------
@@ -904,16 +905,13 @@ def create_linear_initial_guess(self, plot=False):
                     initial_guess[-1] = wert
 
             if self.bounds is None:
-                initial_guess[-1] = 1.0 / (num_nodes-1)
+                initial_guess[-1] = end_time / (num_nodes-1)
 
             elif self.collocator.node_time_interval not in self.bounds.keys():
-                initial_guess[-1] = 1.0 / (num_nodes-1)
+                initial_guess[-1] = end_time / (num_nodes-1)
             else:
                 pass
 
-        if plot:
-            self.plot_trajectories(initial_guess)
-
         return initial_guess
 
 
diff --git a/opty/tests/test_direct_collocation.py b/opty/tests/test_direct_collocation.py
index 15511471..322091ea 100644
--- a/opty/tests/test_direct_collocation.py
+++ b/opty/tests/test_direct_collocation.py
@@ -1948,7 +1948,7 @@ def test_linear_initial_guess(plot=False):
     num_nodes = 61
 
     # A: CONSTANT TIME INTERVAL
-    # A!: normal
+    # A0: no bounds, no instance constraints
     t0, t1, t2, tf = 0.0, 2.0, 4.0, 6.0
     interval_value = tf/(num_nodes - 1)
 
@@ -1961,6 +1961,24 @@ def obj_grad(free):
         grad[0: 2*num_nodes] = 2.0*free[0:2*num_nodes]*interval_value
         return grad
 
+    prob = Problem(
+        obj,
+        obj_grad,
+        eom,
+        state_symbols,
+        num_nodes,
+        interval_value,
+        known_parameter_map=par_map,
+        time_symbol=t,
+        backend='numpy',
+    )
+
+    expected_guess = np.zeros(prob.num_free)
+    initial_guess = prob.create_linear_initial_guess()
+    np.testing.assert_allclose(initial_guess, expected_guess)
+
+    # A1: no bounds
+
     instance_constraints = (
         x.func(t0),
         y.func(t0) - 2.0,
@@ -1978,19 +1996,6 @@ def obj_grad(free):
         ux.func(tf),
     )
 
-    bounds= {
-        a1: (-1.0, 10.0),
-        a2: (-1.0, 12.0),
-
-        u1: (-10.0, 1.0),
-        u2: (-1.0, 10.0),
-
-        x: (-5.0, 5.0 ),
-        ux: (-1.0, 1.0),
-        y: (-4.0, 4.0),
-        uy: (-1.0, 1.0),
-    }
-
     prob = Problem(
         obj,
         obj_grad,
@@ -2000,8 +2005,8 @@ def obj_grad(free):
         interval_value,
         known_parameter_map=par_map,
         instance_constraints=instance_constraints,
-        bounds=bounds,
         time_symbol=t,
+        backend='numpy',
     )
 
     # Set the expected initial guess
@@ -2041,6 +2046,48 @@ def obj_grad(free):
     # uy - guess
     expected_guess[3*num_nodes:4*num_nodes] = 4.5
     # u1 - guess
+    expected_guess[4*num_nodes:5*num_nodes] = 0.0
+    # u2 - guess
+    expected_guess[5*num_nodes:6*num_nodes] = 0.0
+    # a1 - guess
+    expected_guess[6*num_nodes] = 0.0
+    # a2 - guess
+    expected_guess[6*num_nodes+1] = 0.0
+
+    initial_guess = prob.create_linear_initial_guess()
+    np.testing.assert_allclose(initial_guess, expected_guess)
+
+    # A2 normal
+
+    bounds= {
+        a1: (-1.0, 10.0),
+        a2: (-1.0, 12.0),
+
+        u1: (-10.0, 1.0),
+        u2: (-1.0, 10.0),
+
+        x: (-5.0, 5.0 ),
+        ux: (-1.0, 1.0),
+        y: (-4.0, 4.0),
+        uy: (-1.0, 1.0),
+    }
+
+    prob = Problem(
+        obj,
+        obj_grad,
+        eom,
+        state_symbols,
+        num_nodes,
+        interval_value,
+        known_parameter_map=par_map,
+        instance_constraints=instance_constraints,
+        bounds=bounds,
+        time_symbol=t,
+        backend='numpy',
+    )
+
+    # Set new expected guesses as bounds are present
+    # u1 - guess
     expected_guess[4*num_nodes:5*num_nodes] = -9.0/2.0
     # u2 - guess
     expected_guess[5*num_nodes:6*num_nodes] = 9.0/2.0
@@ -2052,7 +2099,7 @@ def obj_grad(free):
     initial_guess = prob.create_linear_initial_guess()
     np.testing.assert_allclose(initial_guess, expected_guess)
 
-    # A2: np.inf, -np.inf in bounds
+    # A3: np.inf, -np.inf in bounds
     bounds[a1] = (-np.inf, 10.0)
     bounds[a2] = (-10.0, np.inf)
 
@@ -2067,6 +2114,7 @@ def obj_grad(free):
         instance_constraints=instance_constraints,
         bounds=bounds,
         time_symbol=t,
+        backend='numpy',
     )
 
     expected_guess[6*num_nodes] = 10.0
@@ -2075,7 +2123,7 @@ def obj_grad(free):
     initial_guess = prob.create_linear_initial_guess()
     np.testing.assert_allclose(initial_guess, expected_guess)
 
-    # A3: no bounds
+    # A4: no bounds
     expected_guess[4*num_nodes: 5*num_nodes] = 0.0
     expected_guess[5*num_nodes: 6*num_nodes] = 0.0
     expected_guess[6*num_nodes] = 0.0
@@ -2091,12 +2139,14 @@ def obj_grad(free):
         known_parameter_map=par_map,
         instance_constraints=instance_constraints,
         time_symbol=t,
+        backend='numpy',
     )
 
     initial_guess = prob.create_linear_initial_guess()
     np.testing.assert_allclose(initial_guess, expected_guess)
 
-    # A4: state instances in instance_constraints
+
+    # A5: state instances in instance_constraints
     instance_constraints = (
         x.func(t0) - 3.0 + ux.func(tf),
         y.func(t0) - 2.0,
@@ -2128,16 +2178,19 @@ def obj_grad(free):
         known_parameter_map=par_map,
         instance_constraints=instance_constraints,
         time_symbol=t,
+        backend='numpy',
     )
 
     initial_guess = prob.create_linear_initial_guess()
     np.testing.assert_allclose(initial_guess, expected_guess)
 
+
     # ========================================================================
     # B: VARIABLE TIME INTERVAL
-    # B1: normal
+    # B0: no bounds no instances
     h = sym.symbols('h')
-    t00, t10, t20, tf0 = 0.0, int(num_nodes/3)*h, int(2*num_nodes/3)*h, (num_nodes - 1)*h
+    t00, t10, t20 = 0.0, int(num_nodes/3)*h, int(2*num_nodes/3)*h
+    tf0 = (num_nodes - 1)*h
     interval_value = h
 
     def obj(free):
@@ -2149,6 +2202,28 @@ def obj_grad(free):
         grad[0: 2*num_nodes] = 2.0*free[0:2*num_nodes]*free[-1]
         return grad
 
+    prob = Problem(
+        obj,
+        obj_grad,
+        eom,
+        state_symbols,
+        num_nodes,
+        interval_value,
+        known_parameter_map=par_map,
+        time_symbol=t,
+        backend='numpy',
+    )
+
+    expected_guess = np.zeros(prob.num_free)
+    expected_guess[-1] = 1.0 / (num_nodes-1)
+    initial_guess = prob.create_linear_initial_guess()
+    np.testing.assert_allclose(initial_guess, expected_guess)
+
+    initial_guess = prob.create_linear_initial_guess(end_time=2.0)
+    expected_guess[-1] = 2.0 / (num_nodes-1)
+    np.testing.assert_allclose(initial_guess, expected_guess)
+
+    # B1: no bounds
     instance_constraints = (
         x.func(t00),
         y.func(t00),
@@ -2166,33 +2241,6 @@ def obj_grad(free):
         ux.func(tf0) + 5.0,
     )
 
-    bounds= {
-        a1: (-1.0, 10.0),
-        a2: (-1.0, 12.0),
-
-        u1: (-10.0, 1.0),
-        u2: (-1.0, 10.0),
-
-        x: (-5.0, 5.0 ),
-        ux: (-1.0, 1.0),
-        y: (-4.0, 4.0),
-        uy: (-1.0, 1.0),
-        h: (1.0, 2.0),
-    }
-
-    prob = Problem(
-        obj,
-        obj_grad,
-        eom,
-        state_symbols,
-        num_nodes,
-        interval_value,
-        known_parameter_map=par_map,
-        instance_constraints=instance_constraints,
-        bounds=bounds,
-        time_symbol=t,
-    )
-
     # Set the expected initial guess
     expected_guess = np.zeros(prob.num_free)
     t0, t1, t2 = int(0.0), int(num_nodes/3), int(2*num_nodes/3)
@@ -2232,6 +2280,68 @@ def obj_grad(free):
     # uy - guess
     expected_guess[3*num_nodes:4*num_nodes] = 4.5
     # u1 - guess
+    expected_guess[4*num_nodes:5*num_nodes] = 0
+    # u2 - guess
+    expected_guess[5*num_nodes:6*num_nodes] = 0
+    # a1 - guess
+    expected_guess[6*num_nodes] = 0
+    # a2 - guess
+    expected_guess[6*num_nodes+1] = 0
+    # h - guess
+    expected_guess[-1] = 1.0 / (num_nodes-1)
+
+    prob = Problem(
+        obj,
+        obj_grad,
+        eom,
+        state_symbols,
+        num_nodes,
+        interval_value,
+        known_parameter_map=par_map,
+        instance_constraints=instance_constraints,
+        time_symbol=t,
+        backend='numpy',
+    )
+
+    initial_guess = prob.create_linear_initial_guess()
+    np.testing.assert_allclose(initial_guess, expected_guess)
+
+    initial_guess = prob.create_linear_initial_guess(end_time=3.0)
+    expected_guess[-1] = 3.0 / (num_nodes-1)
+    np.testing.assert_allclose(initial_guess, expected_guess)
+
+
+    # B3 normal
+    bounds= {
+        a1: (-1.0, 10.0),
+        a2: (-1.0, 12.0),
+
+        u1: (-10.0, 1.0),
+        u2: (-1.0, 10.0),
+
+        x: (-5.0, 5.0 ),
+        ux: (-1.0, 1.0),
+        y: (-4.0, 4.0),
+        uy: (-1.0, 1.0),
+        h: (1.0, 2.0),
+    }
+
+    prob = Problem(
+        obj,
+        obj_grad,
+        eom,
+        state_symbols,
+        num_nodes,
+        interval_value,
+        known_parameter_map=par_map,
+        instance_constraints=instance_constraints,
+        bounds=bounds,
+        time_symbol=t,
+        backend='numpy',
+    )
+
+    # Set new expected guesses as bounds are present
+    # u1 - guess
     expected_guess[4*num_nodes:5*num_nodes] = -9.0/2.0
     # u2 - guess
     expected_guess[5*num_nodes:6*num_nodes] = 9.0/2.0
@@ -2245,6 +2355,11 @@ def obj_grad(free):
     initial_guess = prob.create_linear_initial_guess()
     np.testing.assert_allclose(initial_guess, expected_guess)
 
+    initial_guess = prob.create_linear_initial_guess(end_time=4.0)
+    # as bound for h is given, end_time has no effect.
+    expected_guess[-1] = 1.5
+    np.testing.assert_allclose(initial_guess, expected_guess)
+
     # B2: np.inf, -np.inf in bounds
     bounds[a1] = (-np.inf, 10.0)
     bounds[a2] = (-10.0, np.inf)
@@ -2260,6 +2375,7 @@ def obj_grad(free):
         instance_constraints=instance_constraints,
         bounds=bounds,
         time_symbol=t,
+        backend='numpy',
     )
 
     expected_guess[6*num_nodes] = 10.0
@@ -2285,6 +2401,7 @@ def obj_grad(free):
         known_parameter_map=par_map,
         instance_constraints=instance_constraints,
         time_symbol=t,
+        backend='numpy',
         )
     initial_guess = prob.create_linear_initial_guess()
     np.testing.assert_allclose(initial_guess, expected_guess)
@@ -2321,6 +2438,7 @@ def obj_grad(free):
         known_parameter_map=par_map,
         instance_constraints=instance_constraints,
         time_symbol=t,
+        backend='numpy',
     )
 
     initial_guess = prob.create_linear_initial_guess()

From 34d2a5d2cafac369cd5de9e8a345782c84a5856b Mon Sep 17 00:00:00 2001
From: Peter Stahlecker <peter.stahlecker@gmail.com>
Date: Fri, 28 Feb 2025 14:14:24 +0100
Subject: [PATCH 7/7] removed kwarg plot from pendulum

---
 .../beginner/plot_pendulum_swing_up_fixed_duration.py           | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/examples-gallery/beginner/plot_pendulum_swing_up_fixed_duration.py b/examples-gallery/beginner/plot_pendulum_swing_up_fixed_duration.py
index 491a0fd9..c917c640 100644
--- a/examples-gallery/beginner/plot_pendulum_swing_up_fixed_duration.py
+++ b/examples-gallery/beginner/plot_pendulum_swing_up_fixed_duration.py
@@ -99,7 +99,7 @@
 # %%
 # Use a linear guess.
 # %%
-initial_guess = prob.create_linear_initial_guess(plot=True)
+initial_guess = prob.create_linear_initial_guess()
 # %%
 # Find the optimal solution.
 solution, info = prob.solve(initial_guess)