Skip to content

Commit

Permalink
BIG UPDATE: Implemented and tested successfully mobiman parallelizati…
Browse files Browse the repository at this point in the history 
…on using namespaces. All print-outs are commented out. NUA NOTE: Even the launch is terminated the node mobile_manipulator still remains active, which cause the computer slows down!
  • Loading branch information
@akmandor
akmandor committed Oct 24, 2023
1 parent 1735c33 commit a2a6955
Show file tree
Hide file tree
Showing 21 changed files with 484 additions and 380 deletions.
2 changes: 1 addition & 1 deletion ocs2_core/src/integration/IntegratorBase.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -178,7 +178,7 @@ void IntegratorBase::integrateTimes(OdeBase& system,
}
catch (const std::runtime_error& error)
{
std::cout << "[IntegratorBase::integrateTimes] INTEGRATION TERMINATED!!!" << std::endl << std::endl;
//std::cout << "[IntegratorBase::integrateTimes] INTEGRATION TERMINATED!!!" << std::endl << std::endl;
//std::cout << "[IntegratorBase::integrateTimes] DEBUG INF" << std::endl << std::endl;
//while(1);
}
Expand Down
8 changes: 4 additions & 4 deletions ocs2_core/src/thread_support/ThreadPool.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -103,13 +103,13 @@ void ThreadPool::runTask(std::unique_ptr<TaskBase> taskPtr) {
/**************************************************************************************************/
void ThreadPool::runParallel(std::function<void(int)> taskFunction, int N)
{
std::cout << "[ThreadPool::runParallel] START" << std::endl;
//std::cout << "[ThreadPool::runParallel] START" << std::endl;

// Launch tasks in helper threads
std::vector<std::future<void>> futures;
if (N > 1)
{
std::cout << "[ThreadPool::runParallel] GALAPAKOS 0" << std::endl;
//std::cout << "[ThreadPool::runParallel] GALAPAKOS 0" << std::endl;
const int numHelpers = N - 1;
futures.reserve(numHelpers);
for (int i = 0; i < numHelpers; ++i)
Expand All @@ -125,11 +125,11 @@ void ThreadPool::runParallel(std::function<void(int)> taskFunction, int N)
// Wait for helpers to finish.
for (auto&& fut : futures)
{
std::cout << "[ThreadPool::runParallel] GALAPAKOS 1" << std::endl;
//std::cout << "[ThreadPool::runParallel] GALAPAKOS 1" << std::endl;
fut.get();
}

std::cout << "[ThreadPool::runParallel] END" << std::endl;
//std::cout << "[ThreadPool::runParallel] END" << std::endl;
}

} // namespace ocs2
24 changes: 10 additions & 14 deletions ocs2_ddp/src/GaussNewtonDDP.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -150,14 +150,10 @@ GaussNewtonDDP::GaussNewtonDDP(ddp::Settings ddpSettings,
/******************************************************************************************************/
GaussNewtonDDP::~GaussNewtonDDP()
{
std::cerr << getBenchmarkingInfo() << std::endl;

/*
if (ddpSettings_.displayInfo_ || ddpSettings_.displayShortSummary_)
{
std::cerr << getBenchmarkingInfo() << std::endl;
}
*/
}

/******************************************************************************************************/
Expand Down Expand Up @@ -1270,7 +1266,7 @@ void GaussNewtonDDP::initializeDualSolutionAndMetrics()
/******************************************************************************************************/
void GaussNewtonDDP::takePrimalDualStep(scalar_t lqModelExpectedCost)
{
std::cout << "[GaussNewtonDDP::takePrimalDualStep] START" << std::endl;
//std::cout << "[GaussNewtonDDP::takePrimalDualStep] START" << std::endl;

// update primal: run search strategy and find the optimal stepLength
searchStrategyTimer_.startTimer();
Expand Down Expand Up @@ -1307,7 +1303,7 @@ void GaussNewtonDDP::takePrimalDualStep(scalar_t lqModelExpectedCost)
solution);
//*/

std::cout << "[GaussNewtonDDP::takePrimalDualStep] END searchStrategyPtr_" << std::endl;
//std::cout << "[GaussNewtonDDP::takePrimalDualStep] END searchStrategyPtr_" << std::endl;

if (success)
{
Expand Down Expand Up @@ -1626,7 +1622,7 @@ void GaussNewtonDDP::runImpl(scalar_t initTime, const vector_t& initState, const
// DDP main loop
while (true)
{
std::cout << "[GaussNewtonDDP::runImpl(4)] START DDP LOOP" << std::endl;
//std::cout << "[GaussNewtonDDP::runImpl(4)] START DDP LOOP" << std::endl;

if (ddpSettings_.displayInfo_)
{
Expand All @@ -1649,21 +1645,21 @@ void GaussNewtonDDP::runImpl(scalar_t initTime, const vector_t& initState, const
backwardPassTimer_.endTimer();
//std::cout << "[GaussNewtonDDP::runImpl(4)] END solveSequentialRiccatiEquations" << std::endl;

std::cout << "[GaussNewtonDDP::runImpl(4)] START calculateController" << std::endl;
//std::cout << "[GaussNewtonDDP::runImpl(4)] START calculateController" << std::endl;
// calculate controller and store the result in unoptimizedController_
computeControllerTimer_.startTimer();
calculateController();
computeControllerTimer_.endTimer();
std::cout << "[GaussNewtonDDP::runImpl(4)] END calculateController" << std::endl;
//std::cout << "[GaussNewtonDDP::runImpl(4)] END calculateController" << std::endl;


// the expected cost/merit calculated by the Riccati solution is not reliable
const auto lqModelExpectedCost = initialSolutionExists ? nominalDualData_.valueFunctionTrajectory.front().f : performanceIndex_.merit;

std::cout << "[GaussNewtonDDP::runImpl(4)] START takePrimalDualStep" << std::endl;
//std::cout << "[GaussNewtonDDP::runImpl(4)] START takePrimalDualStep" << std::endl;
// nominal --> optimized: based on the current LQ solution updates the optimized primal and dual solutions
takePrimalDualStep(lqModelExpectedCost);
std::cout << "[GaussNewtonDDP::runImpl(4)] END takePrimalDualStep" << std::endl;
//std::cout << "[GaussNewtonDDP::runImpl(4)] END takePrimalDualStep" << std::endl;

// iteration info
++totalNumIterations_;
Expand All @@ -1679,11 +1675,11 @@ void GaussNewtonDDP::runImpl(scalar_t initTime, const vector_t& initState, const
std::tie(isConverged, convergenceInfo) = searchStrategyPtr_->checkConvergence(!initialSolutionExists, *std::prev(performanceIndexHistory_.end(), 2), performanceIndexHistory_.back());
initialSolutionExists = true;

std::cout << "[GaussNewtonDDP::runImpl(4)] convergenceInfo: " << std::endl;
//std::cout << "[GaussNewtonDDP::runImpl(4)] convergenceInfo: " << std::endl;
//std::cout << convergenceInfo << std::endl;
if (isConverged || (totalNumIterations_ - initIteration) == ddpSettings_.maxNumIterations_)
{
std::cout << "[GaussNewtonDDP::runImpl(4)] END DDP LOOP" << std::endl;
//std::cout << "[GaussNewtonDDP::runImpl(4)] END DDP LOOP" << std::endl;
break;
}
else
Expand All @@ -1703,7 +1699,7 @@ void GaussNewtonDDP::runImpl(scalar_t initTime, const vector_t& initState, const
optimizedPrimalSolution_.swap(nominalPrimalData_.primalSolution);
optimizedProblemMetrics_.swap(nominalPrimalData_.problemMetrics);

std::cout << "[GaussNewtonDDP::runImpl(4)] CONTINUE DDP LOOP" << std::endl << std::endl;;
//std::cout << "[GaussNewtonDDP::runImpl(4)] CONTINUE DDP LOOP" << std::endl << std::endl;;
}
//ctr++;
} // end of while loop
Expand Down
37 changes: 18 additions & 19 deletions ocs2_ddp/src/search_strategy/LineSearchStrategy.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -162,7 +162,7 @@ void LineSearchStrategy::computeSolution(size_t taskId,
const vector_t& initFullState,
search_strategy::Solution& solution)
{
std::cout << "[LineSearchStrategy::computeSolution(4)] START" << std::endl;
//std::cout << "[LineSearchStrategy::computeSolution(4)] START" << std::endl;

auto& problem = optimalControlProblemRefStock_[taskId];
auto& rollout = rolloutRefStock_[taskId];
Expand Down Expand Up @@ -223,7 +223,7 @@ void LineSearchStrategy::computeSolution(size_t taskId,
printString(infoDisplay.str());
}

std::cout << "[LineSearchStrategy::computeSolution(4)] END" << std::endl;
//std::cout << "[LineSearchStrategy::computeSolution(4)] END" << std::endl;
}

/******************************************************************************************************/
Expand Down Expand Up @@ -309,7 +309,7 @@ bool LineSearchStrategy::run(const std::pair<scalar_t, scalar_t>& timePeriod,
const ModeSchedule& modeSchedule,
search_strategy::SolutionRef solutionRef)
{
std::cout << "[LineSearchStrategy::run(8)] START" << std::endl;
//std::cout << "[LineSearchStrategy::run(8)] START" << std::endl;

// initialize lineSearchModule inputs
lineSearchInputRef_.timePeriodPtr = &timePeriod;
Expand All @@ -325,10 +325,10 @@ bool LineSearchStrategy::run(const std::pair<scalar_t, scalar_t>& timePeriod,

try
{
std::cout << "[LineSearchStrategy::run(8)] START computeSolution" << std::endl;
//std::cout << "[LineSearchStrategy::run(8)] START computeSolution" << std::endl;
//computeSolution(taskId, stepLength, workersSolution_[taskId]);
computeSolution(taskId, stepLength, initFullState, workersSolution_[taskId]);
std::cout << "[LineSearchStrategy::run(8)] END computeSolution" << std::endl;
//std::cout << "[LineSearchStrategy::run(8)] END computeSolution" << std::endl;

baselineMerit_ = workersSolution_[taskId].performanceIndex.merit;
unoptimizedControllerUpdateIS_ = computeControllerUpdateIS(unoptimizedController);
Expand All @@ -345,22 +345,22 @@ bool LineSearchStrategy::run(const std::pair<scalar_t, scalar_t>& timePeriod,
" is terminated: " + error.what() + '\n');
}

std::cout << "[LineSearchStrategy::run(8)] BEFORE internalShutDownFlag_: " << internalShutDownFlag_ << std::endl;
//std::cout << "[LineSearchStrategy::run(8)] BEFORE internalShutDownFlag_: " << internalShutDownFlag_ << std::endl;
internalShutDownFlag_ = true;
std::cout << "[LineSearchStrategy::run(8)] AFTER internalShutDownFlag_: " << internalShutDownFlag_ << std::endl;
//std::cout << "[LineSearchStrategy::run(8)] AFTER internalShutDownFlag_: " << internalShutDownFlag_ << std::endl;
//throw std::runtime_error("[LineSearchStrategy::run(8)] DDP controller does not generate a stable rollout!");
}

std::cout << "[LineSearchStrategy::run(8)] START lineSearchTask" << std::endl;
//std::cout << "[LineSearchStrategy::run(8)] START lineSearchTask" << std::endl;
// run workers
nextTaskId_ = 0;
alphaExpNext_ = 0;
alphaProcessed_ = std::vector<bool>(maxNumOfSearches(), false);
//auto task = [&](int) { lineSearchTask(nextTaskId_++); };
auto task = [&](int) { lineSearchTask(nextTaskId_++, initFullState); };
std::cout << "[LineSearchStrategy::run(8)] numThreads: " << threadPoolRef_.numThreads() << std::endl;
//std::cout << "[LineSearchStrategy::run(8)] numThreads: " << threadPoolRef_.numThreads() << std::endl;
threadPoolRef_.runParallel(task, threadPoolRef_.numThreads());
std::cout << "[LineSearchStrategy::run(8)] END lineSearchTask" << std::endl;
//std::cout << "[LineSearchStrategy::run(8)] END lineSearchTask" << std::endl;

//std::cout << "[LineSearchStrategy::run(8)] START reactivateRollout" << std::endl;
// revitalize all integrators
Expand All @@ -376,7 +376,7 @@ bool LineSearchStrategy::run(const std::pair<scalar_t, scalar_t>& timePeriod,
std::cerr << "[LineSearchStrategy::run(8)] The chosen step length is: " + std::to_string(bestStepSize_) << "\n";
}

std::cout << "[LineSearchStrategy::run(8)] END" << std::endl;
//std::cout << "[LineSearchStrategy::run(8)] END" << std::endl;

return true;
}
Expand Down Expand Up @@ -477,12 +477,12 @@ void LineSearchStrategy::lineSearchTask(const size_t taskId)
/******************************************************************************************************/
void LineSearchStrategy::lineSearchTask(const size_t taskId, const vector_t& initFullState)
{
std::cout << "[LineSearchStrategy::lineSearchTask(2)] START" << std::endl;
//std::cout << "[LineSearchStrategy::lineSearchTask(2)] START" << std::endl;

int ctr = 0;
while (true)
{
std::cout << "[LineSearchStrategy::lineSearchTask(2)] ctr: " << ctr << std::endl;
//std::cout << "[LineSearchStrategy::lineSearchTask(2)] ctr: " << ctr << std::endl;

const size_t alphaExp = alphaExpNext_++;
const scalar_t stepLength = settings_.maxStepLength * std::pow(settings_.contractionRate, alphaExp);
Expand All @@ -494,14 +494,14 @@ void LineSearchStrategy::lineSearchTask(const size_t taskId, const vector_t& ini
*/
if (!numerics::almost_ge(stepLength, settings_.minStepLength))
{
std::cout << "[LineSearchStrategy::lineSearchTask(2)] BEFORE break 0" << std::endl;
//std::cout << "[LineSearchStrategy::lineSearchTask(2)] BEFORE break 0" << std::endl;
break;
}

// skip if the current learning rate is less than the best candidate
if (stepLength < bestStepSize_)
{
std::cout << "[LineSearchStrategy::lineSearchTask(2)] BEFORE break 1" << std::endl;
//std::cout << "[LineSearchStrategy::lineSearchTask(2)] BEFORE break 1" << std::endl;
// display
if (baseSettings_.displayInfo)
{
Expand All @@ -515,7 +515,7 @@ void LineSearchStrategy::lineSearchTask(const size_t taskId, const vector_t& ini

try
{
std::cout << "[LineSearchStrategy::lineSearchTask(2)] BEFORE computeSolution" << std::endl;
//std::cout << "[LineSearchStrategy::lineSearchTask(2)] BEFORE computeSolution" << std::endl;
//computeSolution(taskId, stepLength, workersSolution_[taskId]);
computeSolution(taskId, stepLength, initFullState, workersSolution_[taskId]);
}
Expand Down Expand Up @@ -543,7 +543,6 @@ void LineSearchStrategy::lineSearchTask(const size_t taskId, const vector_t& ini
const bool armijoCondition = workersSolution_[taskId].performanceIndex.merit < (baselineMerit_ - settings_.armijoCoefficient * stepLength * unoptimizedControllerUpdateIS_);
if (armijoCondition && stepLength > bestStepSize_)
{ // save solution
std::cout << "[LineSearchStrategy::lineSearchTask(2)] ARMIJOOOOO" << std::endl;
bestStepSize_ = stepLength;
swap(*bestSolutionRef_, workersSolution_[taskId]);
terminateLinesearchTasks = std::all_of(alphaProcessed_.cbegin(), alphaProcessed_.cbegin() + alphaExp, [](bool f) { return f; });
Expand All @@ -557,7 +556,7 @@ void LineSearchStrategy::lineSearchTask(const size_t taskId, const vector_t& ini
{
for (RolloutBase& rollout : rolloutRefStock_)
{
std::cout << "[LineSearchStrategy::lineSearchTask(2)] BEFORE abortRollout" << std::endl;
//std::cout << "[LineSearchStrategy::lineSearchTask(2)] BEFORE abortRollout" << std::endl;
rollout.abortRollout();
}

Expand All @@ -570,7 +569,7 @@ void LineSearchStrategy::lineSearchTask(const size_t taskId, const vector_t& ini
ctr++;
} // end of while loop

std::cout << "[LineSearchStrategy::lineSearchTask(2)] END" << std::endl;
//std::cout << "[LineSearchStrategy::lineSearchTask(2)] END" << std::endl;
}

/******************************************************************************************************/
Expand Down
4 changes: 2 additions & 2 deletions ocs2_pinocchio/ocs2_ext_collision/src/ExtCollisionCppAd.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,7 +38,7 @@ ExtCollisionCppAd::ExtCollisionCppAd(const PinocchioInterface& pinocchioInterfac
distances_(pointsOnRobotPtr->getNumOfPoints()),
emuPtr_(emuPtr)
{
std::cout << "[ExtCollisionCppAd::ExtCollisionCppAd] START" << std::endl;
//std::cout << "[ExtCollisionCppAd::ExtCollisionCppAd] START" << std::endl;

normalize_flag_ = true;

Expand All @@ -57,7 +57,7 @@ ExtCollisionCppAd::ExtCollisionCppAd(const PinocchioInterface& pinocchioInterfac
cppAdInterfaceDistanceCalculation_->loadModelsIfAvailable(CppAdInterface::ApproximationOrder::First, verbose);
}

std::cout << "[ExtCollisionCppAd::ExtCollisionCppAd] END" << std::endl;
//std::cout << "[ExtCollisionCppAd::ExtCollisionCppAd] END" << std::endl;
}

/******************************************************************************************************/
Expand Down
10 changes: 6 additions & 4 deletions ocs2_pinocchio/ocs2_ext_collision/src/PointsOnRobot.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,7 +21,7 @@
PointsOnRobot::PointsOnRobot(const PointsOnRobot::points_radii_t& points_radii)
: points_(), radii_()
{
std::cout << "[PointsOnRobot::PointsOnRobot] START" << std::endl;
//std::cout << "[PointsOnRobot::PointsOnRobot] START" << std::endl;

const auto& pointsAndRadii = points_radii;

Expand Down Expand Up @@ -51,7 +51,7 @@ PointsOnRobot::PointsOnRobot(const PointsOnRobot::points_radii_t& points_radii)
//nh_ = nh;
//pointsOnRobot_visu_pub_ = nh_.advertise<visualization_msgs::MarkerArray>("points_on_robot", 1);

std::cout << "[PointsOnRobot::PointsOnRobot] END" << std::endl;
//std::cout << "[PointsOnRobot::PointsOnRobot] END" << std::endl;
}

//-------------------------------------------------------------------------------------------------------
Expand Down Expand Up @@ -128,11 +128,13 @@ void PointsOnRobot::initialize(ocs2::PinocchioInterface& pinocchioInterface,
break;
}

//std::cout << "[PointsOnRobot::initialize] frameNames_: " << std::endl;
/*
std::cout << "[PointsOnRobot::initialize] frameNames_: " << std::endl;
for (size_t i = 0; i < frameNames_.size(); i++)
{
std::cout << i << " -> " << frameNames_[i] << std::endl;
}
*/

//std::cout << "[PointsOnRobot::initialize] DEBUG INF" << std::endl;
//while(1);
Expand Down Expand Up @@ -418,7 +420,7 @@ Eigen::Matrix<ocs2::scalar_t, 3, 1> PointsOnRobot::QuaternionToEuler(Eigen::Quat
Eigen::Matrix<ocs2::scalar_t, 3, 1> euler;
euler = quat.toRotationMatrix().eulerAngles(3,2,1);

std::cout << "[PointsOnRobot::QuaternionToEuler] Euler from quaternion in yaw, pitch, roll"<< std::endl << euler << std::endl;
//std::cout << "[PointsOnRobot::QuaternionToEuler] Euler from quaternion in yaw, pitch, roll"<< std::endl << euler << std::endl;

return euler;
}
Expand Down
Loading

0 comments on commit a2a6955

Please sign in to comment.