From 2985b5ac4f7a19a6815af275c488ac09a7b34687 Mon Sep 17 00:00:00 2001 From: William Zijie Zhang Date: Mon, 7 Jul 2025 17:13:21 -0700 Subject: [PATCH] fix interface issues thanks to parth --- cvxpy/reductions/expr2smooth/expr2smooth.py | 29 +-- .../expr2smooth/nlp_matrix_stuffing.py | 97 --------- .../solvers/nlp_solvers/ipopt_nlpif.py | 13 +- cvxpy/reductions/solvers/solving_chain.py | 4 +- cvxpy/sandbox/control_of_car.py | 185 ++++++++++++++++++ cvxpy/sandbox/direct_ipopt_call.py | 4 +- 6 files changed, 197 insertions(+), 135 deletions(-) delete mode 100644 cvxpy/reductions/expr2smooth/nlp_matrix_stuffing.py create mode 100644 cvxpy/sandbox/control_of_car.py diff --git a/cvxpy/reductions/expr2smooth/expr2smooth.py b/cvxpy/reductions/expr2smooth/expr2smooth.py index 258d2b7325..15cb2200de 100644 --- a/cvxpy/reductions/expr2smooth/expr2smooth.py +++ b/cvxpy/reductions/expr2smooth/expr2smooth.py @@ -28,7 +28,7 @@ from cvxpy.reductions.solution import Solution -class Expr2smooth(Canonicalization): +class Expr2Smooth(Canonicalization): """Reduce Expressions to an equivalent smooth program This reduction takes as input (minimization) expressions and converts @@ -42,33 +42,6 @@ def __init__(self, problem=None, quad_obj: bool = False) -> None: def accepts(self, problem): """A problem is always accepted""" return True - - def invert(self, solution, inverse_data): - """Retrieves a solution to the original problem""" - var_map = inverse_data.var_offsets - # Flip sign of opt val if maximize. - opt_val = solution.opt_val - if solution.status not in s.ERROR and not inverse_data.minimize: - opt_val = -solution.opt_val - - primal_vars, dual_vars = {}, {} - if solution.status not in s.SOLUTION_PRESENT: - return Solution(solution.status, opt_val, primal_vars, dual_vars, - solution.attr) - - # Split vectorized variable into components. - x_opt = list(solution.primal_vars.values())[0] - for var_id, offset in var_map.items(): - shape = inverse_data.var_shapes[var_id] - size = np.prod(shape, dtype=int) - primal_vars[var_id] = np.reshape(x_opt[offset:offset+size], shape, - order='F') - - solution = super(Expr2smooth, self).invert(solution, inverse_data) - - return Solution(solution.status, opt_val, primal_vars, dual_vars, - solution.attr) - def apply(self, problem): """Converts an expr to a smooth program""" diff --git a/cvxpy/reductions/expr2smooth/nlp_matrix_stuffing.py b/cvxpy/reductions/expr2smooth/nlp_matrix_stuffing.py deleted file mode 100644 index 1aa42e2997..0000000000 --- a/cvxpy/reductions/expr2smooth/nlp_matrix_stuffing.py +++ /dev/null @@ -1,97 +0,0 @@ -""" -Copyright 2013 Steven Diamond - -Licensed under the Apache License, Version 2.0 (the "License"); -you may not use this file except in compliance with the License. -You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - -Unless required by applicable law or agreed to in writing, software -distributed under the License is distributed on an "AS IS" BASIS, -WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -See the License for the specific language governing permissions and -limitations under the License. -""" -from __future__ import annotations - -import numpy as np - -import cvxpy.settings as s -from cvxpy.constraints import ( - PSD, - SOC, - Equality, - ExpCone, - Inequality, - NonNeg, - NonPos, - PowCone3D, - Zero, -) -from cvxpy.expressions.variable import Variable -from cvxpy.problems.objective import Minimize -from cvxpy.reductions import InverseData, Solution, cvx_attr2constr -from cvxpy.reductions.matrix_stuffing import ( - MatrixStuffing, - extract_lower_bounds, - extract_mip_idx, - extract_upper_bounds, -) -from cvxpy.reductions.solvers.solving_chain_utils import get_canon_backend -from cvxpy.reductions.utilities import ( - are_args_affine, - group_constraints, - lower_equality, - lower_ineq_to_nonneg, - nonpos2nonneg, -) -from cvxpy.utilities.coeff_extractor import CoeffExtractor - - -class NLPMatrixStuffing(MatrixStuffing): - """Construct matrices for linear cone problems. - - Linear cone problems are assumed to have a linear objective and cone - constraints which may have zero or more arguments, all of which must be - affine. - """ - CONSTRAINTS = 'ordered_constraints' - - def __init__(self, canon_backend: str | None = None): - self.canon_backend = canon_backend - - def accepts(self, problem): - valid_obj_curv = problem.objective.expr.is_affine() - return (type(problem.objective) == Minimize - and valid_obj_curv - and not cvx_attr2constr.convex_attributes(problem.variables()) - and are_args_affine(problem.constraints) - and problem.is_dpp()) - - def apply(self, problem): - pass - - def invert(self, solution, inverse_data): - """Retrieves a solution to the original problem""" - var_map = inverse_data.var_offsets - # Flip sign of opt val if maximize. - opt_val = solution.opt_val - if solution.status not in s.ERROR and not inverse_data.minimize: - opt_val = -solution.opt_val - - primal_vars, dual_vars = {}, {} - if solution.status not in s.SOLUTION_PRESENT: - return Solution(solution.status, opt_val, primal_vars, dual_vars, - solution.attr) - - # Split vectorized variable into components. - x_opt = list(solution.primal_vars.values())[0] - for var_id, offset in var_map.items(): - shape = inverse_data.var_shapes[var_id] - size = np.prod(shape, dtype=int) - primal_vars[var_id] = np.reshape(x_opt[offset:offset+size], shape, - order='F') - - return Solution(solution.status, opt_val, primal_vars, dual_vars, - solution.attr) diff --git a/cvxpy/reductions/solvers/nlp_solvers/ipopt_nlpif.py b/cvxpy/reductions/solvers/nlp_solvers/ipopt_nlpif.py index ff3e49f2de..f36d2aea46 100644 --- a/cvxpy/reductions/solvers/nlp_solvers/ipopt_nlpif.py +++ b/cvxpy/reductions/solvers/nlp_solvers/ipopt_nlpif.py @@ -86,12 +86,13 @@ def invert(self, solution, inverse_data): if status in s.SOLUTION_PRESENT: primal_val = solution['obj_val'] opt_val = primal_val + inverse_data.offset - """ - primal_vars = { - inverse_data[IPOPT.VAR_ID]: solution['x'] - } - """ - return Solution(status, opt_val, {16: np.array([14., 14., 6.])}, {}, attr) + primal_vars = {} + x_opt = solution['x'] + for id, offset in inverse_data.var_offsets.items(): + shape = inverse_data.var_shapes[id] + size = np.prod(shape, dtype=int) + primal_vars[id] = np.reshape(x_opt[offset:offset+size], shape, order='F') + return Solution(status, opt_val, primal_vars, {}, attr) else: return failure_solution(status, attr) diff --git a/cvxpy/reductions/solvers/solving_chain.py b/cvxpy/reductions/solvers/solving_chain.py index 33a37984e7..55bdb01faf 100644 --- a/cvxpy/reductions/solvers/solving_chain.py +++ b/cvxpy/reductions/solvers/solving_chain.py @@ -36,7 +36,7 @@ Valinvec2mixedint, ) from cvxpy.reductions.eval_params import EvalParams -from cvxpy.reductions.expr2smooth.expr2smooth import Expr2smooth +from cvxpy.reductions.expr2smooth.expr2smooth import Expr2Smooth from cvxpy.reductions.flip_objective import FlipObjective from cvxpy.reductions.qp2quad_form import qp2symbolic_qp from cvxpy.reductions.qp2quad_form.qp_matrix_stuffing import QpMatrixStuffing @@ -145,7 +145,7 @@ def _reductions_for_problem_class( if nlp: if type(problem.objective) == Maximize: reductions += [FlipObjective()] - reductions += [Expr2smooth()] + reductions += [Expr2Smooth()] return reductions if not gp and not problem.is_dcp(): diff --git a/cvxpy/sandbox/control_of_car.py b/cvxpy/sandbox/control_of_car.py new file mode 100644 index 0000000000..6bb427122b --- /dev/null +++ b/cvxpy/sandbox/control_of_car.py @@ -0,0 +1,185 @@ +import cvxpy as cp +import numpy as np +import matplotlib.pyplot as plt + + +def solve_car_control(x_final, L=0.1, N=50, h=0.1, gamma=10): + """ + Solve the nonlinear optimal control problem for car trajectory planning. + + Parameters: + - x_final: tuple (p1, p2, theta) for final position and orientation + - L: wheelbase length + - N: number of time steps + - h: time step size + - gamma: weight for control smoothness term + + Returns: + - x_opt: optimal states (N+1 x 3) + - u_opt: optimal controls (N x 2) + """ + + # Variables + # States: x[k] = [p1(k), p2(k), theta(k)] + x = cp.Variable((N+1, 3)) + # Controls: u[k] = [s(k), phi(k)] + u = cp.Variable((N, 2)) + + # Initial state (starting at origin with zero orientation) + x_init = np.array([0, 0, 0]) + + # Objective function + objective = 0 + + # Sum of squared control inputs + for k in range(N): + objective += cp.sum_squares(u[k, :]) + + # Control smoothness term + for k in range(N-1): + objective += gamma * cp.sum_squares(u[k+1, :] - u[k, :]) + + # Constraints + constraints = [] + + # Initial state constraint + constraints.append(x[0, :] == x_init) + + # Dynamics constraints + # Note: We're pretending cp.sin, cp.cos, and cp.tan exist as atoms + for k in range(N): + # x[k+1] = f(x[k], u[k]) + # where f(x, u) = x + h * [u[0]*cos(x[2]), u[0]*sin(x[2]), u[0]*tan(u[1])/L] + + # Position dynamics + constraints.append(x[k+1, 0] == x[k, 0] + h * u[k, 0] * cp.cos(x[k, 2])) + constraints.append(x[k+1, 1] == x[k, 1] + h * u[k, 0] * cp.sin(x[k, 2])) + + # Orientation dynamics + constraints.append(x[k+1, 2] == x[k, 2] + h * (u[k, 0] / L) * cp.tan(u[k, 1])) + + # Final state constraint + constraints.append(x[N, :] == x_final) + + # Steering angle limits (optional but realistic) + # Assuming max steering angle of 45 degrees + max_steering = np.pi / 4 + constraints.append(u[:, 1] >= -max_steering) + constraints.append(u[:, 1] <= max_steering) + + # Create and solve the problem + problem = cp.Problem(cp.Minimize(objective), constraints) + + # Solve using an appropriate solver + # For nonlinear problems, we might need special solver options + problem.solve(solver=cp.SCS, verbose=True) + + # Extract solution + x_opt = x.value + u_opt = u.value + + return x_opt, u_opt + + +def plot_trajectory(x_opt, u_opt, L, h, title="Car Trajectory"): + """ + Plot the car trajectory with orientation indicators. + """ + fig, ax = plt.subplots(1, 1, figsize=(8, 8)) + + # Plot trajectory + ax.plot(x_opt[:, 0], x_opt[:, 1], 'b-', linewidth=2, label='Trajectory') + + # Plot car position and orientation at several time steps + car_length = L + car_width = L * 0.6 + + # Select time steps to show car outline (every 5th step) + steps_to_show = range(0, len(x_opt), 5) + + for k in steps_to_show: + p1, p2, theta = x_opt[k] + + # Car outline (simplified rectangle) + # Front of car + front_x = p1 + (car_length/2) * np.cos(theta) + front_y = p2 + (car_length/2) * np.sin(theta) + + # Rear of car + rear_x = p1 - (car_length/2) * np.cos(theta) + rear_y = p2 - (car_length/2) * np.sin(theta) + + # Draw car as a line with orientation + ax.plot([rear_x, front_x], [rear_y, front_y], 'k-', linewidth=3, alpha=0.5) + + # Draw steering angle indicator if not at final position + if k < len(u_opt): + phi = u_opt[k, 1] + # Steering direction from front of car + steer_x = front_x + (car_length/3) * np.cos(theta + phi) + steer_y = front_y + (car_length/3) * np.sin(theta + phi) + ax.plot([front_x, steer_x], [front_y, steer_y], 'r-', linewidth=2, alpha=0.7) + + # Mark start and end points + ax.plot(x_opt[0, 0], x_opt[0, 1], 'go', markersize=10, label='Start') + ax.plot(x_opt[-1, 0], x_opt[-1, 1], 'ro', markersize=10, label='Goal') + + ax.set_xlabel('p1') + ax.set_ylabel('p2') + ax.set_title(title) + ax.legend() + ax.grid(True, alpha=0.3) + ax.axis('equal') + + return fig, ax + + +# Example usage +if __name__ == "__main__": + # Test cases from the figure + test_cases = [ + ((0, 1, 0), "Move forward to (0, 1)"), + ((0, 1, np.pi/2), "Move to (0, 1) and turn 90°"), + ((0, 0.5, 0), "Move forward to (0, 0.5)"), + ((0.5, 0.5, -np.pi/2), "Move to (0.5, 0.5) and turn -90°") + ] + + # Solve for each test case + for x_final, description in test_cases: + print(f"\nSolving for: {description}") + print(f"Target state: p1={x_final[0]}, p2={x_final[1]}, theta={x_final[2]:.2f}") + + try: + x_opt, u_opt = solve_car_control(x_final) + + if x_opt is not None and u_opt is not None: + print("Optimization successful!") + print(f"Final position: p1={x_opt[-1, 0]:.3f}, p2={x_opt[-1, 1]:.3f}, theta={x_opt[-1, 2]:.3f}") + + # Plot the trajectory + fig, ax = plot_trajectory(x_opt, u_opt, L=0.1, h=0.1, title=description) + plt.show() + else: + print("Optimization failed!") + + except Exception as e: + print(f"Error: {e}") + + # Additional analysis: plot control inputs + if x_opt is not None and u_opt is not None: + fig, (ax1, ax2) = plt.subplots(2, 1, figsize=(10, 6)) + + time_steps = np.arange(len(u_opt)) * 0.1 # h = 0.1 + + ax1.plot(time_steps, u_opt[:, 0], 'b-', linewidth=2) + ax1.set_ylabel('Speed s(t)') + ax1.set_xlabel('Time') + ax1.grid(True, alpha=0.3) + + ax2.plot(time_steps, u_opt[:, 1], 'r-', linewidth=2) + ax2.set_ylabel('Steering angle φ(t)') + ax2.set_xlabel('Time') + ax2.grid(True, alpha=0.3) + + plt.tight_layout() + plt.show() diff --git a/cvxpy/sandbox/direct_ipopt_call.py b/cvxpy/sandbox/direct_ipopt_call.py index a07c9fa30d..7922a4d93d 100644 --- a/cvxpy/sandbox/direct_ipopt_call.py +++ b/cvxpy/sandbox/direct_ipopt_call.py @@ -10,8 +10,8 @@ problem = cp.Problem(objective, constraints) print(cp.installed_solvers()) -problem.solve(solver=cp.CLARABEL, verbose=True) -#problem.solve(solver=cp.IPOPT, nlp=True, verbose=True) +#problem.solve(solver=cp.CLARABEL, verbose=True) +problem.solve(solver=cp.IPOPT, nlp=True, verbose=True) print(x.value, y.value) print(problem.status) print(problem.value)