GeezerVison is showing github.com — update coeff handling to allow multi-variable basis · python-control/python-control@47262f5
File treeExpand file treeCollapse file tree Open diff view settings Expand file treeCollapse file tree Open diff view settings Collapse file Copy file name to clipboardExpand all lines: control/optimal.py +30-35Lines changed: 30 additions & 35 deletions | Original file line number | Diff line number | Diff line change |
|---|
@@ -268,17 +268,16 @@ def _cost_function(self, coeffs): | 268 | 268 | start_time = time.process_time() | 269 | 269 | logging.info("_cost_function called at: %g", start_time) | 270 | 270 |
| 271 | | - # Retrieve the initial state and reshape the input vector | | 271 | + # Retrieve the saved initial state | 272 | 272 | x = self.x | 273 | | - coeffs = coeffs.reshape((self.system.ninputs, -1)) | 274 | 273 |
| 275 | | - # Compute time points (if basis present) | | 274 | + # Compute inputs | 276 | 275 | if self.basis: | 277 | 276 | if self.log: | 278 | 277 | logging.debug("coefficients = " + str(coeffs)) | 279 | 278 | inputs = self._coeffs_to_inputs(coeffs) | 280 | 279 | else: | 281 | | - inputs = coeffs | | 280 | + inputs = coeffs.reshape((self.system.ninputs, -1)) | 282 | 281 |
| 283 | 282 | # See if we already have a simulation for this condition | 284 | 283 | if np.array_equal(coeffs, self.last_coeffs) and \ | @@ -391,15 +390,14 @@ def _constraint_function(self, coeffs): | 391 | 390 | start_time = time.process_time() | 392 | 391 | logging.info("_constraint_function called at: %g", start_time) | 393 | 392 |
| 394 | | - # Retrieve the initial state and reshape the input vector | | 393 | + # Retrieve the initial state | 395 | 394 | x = self.x | 396 | | - coeffs = coeffs.reshape((self.system.ninputs, -1)) | 397 | 395 |
| 398 | | - # Compute time points (if basis present) | | 396 | + # Compute input at time points | 399 | 397 | if self.basis: | 400 | 398 | inputs = self._coeffs_to_inputs(coeffs) | 401 | 399 | else: | 402 | | - inputs = coeffs | | 400 | + inputs = coeffs.reshape((self.system.ninputs, -1)) | 403 | 401 |
| 404 | 402 | # See if we already have a simulation for this condition | 405 | 403 | if np.array_equal(coeffs, self.last_coeffs) \ | @@ -473,15 +471,14 @@ def _eqconst_function(self, coeffs): | 473 | 471 | start_time = time.process_time() | 474 | 472 | logging.info("_eqconst_function called at: %g", start_time) | 475 | 473 |
| 476 | | - # Retrieve the initial state and reshape the input vector | | 474 | + # Retrieve the initial state | 477 | 475 | x = self.x | 478 | | - coeffs = coeffs.reshape((self.system.ninputs, -1)) | 479 | 476 |
| 480 | | - # Compute time points (if basis present) | | 477 | + # Compute input at time points | 481 | 478 | if self.basis: | 482 | 479 | inputs = self._coeffs_to_inputs(coeffs) | 483 | 480 | else: | 484 | | - inputs = coeffs | | 481 | + inputs = coeffs.reshape((self.system.ninputs, -1)) | 485 | 482 |
| 486 | 483 | # See if we already have a simulation for this condition | 487 | 484 | if np.array_equal(coeffs, self.last_coeffs) and \ | @@ -609,34 +606,36 @@ def _inputs_to_coeffs(self, inputs): | 609 | 606 | return inputs | 610 | 607 |
| 611 | 608 | # Solve least squares problems (M x = b) for coeffs on each input | 612 | | - coeffs = np.zeros((self.system.ninputs, self.basis.N)) | | 609 | + coeffs = [] | 613 | 610 | for i in range(self.system.ninputs): | 614 | 611 | # Set up the matrices to get inputs | 615 | | - M = np.zeros((self.timepts.size, self.basis.N)) | | 612 | + M = np.zeros((self.timepts.size, self.basis.var_ncoefs(i))) | 616 | 613 | b = np.zeros(self.timepts.size) | 617 | 614 |
| 618 | 615 | # Evaluate at each time point and for each basis function | 619 | 616 | # TODO: vectorize | 620 | 617 | for j, t in enumerate(self.timepts): | 621 | | - for k in range(self.basis.N): | | 618 | + for k in range(self.basis.var_ncoefs(i)): | 622 | 619 | M[j, k] = self.basis(k, t) | 623 | | - b[j] = inputs[i, j] | | 620 | + b[j] = inputs[i, j] | 624 | 621 |
| 625 | 622 | # Solve a least squares problem for the coefficients | 626 | 623 | alpha, residuals, rank, s = np.linalg.lstsq(M, b, rcond=None) | 627 | | - coeffs[i, :] = alpha | | 624 | + coeffs.append(alpha) | 628 | 625 |
| 629 | | - return coeffs | | 626 | + return np.hstack(coeffs) | 630 | 627 |
| 631 | 628 | # Utility function to convert coefficient vector to input vector | 632 | 629 | def _coeffs_to_inputs(self, coeffs): | 633 | 630 | # TODO: vectorize | 634 | 631 | inputs = np.zeros((self.system.ninputs, self.timepts.size)) | 635 | | - for i, t in enumerate(self.timepts): | 636 | | - for k in range(self.basis.N): | 637 | | - phi_k = self.basis(k, t) | 638 | | - for inp in range(self.system.ninputs): | 639 | | - inputs[inp, i] += coeffs[inp, k] * phi_k | | 632 | + offset = 0 | | 633 | + for i in range(self.system.ninputs): | | 634 | + length = self.basis.var_ncoefs(i) | | 635 | + for j, t in enumerate(self.timepts): | | 636 | + for k in range(length): | | 637 | + inputs[i, j] += coeffs[offset + k] * self.basis(k, t) | | 638 | + offset += length | 640 | 639 | return inputs | 641 | 640 |
| 642 | 641 | # | @@ -680,7 +679,7 @@ def _print_statistics(self, reset=True): | 680 | 679 |
| 681 | 680 | # Compute the optimal trajectory from the current state | 682 | 681 | def compute_trajectory( | 683 | | - self, x, squeeze=None, transpose=None, return_states=None, | | 682 | + self, x, squeeze=None, transpose=None, return_states=True, | 684 | 683 | initial_guess=None, print_summary=True, **kwargs): | 685 | 684 | """Compute the optimal input at state x | 686 | 685 |
| @@ -689,8 +688,7 @@ def compute_trajectory( | 689 | 688 | x : array-like or number, optional | 690 | 689 | Initial state for the system. | 691 | 690 | return_states : bool, optional | 692 | | - If True, return the values of the state at each time (default = | 693 | | - False). | | 691 | + If True (default), return the values of the state at each time. | 694 | 692 | squeeze : bool, optional | 695 | 693 | If True and if the system has a single output, return the system | 696 | 694 | output as a 1D array rather than a 2D array. If False, return the | @@ -837,7 +835,7 @@ class OptimalControlResult(sp.optimize.OptimizeResult): | 837 | 835 |
| 838 | 836 | """ | 839 | 837 | def __init__( | 840 | | - self, ocp, res, return_states=False, print_summary=False, | | 838 | + self, ocp, res, return_states=True, print_summary=False, | 841 | 839 | transpose=None, squeeze=None): | 842 | 840 | """Create a OptimalControlResult object""" | 843 | 841 |
| @@ -848,14 +846,11 @@ def __init__( | 848 | 846 | # Remember the optimal control problem that we solved | 849 | 847 | self.problem = ocp | 850 | 848 |
| 851 | | - # Reshape and process the input vector | 852 | | - coeffs = res.x.reshape((ocp.system.ninputs, -1)) | 853 | | - | 854 | | - # Compute time points (if basis present) | | 849 | + # Compute input at time points | 855 | 850 | if ocp.basis: | 856 | | - inputs = ocp._coeffs_to_inputs(coeffs) | | 851 | + inputs = ocp._coeffs_to_inputs(res.x) | 857 | 852 | else: | 858 | | - inputs = coeffs | | 853 | + inputs = res.x.reshape((ocp.system.ninputs, -1)) | 859 | 854 |
| 860 | 855 | # See if we got an answer | 861 | 856 | if not res.success: | @@ -894,7 +889,7 @@ def __init__( | 894 | 889 | def solve_ocp( | 895 | 890 | sys, horizon, X0, cost, trajectory_constraints=None, terminal_cost=None, | 896 | 891 | terminal_constraints=[], initial_guess=None, basis=None, squeeze=None, | 897 | | - transpose=None, return_states=False, log=False, **kwargs): | | 892 | + transpose=None, return_states=True, log=False, **kwargs): | 898 | 893 |
| 899 | 894 | """Compute the solution to an optimal control problem | 900 | 895 |
| @@ -949,7 +944,7 @@ def solve_ocp( | 949 | 944 | If `True`, turn on logging messages (using Python logging module). | 950 | 945 |
| 951 | 946 | return_states : bool, optional | 952 | | - If True, return the values of the state at each time (default = False). | | 947 | + If True, return the values of the state at each time (default = True). | 953 | 948 |
| 954 | 949 | squeeze : bool, optional | 955 | 950 | If True and if the system has a single output, return the system | |
Collapse file Copy file name to clipboardExpand all lines: control/tests/optimal_test.py +13-8Lines changed: 13 additions & 8 deletions | Original file line number | Diff line number | Diff line change |
|---|
@@ -300,8 +300,9 @@ def test_terminal_constraints(sys_args): | 300 | 300 | np.testing.assert_almost_equal(res.inputs, u1) | 301 | 301 |
| 302 | 302 | # Re-run using a basis function and see if we get the same answer | 303 | | - res = opt.solve_ocp(sys, time, x0, cost, terminal_constraints=final_point, | 304 | | - basis=flat.BezierFamily(8, Tf)) | | 303 | + res = opt.solve_ocp( | | 304 | + sys, time, x0, cost, terminal_constraints=final_point, | | 305 | + basis=flat.BezierFamily(8, Tf)) | 305 | 306 |
| 306 | 307 | # Final point doesn't affect cost => don't need to test | 307 | 308 | np.testing.assert_almost_equal( | @@ -471,8 +472,12 @@ def test_ocp_argument_errors(): | 471 | 472 | sys, time, x0, cost, terminal_constraints=constraints) | 472 | 473 |
| 473 | 474 |
| 474 | | -def test_optimal_basis_simple(): | 475 | | - sys = ct.ss2io(ct.ss([[1, 1], [0, 1]], [[1], [0.5]], np.eye(2), 0, 1)) | | 475 | +@pytest.mark.parametrize("basis", [ | | 476 | + flat.PolyFamily(4), flat.PolyFamily(6), | | 477 | + flat.BezierFamily(4), flat.BSplineFamily([0, 4, 8], 6) | | 478 | + ]) | | 479 | +def test_optimal_basis_simple(basis): | | 480 | + sys = ct.ss([[1, 1], [0, 1]], [[1], [0.5]], np.eye(2), 0, 1) | 476 | 481 |
| 477 | 482 | # State and input constraints | 478 | 483 | constraints = [ | @@ -492,7 +497,7 @@ def test_optimal_basis_simple(): | 492 | 497 | # Basic optimal control problem | 493 | 498 | res1 = opt.solve_ocp( | 494 | 499 | sys, time, x0, cost, constraints, | 495 | | - basis=flat.BezierFamily(4, Tf), return_x=True) | | 500 | + terminal_cost=cost, basis=basis, return_x=True) | 496 | 501 | assert res1.success | 497 | 502 |
| 498 | 503 | # Make sure the constraints were satisfied | @@ -503,14 +508,14 @@ def test_optimal_basis_simple(): | 503 | 508 | # Pass an initial guess and rerun | 504 | 509 | res2 = opt.solve_ocp( | 505 | 510 | sys, time, x0, cost, constraints, initial_guess=0.99*res1.inputs, | 506 | | - basis=flat.BezierFamily(4, Tf), return_x=True) | | 511 | + terminal_cost=cost, basis=basis, return_x=True) | 507 | 512 | assert res2.success | 508 | 513 | np.testing.assert_allclose(res2.inputs, res1.inputs, atol=0.01, rtol=0.01) | 509 | 514 |
| 510 | 515 | # Run with logging turned on for code coverage | 511 | 516 | res3 = opt.solve_ocp( | 512 | | - sys, time, x0, cost, constraints, | 513 | | - basis=flat.BezierFamily(4, Tf), return_x=True, log=True) | | 517 | + sys, time, x0, cost, constraints, terminal_cost=cost, | | 518 | + basis=basis, return_x=True, log=True) | 514 | 519 | assert res3.success | 515 | 520 | np.testing.assert_almost_equal(res3.inputs, res1.inputs, decimal=3) | 516 | 521 |
| |
|