1   /* Copyright 2022-2025 Romain Serra
2    * Licensed to CS GROUP (CS) under one or more
3    * contributor license agreements.  See the NOTICE file distributed with
4    * this work for additional information regarding copyright ownership.
5    * CS licenses this file to You under the Apache License, Version 2.0
6    * (the "License"); you may not use this file except in compliance with
7    * the License.  You may obtain a copy of the License at
8    *
9    *   http://www.apache.org/licenses/LICENSE-2.0
10   *
11   * Unless required by applicable law or agreed to in writing, software
12   * distributed under the License is distributed on an "AS IS" BASIS,
13   * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14   * See the License for the specific language governing permissions and
15   * limitations under the License.
16   */
17  package org.orekit.control.indirect.adjoint.cost;
18  
19  import org.hipparchus.geometry.euclidean.threed.Vector3D;
20  import org.orekit.errors.OrekitException;
21  import org.orekit.errors.OrekitMessages;
22  
23  /**
24   * Abstract class for fuel cost with a penalty term proportional to a weight parameter epsilon.
25   * This is typically used in a continuation method, starting from epsilon equal to 1
26   * and going towards 0 where the fuel cost is recovered. The point is to enhance convergence.
27   * The control vector is the normalized (by the upper bound on magnitude) thrust force in propagation frame.
28   * See the following reference:
29   * BERTRAND, Régis et EPENOY, Richard. New smoothing techniques for solving bang–bang optimal control problems—numerical results and statistical interpretation.
30   * Optimal Control Applications and Methods, 2002, vol. 23, no 4, p. 171-197.
31   *
32   * @author Romain Serra
33   * @since 13.0
34   * @see CartesianFuelCost
35   */
36  public abstract class PenalizedCartesianFuelCost extends AbstractCartesianCost {
37  
38      /** Maximum value of thrust force Euclidean norm. */
39      private final double maximumThrustMagnitude;
40  
41      /** Penalty weight. */
42      private final double epsilon;
43  
44      /**
45       * Constructor.
46       *
47       * @param name               adjoint name
48       * @param massFlowRateFactor mass flow rate factor
49       * @param maximumThrustMagnitude maximum thrust magnitude
50       * @param epsilon penalty weight
51       */
52      protected PenalizedCartesianFuelCost(final String name, final double massFlowRateFactor,
53                                           final double maximumThrustMagnitude, final double epsilon) {
54          super(name, massFlowRateFactor);
55          if (epsilon < 0 || epsilon > 1) {
56              throw new OrekitException(OrekitMessages.INVALID_PARAMETER_RANGE, "epsilon", epsilon, 0, 1);
57          }
58          this.maximumThrustMagnitude = maximumThrustMagnitude;
59          this.epsilon = epsilon;
60      }
61  
62      /** Getter for the penalty weight epsilon.
63       * @return epsilon
64       */
65      public double getEpsilon() {
66          return epsilon;
67      }
68  
69      /** Getter for maximum thrust magnitude.
70       * @return maximum thrust
71       */
72      public double getMaximumThrustMagnitude() {
73          return maximumThrustMagnitude;
74      }
75  
76      /**
77       * Evaluate the penalty term (without the weight), assumed to be a function of the control norm.
78       * @param controlNorm Euclidean norm of control vector
79       * @return penalty function
80       */
81      public abstract double evaluatePenaltyFunction(double controlNorm);
82  
83      /**
84       * Computes the direction of thrust.
85       * @param adjointVariables adjoint vector
86       * @return thrust direction
87       */
88      protected Vector3D getThrustDirection(final double[] adjointVariables) {
89          return new Vector3D(adjointVariables[3], adjointVariables[4], adjointVariables[5]).normalize();
90      }
91  
92      /** {@inheritDoc} */
93      @Override
94      public double getHamiltonianContribution(final double[] adjointVariables, final double mass) {
95          final Vector3D thrustForce = getThrustAccelerationVector(adjointVariables, mass).scalarMultiply(mass);
96          final double controlNorm = thrustForce.getNorm() / getMaximumThrustMagnitude();
97          return -(controlNorm + getEpsilon() * evaluatePenaltyFunction(controlNorm)) * getMaximumThrustMagnitude();
98      }
99  
100 }