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.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   * @author Romain Serra
34   * @since 13.0
35   * @see FieldCartesianFuelCost
36   * @see PenalizedCartesianFuelCost
37   */
38  public abstract class FieldPenalizedCartesianFuelCost<T extends CalculusFieldElement<T>>
39          extends FieldAbstractCartesianCost<T> {
40  
41      /** Maximum value of thrust force Euclidean norm. */
42      private final T maximumThrustMagnitude;
43  
44      /** Penalty weight. */
45      private final T epsilon;
46  
47      /**
48       * Constructor.
49       *
50       * @param name               adjoint name
51       * @param massFlowRateFactor mass flow rate factor
52       * @param maximumThrustMagnitude maximum thrust magnitude
53       * @param epsilon penalty weight
54       */
55      protected FieldPenalizedCartesianFuelCost(final String name, final T massFlowRateFactor,
56                                                final T maximumThrustMagnitude, final T epsilon) {
57          super(name, massFlowRateFactor);
58          final double epsilonReal = epsilon.getReal();
59          if (epsilonReal < 0 || epsilonReal > 1) {
60              throw new OrekitException(OrekitMessages.INVALID_PARAMETER_RANGE, "epsilon", epsilonReal, 0, 1);
61          }
62          this.maximumThrustMagnitude = maximumThrustMagnitude;
63          this.epsilon = epsilon;
64      }
65  
66      /** Getter for the penalty weight epsilon.
67       * @return epsilon
68       */
69      public T getEpsilon() {
70          return epsilon;
71      }
72  
73      /** Getter for maximum thrust magnitude.
74       * @return maximum thrust
75       */
76      public T getMaximumThrustMagnitude() {
77          return maximumThrustMagnitude;
78      }
79  
80      /**
81       * Evaluate the penalty term (without the weight), assumed to be a function of the control norm.
82       * @param controlNorm Euclidean norm of control vector
83       * @return penalty function
84       */
85      public abstract T evaluateFieldPenaltyFunction(T controlNorm);
86  
87      /**
88       * Computes the direction of thrust.
89       * @param adjointVariables adjoint vector
90       * @return thrust direction
91       */
92      protected FieldVector3D<T> getFieldThrustDirection(final T[] adjointVariables) {
93          return new FieldVector3D<>(adjointVariables[3], adjointVariables[4], adjointVariables[5]).normalize();
94      }
95  
96      /** {@inheritDoc} */
97      @Override
98      public T getFieldHamiltonianContribution(final T[] adjointVariables, final T mass) {
99          final FieldVector3D<T> thrustForce = getFieldThrustAccelerationVector(adjointVariables,
100                 mass).scalarMultiply(mass);
101         final T controlNorm = thrustForce.getNorm().divide(getMaximumThrustMagnitude());
102         return controlNorm.add(getEpsilon().multiply(evaluateFieldPenaltyFunction(controlNorm)))
103                 .multiply(getMaximumThrustMagnitude().negate());
104     }
105 
106 }