Skip to content

Commit c7034d5

Browse files
mayatakamayataka
committed
refactor IpmSolver
1 parent 092e122 commit c7034d5

File tree

1 file changed

+32
-32
lines changed

1 file changed

+32
-32
lines changed

‎ocs2_ipm/src/IpmSolver.cpp‎

Lines changed: 32 additions & 32 deletions
Original file line numberDiff line numberDiff line change
@@ -374,8 +374,7 @@ IpmSolver::OcpSubproblemSolution IpmSolver::getOCPSolution(const vector_t& delta
374374
auto& deltaXSol = solution.deltaXSol;
375375
auto& deltaUSol = solution.deltaUSol;
376376
hpipm_status status;
377-
const bool hasStateInputConstraints = !ocpDefinitions_.front().equalityConstraintPtr->empty();
378-
if (hasStateInputConstraints && !settings_.projectStateInputEqualityConstraints) {
377+
if (!settings_.projectStateInputEqualityConstraints) {
379378
hpipmInterface_.resize(extractSizesFromProblem(dynamics_, lagrangian_, &stateInputEqConstraints_));
380379
status = hpipmInterface_.solve(delta_x0, dynamics_, lagrangian_, &stateInputEqConstraints_, deltaXSol, deltaUSol,
381380
settings_.printSolverStatus);
@@ -405,8 +404,8 @@ IpmSolver::OcpSubproblemSolution IpmSolver::getOCPSolution(const vector_t& delta
405404
deltaDualStateIneq.resize(N + 1);
406405
deltaDualStateInputIneq.resize(N + 1);
407406

408-
scalar_array_t primalStepSizes(N + 1, 1.0);
409-
scalar_array_t dualStepSizes(N + 1, 1.0);
407+
scalar_array_t primalStepSizes(settings_.nThreads, 1.0);
408+
scalar_array_t dualStepSizes(settings_.nThreads, 1.0);
410409

411410
std::atomic_int timeIndex{0};
412411
auto parallelTask = [&](int workerId) {
@@ -421,21 +420,21 @@ IpmSolver::OcpSubproblemSolution IpmSolver::getOCPSolution(const vector_t& delta
421420
ipm::retrieveSlackDirection(stateInputIneqConstraints_[i], deltaXSol[i], deltaUSol[i], barrierParam, slackStateInputIneq[i]);
422421
deltaDualStateInputIneq[i] =
423422
ipm::retrieveDualDirection(barrierParam, slackStateInputIneq[i], dualStateInputIneq[i], deltaSlackStateInputIneq[i]);
424-
primalStepSizes[i] = std::min(
425-
ipm::fractionToBoundaryStepSize(slackStateIneq[i], deltaSlackStateIneq[i], settings_.fractionToBoundaryMargin),
426-
ipm::fractionToBoundaryStepSize(slackStateInputIneq[i], deltaSlackStateInputIneq[i], settings_.fractionToBoundaryMargin));
427-
dualStepSizes[i] =
428-
std::min(ipm::fractionToBoundaryStepSize(dualStateIneq[i], deltaDualStateIneq[i], settings_.fractionToBoundaryMargin),
429-
ipm::fractionToBoundaryStepSize(dualStateInputIneq[i], deltaDualStateInputIneq[i], settings_.fractionToBoundaryMargin));
430-
// Re-map the projected input back to the original space.
423+
primalStepSizes[workerId] = std::min(
424+
{primalStepSizes[workerId],
425+
ipm::fractionToBoundaryStepSize(slackStateIneq[i], deltaSlackStateIneq[i], settings_.fractionToBoundaryMargin),
426+
ipm::fractionToBoundaryStepSize(slackStateInputIneq[i], deltaSlackStateInputIneq[i], settings_.fractionToBoundaryMargin)});
427+
dualStepSizes[workerId] = std::min(
428+
{dualStepSizes[workerId],
429+
ipm::fractionToBoundaryStepSize(dualStateIneq[i], deltaDualStateIneq[i], settings_.fractionToBoundaryMargin),
430+
ipm::fractionToBoundaryStepSize(dualStateInputIneq[i], deltaDualStateInputIneq[i], settings_.fractionToBoundaryMargin)});
431431
if (constraintsProjection_[i].f.size() > 0) {
432432
if (settings_.computeLagrangeMultipliers) {
433-
const auto& dfdx = projectionMultiplierCoefficients_[i].dfdx;
434-
const auto& dfdu = projectionMultiplierCoefficients_[i].dfdu;
435-
const auto& dfdcostate = projectionMultiplierCoefficients_[i].dfdcostate;
436-
const auto& f = projectionMultiplierCoefficients_[i].f;
437-
deltaNuSol[i].noalias() = dfdx * deltaXSol[i] + dfdu * deltaUSol[i] + f;
433+
deltaNuSol[i] = projectionMultiplierCoefficients_[i].f;
434+
deltaNuSol[i].noalias() += projectionMultiplierCoefficients_[i].dfdx * deltaXSol[i];
435+
deltaNuSol[i].noalias() += projectionMultiplierCoefficients_[i].dfdu * deltaUSol[i];
438436
}
437+
// Re-map the projected input back to the original space.
439438
tmp.noalias() = constraintsProjection_[i].dfdu * deltaUSol[i];
440439
deltaUSol[i] = tmp + constraintsProjection_[i].f;
441440
deltaUSol[i].noalias() += constraintsProjection_[i].dfdx * deltaXSol[i];
@@ -447,8 +446,11 @@ IpmSolver::OcpSubproblemSolution IpmSolver::getOCPSolution(const vector_t& delta
447446
if (i == N) { // Only one worker will execute this
448447
deltaSlackStateIneq[i] = ipm::retrieveSlackDirection(stateIneqConstraints_[i], deltaXSol[i], barrierParam, slackStateIneq[i]);
449448
deltaDualStateIneq[i] = ipm::retrieveDualDirection(barrierParam, slackStateIneq[i], dualStateIneq[i], deltaSlackStateIneq[i]);
450-
primalStepSizes[i] = ipm::fractionToBoundaryStepSize(slackStateIneq[i], deltaSlackStateIneq[i], settings_.fractionToBoundaryMargin);
451-
dualStepSizes[i] = ipm::fractionToBoundaryStepSize(dualStateIneq[i], deltaDualStateIneq[i], settings_.fractionToBoundaryMargin);
449+
primalStepSizes[workerId] =
450+
std::min(primalStepSizes[workerId],
451+
ipm::fractionToBoundaryStepSize(slackStateIneq[i], deltaSlackStateIneq[i], settings_.fractionToBoundaryMargin));
452+
dualStepSizes[workerId] = std::min(dualStepSizes[workerId], ipm::fractionToBoundaryStepSize(dualStateIneq[i], deltaDualStateIneq[i],
453+
settings_.fractionToBoundaryMargin));
452454
}
453455
};
454456
runParallel(std::move(parallelTask));
@@ -522,7 +524,6 @@ PerformanceIndex IpmSolver::setupQuadraticSubproblem(const std::vector<Annotated
522524
auto parallelTask = [&](int workerId) {
523525
// Get worker specific resources
524526
OptimalControlProblem& ocpDefinition = ocpDefinitions_[workerId];
525-
PerformanceIndex workerPerformance; // Accumulate performance in local variable
526527

527528
int i = timeIndex++;
528529
while (i < N) {
@@ -536,7 +537,7 @@ PerformanceIndex IpmSolver::setupQuadraticSubproblem(const std::vector<Annotated
536537
settings_.initialDualMarginRate);
537538
}
538539
metrics[i] = multiple_shooting::computeMetrics(result);
539-
workerPerformance += ipm::computePerformanceIndex(result, barrierParam, slackStateIneq[i]);
540+
performance[workerId] += ipm::computePerformanceIndex(result, barrierParam, slackStateIneq[i]);
540541
dynamics_[i] = std::move(result.dynamics);
541542
stateInputEqConstraints_[i].resize(0, x[i].size());
542543
stateIneqConstraints_[i] = std::move(result.ineqConstraints);
@@ -551,9 +552,10 @@ PerformanceIndex IpmSolver::setupQuadraticSubproblem(const std::vector<Annotated
551552

552553
ipm::condenseIneqConstraints(barrierParam, slackStateIneq[i], dualStateIneq[i], stateIneqConstraints_[i], lagrangian_[i]);
553554
if (settings_.computeLagrangeMultipliers) {
554-
workerPerformance.dualFeasibilitiesSSE += multiple_shooting::evaluateDualFeasibilities(lagrangian_[i]);
555+
performance[workerId].dualFeasibilitiesSSE += multiple_shooting::evaluateDualFeasibilities(lagrangian_[i]);
555556
}
556-
workerPerformance.dualFeasibilitiesSSE += ipm::evaluateComplementarySlackness(barrierParam, slackStateIneq[i], dualStateIneq[i]);
557+
performance[workerId].dualFeasibilitiesSSE +=
558+
ipm::evaluateComplementarySlackness(barrierParam, slackStateIneq[i], dualStateIneq[i]);
557559
} else {
558560
// Normal, intermediate node
559561
const scalar_t ti = getIntervalStart(time[i]);
@@ -575,7 +577,7 @@ PerformanceIndex IpmSolver::setupQuadraticSubproblem(const std::vector<Annotated
575577
settings_.initialDualMarginRate);
576578
}
577579
metrics[i] = multiple_shooting::computeMetrics(result);
578-
workerPerformance += ipm::computePerformanceIndex(result, dt, barrierParam, slackStateIneq[i], slackStateInputIneq[i]);
580+
performance[workerId] += ipm::computePerformanceIndex(result, dt, barrierParam, slackStateIneq[i], slackStateInputIneq[i]);
579581
if (settings_.projectStateInputEqualityConstraints) {
580582
multiple_shooting::projectTranscription(result, settings_.computeLagrangeMultipliers);
581583
}
@@ -596,10 +598,11 @@ PerformanceIndex IpmSolver::setupQuadraticSubproblem(const std::vector<Annotated
596598
ipm::condenseIneqConstraints(barrierParam, slackStateInputIneq[i], dualStateInputIneq[i], stateInputIneqConstraints_[i],
597599
lagrangian_[i]);
598600
if (settings_.computeLagrangeMultipliers) {
599-
workerPerformance.dualFeasibilitiesSSE += multiple_shooting::evaluateDualFeasibilities(lagrangian_[i]);
601+
performance[workerId].dualFeasibilitiesSSE += multiple_shooting::evaluateDualFeasibilities(lagrangian_[i]);
600602
}
601-
workerPerformance.dualFeasibilitiesSSE += ipm::evaluateComplementarySlackness(barrierParam, slackStateIneq[i], dualStateIneq[i]);
602-
workerPerformance.dualFeasibilitiesSSE +=
603+
performance[workerId].dualFeasibilitiesSSE +=
604+
ipm::evaluateComplementarySlackness(barrierParam, slackStateIneq[i], dualStateIneq[i]);
605+
performance[workerId].dualFeasibilitiesSSE +=
603606
ipm::evaluateComplementarySlackness(barrierParam, slackStateInputIneq[i], dualStateInputIneq[i]);
604607
}
605608

@@ -616,7 +619,7 @@ PerformanceIndex IpmSolver::setupQuadraticSubproblem(const std::vector<Annotated
616619
ipm::initializeDualVariable(slackStateIneq[N], barrierParam, settings_.initialDualLowerBound, settings_.initialDualMarginRate);
617620
}
618621
metrics[i] = multiple_shooting::computeMetrics(result);
619-
workerPerformance += ipm::computePerformanceIndex(result, barrierParam, slackStateIneq[N]);
622+
performance[workerId] += ipm::computePerformanceIndex(result, barrierParam, slackStateIneq[N]);
620623
stateInputEqConstraints_[i].resize(0, x[i].size());
621624
stateIneqConstraints_[i] = std::move(result.ineqConstraints);
622625
if (settings_.computeLagrangeMultipliers) {
@@ -626,13 +629,10 @@ PerformanceIndex IpmSolver::setupQuadraticSubproblem(const std::vector<Annotated
626629
}
627630
ipm::condenseIneqConstraints(barrierParam, slackStateIneq[N], dualStateIneq[N], stateIneqConstraints_[N], lagrangian_[N]);
628631
if (settings_.computeLagrangeMultipliers) {
629-
workerPerformance.dualFeasibilitiesSSE += multiple_shooting::evaluateDualFeasibilities(lagrangian_[N]);
632+
performance[workerId].dualFeasibilitiesSSE += multiple_shooting::evaluateDualFeasibilities(lagrangian_[N]);
630633
}
631-
workerPerformance.dualFeasibilitiesSSE += ipm::evaluateComplementarySlackness(barrierParam, slackStateIneq[N], dualStateIneq[N]);
634+
performance[workerId].dualFeasibilitiesSSE += ipm::evaluateComplementarySlackness(barrierParam, slackStateIneq[N], dualStateIneq[N]);
632635
}
633-
634-
// Accumulate! Same worker might run multiple tasks
635-
performance[workerId] += workerPerformance;
636636
};
637637
runParallel(std::move(parallelTask));
638638

0 commit comments

Comments
 (0)