1   /* Copyright 2022-2026 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.CalculusFieldElement;
20  import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
21  import org.orekit.errors.OrekitException;
22  import org.orekit.errors.OrekitMessages;
23  
24  /**
25   * Abstract class for fuel cost with a penalty term proportional to a weight parameter epsilon.
26   * This is typically used in a continuation method, starting from epsilon equal to 1
27   * and going towards 0 where the fuel cost is recovered. The point is to enhance convergence.
28   * The control vector is the normalized (by the upper bound on magnitude) thrust force in propagation frame.
29   * See the following reference:
30   * BERTRAND, Régis et EPENOY, Richard. New smoothing techniques for solving bang–bang optimal control problems—numerical results and statistical interpretation.
31   * Optimal Control Applications and Methods, 2002, vol. 23, no 4, p. 171-197.
32   *
33   * @param <T> type of the field elements
34   * @author Romain Serra
35   * @since 13.0
36   * @see FieldCartesianFuelCost
37   * @see PenalizedCartesianFuelCost
38   */
39  public abstract class FieldPenalizedCartesianFuelCost<T extends CalculusFieldElement<T>>
40          extends FieldAbstractCartesianCost<T> {
41  
42      /** Maximum value of thrust force Euclidean norm. */
43      private final T maximumThrustMagnitude;
44  
45      /** Penalty weight. */
46      private final T epsilon;
47  
48      /**
49       * Constructor.
50       *
51       * @param name               adjoint name
52       * @param massFlowRateFactor mass flow rate factor
53       * @param maximumThrustMagnitude maximum thrust magnitude
54       * @param epsilon penalty weight
55       */
56      protected FieldPenalizedCartesianFuelCost(final String name, final T massFlowRateFactor,
57                                                final T maximumThrustMagnitude, final T epsilon) {
58          super(name, massFlowRateFactor);
59          final double epsilonReal = epsilon.getReal();
60          if (epsilonReal < 0 || epsilonReal > 1) {
61              throw new OrekitException(OrekitMessages.INVALID_PARAMETER_RANGE, "epsilon", epsilonReal, 0, 1);
62          }
63          this.maximumThrustMagnitude = maximumThrustMagnitude;
64          this.epsilon = epsilon;
65      }
66  
67      /** Getter for the penalty weight epsilon.
68       * @return epsilon
69       */
70      public T getEpsilon() {
71          return epsilon;
72      }
73  
74      /** Getter for maximum thrust magnitude.
75       * @return maximum thrust
76       */
77      public T getMaximumThrustMagnitude() {
78          return maximumThrustMagnitude;
79      }
80  
81      /**
82       * Evaluate the penalty term (without the weight), assumed to be a function of the control norm.
83       * @param controlNorm Euclidean norm of control vector
84       * @return penalty function
85       */
86      public abstract T evaluateFieldPenaltyFunction(T controlNorm);
87  
88      /**
89       * Computes the direction of thrust.
90       * @param adjointVariables adjoint vector
91       * @return thrust direction
92       */
93      protected FieldVector3D<T> getFieldThrustDirection(final T[] adjointVariables) {
94          return new FieldVector3D<>(adjointVariables[3], adjointVariables[4], adjointVariables[5]).normalize();
95      }
96  
97      /** {@inheritDoc} */
98      @Override
99      public T getFieldHamiltonianContribution(final T[] adjointVariables, final T mass) {
100         final FieldVector3D<T> thrustForce = getFieldThrustAccelerationVector(adjointVariables,
101                 mass).scalarMultiply(mass);
102         final T controlNorm = thrustForce.getNorm().divide(getMaximumThrustMagnitude());
103         return controlNorm.add(getEpsilon().multiply(evaluateFieldPenaltyFunction(controlNorm)))
104                 .multiply(getMaximumThrustMagnitude().negate());
105     }
106 
107 }