fixing Transcription

Author: farbod-farshidian

ocs2_oc/src/multiple_shooting/Transcription.cpp

@@ -44,11 +44,11 @@ Transcription setupIntermediateNode(const OptimalControlProblem& optimalControlP
   Transcription transcription;
   auto& cost = transcription.cost;
   auto& dynamics = transcription.dynamics;
+  auto& constraintsSize = transcription.constraintsSize;
   auto& stateEqConstraints = transcription.stateEqConstraints;
   auto& stateInputEqConstraints = transcription.stateInputEqConstraints;
   auto& stateIneqConstraints = transcription.stateIneqConstraints;
   auto& stateInputIneqConstraints = transcription.stateInputIneqConstraints;
-  auto& constraintsSize = transcription.constraintsSize;
 
   // Dynamics
   // Discretization returns x_{k+1} = A_{k} * dx_{k} + B_{k} * du_{k} + b_{k}
@@ -65,26 +65,30 @@ Transcription setupIntermediateNode(const OptimalControlProblem& optimalControlP
 
   // State equality constraints
   if (!optimalControlProblem.stateEqualityConstraintPtr->empty()) {
-    stateEqConstraints = optimalControlProblem.stateEqualityConstraintPtr->getLinearApproximation(
-        t, x, *optimalControlProblem.preComputationPtr, &constraintsSize.stateEq);
+    constraintsSize.stateEq = optimalControlProblem.stateEqualityConstraintPtr->getTermsSize(t);
+    stateEqConstraints =
+        optimalControlProblem.stateEqualityConstraintPtr->getLinearApproximation(t, x, *optimalControlProblem.preComputationPtr);
   }
 
   // State-input equality constraints
   if (!optimalControlProblem.equalityConstraintPtr->empty()) {
-    stateInputEqConstraints = optimalControlProblem.equalityConstraintPtr->getLinearApproximation(
-        t, x, u, *optimalControlProblem.preComputationPtr, &constraintsSize.stateInputEq);
+    constraintsSize.stateInputEq = optimalControlProblem.equalityConstraintPtr->getTermsSize(t);
+    stateInputEqConstraints =
+        optimalControlProblem.equalityConstraintPtr->getLinearApproximation(t, x, u, *optimalControlProblem.preComputationPtr);
   }
 
   // State inequality constraints.
   if (!optimalControlProblem.stateInequalityConstraintPtr->empty()) {
-    stateIneqConstraints = optimalControlProblem.stateInequalityConstraintPtr->getLinearApproximation(
-        t, x, *optimalControlProblem.preComputationPtr, &constraintsSize.stateIneq);
+    constraintsSize.stateIneq = optimalControlProblem.stateInequalityConstraintPtr->getTermsSize(t);
+    stateIneqConstraints =
+        optimalControlProblem.stateInequalityConstraintPtr->getLinearApproximation(t, x, *optimalControlProblem.preComputationPtr);
   }
 
   // State-input inequality constraints.
   if (!optimalControlProblem.inequalityConstraintPtr->empty()) {
-    stateInputIneqConstraints = optimalControlProblem.inequalityConstraintPtr->getLinearApproximation(
-        t, x, u, *optimalControlProblem.preComputationPtr, &constraintsSize.stateInputIneq);
+    constraintsSize.stateInputIneq = optimalControlProblem.inequalityConstraintPtr->getTermsSize(t);
+    stateInputIneqConstraints =
+        optimalControlProblem.inequalityConstraintPtr->getLinearApproximation(t, x, u, *optimalControlProblem.preComputationPtr);
   }
 
   return transcription;
@@ -123,9 +127,9 @@ TerminalTranscription setupTerminalNode(const OptimalControlProblem& optimalCont
   // Results and short-hand notation
   TerminalTranscription transcription;
   auto& cost = transcription.cost;
+  auto& constraintsSize = transcription.constraintsSize;
   auto& eqConstraints = transcription.eqConstraints;
   auto& ineqConstraints = transcription.ineqConstraints;
-  auto& constraintsSize = transcription.constraintsSize;
 
   constexpr auto request = Request::Cost + Request::SoftConstraint + Request::Approximation;
   optimalControlProblem.preComputationPtr->requestFinal(request, t, x);
@@ -135,14 +139,16 @@ TerminalTranscription setupTerminalNode(const OptimalControlProblem& optimalCont
 
   // State equality constraints.
   if (!optimalControlProblem.finalEqualityConstraintPtr->empty()) {
-    eqConstraints = optimalControlProblem.finalEqualityConstraintPtr->getLinearApproximation(t, x, *optimalControlProblem.preComputationPtr,
-                                                                                             &constraintsSize.stateEq);
+    constraintsSize.stateEq = optimalControlProblem.finalEqualityConstraintPtr->getTermsSize(t);
+    eqConstraints =
+        optimalControlProblem.finalEqualityConstraintPtr->getLinearApproximation(t, x, *optimalControlProblem.preComputationPtr);
   }
 
   // State inequality constraints.
   if (!optimalControlProblem.finalInequalityConstraintPtr->empty()) {
-    ineqConstraints = optimalControlProblem.finalInequalityConstraintPtr->getLinearApproximation(
-        t, x, *optimalControlProblem.preComputationPtr, &constraintsSize.stateIneq);
+    constraintsSize.stateIneq = optimalControlProblem.finalInequalityConstraintPtr->getTermsSize(t);
+    ineqConstraints =
+        optimalControlProblem.finalInequalityConstraintPtr->getLinearApproximation(t, x, *optimalControlProblem.preComputationPtr);
   }
 
   return transcription;
@@ -154,9 +160,9 @@ EventTranscription setupEventNode(const OptimalControlProblem& optimalControlPro
   EventTranscription transcription;
   auto& cost = transcription.cost;
   auto& dynamics = transcription.dynamics;
+  auto& constraintsSize = transcription.constraintsSize;
   auto& eqConstraints = transcription.eqConstraints;
   auto& ineqConstraints = transcription.ineqConstraints;
-  auto& constraintsSize = transcription.constraintsSize;
 
   constexpr auto request = Request::Cost + Request::SoftConstraint + Request::Dynamics + Request::Approximation;
   optimalControlProblem.preComputationPtr->requestPreJump(request, t, x);
@@ -172,14 +178,16 @@ EventTranscription setupEventNode(const OptimalControlProblem& optimalControlPro
 
   // State equality constraints.
   if (!optimalControlProblem.preJumpEqualityConstraintPtr->empty()) {
-    eqConstraints = optimalControlProblem.preJumpEqualityConstraintPtr->getLinearApproximation(
-        t, x, *optimalControlProblem.preComputationPtr, &constraintsSize.stateEq);
+    constraintsSize.stateEq = optimalControlProblem.preJumpEqualityConstraintPtr->getTermsSize(t);
+    eqConstraints =
+        optimalControlProblem.preJumpEqualityConstraintPtr->getLinearApproximation(t, x, *optimalControlProblem.preComputationPtr);
   }
 
   // State inequality constraints.
   if (!optimalControlProblem.preJumpInequalityConstraintPtr->empty()) {
-    ineqConstraints = optimalControlProblem.preJumpInequalityConstraintPtr->getLinearApproximation(
-        t, x, *optimalControlProblem.preComputationPtr, &constraintsSize.stateIneq);
+    constraintsSize.stateIneq = optimalControlProblem.preJumpInequalityConstraintPtr->getTermsSize(t);
+    ineqConstraints =
+        optimalControlProblem.preJumpInequalityConstraintPtr->getLinearApproximation(t, x, *optimalControlProblem.preComputationPtr);
   }
 
   return transcription;