From f17328bcdc0e356af23e1cb6cdf4e9e467bb9a03 Mon Sep 17 00:00:00 2001 From: Dario Izzo Date: Wed, 12 Jun 2024 13:45:34 +0200 Subject: [PATCH] Bugfix in the mismatch constraint gradient --- doc/notebooks/udp_point2point.ipynb | 586 +++++++++++++++------------- src/leg/sims_flanagan.cpp | 4 +- test/leg_sims_flanagan_udp.hpp | 6 +- 3 files changed, 316 insertions(+), 280 deletions(-) diff --git a/doc/notebooks/udp_point2point.ipynb b/doc/notebooks/udp_point2point.ipynb index 43ec548b..16694563 100644 --- a/doc/notebooks/udp_point2point.ipynb +++ b/doc/notebooks/udp_point2point.ipynb @@ -9,7 +9,7 @@ }, { "cell_type": "code", - "execution_count": 248, + "execution_count": 1, "metadata": {}, "outputs": [], "source": [ @@ -24,7 +24,7 @@ }, { "cell_type": "code", - "execution_count": 249, + "execution_count": 225, "metadata": {}, "outputs": [], "source": [ @@ -35,7 +35,7 @@ "\n", " The decision vector is::\n", "\n", - " z = [tof, mf, throttles]\n", + " z = [mf, throttles, tof]\n", "\n", " where throttles is a vector [u0x, u0y,u0z, ...]\n", "\n", @@ -56,9 +56,10 @@ " max_thrust=0.12,\n", " isp=3000,\n", " tof_bounds=[80, 400],\n", - " mf_bounds=[1000.0, 200.0],\n", + " mf_bounds=[200.0, 1000.0],\n", " nseg=10,\n", " cut=0.6,\n", + " mass_scaling = 1000\n", " ):\n", " # We add as data member one single Sims-Flanagan leg using the problem data\n", " # and some temporary (and unused, thus irrelelvant) values for the to-be-optimzed parameters throttles, tof and mf.\n", @@ -74,21 +75,26 @@ " isp=isp,\n", " mu=mu,\n", " cut=cut,\n", - " )\n", + " )\n", " self.tof_bounds = tof_bounds\n", " self.mf_bounds = mf_bounds\n", + " self.mass_scaling = mass_scaling\n", "\n", " def get_bounds(self):\n", - " lb = [self.tof_bounds[0], self.mf_bounds[0]] + [-1, -1, -1] * self.leg.nseg\n", - " ub = [self.tof_bounds[1], self.mf_bounds[1]] + [1, 1, 1] * self.leg.nseg\n", + " lb = [self.mf_bounds[0]] + [-1, -1, -1] * self.leg.nseg + [self.tof_bounds[0]]\n", + " ub = [self.mf_bounds[1]] + [1, 1, 1] * self.leg.nseg + [self.tof_bounds[1]]\n", " return (lb, ub)\n", "\n", + " def _set_leg_from_x(self, x):\n", + " # We set the leg using data in the decision vector\n", + " self.leg.tof = x[-1] * pk.DAY2SEC\n", + " self.leg.mf = x[0]\n", + " self.leg.throttles = x[1:-1]\n", + "\n", " def fitness(self, x):\n", " # 1 - We set the leg using data in the decision vector\n", - " self.leg.tof = x[0] * pk.DAY2SEC\n", - " self.leg.mf = x[1]\n", - " self.leg.throttles = x[2:]\n", - " obj = -x[1]\n", + " self._set_leg_from_x(x)\n", + " obj = -x[0] / self.mass_scaling\n", "\n", " # 2 - We compute the constraints violations (mismatch+throttle)\n", " ceq = self.leg.compute_mismatch_constraints()\n", @@ -98,20 +104,174 @@ " # 3 - We scale the values in nd units (numerical solvers are sensitive to well-scaled values)\n", " retval[1:4] /= pk.AU\n", " retval[4:7] /= pk.EARTH_VELOCITY\n", - " retval[7] /= 1000\n", - " \n", + " retval[7] /= self.mass_scaling\n", + "\n", " return retval\n", "\n", + " def gradientTMP(self, x):\n", + " self._set_leg_from_x(x)\n", + " mcg_xs, mcg_xf, mcg_th_tof = self.leg.compute_mc_grad()\n", + " tcg_th = self.leg.compute_tc_grad()\n", + " \n", + " # The gradient of the objective function (obj = -mf)\n", + " retval = [-1. / self.mass_scaling]\n", + " # The gradient of the mismatch contraints (mcg). We divide them in pos, vel mass as they have different scaling units\n", + " # pos\n", + " for i in range(3):\n", + " # First w.r.t. mf\n", + " retval.append(mcg_xf[i,-1] / pk.AU)\n", + " # Then the [throttles, tof]\n", + " retval.extend(mcg_th_tof[i,:] / pk.AU)\n", + " retval[-1] *= pk.DAY2SEC\n", + " # vel\n", + " for i in range(3,6):\n", + " # First w.r.t. mf\n", + " retval.append(mcg_xf[i,-1]/ pk.EARTH_VELOCITY)\n", + " # Then the [throttles, tof]\n", + " retval.extend(mcg_th_tof[i,:]/ pk.EARTH_VELOCITY)\n", + " retval[-1] *= pk.DAY2SEC\n", + " # mass\n", + " for i in range(6,7):\n", + " # First w.r.t. mf\n", + " retval.append(mcg_xf[i,-1] / self.mass_scaling)\n", + " # Then the [throttles, tof]\n", + " retval.extend(mcg_th_tof[i,:]/self.mass_scaling)\n", + " retval[-1] *= pk.DAY2SEC\n", + " \n", + " return retval\n", + " \n", + " def sparsity_pattern(self):\n", + " dim = 2 + 3 * self.leg.nseg\n", + " # The objective function only depends on the final mass, which is in the chromosome.\n", + " retval = [[0,0]]\n", + " # The mismatch constraints depend on all variables.\n", + " for i in range(1,8):\n", + " for j in range(dim):\n", + " retval.append([i, j])\n", + " # The throttle constraints only depend on the specific throttles (3).\n", + " for i in range(self.leg.nseg):\n", + " retval.append([8+i, 3*i+1])\n", + " retval.append([8+i, 3*i+2])\n", + " retval.append([8+i, 3*i+3])\n", + " # We return the sparsity pattern\n", + " return retval\n", + " \n", " def get_nec(self):\n", " return 7\n", "\n", " def get_nic(self):\n", - " return self.leg.nseg" + " return self.leg.nseg\n", + " \n", + "udp = direct_point2point(\n", + " rvs=[rs, vs],\n", + " rvf=[rf, vf],\n", + " mu=pk.MU_SUN,\n", + " max_thrust=0.12,\n", + " isp=3000,\n", + " tof_bounds=[80, 200],\n", + " mf_bounds=[200.0, 1000.0],\n", + " nseg=nseg,\n", + " cut=0.6,\n", + ")\n", + "x = pg.population(prob, 1).get_x()[0]\n", + "\n" + ] + }, + { + "cell_type": "code", + "execution_count": 226, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "[-1.4694721245265518e-05,\n", + " 0.006813221805208961,\n", + " 0.00024430842703371714,\n", + " 4.925681352913288e-07,\n", + " -0.01311342691525777,\n", + " -0.0005014600058762055,\n", + " -7.176711271919772e-07,\n", + " -0.012056461006290039,\n", + " 1.0223476528838442e-05,\n", + " 0.0002459535545847036,\n", + " 0.0066233232757768315,\n", + " 3.6692385943127215e-07,\n", + " -0.0004978984473423457,\n", + " -0.013475900194643495]" + ] + }, + "execution_count": 226, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "udp.gradientTMP(x)[1:15]" + ] + }, + { + "cell_type": "code", + "execution_count": 227, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "array([ 1.46947212e-05, 6.81322181e-03, 2.44308427e-04, 4.92568139e-07,\n", + " -1.31134269e-02, -5.01460006e-04, -7.17671120e-07, -1.20564610e-02,\n", + " -1.02234765e-05, 2.45953555e-04, 6.62332328e-03, 3.66923860e-07,\n", + " -4.97898447e-04, -1.34759002e-02, -9.38387522e-07, 1.22893180e-02,\n", + " 4.30503734e-06, 5.01568579e-07, 3.70761390e-07, 6.45138920e-03,\n", + " -7.25251065e-07, -9.56086319e-07, -1.27542482e-02, 2.88265433e-06,\n", + " -3.48545582e-05, 1.75302490e-02, 1.88221314e-03, 4.60277248e-06,\n", + " 3.29808297e-02, 3.88879645e-03, 7.60535628e-06, -1.17575587e-02,\n", + " 2.33262850e-05, 1.90636474e-03, 1.66101347e-02, 3.87551494e-06,\n", + " 3.83641005e-03, 3.46299510e-02, 8.75315844e-06, -1.20734686e-02,\n", + " -1.00550366e-05, 4.73490516e-06, 3.93185252e-06, 1.50889482e-02,\n", + " 7.71684839e-06, 9.01348745e-06, 2.97849419e-02, 9.27052114e-05,\n", + " -9.99709368e-04, -1.50740597e-02, 2.55059386e-03, -5.64533104e-03,\n", + " -1.35506791e-02, 9.43088628e-03, -3.97304270e-03, -3.10933867e-04,\n", + " 0.00000000e+00, 1.91521373e+00, -3.24062161e-01, 7.17259696e-01,\n", + " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", + " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", + " 1.16387585e+00, -8.10024404e-01, 3.41246989e-01, 0.00000000e+00])" + ] + }, + "execution_count": 227, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "pg.estimate_gradient_h(udp.fitness, x)[8:]" + ] + }, + { + "cell_type": "code", + "execution_count": 203, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "0.00013955916376220278" + ] + }, + "execution_count": 203, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "udp._set_leg_from_x(x)\n", + "mcg_xs, mcg_xf, mcg_th_tof = udp.leg.compute_mc_grad()\n", + "mcg_xf[1,-1]/pk.AU\n" ] }, { "cell_type": "code", - "execution_count": 250, + "execution_count": 207, "metadata": {}, "outputs": [], "source": [ @@ -122,25 +282,47 @@ "\n", "# Initial state\n", "ms = 1500.0\n", - "rs = np.array([1, 0., -0.]) * pk.AU\n", - "vs = np.array([0.0, 1, -0.0]) * pk.EARTH_VELOCITY\n", + "rs = np.array([1, 0., -0.0]) * pk.AU\n", + "vs = np.array([0.01, 1, -0.0]) * pk.EARTH_VELOCITY\n", "\n", "# Final state\n", "mf = 1300.0\n", - "rf = np.array([-0.0, 1.0, 0.]) * pk.AU\n", + "rf = np.array([-0.0, 1.0, 0.0]) * pk.AU\n", "vf = np.array([-1, -0, 0]) * pk.EARTH_VELOCITY\n", "\n", "# Throttles and tof\n", - "nseg = 20\n", + "nseg = 2\n", "throttles = np.random.uniform(-1, 1, size=(nseg * 3))\n", - "tof = 90.0 * pk.DAY2SEC\n", + "tof = 2 * np.pi * np.sqrt(pk.AU**3 / pk.MU_SUN) / 4\n", "\n", - "sf = pk.leg.sims_flanagan(rvs = [rs, vs], ms = ms, throttles = throttles, rvf = [rf, vf], mf = mf, tof = tof, max_thrust = max_thrust, isp = isp, mu=mu, cut = 0.6)" + "sf = pk.leg.sims_flanagan(\n", + " rvs=[rs, vs],\n", + " ms=ms,\n", + " throttles=throttles,\n", + " rvf=[rf, vf],\n", + " mf=mf,\n", + " tof=tof,\n", + " max_thrust=max_thrust,\n", + " isp=isp,\n", + " mu=mu,\n", + " cut=0.6,\n", + ")\n", + "udp = direct_point2point(\n", + " rvs=[rs, vs],\n", + " rvf=[rf, vf],\n", + " mu=pk.MU_SUN,\n", + " max_thrust=0.12,\n", + " isp=3000,\n", + " tof_bounds=[80, 200],\n", + " mf_bounds=[200.0, 1000.0],\n", + " nseg=nseg,\n", + " cut=0.6,\n", + ")" ] }, { "cell_type": "code", - "execution_count": 252, + "execution_count": 209, "metadata": {}, "outputs": [], "source": [ @@ -149,13 +331,13 @@ "uda.set_integer_option(\"Major iterations limit\", 700)\n", "uda.set_integer_option(\"Iterations limit\", 20000)\n", "uda.set_numeric_option(\"Major optimality tolerance\", 1e-3)\n", - "uda.set_numeric_option(\"Major feasibility tolerance\", 1e-11)\n", + "uda.set_numeric_option(\"Major feasibility tolerance\", 1e-9)\n", "algo = pg.algorithm(uda)" ] }, { "cell_type": "code", - "execution_count": 253, + "execution_count": 211, "metadata": {}, "outputs": [ { @@ -172,276 +354,79 @@ " SNMEMA INFO 104 -- memory requirements estimated\n", " \n", "\n", - " Nonlinear constraints 27 Linear constraints 0\n", - " Nonlinear variables 62 Linear variables 0\n", - " Jacobian variables 62 Objective variables 62\n", - " Total constraints 27 Total variables 62\n", + " Nonlinear constraints 9 Linear constraints 0\n", + " Nonlinear variables 8 Linear variables 0\n", + " Jacobian variables 8 Objective variables 8\n", + " Total constraints 9 Total variables 8\n", " \n", "\n", " \n", - " The user has defined 0 out of 1736 first derivatives\n", + " The user has defined 0 out of 80 first derivatives\n", " \n", "\n", - " Minor QPmult nInf SumInf rgNorm Elastic QPobj\n", - " 100 6.9E+01 5 6.2E+00 7.5281412E+04\n", - "\n", - " Major Minors Step nCon Feasible Optimal MeritFunction nS Penalty\n", - " 0 153 1 1.4E-02 6.0E-01 9.3947490E+04 2 r i\n", - " 1 52 3.0E-02 2 1.4E-02 5.8E-01 9.2988976E+04 30 n rli\n", - " 2 14 3.1E-01 4 1.4E-02 4.0E-01 8.5914224E+04 29 n rli\n", - " 3 34 1.0E+00 6 1.7E-02 1.7E-02 6.9496045E+04 11 sM i\n", - " 4 71 1.0E+00 8 1.9E-02 2.5E-02 6.6454546E+04 8 m i\n", - " 5 84 1.0E+00 10 1.8E-02 1.2E+00 6.0824289E+04 4 m i\n", - "\n", - " Minor QPmult nInf SumInf rgNorm Elastic QPobj nS\n", - " 100 2 1.4E+00 5.3E+00 1.5814029E+04 4\n", - "\n", - " Major Minors Step nCon Feasible Optimal MeritFunction nS Penalty\n", - " 6 107 1.0E+00 12 2.4E-02 5.4E-02 -2.4388341E+03 8 m i\n", - " 7 37 4.2E-03 13 2.4E-02 6.0E-02 1.6979128E+04 6 5.0E+03 i\n", - " 8 20 2.5E-03 14 2.4E-02 2.9E-02 1.7818301E+04 4 5.2E+03 i\n", - " 9 21 8.6E-03 15 2.4E-02 2.7E-02 1.7498856E+04 8 5.2E+03 i\n", - "\n", - " Major Minors Step nCon Feasible Optimal MeritFunction nS Penalty\n", - " 10 9 1.5E-02 17 2.3E-02 2.8E-02 1.7073102E+04 6 5.2E+03 M i\n", - " 11 21 2.2E-02 18 2.3E-02 3.5E-02 1.6937181E+04 10 5.3E+03 i\n", - " 12 23 1.3E-02 19 2.3E-02 3.5E-02 1.7096090E+04 6 5.4E+03 i\n", - " 13 1 1.0E-02 21 2.2E-02 3.5E-02 1.6922867E+04 6 5.5E+03 n i\n", - " 14 11 1.1E-02 23 2.2E-02 3.7E-02 1.6910177E+04 6 5.5E+03 M i\n", - " 15 1 1.2E-02 25 2.2E-02 3.7E-02 1.6867281E+04 6 5.6E+03 M i\n", - " 16 5 1.3E-02 27 2.2E-02 3.6E-02 1.6840245E+04 6 5.7E+03 M i\n", - " 17 3 1.3E-02 29 2.1E-02 3.6E-02 1.6788327E+04 7 5.8E+03 M i\n", - " 18 4 1.4E-02 31 2.1E-02 3.7E-02 1.6745913E+04 6 5.9E+03 M i\n", - " 19 2 1.5E-02 33 2.1E-02 3.7E-02 1.6644812E+04 7 5.9E+03 M i\n", - "\n", - " Major Minors Step nCon Feasible Optimal MeritFunction nS Penalty\n", - " 20 1 1.6E-02 35 2.0E-02 3.7E-02 1.6560280E+04 7 6.0E+03 M i\n", - " 21 1 1.6E-02 37 2.0E-02 3.7E-02 1.6480573E+04 7 6.0E+03 M i\n", - " 22 1 1.7E-02 39 2.0E-02 3.7E-02 1.6372793E+04 7 6.1E+03 M i\n", - " 23 4 1.6E-02 41 1.9E-02 3.4E-02 1.6264017E+04 6 6.1E+03 M i\n", - " 24 3 1.7E-02 43 1.9E-02 3.1E-02 1.6165832E+04 8 6.1E+03 M i\n", - " 25 1 1.9E-02 45 1.9E-02 3.2E-02 1.6045813E+04 8 6.2E+03 M i\n", - " 26 3 1.3E-02 47 1.8E-02 2.8E-02 1.5679697E+04 8 5.7E+03 M i\n", - " 27 2 1.2E-02 49 1.8E-02 2.7E-02 1.5653818E+04 7 5.7E+03 M i\n", - " 28 3 1.4E-02 51 1.8E-02 2.7E-02 1.5592060E+04 5 5.7E+03 M i\n", - " 29 7 1.3E-02 53 1.8E-02 2.8E-02 1.5538466E+04 9 5.7E+03 M i\n", - "\n", - " Major Minors Step nCon Feasible Optimal MeritFunction nS Penalty\n", - " 30 7 1.4E-02 55 1.7E-02 2.7E-02 1.5489350E+04 5 5.7E+03 M i\n", - " 31 7 1.2E-02 57 1.7E-02 2.7E-02 1.5483408E+04 5 5.8E+03 M i\n", - " 32 2 1.4E-02 59 1.7E-02 2.7E-02 1.5438496E+04 6 5.8E+03 M i\n", - " 33 6 1.3E-02 61 1.7E-02 2.7E-02 1.5342217E+04 7 5.6E+03 M i\n", - " 34 4 1.5E-02 63 1.6E-02 2.7E-02 1.5303465E+04 8 5.6E+03 M i\n", - " 35 2 1.5E-02 65 1.6E-02 2.7E-02 1.5265060E+04 9 5.6E+03 M i\n", - " 36 1 1.8E-02 67 1.6E-02 2.7E-02 1.5222727E+04 9 5.6E+03 M i\n", - " 37 2 2.1E-02 69 1.6E-02 2.7E-02 1.5174721E+04 10 5.6E+03 M i\n", - " 38 6 2.5E-02 71 1.5E-02 2.6E-02 1.5111849E+04 9 5.6E+03 M i\n", - " 39 3 3.4E-02 73 1.5E-02 2.6E-02 1.5050025E+04 7 5.6E+03 M i\n", - "\n", " Major Minors Step nCon Feasible Optimal MeritFunction nS Penalty\n", - " 40 1 3.3E-02 75 1.4E-02 2.6E-02 1.4992422E+04 7 5.6E+03 M i\n", - " 41 5 3.3E-02 77 1.4E-02 2.6E-02 1.4936353E+04 9 5.6E+03 M i\n", - " 42 3 3.5E-02 79 1.3E-02 2.4E-02 1.4879287E+04 11 5.6E+03 M i\n", - " 43 8 4.4E-02 81 1.3E-02 2.7E-02 1.4809074E+04 8 5.6E+03 m i\n", - " 44 12 4.2E-02 83 1.3E-02 2.2E-02 1.4742912E+04 7 5.6E+03 M i\n", - " 45 15 5.7E-02 85 1.3E-02 2.8E-02 1.4655122E+04 10 5.6E+03 M i\n", - " 46 14 5.4E-02 87 1.3E-02 4.5E-02 1.4588969E+04 7 5.6E+03 M i\n", - " 47 32 6.7E-02 89 1.3E-02 1.2E-01 1.4536261E+04 11 5.6E+03 m i\n", - " 48 21 5.8E-02 91 1.3E-02 8.4E-02 1.4457906E+04 9 5.6E+03 M i\n", - " 49 17 8.3E-02 93 1.3E-02 5.0E-02 1.4316532E+04 9 5.6E+03 M i\n", + " 0 10 1 3.0E-03 1.9E+00 4.4281307E+04 r i\n", + " 1 12 7.3E-03 2 3.0E-03 6.1E-01 4.3958919E+04 2 n rli\n", + " 2 1 3.4E-01 4 2.9E-03 4.0E-01 4.0784665E+04 2 n rli\n", + " 3 4 1.0E+00 6 3.2E-03 3.8E-02 3.3622881E+04 3 n r i\n", + " 4 14 1.0E+00 8 2.1E-03 1.3E+00 2.7226988E+04 2 sm i\n", + " 5 20 1.0E+00 10 8.9E-04 2.8E-02 2.4698458E+03 2 m i\n", + " 6 2 1.0E+00 12 8.7E-04 1.3E-02 2.4900716E+03 1 3.5E+04 m i\n", + " 7 7 1.0E+00 14 8.4E-04 3.2E-02 2.4509449E+03 1 7.5E+06 m i\n", + " 8 7 1.0E+00 16 5.5E-04 1.0E-01 1.2812529E+03 1 3.6E+03 m i\n", + " 9 4 1.0E+00 18 1.7E-03 2.1E-02 8.8880465E+01 1 1.2E+02 m i\n", "\n", " Major Minors Step nCon Feasible Optimal MeritFunction nS Penalty\n", - " 50 36 9.8E-02 95 1.3E-02 8.8E-02 1.4178504E+04 12 5.6E+03 M i\n", - " 51 31 1.1E-01 97 1.3E-02 7.6E-02 1.4036619E+04 12 5.6E+03 M i\n", - " 52 23 8.9E-02 99 1.3E-02 9.6E-02 1.3900353E+04 10 5.6E+03 M i\n", - " 53 14 7.9E-02 101 1.3E-02 1.0E-01 1.3798335E+04 16 5.6E+03 M i\n", - " 54 21 1.3E-01 103 1.3E-02 8.6E-02 1.3654861E+04 15 5.6E+03 M i\n", - " 55 15 8.2E-02 104 1.3E-02 2.3E-02 1.3557859E+04 13 5.6E+03 i\n", - " 56 11 7.3E-02 105 1.3E-02 2.5E-02 1.3290754E+04 12 4.7E+03 i\n", - " 57 18 6.4E-02 106 1.2E-02 2.3E-02 1.3221189E+04 13 4.7E+03 i\n", - " 58 5 5.7E-02 107 1.2E-02 2.2E-02 1.3163758E+04 15 4.7E+03 i\n", - " 59 19 6.0E-02 108 1.2E-02 1.9E-02 1.3116751E+04 16 4.7E+03 i\n", + " 10 3 1.0E+00 19 4.2E-04 9.9E-03 7.5243681E+01 1.5E+02\n", + " 11 0 1.0E+00 20 7.3E-05 2.8E-02 -6.7639996E-01 3.3E+01\n", + " 12 1 1.0E+00 21 5.6E-07 1.4E-02 -9.7255476E-01 1 3.3E+01\n", + " 13 1 1.0E+00 22 (4.2E-10) 1.6E-02 -9.7275294E-01 1 3.3E+01\n", + " 14 1 6.7E-02 23 1.3E-07 6.6E-03 -9.7410775E-01 1 3.3E+01\n", + " 15 1 7.2E-02 24 2.6E-07 5.6E-03 -9.7532076E-01 1 3.3E+01\n", + " 16 1 8.2E-02 25 3.7E-07 4.7E-03 -9.7639079E-01 1 3.3E+01\n", + " 17 1 9.1E-02 26 4.6E-07 4.0E-03 -9.7732654E-01 1 3.3E+01\n", + " 18 1 1.0E-01 27 5.3E-07 3.7E-03 -9.7813750E-01 1 3.3E+01\n", + " 19 1 1.1E-01 28 5.8E-07 3.6E-03 -9.7883376E-01 1 3.3E+01\n", "\n", " Major Minors Step nCon Feasible Optimal MeritFunction nS Penalty\n", - " 60 12 5.1E-02 109 1.2E-02 2.2E-02 1.3073487E+04 15 4.7E+03 i\n", - " 61 7 6.1E-02 110 1.2E-02 1.7E-02 1.3026580E+04 13 4.7E+03 i\n", - " 62 13 4.8E-02 111 1.2E-02 1.5E-02 1.2994773E+04 16 4.7E+03 i\n", - " 63 6 4.9E-02 112 1.2E-02 1.8E-02 1.2966213E+04 17 4.7E+03 i\n", - " 64 11 5.7E-02 113 1.2E-02 1.9E-02 1.2933547E+04 17 4.7E+03 i\n", - " 65 6 5.6E-02 114 1.2E-02 1.7E-02 1.2906860E+04 15 4.7E+03 i\n", - " 66 9 4.3E-02 115 1.2E-02 1.4E-02 1.2887087E+04 15 4.7E+03 i\n", - " 67 12 3.9E-02 116 1.2E-02 2.0E-02 1.2871017E+04 19 4.7E+03 i\n", - " 68 9 4.1E-02 117 1.2E-02 1.1E-02 1.2854987E+04 17 4.7E+03 i\n", - " 69 8 4.1E-02 118 1.2E-02 1.1E-02 1.2840099E+04 21 4.7E+03 i\n", + " 20 1 1.2E-01 29 6.2E-07 3.4E-03 -9.7942573E-01 1 3.3E+01\n", + " 21 1 1.3E-01 30 6.3E-07 3.1E-03 -9.7992370E-01 1 3.3E+01\n", + " 22 1 1.4E-01 31 6.2E-07 2.8E-03 -9.8033741E-01 1 3.3E+01\n", + " 23 1 1.5E-01 32 6.0E-07 2.4E-03 -9.8067570E-01 1 3.3E+01\n", + " 24 1 1.7E-01 33 5.7E-07 1.9E-03 -9.8094624E-01 1 3.3E+01\n", + " 25 1 2.0E-01 34 5.2E-07 1.5E-03 -9.8115537E-01 1 3.3E+01\n", + " 26 1 1.3E-01 35 4.7E-07 1.1E-03 -9.8124091E-01 1 3.3E+01\n", + " 27 1 1.7E-01 36 4.1E-07 (9.4E-04)-9.8131835E-01 1 3.3E+01\n", + " 28 1 1.4E-01 37 3.6E-07 (8.0E-04)-9.8136073E-01 1 3.3E+01\n", + " 29 1 2.0E-01 38 3.0E-07 (6.4E-04)-9.8140057E-01 1 3.3E+01\n", "\n", " Major Minors Step nCon Feasible Optimal MeritFunction nS Penalty\n", - " 70 6 3.5E-02 119 1.2E-02 1.0E-02 1.2828678E+04 20 4.7E+03 i\n", - " 71 9 3.3E-02 120 1.2E-02 1.5E-02 1.2818136E+04 20 4.7E+03 i\n", - " 72 4 4.0E-02 121 1.2E-02 1.1E-02 1.2805815E+04 23 4.7E+03 i\n", - " 73 2 4.8E-02 122 1.2E-02 1.6E-02 1.2792913E+04 22 4.7E+03 i\n", - " 74 4 4.7E-02 123 1.2E-02 1.9E-02 1.2781603E+04 21 4.7E+03 i\n", - " 75 4 3.5E-02 124 1.2E-02 1.1E-02 1.2771646E+04 24 4.7E+03 i\n", - " 76 7 2.8E-02 125 1.2E-02 1.6E-02 1.2765019E+04 23 4.7E+03 i\n", - " 77 4 2.5E-02 126 1.2E-02 1.4E-02 1.2759180E+04 24 4.7E+03 i\n", - " 78 3 2.5E-02 127 1.2E-02 1.1E-02 1.2753341E+04 24 4.7E+03 i\n", - " 79 3 2.7E-02 128 1.2E-02 1.5E-02 1.2747298E+04 24 4.7E+03 i\n", - "\n", - " Major Minors Step nCon Feasible Optimal MeritFunction nS Penalty\n", - " 80 3 2.8E-02 129 1.2E-02 8.9E-03 1.2740944E+04 24 4.7E+03 i\n", - " 81 4 2.7E-02 130 1.2E-02 1.4E-02 1.2735123E+04 23 4.7E+03 i\n", - " 82 5 3.0E-02 131 1.2E-02 9.8E-03 1.2729713E+04 25 4.7E+03 i\n", - " 83 8 4.1E-02 132 1.2E-02 1.1E-02 1.2721578E+04 24 4.7E+03 i\n", - " 84 8 2.9E-02 133 1.2E-02 1.2E-02 1.2718013E+04 25 4.8E+03 i\n", - " 85 6 9.9E-02 134 1.2E-02 1.7E-02 1.2702381E+04 26 4.8E+03 i\n", - " 86 7 9.7E-02 135 1.2E-02 2.1E-02 1.2700706E+04 28 5.1E+03 i\n", - " 87 22 2.3E-01 136 1.2E-02 2.2E-02 1.2688092E+04 29 5.4E+03 i\n", - " 88 17 4.4E-02 137 1.2E-02 2.0E-02 1.2689001E+04 27 5.8E+03 i\n", - " 89 6 5.9E-02 138 1.2E-02 1.5E-02 1.2684507E+04 30 5.8E+03 i\n", - "\n", - " Major Minors Step nCon Feasible Optimal MeritFunction nS Penalty\n", - " 90 2 1.1E-01 139 1.2E-02 1.2E-02 1.2677848E+04 29 5.8E+03 i\n", - " 91 10 1.4E-01 140 1.2E-02 1.2E-02 1.2668996E+04 30 5.8E+03 i\n", - " 92 8 2.0E-01 141 1.2E-02 7.8E-03 1.2660215E+04 27 5.9E+03 i\n", - " 93 4 3.8E-01 142 1.2E-02 1.1E-02 1.2646203E+04 28 6.0E+03 i\n", - " 94 4 1.5E-01 143 1.2E-02 7.1E-03 1.2644581E+04 27 6.4E+03 i\n", - "\n", - " Minor QPmult nInf SumInf rgNorm Elastic QPobj nS\n", - " 100 3 1.1E+00 3.1E-12 1.2642078E+04 22\n", - "\n", - " Major Minors Step nCon Feasible Optimal MeritFunction nS Penalty\n", - " 95 106 1.5E-01 144 1.2E-02 1.1E-02 1.2640781E+04 28 6.4E+03 i\n", - " 96 5 1.3E-01 145 1.2E-02 8.3E-03 1.2639088E+04 30 6.5E+03 i\n", - " 97 1 1.1E-01 146 1.2E-02 6.9E-03 1.2638598E+04 30 6.8E+03 i\n", - " 98 1 1.5E-01 147 1.2E-02 8.0E-03 1.2636758E+04 30 6.8E+03 i\n", - " 99 6 3.1E-01 148 1.2E-02 6.1E-03 1.2635361E+04 33 7.2E+03 i\n", - "\n", - " Major Minors Step nCon Feasible Optimal MeritFunction nS Penalty\n", - " 100 4 2.1E-01 149 1.2E-02 5.3E-03 1.2634158E+04 32 7.2E+03 i\n", - " 101 6 4.5E-01 150 1.2E-02 3.6E-03 1.2632646E+04 33 7.3E+03 i\n", - " 102 8 1.7E-01 151 1.2E-02 3.2E-03 1.2632124E+04 32 7.3E+03 i\n", - " 103 3 2.7E-01 152 1.2E-02 1.4E-03 1.2631549E+04 32 7.3E+03 i\n", - " 104 34 1.5E-01 153 1.2E-02 2.4E-03 1.2631077E+04 31 7.3E+03 i\n", - " 105 6 9.1E-02 154 1.2E-02 1.6E-03 1.2630726E+04 34 7.3E+03 i\n", - " 106 1 1.3E-01 155 1.2E-02 1.4E-03 1.2630974E+04 34 9.6E+03 i\n", - " 107 5 8.6E-02 156 1.2E-02 1.3E-03 1.2631260E+04 34 1.6E+04 i\n", - " 108 5 7.7E-02 157 1.2E-02 (9.5E-04) 1.2631225E+04 34 1.6E+04 i\n", - " 109 4 9.1E-02 158 1.2E-02 1.1E-03 1.2631213E+04 35 1.6E+04 i\n", - "\n", - " Major Minors Step nCon Feasible Optimal MeritFunction nS Penalty\n", - " 110 3 4.2E-02 159 1.2E-02 (8.2E-04) 1.2631209E+04 35 1.6E+04 i\n", - " 111 3 1.5E-01 160 1.2E-02 (5.3E-04) 1.2631180E+04 37 1.6E+04 i\n", - " 112 2 6.5E-02 161 1.2E-02 (7.1E-04) 1.2631251E+04 36 2.1E+04 i\n", - " 113 2 1.1E-01 162 1.2E-02 (5.7E-04) 1.2631220E+04 37 2.1E+04 i\n", - " 114 2 5.8E-02 163 1.2E-02 (4.7E-04) 1.2631223E+04 36 2.1E+04 i\n", - " 115 2 1.9E-01 164 1.2E-02 (3.3E-04) 1.2631210E+04 37 2.1E+04 i\n", - " 116 1 2.4E-01 165 1.2E-02 (2.1E-04) 1.2631216E+04 37 2.3E+04 i\n", - " 117 2 3.7E-01 166 1.2E-02 (1.8E-04) 1.2631210E+04 37 2.3E+04 i\n", - " 118 1 1.0E+00 167 1.2E-02 (7.7E-05) 1.2631205E+04 37 2.3E+04 i c\n", - " 118 2 1.0E+00 167 1.2E-02 (7.7E-05) 1.2631205E+04 37 2.3E+04 i c\n", - " 119 1 1.0E+00 168 1.2E-02 (4.2E-05) 1.2631198E+04 37 1.1E+04 i c\n", - "\n", - " Major Minors Step nCon Feasible Optimal MeritFunction nS Penalty\n", - " 120 1 2.6E-01 169 1.2E-02 (4.6E-05) 1.2631197E+04 37 1.1E+04 i c\n", - " 121 1 3.0E-01 170 1.2E-02 (2.8E-05) 1.2631196E+04 37 1.1E+04 i c\n", - " 122 1 1.0E+00 171 1.2E-02 (1.9E-05) 1.2631196E+04 37 1.1E+04 i c\n", - " 123 1 1.0E+00 172 1.2E-02 (1.1E-05) 1.2631196E+04 37 1.1E+04 i c\n", - " 124 1 1.0E+00 173 1.2E-02 (9.0E-06) 1.2631196E+04 37 1.1E+04 i c\n", - " 125 1 1.0E+00 174 1.2E-02 (8.7E-06) 1.2631196E+04 37 1.1E+04 i c\n", - " 126 1 1.0E+00 175 1.2E-02 (3.7E-06) 1.2631195E+04 37 1.1E+04 i c\n", - " 127 1 1.0E+00 176 1.2E-02 (2.7E-06) 1.2631195E+04 37 1.1E+04 i c\n", - " 128 1 1.0E+00 177 1.2E-02 (1.4E-06) 1.2631195E+04 37 1.1E+04 i c\n", - " 129 1 4.4E-01 178 1.2E-02 (4.7E-07) 1.2631195E+04 37 1.1E+04 i c\n", - "\n", - " Major Minors Step nCon Feasible Optimal MeritFunction nS Penalty\n", - " 130 1 1.0E+00 179 1.2E-02 (4.1E-07) 1.2631195E+04 37 1.1E+04 i c\n", - " 131 8 1.0E+00 180 1.2E-02 1.2E-03 1.2883502E+05 31 1.1E+04 i c\n", - " 132 4 6.8E-02 181 1.2E-02 (5.5E-04) 1.2260993E+05 32 1.1E+04 i c\n", - " 133 4 2.3E-02 182 1.2E-02 (4.2E-04) 2.6987485E+04 33 4.7E+06 i c\n", - " 134 7 4.2E-01 183 1.2E-02 1.7E-03 1.1768374E+05 35 2.3E+05 i\n", - " 135 4 3.0E-01 184 1.2E-02 1.3E-03 1.2501773E+05 32 4.3E+05 i\n", - " 136 4 1.7E-02 185 1.2E-02 1.4E-03 1.2682451E+05 31 1.3E+06 i\n", - " 137 3 2.3E-02 187 1.2E-02 1.2E-03 1.2789281E+05 33 2.3E+06 M i\n", - " 138 1 3.9E-03 189 1.2E-02 1.0E-03 1.2871869E+05 33 3.7E+06 M i\n", - " 139 9 1.6E-02 191 1.2E-02 (8.7E-04) 1.2902476E+05 33 4.3E+06 M i\n", - "\n", - " Major Minors Step nCon Feasible Optimal MeritFunction nS Penalty\n", - " 140 3 8.5E-03 192 1.2E-02 (7.6E-04) 1.2902811E+05 31 4.3E+06 i\n", - " 141 5 1.7E-02 193 1.2E-02 (7.2E-04) 1.2900881E+05 33 4.3E+06 i\n", - " 142 3 2.4E-02 194 1.2E-02 (6.2E-04) 1.2899307E+05 31 4.3E+06 i\n", - " 143 4 2.3E-02 195 1.2E-02 (6.7E-04) 1.2899526E+05 34 4.4E+06 i\n", - " 144 2 5.1E-02 196 1.2E-02 (7.4E-04) 1.2898287E+05 33 4.4E+06 i\n", - " 145 3 1.7E-02 197 1.2E-02 (7.8E-04) 1.2897100E+05 35 4.4E+06 i\n", - " 146 3 3.9E-01 198 1.2E-02 (8.4E-04) 1.2888492E+05 33 4.4E+06 i\n", - " 147 7 6.1E-03 199 1.2E-02 (9.7E-04) 1.2892486E+05 35 4.6E+06 i\n", - " 148 1 7.0E-02 200 1.2E-02 (7.2E-04) 1.2890864E+05 35 4.6E+06 i\n", - " 149 1 3.3E-02 201 1.2E-02 (6.0E-04) 1.2890889E+05 35 4.7E+06 i\n", - "\n", - " Major Minors Step nCon Feasible Optimal MeritFunction nS Penalty\n", - " 150 1 2.2E-01 202 1.2E-02 (6.5E-04) 1.2888088E+05 35 4.7E+06 i\n", - " 151 2 1.0E+00 203 1.2E-02 1.4E-03 1.2887164E+05 36 4.8E+06 i\n", - " 152 2 3.9E-02 204 1.2E-02 1.5E-03 1.2883021E+05 35 4.4E+05 i\n", - " 153 3 1.2E-01 205 1.2E-02 (7.4E-04) 1.2882285E+05 35 2.7E+05 i\n", - " 154 4 2.3E-02 206 1.2E-02 (8.7E-04) 1.2882198E+05 36 2.7E+05 i\n", - " 155 2 1.2E-01 207 1.2E-02 (7.8E-04) 1.2882075E+05 35 2.7E+05 i\n", - " 156 2 1.5E-01 208 1.2E-02 (7.4E-04) 1.2881938E+05 36 2.7E+05 i\n", - " 157 3 1.2E-01 209 1.2E-02 (5.3E-04) 1.2881852E+05 36 2.7E+05 i\n", - " 158 2 1.1E-01 210 1.2E-02 (3.4E-04) 1.2881807E+05 35 2.7E+05 i\n", - " 159 3 2.6E-01 211 1.2E-02 (4.2E-04) 1.2881767E+05 37 2.7E+05 i\n", - "\n", - " Major Minors Step nCon Feasible Optimal MeritFunction nS Penalty\n", - " 160 2 2.4E-01 212 1.2E-02 (3.7E-04) 1.2881741E+05 36 2.7E+05 i\n", - " 161 2 1.7E-01 213 1.2E-02 (3.3E-04) 1.2881728E+05 37 2.7E+05 i\n", - " 162 2 1.8E-01 214 1.2E-02 (2.7E-04) 1.2881706E+05 36 2.7E+05 i\n", - " 163 3 2.5E-01 215 1.2E-02 (2.2E-04) 1.2881678E+05 36 2.7E+05 i\n", - " 164 2 2.5E-01 216 1.2E-02 (1.8E-04) 1.2881665E+05 37 2.7E+05 i\n", - " 165 1 2.7E-01 217 1.2E-02 (8.0E-05) 1.2881657E+05 37 2.7E+05 i c\n", - " 165 2 2.7E-01 217 1.2E-02 (8.0E-05) 1.2881657E+05 37 2.7E+05 i c\n", - " 166 1 2.2E-01 218 1.2E-02 (5.5E-05) 1.2881650E+05 37 2.7E+05 i c\n", - " 167 1 1.6E-01 219 1.2E-02 (6.0E-05) 1.2881648E+05 37 2.7E+05 i c\n", - " 168 1 4.3E-01 220 1.2E-02 (3.8E-05) 1.2881646E+05 37 2.7E+05 i c\n", - " 169 1 1.3E-01 221 1.2E-02 (2.4E-05) 1.2881645E+05 37 2.7E+05 i c\n", - "\n", - " Major Minors Step nCon Feasible Optimal MeritFunction nS Penalty\n", - " 170 1 2.8E-01 222 1.2E-02 (2.4E-05) 1.2881645E+05 37 2.7E+05 i c\n", - " 171 1 1.0E+00 223 1.2E-02 (2.1E-05) 1.2881645E+05 37 2.7E+05 i c\n", - " 172 1 2.0E-01 224 1.2E-02 (1.3E-05) 1.2881645E+05 37 2.7E+05 i c\n", - " 173 1 4.0E-01 225 1.2E-02 (1.2E-05) 1.2881645E+05 37 2.7E+05 i c\n", - " 174 1 1.2E-01 226 1.2E-02 (2.7E-06) 1.2881645E+05 37 2.7E+05 i c\n", - " 175 1 1.9E-01 227 1.2E-02 (1.9E-06) 1.2881645E+05 37 2.7E+05 i c\n", - " 176 1 3.0E-01 228 1.2E-02 (3.9E-07) 1.2881645E+05 37 2.7E+05 i c\n", - " 177 1 1.2E-01 229 1.2E-02 (3.7E-07) 1.2881645E+05 37 2.7E+05 i c\n", - " 178 1 4.4E-01 230 1.2E-02 (2.6E-07) 1.2881645E+05 37 2.7E+05 i c\n", - " 179 1 2.0E-01 231 1.2E-02 (6.6E-08) 1.2881645E+05 37 2.7E+05 i c\n", - "\n", - " Major Minors Step nCon Feasible Optimal MeritFunction nS Penalty\n", - " 180 1 3.7E-01 232 1.2E-02 (8.9E-08) 1.2881645E+05 37 2.7E+05 i c\n", - " 181 1 2.6E-01 233 1.2E-02 (2.3E-08) 1.2881645E+05 37 2.7E+05 i c\n", - " 182 1 1.9E-01 234 1.2E-02 (1.4E-08) 1.2881645E+05 37 2.7E+05 i c\n", - " 183 34 1.0E+00 235 1.2E-02 (1.4E-04) 1.2909240E+10 18 2.7E+05 i c\n", + " 30 1 2.4E-01 39 2.4E-07 (4.8E-04)-9.8142918E-01 1 3.3E+01\n", + " 31 1 3.9E-01 40 1.6E-07 (2.8E-04)-9.8145158E-01 1 3.3E+01\n", + " 32 1 1.0E+00 41 3.4E-08 (3.2E-05)-9.8146287E-01 1 3.3E+01\n", + " 33 1 1.0E+00 42 (1.6E-10)(1.5E-06)-9.8146293E-01 1 3.3E+01\n", " \n", - " SNOPTA EXIT 10 -- the problem appears to be infeasible\n", - " SNOPTA INFO 13 -- nonlinear infeasibilities minimized\n", + " SNOPTA EXIT 0 -- finished successfully\n", + " SNOPTA INFO 1 -- optimality conditions satisfied\n", "\n", " Problem name ::const_iterator th1, std::vecto = gradients_multiple_impulses(reversed_throttles.begin(), reversed_throttles.end(), rvf, mf, c, a, dt); // 5) We have computed dxf/dxs and dxf/dus, but the initial and final velocites (and us) had their sign // inverted! We thus need to account for that and change sign once again of the relevant entries. + // We also must account for changes in the mass equation (now -a) auto xgrad_rvm = xt::adapt(grad_rvm, {7u, 7u}); xt::view(xgrad_rvm, xt::range(3, 6), xt::all()) *= -1; // vs - xt::view(xgrad_rvm, xt::all(), xt::range(3, 6)) *= -1; // vf + xt::view(xgrad_rvm, xt::all(), xt::range(3, 7)) *= -1; // vf, mf + auto xgrad = xt::adapt(grad, {7u, size + 1u}); xt::view(xgrad, xt::range(3, 6), xt::all()) *= -1; // vf xt::view(xgrad, xt::all(), xt::range(0, size)) *= -1; // us diff --git a/test/leg_sims_flanagan_udp.hpp b/test/leg_sims_flanagan_udp.hpp index ea31a171..296c7e43 100644 --- a/test/leg_sims_flanagan_udp.hpp +++ b/test/leg_sims_flanagan_udp.hpp @@ -110,13 +110,13 @@ struct sf_test_udp { // [1:4,-1] - fitness gradient, position mismatch w.r.t. mf xt::view(xgradient, xt::range(1u, 4u), xt::range(m_nseg * 3u + 1u, m_nseg * 3u + 2u)) - = -xt::view(xgrad_mc_xf, xt::range(0u, 3u), xt::range(6u, 7u)) / kep3::AU; // mf + = xt::view(xgrad_mc_xf, xt::range(0u, 3u), xt::range(6u, 7u)) / kep3::AU; // mf // [4:7,-1] - fitness gradient - velocity mismatch w.r.t. mf xt::view(xgradient, xt::range(4u, 7u), xt::range(m_nseg * 3u + 1u, m_nseg * 3u + 2u)) - = -xt::view(xgrad_mc_xf, xt::range(3u, 6u), xt::range(6u, 7u)) / kep3::EARTH_VELOCITY; // mf + = xt::view(xgrad_mc_xf, xt::range(3u, 6u), xt::range(6u, 7u)) / kep3::EARTH_VELOCITY; // mf // [7:8,-1] - fitness gradient - mass mismatch w.r.t. mf xt::view(xgradient, xt::range(7u, 8u), xt::range(m_nseg * 3u + 1u, m_nseg * 3u + 2u)) - = -xt::view(xgrad_mc_xf, xt::range(6u, 7u), xt::range(6u, 7u)) / 1e8; // mf + = xt::view(xgrad_mc_xf, xt::range(6u, 7u), xt::range(6u, 7u)) / 1e8; // mf // Units for the tof xt::view(xgradient, xt::all(), xt::range(m_nseg * 3u, m_nseg * 3u + 1u)) *= kep3::DAY2SEC;