Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/crocoddyl/core/optctrl/shooting.hxx	Lines:	129	241	53.5 %
Date:	2024-02-13 11:12:33	Branches:	87	748	11.6 %


///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2019-2022, LAAS-CNRS, University of Edinburgh,
//                          University of Oxford, Heriot-Watt University
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////

#include <iostream>
#ifdef CROCODDYL_WITH_MULTITHREADING
#include <omp.h>
#endif  // CROCODDYL_WITH_MULTITHREADING
#include "crocoddyl/core/utils/stop-watch.hpp"

namespace crocoddyl {

template <typename Scalar>
ShootingProblemTpl<Scalar>::ShootingProblemTpl(
    const VectorXs& x0,
    const std::vector<boost::shared_ptr<ActionModelAbstract> >& running_models,
    boost::shared_ptr<ActionModelAbstract> terminal_model)
    : cost_(Scalar(0.)),
      T_(running_models.size()),
      x0_(x0),
      terminal_model_(terminal_model),
      running_models_(running_models),
      nx_(running_models[0]->get_state()->get_nx()),
      ndx_(running_models[0]->get_state()->get_ndx()),
      nu_max_(running_models[0]->get_nu()),
      nthreads_(1),
      is_updated_(false) {
  for (std::size_t i = 1; i < T_; ++i) {
    const boost::shared_ptr<ActionModelAbstract>& model = running_models_[i];
    const std::size_t nu = model->get_nu();
    if (nu_max_ < nu) {
      nu_max_ = nu;
    }
  }
  if (static_cast<std::size_t>(x0.size()) != nx_) {
    throw_pretty("Invalid argument: "
                 << "x0 has wrong dimension (it should be " +
                        std::to_string(nx_) + ")");
  }
  for (std::size_t i = 1; i < T_; ++i) {
    const boost::shared_ptr<ActionModelAbstract>& model = running_models_[i];
    if (model->get_state()->get_nx() != nx_) {
      throw_pretty("Invalid argument: "
                   << "nx in " << i
                   << " node is not consistent with the other nodes")
    }
    if (model->get_state()->get_ndx() != ndx_) {
      throw_pretty("Invalid argument: "
                   << "ndx in " << i
                   << " node is not consistent with the other nodes")
    }
  }
  if (terminal_model_->get_state()->get_nx() != nx_) {
    throw_pretty(
        "Invalid argument: "
        << "nx in terminal node is not consistent with the other nodes")
  }
  if (terminal_model_->get_state()->get_ndx() != ndx_) {
    throw_pretty(
        "Invalid argument: "
        << "ndx in terminal node is not consistent with the other nodes")
  }
  allocateData();

#ifdef CROCODDYL_WITH_MULTITHREADING
  if (enableMultithreading()) {
    nthreads_ = CROCODDYL_WITH_NTHREADS;
  }
#endif
}

template <typename Scalar>
ShootingProblemTpl<Scalar>::ShootingProblemTpl(
    const VectorXs& x0,
    const std::vector<boost::shared_ptr<ActionModelAbstract> >& running_models,
    boost::shared_ptr<ActionModelAbstract> terminal_model,
    const std::vector<boost::shared_ptr<ActionDataAbstract> >& running_datas,
    boost::shared_ptr<ActionDataAbstract> terminal_data)
    : cost_(Scalar(0.)),
      T_(running_models.size()),
      x0_(x0),
      terminal_model_(terminal_model),
      terminal_data_(terminal_data),
      running_models_(running_models),
      running_datas_(running_datas),
      nx_(running_models[0]->get_state()->get_nx()),
      ndx_(running_models[0]->get_state()->get_ndx()),
      nu_max_(running_models[0]->get_nu()),
      nthreads_(1) {
  for (std::size_t i = 1; i < T_; ++i) {
    const boost::shared_ptr<ActionModelAbstract>& model = running_models_[i];
    const std::size_t nu = model->get_nu();
    if (nu_max_ < nu) {
      nu_max_ = nu;
    }
  }
  if (static_cast<std::size_t>(x0.size()) != nx_) {
    throw_pretty("Invalid argument: "
                 << "x0 has wrong dimension (it should be " +
                        std::to_string(nx_) + ")");
  }
  const std::size_t Td = running_datas.size();
  if (Td != T_) {
    throw_pretty(
        "Invalid argument: "
        << "the number of running models and datas are not the same (" +
               std::to_string(T_) + " != " + std::to_string(Td) + ")")
  }
  for (std::size_t i = 0; i < T_; ++i) {
    const boost::shared_ptr<ActionModelAbstract>& model = running_models_[i];
    const boost::shared_ptr<ActionDataAbstract>& data = running_datas_[i];
    if (model->get_state()->get_nx() != nx_) {
      throw_pretty("Invalid argument: "
                   << "nx in " << i
                   << " node is not consistent with the other nodes")
    }
    if (model->get_state()->get_ndx() != ndx_) {
      throw_pretty("Invalid argument: "
                   << "ndx in " << i
                   << " node is not consistent with the other nodes")
    }
    if (!model->checkData(data)) {
      throw_pretty("Invalid argument: "
                   << "action data in " << i
                   << " node is not consistent with the action model")
    }
  }
  if (!terminal_model->checkData(terminal_data)) {
    throw_pretty("Invalid argument: "
                 << "terminal action data is not consistent with the terminal "
                    "action model")
  }

#ifdef CROCODDYL_WITH_MULTITHREADING
  if (enableMultithreading()) {
    nthreads_ = CROCODDYL_WITH_NTHREADS;
  }
#endif
}

template <typename Scalar>
ShootingProblemTpl<Scalar>::ShootingProblemTpl(
    const ShootingProblemTpl<Scalar>& problem)
    : cost_(Scalar(0.)),
      T_(problem.get_T()),
      x0_(problem.get_x0()),
      terminal_model_(problem.get_terminalModel()),
      terminal_data_(problem.get_terminalData()),
      running_models_(problem.get_runningModels()),
      running_datas_(problem.get_runningDatas()),
      nx_(problem.get_nx()),
      ndx_(problem.get_ndx()),
      nu_max_(problem.get_nu_max()) {}

template <typename Scalar>
ShootingProblemTpl<Scalar>::~ShootingProblemTpl() {}

template <typename Scalar>
Scalar ShootingProblemTpl<Scalar>::calc(const std::vector<VectorXs>& xs,
                                        const std::vector<VectorXs>& us) {
  if (xs.size() != T_ + 1) {
    throw_pretty("Invalid argument: "
                 << "xs has wrong dimension (it should be " +
                        std::to_string(T_ + 1) + ")");
  }
  if (us.size() != T_) {
    throw_pretty("Invalid argument: "
                 << "us has wrong dimension (it should be " +
                        std::to_string(T_) + ")");
  }
  START_PROFILER("ShootingProblem::calc");

#ifdef CROCODDYL_WITH_MULTITHREADING
#pragma omp parallel for num_threads(nthreads_)
#endif
  for (std::size_t i = 0; i < T_; ++i) {
    running_models_[i]->calc(running_datas_[i], xs[i], us[i]);
  }
  terminal_model_->calc(terminal_data_, xs.back());

  cost_ = Scalar(0.);
#ifdef CROCODDYL_WITH_MULTITHREADING
#pragma omp simd reduction(+ : cost_)
#endif
  for (std::size_t i = 0; i < T_; ++i) {
    cost_ += running_datas_[i]->cost;
  }
  cost_ += terminal_data_->cost;
  STOP_PROFILER("ShootingProblem::calc");
  return cost_;
}

template <typename Scalar>
Scalar ShootingProblemTpl<Scalar>::calcDiff(const std::vector<VectorXs>& xs,
                                            const std::vector<VectorXs>& us) {
  if (xs.size() != T_ + 1) {
    throw_pretty("Invalid argument: "
                 << "xs has wrong dimension (it should be " +
                        std::to_string(T_ + 1) + ")");
  }
  if (us.size() != T_) {
    throw_pretty("Invalid argument: "
                 << "us has wrong dimension (it should be " +
                        std::to_string(T_) + ")");
  }
  START_PROFILER("ShootingProblem::calcDiff");

#ifdef CROCODDYL_WITH_MULTITHREADING
#pragma omp parallel for num_threads(nthreads_)
#endif
  for (std::size_t i = 0; i < T_; ++i) {
    running_models_[i]->calcDiff(running_datas_[i], xs[i], us[i]);
  }
  terminal_model_->calcDiff(terminal_data_, xs.back());

  cost_ = Scalar(0.);
#ifdef CROCODDYL_WITH_MULTITHREADING
#pragma omp simd reduction(+ : cost_)
#endif
  for (std::size_t i = 0; i < T_; ++i) {
    cost_ += running_datas_[i]->cost;
  }
  cost_ += terminal_data_->cost;

  STOP_PROFILER("ShootingProblem::calcDiff");
  return cost_;
}

template <typename Scalar>
void ShootingProblemTpl<Scalar>::rollout(const std::vector<VectorXs>& us,
                                         std::vector<VectorXs>& xs) {
  if (xs.size() != T_ + 1) {
    throw_pretty("Invalid argument: "
                 << "xs has wrong dimension (it should be " +
                        std::to_string(T_ + 1) + ")");
  }
  if (us.size() != T_) {
    throw_pretty("Invalid argument: "
                 << "us has wrong dimension (it should be " +
                        std::to_string(T_) + ")");
  }
  START_PROFILER("ShootingProblem::rollout");

  xs[0] = x0_;
  for (std::size_t i = 0; i < T_; ++i) {
    const boost::shared_ptr<ActionDataAbstract>& data = running_datas_[i];
    running_models_[i]->calc(data, xs[i], us[i]);
    xs[i + 1] = data->xnext;
  }
  terminal_model_->calc(terminal_data_, xs.back());
  STOP_PROFILER("ShootingProblem::rollout");
}

template <typename Scalar>
std::vector<typename MathBaseTpl<Scalar>::VectorXs>
ShootingProblemTpl<Scalar>::rollout_us(const std::vector<VectorXs>& us) {
  std::vector<VectorXs> xs;
  xs.resize(T_ + 1);
  rollout(us, xs);
  return xs;
}

template <typename Scalar>
void ShootingProblemTpl<Scalar>::quasiStatic(std::vector<VectorXs>& us,
                                             const std::vector<VectorXs>& xs) {
  if (xs.size() != T_) {
    throw_pretty("Invalid argument: "
                 << "xs has wrong dimension (it should be " +
                        std::to_string(T_) + ")");
  }
  if (us.size() != T_) {
    throw_pretty("Invalid argument: "
                 << "us has wrong dimension (it should be " +
                        std::to_string(T_) + ")");
  }

#ifdef CROCODDYL_WITH_MULTITHREADING
#pragma omp parallel for num_threads(nthreads_)
#endif
  for (std::size_t i = 0; i < T_; ++i) {
    running_models_[i]->quasiStatic(running_datas_[i], us[i], xs[i]);
  }
}

template <typename Scalar>
std::vector<typename MathBaseTpl<Scalar>::VectorXs>
ShootingProblemTpl<Scalar>::quasiStatic_xs(const std::vector<VectorXs>& xs) {
  std::vector<VectorXs> us;
  us.resize(T_);
  for (std::size_t i = 0; i < T_; ++i) {
    us[i] = VectorXs::Zero(running_models_[i]->get_nu());
  }
  quasiStatic(us, xs);
  return us;
}

template <typename Scalar>
void ShootingProblemTpl<Scalar>::circularAppend(
    boost::shared_ptr<ActionModelAbstract> model,
    boost::shared_ptr<ActionDataAbstract> data) {
  if (!model->checkData(data)) {
    throw_pretty("Invalid argument: "
                 << "action data is not consistent with the action model")
  }
  if (model->get_state()->get_nx() != nx_) {
    throw_pretty("Invalid argument: "
                 << "nx is not consistent with the other nodes")
  }
  if (model->get_state()->get_ndx() != ndx_) {
    throw_pretty("Invalid argument: "
                 << "ndx node is not consistent with the other nodes")
  }
  is_updated_ = true;
  for (std::size_t i = 0; i < T_ - 1; ++i) {
    running_models_[i] = running_models_[i + 1];
    running_datas_[i] = running_datas_[i + 1];
  }
  running_models_.back() = model;
  running_datas_.back() = data;
}

template <typename Scalar>
void ShootingProblemTpl<Scalar>::circularAppend(
    boost::shared_ptr<ActionModelAbstract> model) {
  if (model->get_state()->get_nx() != nx_) {
    throw_pretty("Invalid argument: "
                 << "nx is not consistent with the other nodes")
  }
  if (model->get_state()->get_ndx() != ndx_) {
    throw_pretty("Invalid argument: "
                 << "ndx node is not consistent with the other nodes")
  }
  is_updated_ = true;
  for (std::size_t i = 0; i < T_ - 1; ++i) {
    running_models_[i] = running_models_[i + 1];
    running_datas_[i] = running_datas_[i + 1];
  }
  running_models_.back() = model;
  running_datas_.back() = model->createData();
}

template <typename Scalar>
void ShootingProblemTpl<Scalar>::updateNode(
    const std::size_t i, boost::shared_ptr<ActionModelAbstract> model,
    boost::shared_ptr<ActionDataAbstract> data) {
  if (i >= T_ + 1) {
    throw_pretty("Invalid argument: "
                 << "i is bigger than the allocated horizon (it should be less "
                    "than or equal to " +
                        std::to_string(T_ + 1) + ")");
  }
  if (!model->checkData(data)) {
    throw_pretty("Invalid argument: "
                 << "action data is not consistent with the action model")
  }
  if (model->get_state()->get_nx() != nx_) {
    throw_pretty("Invalid argument: "
                 << "nx is not consistent with the other nodes")
  }
  if (model->get_state()->get_ndx() != ndx_) {
    throw_pretty("Invalid argument: "
                 << "ndx node is not consistent with the other nodes")
  }
  is_updated_ = true;
  if (i == T_) {
    terminal_model_ = model;
    terminal_data_ = data;
  } else {
    running_models_[i] = model;
    running_datas_[i] = data;
  }
}

template <typename Scalar>
void ShootingProblemTpl<Scalar>::updateModel(
    const std::size_t i, boost::shared_ptr<ActionModelAbstract> model) {
  if (i >= T_ + 1) {
    throw_pretty(
        "Invalid argument: "
        << "i is bigger than the allocated horizon (it should be lower than " +
               std::to_string(T_ + 1) + ")");
  }
  if (model->get_state()->get_nx() != nx_) {
    throw_pretty("Invalid argument: "
                 << "nx is not consistent with the other nodes")
  }
  if (model->get_state()->get_ndx() != ndx_) {
    throw_pretty("Invalid argument: "
                 << "ndx is not consistent with the other nodes")
  }
  is_updated_ = true;
  if (i == T_) {
    terminal_model_ = model;
    terminal_data_ = terminal_model_->createData();
  } else {
    running_models_[i] = model;
    running_datas_[i] = model->createData();
  }
}

template <typename Scalar>
std::size_t ShootingProblemTpl<Scalar>::get_T() const {
  return T_;
}

template <typename Scalar>
const typename MathBaseTpl<Scalar>::VectorXs&
ShootingProblemTpl<Scalar>::get_x0() const {
  return x0_;
}

template <typename Scalar>
void ShootingProblemTpl<Scalar>::allocateData() {
  running_datas_.resize(T_);
  for (std::size_t i = 0; i < T_; ++i) {
    const boost::shared_ptr<ActionModelAbstract>& model = running_models_[i];
    running_datas_[i] = model->createData();
  }
  terminal_data_ = terminal_model_->createData();
}

template <typename Scalar>
const std::vector<
    boost::shared_ptr<crocoddyl::ActionModelAbstractTpl<Scalar> > >&
ShootingProblemTpl<Scalar>::get_runningModels() const {
  return running_models_;
}

template <typename Scalar>
const boost::shared_ptr<crocoddyl::ActionModelAbstractTpl<Scalar> >&
ShootingProblemTpl<Scalar>::get_terminalModel() const {
  return terminal_model_;
}

template <typename Scalar>
const std::vector<
    boost::shared_ptr<crocoddyl::ActionDataAbstractTpl<Scalar> > >&
ShootingProblemTpl<Scalar>::get_runningDatas() const {
  return running_datas_;
}

template <typename Scalar>
const boost::shared_ptr<crocoddyl::ActionDataAbstractTpl<Scalar> >&
ShootingProblemTpl<Scalar>::get_terminalData() const {
  return terminal_data_;
}

template <typename Scalar>
void ShootingProblemTpl<Scalar>::set_x0(const VectorXs& x0_in) {
  if (x0_in.size() != x0_.size()) {
    throw_pretty("Invalid argument: "
                 << "invalid size of x0 provided: Expected " << x0_.size()
                 << ", received " << x0_in.size());
  }
  x0_ = x0_in;
}

template <typename Scalar>
void ShootingProblemTpl<Scalar>::set_runningModels(
    const std::vector<boost::shared_ptr<ActionModelAbstract> >& models) {
  for (std::size_t i = 0; i < T_; ++i) {
    const boost::shared_ptr<ActionModelAbstract>& model = running_models_[i];
    if (model->get_state()->get_nx() != nx_) {
      throw_pretty("Invalid argument: "
                   << "nx in " << i
                   << " node is not consistent with the other nodes")
    }
    if (model->get_state()->get_ndx() != ndx_) {
      throw_pretty("Invalid argument: "
                   << "ndx in " << i
                   << " node is not consistent with the other nodes")
    }
  }
  is_updated_ = true;
  T_ = models.size();
  running_models_.clear();
  running_datas_.clear();
  for (std::size_t i = 0; i < T_; ++i) {
    const boost::shared_ptr<ActionModelAbstract>& model = running_models_[i];
    running_datas_.push_back(model->createData());
  }
}

template <typename Scalar>
void ShootingProblemTpl<Scalar>::set_terminalModel(
    boost::shared_ptr<ActionModelAbstract> model) {
  if (model->get_state()->get_nx() != nx_) {
    throw_pretty("Invalid argument: "
                 << "nx is not consistent with the other nodes")
  }
  if (model->get_state()->get_ndx() != ndx_) {
    throw_pretty("Invalid argument: "
                 << "ndx is not consistent with the other nodes")
  }
  is_updated_ = true;
  terminal_model_ = model;
  terminal_data_ = terminal_model_->createData();
}

template <typename Scalar>
void ShootingProblemTpl<Scalar>::set_nthreads(const int nthreads) {
#ifndef CROCODDYL_WITH_MULTITHREADING
  (void)nthreads;
  std::cerr << "Warning: the number of threads won't affect the computational "
               "performance as multithreading "
               "support is not enabled."
            << std::endl;
#else
  if (nthreads < 1) {
    nthreads_ = CROCODDYL_WITH_NTHREADS;
  } else {
    nthreads_ = static_cast<std::size_t>(nthreads);
  }
  if (!enableMultithreading()) {
    std::cerr << "Warning: the number of threads won't affect the "
                 "computational performance as multithreading "
                 "support is not enabled."
              << std::endl;
    nthreads_ = 1;
  }
#endif
}

template <typename Scalar>
std::size_t ShootingProblemTpl<Scalar>::get_nx() const {
  return nx_;
}

template <typename Scalar>
std::size_t ShootingProblemTpl<Scalar>::get_ndx() const {
  return ndx_;
}

template <typename Scalar>
std::size_t ShootingProblemTpl<Scalar>::get_nu_max() const {
  return nu_max_;
}

template <typename Scalar>
std::size_t ShootingProblemTpl<Scalar>::get_nthreads() const {
#ifndef CROCODDYL_WITH_MULTITHREADING
  std::cerr << "Warning: the number of threads won't affect the computational "
               "performance as multithreading "
               "support is not enabled."
            << std::endl;
#endif
  return nthreads_;
}

template <typename Scalar>
bool ShootingProblemTpl<Scalar>::is_updated() {
  const bool status = is_updated_;
  is_updated_ = false;
  return status;
}

template <typename Scalar>
std::ostream& operator<<(std::ostream& os,
                         const ShootingProblemTpl<Scalar>& problem) {
  os << "ShootingProblem (T=" << problem.get_T() << ", nx=" << problem.get_nx()
     << ", ndx=" << problem.get_ndx() << ") " << std::endl
     << "  Models:" << std::endl;
  const std::vector<
      boost::shared_ptr<crocoddyl::ActionModelAbstractTpl<Scalar> > >&
      runningModels = problem.get_runningModels();
  for (std::size_t t = 0; t < problem.get_T(); ++t) {
    os << "    " << t << ": " << *runningModels[t] << std::endl;
  }
  os << "    " << problem.get_T() << ": " << *problem.get_terminalModel();
  return os;
}

}  // namespace crocoddyl


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			///////////////////////////////////////////////////////////////////////////////
2			// BSD 3-Clause License
3			//
4			// Copyright (C) 2019-2022, LAAS-CNRS, University of Edinburgh,
5			// University of Oxford, Heriot-Watt University
6			// Copyright note valid unless otherwise stated in individual files.
7			// All rights reserved.
8			///////////////////////////////////////////////////////////////////////////////
9
10			#include <iostream>
11			#ifdef CROCODDYL_WITH_MULTITHREADING
12			#include <omp.h>
13			#endif // CROCODDYL_WITH_MULTITHREADING
14			#include "crocoddyl/core/utils/stop-watch.hpp"
15
16			namespace crocoddyl {
17
18			template <typename Scalar>
19		477	ShootingProblemTpl<Scalar>::ShootingProblemTpl(
20			const VectorXs& x0,
21			const std::vector<boost::shared_ptr<ActionModelAbstract> >& running_models,
22			boost::shared_ptr<ActionModelAbstract> terminal_model)
23			: cost_(Scalar(0.)),
24			T_(running_models.size()),
25			x0_(x0),
26			terminal_model_(terminal_model),
27			running_models_(running_models),
28		477	nx_(running_models[0]->get_state()->get_nx()),
29		477	ndx_(running_models[0]->get_state()->get_ndx()),
30		477	nu_max_(running_models[0]->get_nu()),
31			nthreads_(1),
32	✓✗	1431	is_updated_(false) {
33	✓✓	9246	for (std::size_t i = 1; i < T_; ++i) {
34		8769	const boost::shared_ptr<ActionModelAbstract>& model = running_models_[i];
35		8769	const std::size_t nu = model->get_nu();
36	✓✓	8769	if (nu_max_ < nu) {
37		24	nu_max_ = nu;
38			}
39			}
40	✓✗✗✓	477	if (static_cast<std::size_t>(x0.size()) != nx_) {
41			throw_pretty("Invalid argument: "
42			<< "x0 has wrong dimension (it should be " +
43			std::to_string(nx_) + ")");
44			}
45	✓✓	9246	for (std::size_t i = 1; i < T_; ++i) {
46		8769	const boost::shared_ptr<ActionModelAbstract>& model = running_models_[i];
47	✗✓	8769	if (model->get_state()->get_nx() != nx_) {
48			throw_pretty("Invalid argument: "
49			<< "nx in " << i
50			<< " node is not consistent with the other nodes")
51			}
52	✗✓	8769	if (model->get_state()->get_ndx() != ndx_) {
53			throw_pretty("Invalid argument: "
54			<< "ndx in " << i
55			<< " node is not consistent with the other nodes")
56			}
57			}
58	✗✓	477	if (terminal_model_->get_state()->get_nx() != nx_) {
59			throw_pretty(
60			"Invalid argument: "
61			<< "nx in terminal node is not consistent with the other nodes")
62			}
63	✗✓	477	if (terminal_model_->get_state()->get_ndx() != ndx_) {
64			throw_pretty(
65			"Invalid argument: "
66			<< "ndx in terminal node is not consistent with the other nodes")
67			}
68	✓✗	477	allocateData();
69
70			#ifdef CROCODDYL_WITH_MULTITHREADING
71			if (enableMultithreading()) {
72			nthreads_ = CROCODDYL_WITH_NTHREADS;
73			}
74			#endif
75		477	}
76
77			template <typename Scalar>
78		279	ShootingProblemTpl<Scalar>::ShootingProblemTpl(
79			const VectorXs& x0,
80			const std::vector<boost::shared_ptr<ActionModelAbstract> >& running_models,
81			boost::shared_ptr<ActionModelAbstract> terminal_model,
82			const std::vector<boost::shared_ptr<ActionDataAbstract> >& running_datas,
83			boost::shared_ptr<ActionDataAbstract> terminal_data)
84			: cost_(Scalar(0.)),
85			T_(running_models.size()),
86			x0_(x0),
87			terminal_model_(terminal_model),
88			terminal_data_(terminal_data),
89			running_models_(running_models),
90			running_datas_(running_datas),
91		279	nx_(running_models[0]->get_state()->get_nx()),
92		279	ndx_(running_models[0]->get_state()->get_ndx()),
93		279	nu_max_(running_models[0]->get_nu()),
94	✓✗✓✗	837	nthreads_(1) {
95	✓✓	5580	for (std::size_t i = 1; i < T_; ++i) {
96		5301	const boost::shared_ptr<ActionModelAbstract>& model = running_models_[i];
97		5301	const std::size_t nu = model->get_nu();
98	✗✓	5301	if (nu_max_ < nu) {
99			nu_max_ = nu;
100			}
101			}
102	✓✗✗✓	279	if (static_cast<std::size_t>(x0.size()) != nx_) {
103			throw_pretty("Invalid argument: "
104			<< "x0 has wrong dimension (it should be " +
105			std::to_string(nx_) + ")");
106			}
107		279	const std::size_t Td = running_datas.size();
108	✗✓	279	if (Td != T_) {
109			throw_pretty(
110			"Invalid argument: "
111			<< "the number of running models and datas are not the same (" +
112			std::to_string(T_) + " != " + std::to_string(Td) + ")")
113			}
114	✓✓	5859	for (std::size_t i = 0; i < T_; ++i) {
115		5580	const boost::shared_ptr<ActionModelAbstract>& model = running_models_[i];
116		5580	const boost::shared_ptr<ActionDataAbstract>& data = running_datas_[i];
117	✗✓	5580	if (model->get_state()->get_nx() != nx_) {
118			throw_pretty("Invalid argument: "
119			<< "nx in " << i
120			<< " node is not consistent with the other nodes")
121			}
122	✗✓	5580	if (model->get_state()->get_ndx() != ndx_) {
123			throw_pretty("Invalid argument: "
124			<< "ndx in " << i
125			<< " node is not consistent with the other nodes")
126			}
127	✓✗✗✓	5580	if (!model->checkData(data)) {
128			throw_pretty("Invalid argument: "
129			<< "action data in " << i
130			<< " node is not consistent with the action model")
131			}
132			}
133	✓✗✗✓	279	if (!terminal_model->checkData(terminal_data)) {
134			throw_pretty("Invalid argument: "
135			<< "terminal action data is not consistent with the terminal "
136			"action model")
137			}
138
139			#ifdef CROCODDYL_WITH_MULTITHREADING
140			if (enableMultithreading()) {
141			nthreads_ = CROCODDYL_WITH_NTHREADS;
142			}
143			#endif
144		279	}
145
146			template <typename Scalar>
147		2	ShootingProblemTpl<Scalar>::ShootingProblemTpl(
148			const ShootingProblemTpl<Scalar>& problem)
149			: cost_(Scalar(0.)),
150			T_(problem.get_T()),
151			x0_(problem.get_x0()),
152			terminal_model_(problem.get_terminalModel()),
153			terminal_data_(problem.get_terminalData()),
154			running_models_(problem.get_runningModels()),
155			running_datas_(problem.get_runningDatas()),
156			nx_(problem.get_nx()),
157			ndx_(problem.get_ndx()),
158	✓✗✓✗	2	nu_max_(problem.get_nu_max()) {}
159
160			template <typename Scalar>
161		758	ShootingProblemTpl<Scalar>::~ShootingProblemTpl() {}
162
163			template <typename Scalar>
164		545	Scalar ShootingProblemTpl<Scalar>::calc(const std::vector<VectorXs>& xs,
165			const std::vector<VectorXs>& us) {
166	✗✓	545	if (xs.size() != T_ + 1) {
167			throw_pretty("Invalid argument: "
168			<< "xs has wrong dimension (it should be " +
169			std::to_string(T_ + 1) + ")");
170			}
171	✗✓	545	if (us.size() != T_) {
172			throw_pretty("Invalid argument: "
173			<< "us has wrong dimension (it should be " +
174			std::to_string(T_) + ")");
175			}
176	✗✓✗✗ ✗✗✗✗ ✗✓✗✓ ✗✗✗✗	545	START_PROFILER("ShootingProblem::calc");
177
178			#ifdef CROCODDYL_WITH_MULTITHREADING
179			#pragma omp parallel for num_threads(nthreads_)
180			#endif
181	✓✓	10068	for (std::size_t i = 0; i < T_; ++i) {
182	✓✗✓✗	9523	running_models_[i]->calc(running_datas_[i], xs[i], us[i]);
183			}
184	✓✗	545	terminal_model_->calc(terminal_data_, xs.back());
185
186		545	cost_ = Scalar(0.);
187			#ifdef CROCODDYL_WITH_MULTITHREADING
188			#pragma omp simd reduction(+ : cost_)
189			#endif
190	✓✓	10068	for (std::size_t i = 0; i < T_; ++i) {
191		9523	cost_ += running_datas_[i]->cost;
192			}
193		545	cost_ += terminal_data_->cost;
194	✗✓✗✗ ✗✗✗✗ ✗✓✗✓ ✗✗✗✗	545	STOP_PROFILER("ShootingProblem::calc");
195		545	return cost_;
196			}
197
198			template <typename Scalar>
199		387	Scalar ShootingProblemTpl<Scalar>::calcDiff(const std::vector<VectorXs>& xs,
200			const std::vector<VectorXs>& us) {
201	✗✓	387	if (xs.size() != T_ + 1) {
202			throw_pretty("Invalid argument: "
203			<< "xs has wrong dimension (it should be " +
204			std::to_string(T_ + 1) + ")");
205			}
206	✗✓	387	if (us.size() != T_) {
207			throw_pretty("Invalid argument: "
208			<< "us has wrong dimension (it should be " +
209			std::to_string(T_) + ")");
210			}
211	✗✓✗✗ ✗✗✗✗ ✗✓✗✓ ✗✗✗✗	387	START_PROFILER("ShootingProblem::calcDiff");
212
213			#ifdef CROCODDYL_WITH_MULTITHREADING
214			#pragma omp parallel for num_threads(nthreads_)
215			#endif
216	✓✓	6063	for (std::size_t i = 0; i < T_; ++i) {
217	✓✗✓✗	5676	running_models_[i]->calcDiff(running_datas_[i], xs[i], us[i]);
218			}
219	✓✗	387	terminal_model_->calcDiff(terminal_data_, xs.back());
220
221		387	cost_ = Scalar(0.);
222			#ifdef CROCODDYL_WITH_MULTITHREADING
223			#pragma omp simd reduction(+ : cost_)
224			#endif
225	✓✓	6063	for (std::size_t i = 0; i < T_; ++i) {
226		5676	cost_ += running_datas_[i]->cost;
227			}
228		387	cost_ += terminal_data_->cost;
229
230	✗✓✗✗ ✗✗✗✗ ✗✓✗✓ ✗✗✗✗	387	STOP_PROFILER("ShootingProblem::calcDiff");
231		387	return cost_;
232			}
233
234			template <typename Scalar>
235		97	void ShootingProblemTpl<Scalar>::rollout(const std::vector<VectorXs>& us,
236			std::vector<VectorXs>& xs) {
237	✗✓	97	if (xs.size() != T_ + 1) {
238			throw_pretty("Invalid argument: "
239			<< "xs has wrong dimension (it should be " +
240			std::to_string(T_ + 1) + ")");
241			}
242	✗✓	97	if (us.size() != T_) {
243			throw_pretty("Invalid argument: "
244			<< "us has wrong dimension (it should be " +
245			std::to_string(T_) + ")");
246			}
247	✗✓✗✗ ✗✗✗✗ ✗✓✗✓ ✗✗✗✗	97	START_PROFILER("ShootingProblem::rollout");
248
249		97	xs[0] = x0_;
250	✓✓	2139	for (std::size_t i = 0; i < T_; ++i) {
251		2042	const boost::shared_ptr<ActionDataAbstract>& data = running_datas_[i];
252	✓✗✓✗	2042	running_models_[i]->calc(data, xs[i], us[i]);
253		2042	xs[i + 1] = data->xnext;
254			}
255	✓✗	97	terminal_model_->calc(terminal_data_, xs.back());
256	✗✓✗✗ ✗✗✗✗ ✗✓✗✓ ✗✗✗✗	97	STOP_PROFILER("ShootingProblem::rollout");
257		97	}
258
259			template <typename Scalar>
260			std::vector<typename MathBaseTpl<Scalar>::VectorXs>
261		4	ShootingProblemTpl<Scalar>::rollout_us(const std::vector<VectorXs>& us) {
262		4	std::vector<VectorXs> xs;
263	✓✗	4	xs.resize(T_ + 1);
264	✓✗	4	rollout(us, xs);
265		4	return xs;
266			}
267
268			template <typename Scalar>
269		186	void ShootingProblemTpl<Scalar>::quasiStatic(std::vector<VectorXs>& us,
270			const std::vector<VectorXs>& xs) {
271	✗✓	186	if (xs.size() != T_) {
272			throw_pretty("Invalid argument: "
273			<< "xs has wrong dimension (it should be " +
274			std::to_string(T_) + ")");
275			}
276	✗✓	186	if (us.size() != T_) {
277			throw_pretty("Invalid argument: "
278			<< "us has wrong dimension (it should be " +
279			std::to_string(T_) + ")");
280			}
281
282			#ifdef CROCODDYL_WITH_MULTITHREADING
283			#pragma omp parallel for num_threads(nthreads_)
284			#endif
285	✓✓	3906	for (std::size_t i = 0; i < T_; ++i) {
286	✓✗✓✗	3720	running_models_[i]->quasiStatic(running_datas_[i], us[i], xs[i]);
287			}
288		186	}
289
290			template <typename Scalar>
291			std::vector<typename MathBaseTpl<Scalar>::VectorXs>
292			ShootingProblemTpl<Scalar>::quasiStatic_xs(const std::vector<VectorXs>& xs) {
293			std::vector<VectorXs> us;
294			us.resize(T_);
295			for (std::size_t i = 0; i < T_; ++i) {
296			us[i] = VectorXs::Zero(running_models_[i]->get_nu());
297			}
298			quasiStatic(us, xs);
299			return us;
300			}
301
302			template <typename Scalar>
303			void ShootingProblemTpl<Scalar>::circularAppend(
304			boost::shared_ptr<ActionModelAbstract> model,
305			boost::shared_ptr<ActionDataAbstract> data) {
306			if (!model->checkData(data)) {
307			throw_pretty("Invalid argument: "
308			<< "action data is not consistent with the action model")
309			}
310			if (model->get_state()->get_nx() != nx_) {
311			throw_pretty("Invalid argument: "
312			<< "nx is not consistent with the other nodes")
313			}
314			if (model->get_state()->get_ndx() != ndx_) {
315			throw_pretty("Invalid argument: "
316			<< "ndx node is not consistent with the other nodes")
317			}
318			is_updated_ = true;
319			for (std::size_t i = 0; i < T_ - 1; ++i) {
320			running_models_[i] = running_models_[i + 1];
321			running_datas_[i] = running_datas_[i + 1];
322			}
323			running_models_.back() = model;
324			running_datas_.back() = data;
325			}
326
327			template <typename Scalar>
328			void ShootingProblemTpl<Scalar>::circularAppend(
329			boost::shared_ptr<ActionModelAbstract> model) {
330			if (model->get_state()->get_nx() != nx_) {
331			throw_pretty("Invalid argument: "
332			<< "nx is not consistent with the other nodes")
333			}
334			if (model->get_state()->get_ndx() != ndx_) {
335			throw_pretty("Invalid argument: "
336			<< "ndx node is not consistent with the other nodes")
337			}
338			is_updated_ = true;
339			for (std::size_t i = 0; i < T_ - 1; ++i) {
340			running_models_[i] = running_models_[i + 1];
341			running_datas_[i] = running_datas_[i + 1];
342			}
343			running_models_.back() = model;
344			running_datas_.back() = model->createData();
345			}
346
347			template <typename Scalar>
348			void ShootingProblemTpl<Scalar>::updateNode(
349			const std::size_t i, boost::shared_ptr<ActionModelAbstract> model,
350			boost::shared_ptr<ActionDataAbstract> data) {
351			if (i >= T_ + 1) {
352			throw_pretty("Invalid argument: "
353			<< "i is bigger than the allocated horizon (it should be less "
354			"than or equal to " +
355			std::to_string(T_ + 1) + ")");
356			}
357			if (!model->checkData(data)) {
358			throw_pretty("Invalid argument: "
359			<< "action data is not consistent with the action model")
360			}
361			if (model->get_state()->get_nx() != nx_) {
362			throw_pretty("Invalid argument: "
363			<< "nx is not consistent with the other nodes")
364			}
365			if (model->get_state()->get_ndx() != ndx_) {
366			throw_pretty("Invalid argument: "
367			<< "ndx node is not consistent with the other nodes")
368			}
369			is_updated_ = true;
370			if (i == T_) {
371			terminal_model_ = model;
372			terminal_data_ = data;
373			} else {
374			running_models_[i] = model;
375			running_datas_[i] = data;
376			}
377			}
378
379			template <typename Scalar>
380			void ShootingProblemTpl<Scalar>::updateModel(
381			const std::size_t i, boost::shared_ptr<ActionModelAbstract> model) {
382			if (i >= T_ + 1) {
383			throw_pretty(
384			"Invalid argument: "
385			<< "i is bigger than the allocated horizon (it should be lower than " +
386			std::to_string(T_ + 1) + ")");
387			}
388			if (model->get_state()->get_nx() != nx_) {
389			throw_pretty("Invalid argument: "
390			<< "nx is not consistent with the other nodes")
391			}
392			if (model->get_state()->get_ndx() != ndx_) {
393			throw_pretty("Invalid argument: "
394			<< "ndx is not consistent with the other nodes")
395			}
396			is_updated_ = true;
397			if (i == T_) {
398			terminal_model_ = model;
399			terminal_data_ = terminal_model_->createData();
400			} else {
401			running_models_[i] = model;
402			running_datas_[i] = model->createData();
403			}
404			}
405
406			template <typename Scalar>
407		3888	std::size_t ShootingProblemTpl<Scalar>::get_T() const {
408		3888	return T_;
409			}
410
411			template <typename Scalar>
412			const typename MathBaseTpl<Scalar>::VectorXs&
413		371	ShootingProblemTpl<Scalar>::get_x0() const {
414		371	return x0_;
415			}
416
417			template <typename Scalar>
418		477	void ShootingProblemTpl<Scalar>::allocateData() {
419		477	running_datas_.resize(T_);
420	✓✓	9723	for (std::size_t i = 0; i < T_; ++i) {
421		9246	const boost::shared_ptr<ActionModelAbstract>& model = running_models_[i];
422		9246	running_datas_[i] = model->createData();
423			}
424		477	terminal_data_ = terminal_model_->createData();
425		477	}
426
427			template <typename Scalar>
428			const std::vector<
429			boost::shared_ptr<crocoddyl::ActionModelAbstractTpl<Scalar> > >&
430		3575	ShootingProblemTpl<Scalar>::get_runningModels() const {
431		3575	return running_models_;
432			}
433
434			template <typename Scalar>
435			const boost::shared_ptr<crocoddyl::ActionModelAbstractTpl<Scalar> >&
436		1108	ShootingProblemTpl<Scalar>::get_terminalModel() const {
437		1108	return terminal_model_;
438			}
439
440			template <typename Scalar>
441			const std::vector<
442			boost::shared_ptr<crocoddyl::ActionDataAbstractTpl<Scalar> > >&
443		34561	ShootingProblemTpl<Scalar>::get_runningDatas() const {
444		34561	return running_datas_;
445			}
446
447			template <typename Scalar>
448			const boost::shared_ptr<crocoddyl::ActionDataAbstractTpl<Scalar> >&
449		1427	ShootingProblemTpl<Scalar>::get_terminalData() const {
450		1427	return terminal_data_;
451			}
452
453			template <typename Scalar>
454			void ShootingProblemTpl<Scalar>::set_x0(const VectorXs& x0_in) {
455			if (x0_in.size() != x0_.size()) {
456			throw_pretty("Invalid argument: "
457			<< "invalid size of x0 provided: Expected " << x0_.size()
458			<< ", received " << x0_in.size());
459			}
460			x0_ = x0_in;
461			}
462
463			template <typename Scalar>
464			void ShootingProblemTpl<Scalar>::set_runningModels(
465			const std::vector<boost::shared_ptr<ActionModelAbstract> >& models) {
466			for (std::size_t i = 0; i < T_; ++i) {
467			const boost::shared_ptr<ActionModelAbstract>& model = running_models_[i];
468			if (model->get_state()->get_nx() != nx_) {
469			throw_pretty("Invalid argument: "
470			<< "nx in " << i
471			<< " node is not consistent with the other nodes")
472			}
473			if (model->get_state()->get_ndx() != ndx_) {
474			throw_pretty("Invalid argument: "
475			<< "ndx in " << i
476			<< " node is not consistent with the other nodes")
477			}
478			}
479			is_updated_ = true;
480			T_ = models.size();
481			running_models_.clear();
482			running_datas_.clear();
483			for (std::size_t i = 0; i < T_; ++i) {
484			const boost::shared_ptr<ActionModelAbstract>& model = running_models_[i];
485			running_datas_.push_back(model->createData());
486			}
487			}
488
489			template <typename Scalar>
490			void ShootingProblemTpl<Scalar>::set_terminalModel(
491			boost::shared_ptr<ActionModelAbstract> model) {
492			if (model->get_state()->get_nx() != nx_) {
493			throw_pretty("Invalid argument: "
494			<< "nx is not consistent with the other nodes")
495			}
496			if (model->get_state()->get_ndx() != ndx_) {
497			throw_pretty("Invalid argument: "
498			<< "ndx is not consistent with the other nodes")
499			}
500			is_updated_ = true;
501			terminal_model_ = model;
502			terminal_data_ = terminal_model_->createData();
503			}
504
505			template <typename Scalar>
506			void ShootingProblemTpl<Scalar>::set_nthreads(const int nthreads) {
507			#ifndef CROCODDYL_WITH_MULTITHREADING
508			(void)nthreads;
509			std::cerr << "Warning: the number of threads won't affect the computational "
510			"performance as multithreading "
511			"support is not enabled."
512			<< std::endl;
513			#else
514			if (nthreads < 1) {
515			nthreads_ = CROCODDYL_WITH_NTHREADS;
516			} else {
517			nthreads_ = static_cast<std::size_t>(nthreads);
518			}
519			if (!enableMultithreading()) {
520			std::cerr << "Warning: the number of threads won't affect the "
521			"computational performance as multithreading "
522			"support is not enabled."
523			<< std::endl;
524			nthreads_ = 1;
525			}
526			#endif
527			}
528
529			template <typename Scalar>
530		33	std::size_t ShootingProblemTpl<Scalar>::get_nx() const {
531		33	return nx_;
532			}
533
534			template <typename Scalar>
535		178	std::size_t ShootingProblemTpl<Scalar>::get_ndx() const {
536		178	return ndx_;
537			}
538
539			template <typename Scalar>
540		2	std::size_t ShootingProblemTpl<Scalar>::get_nu_max() const {
541		2	return nu_max_;
542			}
543
544			template <typename Scalar>
545			std::size_t ShootingProblemTpl<Scalar>::get_nthreads() const {
546			#ifndef CROCODDYL_WITH_MULTITHREADING
547			std::cerr << "Warning: the number of threads won't affect the computational "
548			"performance as multithreading "
549			"support is not enabled."
550			<< std::endl;
551			#endif
552			return nthreads_;
553			}
554
555			template <typename Scalar>
556		24	bool ShootingProblemTpl<Scalar>::is_updated() {
557		24	const bool status = is_updated_;
558		24	is_updated_ = false;
559		24	return status;
560			}
561
562			template <typename Scalar>
563		5	std::ostream& operator<<(std::ostream& os,
564			const ShootingProblemTpl<Scalar>& problem) {
565		5	os << "ShootingProblem (T=" << problem.get_T() << ", nx=" << problem.get_nx()
566		5	<< ", ndx=" << problem.get_ndx() << ") " << std::endl
567		5	<< " Models:" << std::endl;
568			const std::vector<
569			boost::shared_ptr<crocoddyl::ActionModelAbstractTpl<Scalar> > >&
570		5	runningModels = problem.get_runningModels();
571	✓✓	105	for (std::size_t t = 0; t < problem.get_T(); ++t) {
572		100	os << " " << t << ": " << *runningModels[t] << std::endl;
573			}
574		5	os << " " << problem.get_T() << ": " << *problem.get_terminalModel();
575		5	return os;
576			}
577
578			} // namespace crocoddyl