diff --git a/benchmark/unicycle.cpp b/benchmark/unicycle.cpp index 0556af1c692ddb201b347b9d0ba4b2f734bc934f..87bc5c2cb06922d64c5dda81f31e1e54cfb3dd68 100644 --- a/benchmark/unicycle.cpp +++ b/benchmark/unicycle.cpp @@ -1,17 +1,16 @@ #include "crocoddyl/core/actions/unicycle.hpp" #include "crocoddyl/core/utils/callbacks.hpp" #include "crocoddyl/core/solvers/ddp.hpp" -#include<time.h> +#include <time.h> #ifdef WITH_MULTITHREADING -#include<omp.h> -#endif //WITH_MULTITHREADING - +#include <omp.h> +#endif // WITH_MULTITHREADING int main() { bool CALLBACKS = false; unsigned int N = 200; // number of nodes - unsigned int T = 10; // number of trials + unsigned int T = 10; // number of trials unsigned int MAXITER = 1; using namespace crocoddyl; @@ -48,28 +47,30 @@ int main() { clock_gettime(CLOCK_MONOTONIC, &start); ddp.solve(xs, us, MAXITER); clock_gettime(CLOCK_MONOTONIC, &finish); - elapsed = (finish.tv_sec - start.tv_sec)*1000000.0; - elapsed += (finish.tv_nsec - start.tv_nsec)/ 1000.0; + elapsed = (finish.tv_sec - start.tv_sec) * 1000000.0; + elapsed += (finish.tv_nsec - start.tv_nsec) / 1000.0; - duration[i] = elapsed; //in us + duration[i] = elapsed; // in us } double avrg_duration = duration.sum() / T; double min_duration = duration.minCoeff(); double max_duration = duration.maxCoeff(); - std::cout << "Wall time solve [us]: " << avrg_duration << " (" << min_duration << "-" << max_duration << ")" << std::endl; + std::cout << "Wall time solve [us]: " << avrg_duration << " (" << min_duration << "-" << max_duration << ")" + << std::endl; for (unsigned int i = 0; i < T; ++i) { clock_gettime(CLOCK_MONOTONIC, &start); problem.calcDiff(xs, us); clock_gettime(CLOCK_MONOTONIC, &finish); - elapsed = (finish.tv_sec - start.tv_sec)*1000000.0; - elapsed += (finish.tv_nsec - start.tv_nsec)/ 1000.0; - duration[i] = elapsed; //in us + elapsed = (finish.tv_sec - start.tv_sec) * 1000000.0; + elapsed += (finish.tv_nsec - start.tv_nsec) / 1000.0; + duration[i] = elapsed; // in us } avrg_duration = duration.sum() / T; min_duration = duration.minCoeff(); max_duration = duration.maxCoeff(); - std::cout << "Wall time calcDiff [us]: " << avrg_duration << " (" << min_duration << "-" << max_duration << ")" << std::endl; + std::cout << "Wall time calcDiff [us]: " << avrg_duration << " (" << min_duration << "-" << max_duration << ")" + << std::endl; } diff --git a/src/core/optctrl/shooting.cpp b/src/core/optctrl/shooting.cpp index a7080835e2991d14a692fb1f5866447ef57131af..aaf79b1567b17316d8e20721da6511e2ef2701da 100644 --- a/src/core/optctrl/shooting.cpp +++ b/src/core/optctrl/shooting.cpp @@ -11,12 +11,11 @@ #ifdef WITH_MULTITHREADING #include <omp.h> #define NUM_THREADS 4 -#endif //WITH_MULTITHREADING +#endif // WITH_MULTITHREADING namespace crocoddyl { -ShootingProblem::ShootingProblem(const Eigen::VectorXd& x0, - const std::vector<ActionModelAbstract*>& running_models, +ShootingProblem::ShootingProblem(const Eigen::VectorXd& x0, const std::vector<ActionModelAbstract*>& running_models, ActionModelAbstract* const terminal_model) : terminal_model_(terminal_model), running_models_(running_models), @@ -66,7 +65,7 @@ double ShootingProblem::calcDiff(const std::vector<Eigen::VectorXd>& xs, const s for (unsigned int i = 0; i < T_; ++i) { cost_ += running_datas_[i]->cost; } - + terminal_model_->calcDiff(terminal_data_, xs.back()); cost_ += terminal_data_->cost; return cost_;