Unverified Commit a66dbbf3 authored by Carlos Mastalli's avatar Carlos Mastalli Committed by GitHub
Browse files

Merge pull request #1021 from wxmerkt/topic/reduce-temporary-allocations

Reduce temporary allocations
parents 8c5b40ea f02b3725
Pipeline #17276 passed with stage
in 261 minutes and 57 seconds
......@@ -87,9 +87,13 @@ struct ActionDataLQRTpl : public ActionDataAbstractTpl<_Scalar> {
typedef _Scalar Scalar;
typedef MathBaseTpl<Scalar> MathBase;
typedef ActionDataAbstractTpl<Scalar> Base;
typedef typename MathBase::VectorXs VectorXs;
template <template <typename Scalar> class Model>
explicit ActionDataLQRTpl(Model<Scalar>* const model) : Base(model) {
explicit ActionDataLQRTpl(Model<Scalar>* const model)
: Base(model),
Luu_u_tmp(VectorXs::Zero(static_cast<Eigen::Index>(model->get_nu()))),
Lxx_x_tmp(VectorXs::Zero(static_cast<Eigen::Index>(model->get_state()->get_ndx()))) {
// Setting the linear model and quadratic cost here because they are constant
Fx = model->get_Fx();
Fu = model->get_Fu();
......@@ -108,6 +112,8 @@ struct ActionDataLQRTpl : public ActionDataAbstractTpl<_Scalar> {
using Base::Lxx;
using Base::r;
using Base::xnext;
VectorXs Luu_u_tmp; // Temporary variable for storing Hessian-vector product (size: nu)
VectorXs Lxx_x_tmp; // Temporary variable for storing Hessian-vector product (size: nx)
};
} // namespace crocoddyl
......
......@@ -38,6 +38,7 @@ void ActionModelLQRTpl<Scalar>::calc(const boost::shared_ptr<ActionDataAbstract>
throw_pretty("Invalid argument: "
<< "u has wrong dimension (it should be " + std::to_string(nu_) + ")");
}
Data* d = static_cast<Data*>(data.get());
if (drift_free_) {
data->xnext.noalias() = Fx_ * x;
......@@ -47,8 +48,16 @@ void ActionModelLQRTpl<Scalar>::calc(const boost::shared_ptr<ActionDataAbstract>
data->xnext.noalias() += Fu_ * u;
data->xnext += f0_;
}
data->cost =
Scalar(0.5) * x.dot(Lxx_ * x) + Scalar(0.5) * u.dot(Luu_ * u) + x.dot(Lxu_ * u) + lx_.dot(x) + lu_.dot(u);
// cost = 0.5 * x^T*Lxx*x + 0.5 * u^T*Luu*u + x^T*Lxu*u + lx^T*x + lu^T*u
d->Lxx_x_tmp.noalias() = Lxx_ * x;
data->cost = Scalar(0.5) * x.dot(d->Lxx_x_tmp);
d->Luu_u_tmp.noalias() = Luu_ * u;
data->cost += Scalar(0.5) * u.dot(d->Luu_u_tmp);
d->Lxx_x_tmp.noalias() = Lxu_ * u;
data->cost += x.dot(d->Lxx_x_tmp);
data->cost += lx_.transpose() * x;
data->cost += lu_.transpose() * u;
}
template <typename Scalar>
......@@ -58,8 +67,12 @@ void ActionModelLQRTpl<Scalar>::calc(const boost::shared_ptr<ActionDataAbstract>
throw_pretty("Invalid argument: "
<< "x has wrong dimension (it should be " + std::to_string(state_->get_nx()) + ")");
}
Data* d = static_cast<Data*>(data.get());
data->cost = Scalar(0.5) * x.dot(Lxx_ * x) + lx_.dot(x);
// cost = 0.5 * x^T*Lxx*x + lx^T*x
d->Lxx_x_tmp.noalias() = Lxx_ * x;
data->cost = Scalar(0.5) * x.dot(d->Lxx_x_tmp);
data->cost += lx_.dot(x);
}
template <typename Scalar>
......@@ -101,7 +114,7 @@ void ActionModelLQRTpl<Scalar>::calcDiff(const boost::shared_ptr<ActionDataAbstr
}
template <typename Scalar>
boost::shared_ptr<ActionDataAbstractTpl<Scalar> > ActionModelLQRTpl<Scalar>::createData() {
boost::shared_ptr<ActionDataAbstractTpl<Scalar>> ActionModelLQRTpl<Scalar>::createData() {
return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
}
......
......@@ -60,12 +60,12 @@ void CostModelResidualTpl<Scalar>::calcDiff(const boost::shared_ptr<CostDataAbst
const std::size_t nv = state_->get_nv();
if (is_ru) {
data->Lu.noalias() = data->residual->Ru.transpose() * data->activation->Ar;
d->Arr_Ru.noalias() = data->activation->Arr * data->residual->Ru;
d->Arr_Ru.noalias() = data->activation->Arr.diagonal().asDiagonal() * data->residual->Ru;
data->Luu.noalias() = data->residual->Ru.transpose() * d->Arr_Ru;
}
if (is_rq && is_rv) {
data->Lx.noalias() = data->residual->Rx.transpose() * data->activation->Ar;
d->Arr_Rx.noalias() = data->activation->Arr * data->residual->Rx;
d->Arr_Rx.noalias() = data->activation->Arr.diagonal().asDiagonal() * data->residual->Rx;
data->Lxx.noalias() = data->residual->Rx.transpose() * d->Arr_Rx;
if (is_ru) {
data->Lxu.noalias() = data->residual->Rx.transpose() * d->Arr_Ru;
......@@ -73,7 +73,7 @@ void CostModelResidualTpl<Scalar>::calcDiff(const boost::shared_ptr<CostDataAbst
} else if (is_rq) {
Eigen::Block<MatrixXs, Eigen::Dynamic, Eigen::Dynamic, true> Rq = data->residual->Rx.leftCols(nv);
data->Lx.head(nv).noalias() = Rq.transpose() * data->activation->Ar;
d->Arr_Rx.leftCols(nv).noalias() = data->activation->Arr * Rq;
d->Arr_Rx.leftCols(nv).noalias() = data->activation->Arr.diagonal().asDiagonal() * Rq;
data->Lxx.topLeftCorner(nv, nv).noalias() = Rq.transpose() * d->Arr_Rx.leftCols(nv);
if (is_ru) {
data->Lxu.topRows(nv).noalias() = Rq.transpose() * d->Arr_Ru;
......@@ -81,7 +81,7 @@ void CostModelResidualTpl<Scalar>::calcDiff(const boost::shared_ptr<CostDataAbst
} else if (is_rv) {
Eigen::Block<MatrixXs, Eigen::Dynamic, Eigen::Dynamic, true> Rv = data->residual->Rx.rightCols(nv);
data->Lx.tail(nv).noalias() = Rv.transpose() * data->activation->Ar;
d->Arr_Rx.rightCols(nv).noalias() = data->activation->Arr * Rv;
d->Arr_Rx.rightCols(nv).noalias() = data->activation->Arr.diagonal().asDiagonal() * Rv;
data->Lxx.bottomRightCorner(nv, nv).noalias() = Rv.transpose() * d->Arr_Rx.rightCols(nv);
if (is_ru) {
data->Lxu.bottomRows(nv).noalias() = Rv.transpose() * d->Arr_Ru;
......@@ -103,17 +103,17 @@ void CostModelResidualTpl<Scalar>::calcDiff(const boost::shared_ptr<CostDataAbst
const std::size_t nv = state_->get_nv();
if (is_rq && is_rv) {
data->Lx.noalias() = data->residual->Rx.transpose() * data->activation->Ar;
d->Arr_Rx.noalias() = data->activation->Arr * data->residual->Rx;
d->Arr_Rx.noalias() = data->activation->Arr.diagonal().asDiagonal() * data->residual->Rx;
data->Lxx.noalias() = data->residual->Rx.transpose() * d->Arr_Rx;
} else if (is_rq) {
Eigen::Block<MatrixXs, Eigen::Dynamic, Eigen::Dynamic, true> Rq = data->residual->Rx.leftCols(nv);
data->Lx.head(nv).noalias() = Rq.transpose() * data->activation->Ar;
d->Arr_Rx.leftCols(nv).noalias() = data->activation->Arr * Rq;
d->Arr_Rx.leftCols(nv).noalias() = data->activation->Arr.diagonal().asDiagonal() * Rq;
data->Lxx.topLeftCorner(nv, nv).noalias() = Rq.transpose() * d->Arr_Rx.leftCols(nv);
} else if (is_rv) {
Eigen::Block<MatrixXs, Eigen::Dynamic, Eigen::Dynamic, true> Rv = data->residual->Rx.rightCols(nv);
data->Lx.tail(nv).noalias() = Rv.transpose() * data->activation->Ar;
d->Arr_Rx.rightCols(nv).noalias() = data->activation->Arr * Rv;
d->Arr_Rx.rightCols(nv).noalias() = data->activation->Arr.diagonal().asDiagonal() * Rv;
data->Lxx.bottomRightCorner(nv, nv).noalias() = Rv.transpose() * d->Arr_Rx.rightCols(nv);
}
}
......
......@@ -187,7 +187,7 @@ double SolverKKT::calcDiff() {
std::size_t ix = 0;
std::size_t iu = 0;
const std::size_t T = problem_->get_T();
kkt_.block(ndx_ + nu_, 0, ndx_, ndx_) = Eigen::MatrixXd::Identity(ndx_, ndx_);
kkt_.block(ndx_ + nu_, 0, ndx_, ndx_).setIdentity();
for (std::size_t t = 0; t < T; ++t) {
const boost::shared_ptr<ActionModelAbstract>& m = problem_->get_runningModels()[t];
const boost::shared_ptr<ActionDataAbstract>& d = problem_->get_runningDatas()[t];
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment