| Directory: | ./ |
|---|---|
| File: | include/crocoddyl/core/actions/diff-lqr.hxx |
| Date: | 2025-01-16 08:47:40 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 196 | 246 | 79.7% |
| Branches: | 244 | 940 | 26.0% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | /////////////////////////////////////////////////////////////////////////////// | ||
| 2 | // BSD 3-Clause License | ||
| 3 | // | ||
| 4 | // Copyright (C) 2019-2024, LAAS-CNRS, University of Edinburgh, | ||
| 5 | // Heriot-Watt University | ||
| 6 | // Copyright note valid unless otherwise stated in individual files. | ||
| 7 | // All rights reserved. | ||
| 8 | /////////////////////////////////////////////////////////////////////////////// | ||
| 9 | |||
| 10 | #include "crocoddyl/core/actions/diff-lqr.hpp" | ||
| 11 | #include "crocoddyl/core/utils/exception.hpp" | ||
| 12 | |||
| 13 | namespace crocoddyl { | ||
| 14 | |||
| 15 | template <typename Scalar> | ||
| 16 | ✗ | DifferentialActionModelLQRTpl<Scalar>::DifferentialActionModelLQRTpl( | |
| 17 | const MatrixXs& Aq, const MatrixXs& Av, const MatrixXs& B, | ||
| 18 | const MatrixXs& Q, const MatrixXs& R, const MatrixXs& N) | ||
| 19 | ✗ | : Base(boost::make_shared<StateVector>(2 * Aq.cols()), B.cols(), 0), | |
| 20 | ✗ | drift_free_(true), | |
| 21 | ✗ | updated_lqr_(false) { | |
| 22 | ✗ | const std::size_t nq = state_->get_nq(); | |
| 23 | ✗ | MatrixXs G = MatrixXs::Zero(ng_, 2 * nq + nu_); | |
| 24 | ✗ | MatrixXs H = MatrixXs::Zero(nh_, 2 * nq + nu_); | |
| 25 | ✗ | VectorXs f = VectorXs::Zero(nq); | |
| 26 | ✗ | VectorXs q = VectorXs::Zero(2 * nq); | |
| 27 | ✗ | VectorXs r = VectorXs::Zero(nu_); | |
| 28 | ✗ | VectorXs g = VectorXs::Zero(ng_); | |
| 29 | ✗ | VectorXs h = VectorXs::Zero(nh_); | |
| 30 | ✗ | set_LQR(Aq, Av, B, Q, R, N, G, H, f, q, r, g, h); | |
| 31 | } | ||
| 32 | |||
| 33 | template <typename Scalar> | ||
| 34 | ✗ | DifferentialActionModelLQRTpl<Scalar>::DifferentialActionModelLQRTpl( | |
| 35 | const MatrixXs& Aq, const MatrixXs& Av, const MatrixXs& B, | ||
| 36 | const MatrixXs& Q, const MatrixXs& R, const MatrixXs& N, const VectorXs& f, | ||
| 37 | const VectorXs& q, const VectorXs& r) | ||
| 38 | ✗ | : Base(boost::make_shared<StateVector>(2 * Aq.cols()), B.cols(), 0), | |
| 39 | ✗ | drift_free_(false), | |
| 40 | ✗ | updated_lqr_(false) { | |
| 41 | ✗ | const std::size_t nq = state_->get_nq(); | |
| 42 | ✗ | MatrixXs G = MatrixXs::Zero(ng_, 2 * nq + nu_); | |
| 43 | ✗ | MatrixXs H = MatrixXs::Zero(ng_, 2 * nq + nu_); | |
| 44 | ✗ | VectorXs g = VectorXs::Zero(ng_); | |
| 45 | ✗ | VectorXs h = VectorXs::Zero(nh_); | |
| 46 | ✗ | set_LQR(Aq, Av, B, Q, R, N, G, H, f, q, r, g, h); | |
| 47 | } | ||
| 48 | |||
| 49 | template <typename Scalar> | ||
| 50 | 51 | DifferentialActionModelLQRTpl<Scalar>::DifferentialActionModelLQRTpl( | |
| 51 | const MatrixXs& Aq, const MatrixXs& Av, const MatrixXs& B, | ||
| 52 | const MatrixXs& Q, const MatrixXs& R, const MatrixXs& N, const MatrixXs& G, | ||
| 53 | const MatrixXs& H, const VectorXs& f, const VectorXs& q, const VectorXs& r, | ||
| 54 | const VectorXs& g, const VectorXs& h) | ||
| 55 | ✗ | : Base(boost::make_shared<StateVector>(2 * Aq.cols()), B.cols(), 0, | |
| 56 | 204 | G.rows(), H.rows(), G.rows(), H.rows()), | |
| 57 | 51 | drift_free_(false), | |
| 58 |
16/32✓ Branch 7 taken 51 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 51 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 51 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 51 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 51 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 51 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 51 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 51 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 51 times.
✗ Branch 35 not taken.
✓ Branch 37 taken 51 times.
✗ Branch 38 not taken.
✓ Branch 40 taken 51 times.
✗ Branch 41 not taken.
✓ Branch 43 taken 51 times.
✗ Branch 44 not taken.
✓ Branch 46 taken 51 times.
✗ Branch 47 not taken.
✓ Branch 49 taken 51 times.
✗ Branch 50 not taken.
✓ Branch 52 taken 51 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 51 times.
✗ Branch 56 not taken.
|
51 | updated_lqr_(false) { |
| 59 |
1/2✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
|
51 | set_LQR(Aq, Av, B, Q, R, N, G, H, f, q, r, g, h); |
| 60 | 51 | } | |
| 61 | |||
| 62 | template <typename Scalar> | ||
| 63 | 105 | DifferentialActionModelLQRTpl<Scalar>::DifferentialActionModelLQRTpl( | |
| 64 | const std::size_t nq, const std::size_t nu, const bool drift_free) | ||
| 65 | ✗ | : Base(boost::make_shared<StateVector>(2 * nq), nu), | |
| 66 |
1/2✓ Branch 1 taken 105 times.
✗ Branch 2 not taken.
|
105 | Aq_(MatrixXs::Identity(nq, nq)), |
| 67 |
2/4✓ Branch 1 taken 105 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 105 times.
✗ Branch 5 not taken.
|
105 | Av_(MatrixXs::Identity(nq, nq)), |
| 68 |
2/4✓ Branch 1 taken 105 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 105 times.
✗ Branch 5 not taken.
|
105 | B_(MatrixXs::Identity(nq, nu)), |
| 69 |
2/4✓ Branch 1 taken 105 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 105 times.
✗ Branch 5 not taken.
|
105 | Q_(MatrixXs::Identity(2 * nq, 2 * nq)), |
| 70 |
2/4✓ Branch 1 taken 105 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 105 times.
✗ Branch 5 not taken.
|
105 | R_(MatrixXs::Identity(nu, nu)), |
| 71 |
2/4✓ Branch 1 taken 105 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 105 times.
✗ Branch 5 not taken.
|
105 | N_(MatrixXs::Zero(2 * nq, nu)), |
| 72 |
2/4✓ Branch 1 taken 105 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 105 times.
✗ Branch 5 not taken.
|
105 | G_(MatrixXs::Zero(0, 2 * nq + nu)), |
| 73 |
2/4✓ Branch 1 taken 105 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 105 times.
✗ Branch 5 not taken.
|
105 | H_(MatrixXs::Zero(0, 2 * nq + nu)), |
| 74 |
5/8✓ Branch 0 taken 55 times.
✓ Branch 1 taken 50 times.
✓ Branch 3 taken 55 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 50 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 105 times.
✗ Branch 10 not taken.
|
105 | f_(drift_free ? VectorXs::Zero(nq) : VectorXs::Ones(nq)), |
| 75 |
2/4✓ Branch 1 taken 105 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 105 times.
✗ Branch 5 not taken.
|
105 | q_(VectorXs::Ones(2 * nq)), |
| 76 |
2/4✓ Branch 1 taken 105 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 105 times.
✗ Branch 5 not taken.
|
105 | r_(VectorXs::Ones(nu)), |
| 77 |
2/4✓ Branch 1 taken 105 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 105 times.
✗ Branch 5 not taken.
|
105 | g_(VectorXs::Zero(0)), |
| 78 |
2/4✓ Branch 1 taken 105 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 105 times.
✗ Branch 5 not taken.
|
105 | h_(VectorXs::Zero(0)), |
| 79 | 105 | drift_free_(drift_free), | |
| 80 |
4/8✓ Branch 1 taken 105 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 105 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 105 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 105 times.
✗ Branch 14 not taken.
|
210 | updated_lqr_(false) {} |
| 81 | |||
| 82 | template <typename Scalar> | ||
| 83 | 53 | DifferentialActionModelLQRTpl<Scalar>::DifferentialActionModelLQRTpl( | |
| 84 | const DifferentialActionModelLQRTpl& copy) | ||
| 85 | 53 | : Base(boost::make_shared<StateVector>(2 * copy.get_Aq().cols()), | |
| 86 | 159 | copy.get_B().cols(), 0, copy.get_G().rows(), copy.get_H().rows(), | |
| 87 | 106 | copy.get_G().rows(), copy.get_H().rows()), | |
| 88 | 53 | drift_free_(false), | |
| 89 |
16/32✓ Branch 7 taken 53 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 53 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 53 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 53 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 53 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 53 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 53 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 53 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 53 times.
✗ Branch 35 not taken.
✓ Branch 37 taken 53 times.
✗ Branch 38 not taken.
✓ Branch 40 taken 53 times.
✗ Branch 41 not taken.
✓ Branch 43 taken 53 times.
✗ Branch 44 not taken.
✓ Branch 46 taken 53 times.
✗ Branch 47 not taken.
✓ Branch 49 taken 53 times.
✗ Branch 50 not taken.
✓ Branch 52 taken 53 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 53 times.
✗ Branch 56 not taken.
|
371 | updated_lqr_(false) { |
| 90 |
1/2✓ Branch 5 taken 53 times.
✗ Branch 6 not taken.
|
53 | set_LQR(copy.get_Aq(), copy.get_Av(), copy.get_B(), copy.get_Q(), |
| 91 | 53 | copy.get_R(), copy.get_N(), copy.get_G(), copy.get_H(), copy.get_f(), | |
| 92 | 53 | copy.get_q(), copy.get_r(), copy.get_g(), copy.get_h()); | |
| 93 | 53 | } | |
| 94 | |||
| 95 | template <typename Scalar> | ||
| 96 | 418 | DifferentialActionModelLQRTpl<Scalar>::~DifferentialActionModelLQRTpl() {} | |
| 97 | |||
| 98 | template <typename Scalar> | ||
| 99 | 19471 | void DifferentialActionModelLQRTpl<Scalar>::calc( | |
| 100 | const boost::shared_ptr<DifferentialActionDataAbstract>& data, | ||
| 101 | const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) { | ||
| 102 |
1/2✗ Branch 3 not taken.
✓ Branch 4 taken 19471 times.
|
19471 | if (static_cast<std::size_t>(x.size()) != state_->get_nx()) { |
| 103 | ✗ | throw_pretty( | |
| 104 | "Invalid argument: " << "x has wrong dimension (it should be " + | ||
| 105 | std::to_string(state_->get_nx()) + ")"); | ||
| 106 | } | ||
| 107 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 19471 times.
|
19471 | if (static_cast<std::size_t>(u.size()) != nu_) { |
| 108 | ✗ | throw_pretty( | |
| 109 | "Invalid argument: " << "u has wrong dimension (it should be " + | ||
| 110 | std::to_string(nu_) + ")"); | ||
| 111 | } | ||
| 112 | const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q = | ||
| 113 |
1/2✓ Branch 3 taken 19471 times.
✗ Branch 4 not taken.
|
19471 | x.head(state_->get_nq()); |
| 114 | const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> v = | ||
| 115 |
1/2✓ Branch 3 taken 19471 times.
✗ Branch 4 not taken.
|
19471 | x.tail(state_->get_nv()); |
| 116 | |||
| 117 |
3/6✓ Branch 1 taken 19471 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 19471 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 19471 times.
✗ Branch 9 not taken.
|
19471 | data->xout.noalias() = Aq_ * q; |
| 118 |
3/6✓ Branch 1 taken 19471 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 19471 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 19471 times.
✗ Branch 9 not taken.
|
19471 | data->xout.noalias() += Av_ * v; |
| 119 |
3/6✓ Branch 1 taken 19471 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 19471 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 19471 times.
✗ Branch 9 not taken.
|
19471 | data->xout.noalias() += B_ * u; |
| 120 |
1/2✓ Branch 2 taken 19471 times.
✗ Branch 3 not taken.
|
19471 | data->xout += f_; |
| 121 | |||
| 122 | // cost = 0.5 * x^T * Q * x + 0.5 * u^T * R * u + x^T * N * u + q^T * x + r^T | ||
| 123 | // * u | ||
| 124 |
2/4✓ Branch 1 taken 19471 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 19471 times.
✗ Branch 5 not taken.
|
19471 | data->cost = Scalar(0.5) * x.dot(Q_ * x); |
| 125 |
2/4✓ Branch 1 taken 19471 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 19471 times.
✗ Branch 5 not taken.
|
19471 | data->cost += Scalar(0.5) * u.dot(R_ * u); |
| 126 |
2/4✓ Branch 1 taken 19471 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 19471 times.
✗ Branch 5 not taken.
|
19471 | data->cost += x.dot(N_ * u); |
| 127 |
1/2✓ Branch 1 taken 19471 times.
✗ Branch 2 not taken.
|
19471 | data->cost += q_.dot(x); |
| 128 |
1/2✓ Branch 1 taken 19471 times.
✗ Branch 2 not taken.
|
19471 | data->cost += r_.dot(u); |
| 129 | |||
| 130 | // constraints | ||
| 131 | 19471 | const std::size_t nq = state_->get_nq(); | |
| 132 |
4/8✓ Branch 1 taken 19471 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 19471 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 19471 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 19471 times.
✗ Branch 12 not taken.
|
19471 | data->g.noalias() = G_.leftCols(nq) * q; |
| 133 |
4/8✓ Branch 1 taken 19471 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 19471 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 19471 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 19471 times.
✗ Branch 12 not taken.
|
19471 | data->g.noalias() += G_.middleCols(nq, nq) * v; |
| 134 |
4/8✓ Branch 1 taken 19471 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 19471 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 19471 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 19471 times.
✗ Branch 12 not taken.
|
19471 | data->g.noalias() += G_.rightCols(nu_) * u; |
| 135 |
1/2✓ Branch 2 taken 19471 times.
✗ Branch 3 not taken.
|
19471 | data->g += g_; |
| 136 |
4/8✓ Branch 1 taken 19471 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 19471 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 19471 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 19471 times.
✗ Branch 12 not taken.
|
19471 | data->h.noalias() = H_.leftCols(nq) * q; |
| 137 |
4/8✓ Branch 1 taken 19471 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 19471 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 19471 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 19471 times.
✗ Branch 12 not taken.
|
19471 | data->h.noalias() += H_.middleCols(nq, nq) * v; |
| 138 |
4/8✓ Branch 1 taken 19471 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 19471 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 19471 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 19471 times.
✗ Branch 12 not taken.
|
19471 | data->h.noalias() += H_.rightCols(nu_) * u; |
| 139 |
1/2✓ Branch 2 taken 19471 times.
✗ Branch 3 not taken.
|
19471 | data->h += h_; |
| 140 | 19471 | } | |
| 141 | |||
| 142 | template <typename Scalar> | ||
| 143 | 2575 | void DifferentialActionModelLQRTpl<Scalar>::calc( | |
| 144 | const boost::shared_ptr<DifferentialActionDataAbstract>& data, | ||
| 145 | const Eigen::Ref<const VectorXs>& x) { | ||
| 146 |
1/2✗ Branch 3 not taken.
✓ Branch 4 taken 2575 times.
|
2575 | if (static_cast<std::size_t>(x.size()) != state_->get_nx()) { |
| 147 | ✗ | throw_pretty( | |
| 148 | "Invalid argument: " << "x has wrong dimension (it should be " + | ||
| 149 | std::to_string(state_->get_nx()) + ")"); | ||
| 150 | } | ||
| 151 | const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q = | ||
| 152 |
1/2✓ Branch 3 taken 2575 times.
✗ Branch 4 not taken.
|
2575 | x.head(state_->get_nq()); |
| 153 | const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> v = | ||
| 154 |
1/2✓ Branch 3 taken 2575 times.
✗ Branch 4 not taken.
|
2575 | x.tail(state_->get_nv()); |
| 155 | |||
| 156 | // cost = 0.5 * x^T * Q * x + q^T * x | ||
| 157 |
2/4✓ Branch 1 taken 2575 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2575 times.
✗ Branch 5 not taken.
|
2575 | data->cost = Scalar(0.5) * x.dot(Q_ * x); |
| 158 |
1/2✓ Branch 1 taken 2575 times.
✗ Branch 2 not taken.
|
2575 | data->cost += q_.dot(x); |
| 159 | |||
| 160 | // constraints | ||
| 161 | 2575 | const std::size_t nq = state_->get_nq(); | |
| 162 |
4/8✓ Branch 1 taken 2575 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2575 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 2575 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 2575 times.
✗ Branch 12 not taken.
|
2575 | data->g.noalias() = G_.leftCols(nq) * q; |
| 163 |
4/8✓ Branch 1 taken 2575 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2575 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 2575 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 2575 times.
✗ Branch 12 not taken.
|
2575 | data->g.noalias() += G_.middleCols(nq, nq) * v; |
| 164 |
1/2✓ Branch 2 taken 2575 times.
✗ Branch 3 not taken.
|
2575 | data->g += g_; |
| 165 |
4/8✓ Branch 1 taken 2575 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2575 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 2575 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 2575 times.
✗ Branch 12 not taken.
|
2575 | data->h.noalias() = H_.leftCols(nq) * q; |
| 166 |
4/8✓ Branch 1 taken 2575 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2575 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 2575 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 2575 times.
✗ Branch 12 not taken.
|
2575 | data->h.noalias() += H_.middleCols(nq, nq) * v; |
| 167 |
1/2✓ Branch 2 taken 2575 times.
✗ Branch 3 not taken.
|
2575 | data->h += h_; |
| 168 | 2575 | } | |
| 169 | |||
| 170 | template <typename Scalar> | ||
| 171 | 3809 | void DifferentialActionModelLQRTpl<Scalar>::calcDiff( | |
| 172 | const boost::shared_ptr<DifferentialActionDataAbstract>& data, | ||
| 173 | const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) { | ||
| 174 |
1/2✗ Branch 3 not taken.
✓ Branch 4 taken 3809 times.
|
3809 | if (static_cast<std::size_t>(x.size()) != state_->get_nx()) { |
| 175 | ✗ | throw_pretty( | |
| 176 | "Invalid argument: " << "x has wrong dimension (it should be " + | ||
| 177 | std::to_string(state_->get_nx()) + ")"); | ||
| 178 | } | ||
| 179 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 3809 times.
|
3809 | if (static_cast<std::size_t>(u.size()) != nu_) { |
| 180 | ✗ | throw_pretty( | |
| 181 | "Invalid argument: " << "u has wrong dimension (it should be " + | ||
| 182 | std::to_string(nu_) + ")"); | ||
| 183 | } | ||
| 184 | |||
| 185 | 3809 | const std::size_t nq = state_->get_nq(); | |
| 186 |
2/2✓ Branch 0 taken 59 times.
✓ Branch 1 taken 3750 times.
|
3809 | if (!updated_lqr_) { |
| 187 |
1/2✓ Branch 3 taken 59 times.
✗ Branch 4 not taken.
|
59 | data->Fx.leftCols(nq) = Aq_; |
| 188 |
1/2✓ Branch 3 taken 59 times.
✗ Branch 4 not taken.
|
59 | data->Fx.rightCols(nq) = Av_; |
| 189 | 59 | data->Fu = B_; | |
| 190 | 59 | data->Lxx = Q_; | |
| 191 | 59 | data->Luu = R_; | |
| 192 | 59 | data->Lxu = N_; | |
| 193 |
1/2✓ Branch 3 taken 59 times.
✗ Branch 4 not taken.
|
59 | data->Gx = G_.leftCols(2 * nq); |
| 194 |
1/2✓ Branch 3 taken 59 times.
✗ Branch 4 not taken.
|
59 | data->Gu = G_.rightCols(nu_); |
| 195 |
1/2✓ Branch 3 taken 59 times.
✗ Branch 4 not taken.
|
59 | data->Hx = H_.leftCols(2 * nq); |
| 196 |
1/2✓ Branch 3 taken 59 times.
✗ Branch 4 not taken.
|
59 | data->Hu = H_.rightCols(nu_); |
| 197 | 59 | updated_lqr_ = true; | |
| 198 | } | ||
| 199 | 3809 | data->Lx = q_; | |
| 200 |
2/4✓ Branch 3 taken 3809 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 3809 times.
✗ Branch 7 not taken.
|
3809 | data->Lx.noalias() += Q_ * x; |
| 201 |
2/4✓ Branch 3 taken 3809 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 3809 times.
✗ Branch 7 not taken.
|
3809 | data->Lx.noalias() += N_ * u; |
| 202 | 3809 | data->Lu = r_; | |
| 203 |
3/6✓ Branch 2 taken 3809 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 3809 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 3809 times.
✗ Branch 10 not taken.
|
3809 | data->Lu.noalias() += N_.transpose() * x; |
| 204 |
2/4✓ Branch 3 taken 3809 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 3809 times.
✗ Branch 7 not taken.
|
3809 | data->Lu.noalias() += R_ * u; |
| 205 | 3809 | } | |
| 206 | |||
| 207 | template <typename Scalar> | ||
| 208 | 68 | void DifferentialActionModelLQRTpl<Scalar>::calcDiff( | |
| 209 | const boost::shared_ptr<DifferentialActionDataAbstract>& data, | ||
| 210 | const Eigen::Ref<const VectorXs>& x) { | ||
| 211 |
1/2✗ Branch 3 not taken.
✓ Branch 4 taken 68 times.
|
68 | if (static_cast<std::size_t>(x.size()) != state_->get_nx()) { |
| 212 | ✗ | throw_pretty( | |
| 213 | "Invalid argument: " << "x has wrong dimension (it should be " + | ||
| 214 | std::to_string(state_->get_nx()) + ")"); | ||
| 215 | } | ||
| 216 | |||
| 217 | 68 | const std::size_t nq = state_->get_nq(); | |
| 218 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 68 times.
|
68 | if (!updated_lqr_) { |
| 219 | ✗ | data->Lxx = Q_; | |
| 220 | ✗ | data->Gx = G_.leftCols(2 * nq); | |
| 221 | ✗ | data->Hx = H_.leftCols(2 * nq); | |
| 222 | ✗ | updated_lqr_ = true; | |
| 223 | } | ||
| 224 | 68 | data->Lx = q_; | |
| 225 |
2/4✓ Branch 3 taken 68 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 68 times.
✗ Branch 7 not taken.
|
68 | data->Lx.noalias() += Q_ * x; |
| 226 | 68 | } | |
| 227 | |||
| 228 | template <typename Scalar> | ||
| 229 | boost::shared_ptr<DifferentialActionDataAbstractTpl<Scalar> > | ||
| 230 | 20322 | DifferentialActionModelLQRTpl<Scalar>::createData() { | |
| 231 |
1/2✓ Branch 2 taken 20322 times.
✗ Branch 3 not taken.
|
20322 | return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this); |
| 232 | } | ||
| 233 | |||
| 234 | template <typename Scalar> | ||
| 235 | 1971 | bool DifferentialActionModelLQRTpl<Scalar>::checkData( | |
| 236 | const boost::shared_ptr<DifferentialActionDataAbstract>& data) { | ||
| 237 | 1971 | boost::shared_ptr<Data> d = boost::dynamic_pointer_cast<Data>(data); | |
| 238 |
1/2✓ Branch 1 taken 1971 times.
✗ Branch 2 not taken.
|
1971 | if (d != NULL) { |
| 239 | 1971 | return true; | |
| 240 | } else { | ||
| 241 | ✗ | return false; | |
| 242 | } | ||
| 243 | 1971 | } | |
| 244 | |||
| 245 | template <typename Scalar> | ||
| 246 | DifferentialActionModelLQRTpl<Scalar> | ||
| 247 | 51 | DifferentialActionModelLQRTpl<Scalar>::Random(const std::size_t nq, | |
| 248 | const std::size_t nu, | ||
| 249 | const std::size_t ng, | ||
| 250 | const std::size_t nh) { | ||
| 251 |
2/4✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 51 times.
✗ Branch 5 not taken.
|
51 | MatrixXs Aq = MatrixXs::Random(nq, nq); |
| 252 |
2/4✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 51 times.
✗ Branch 5 not taken.
|
51 | MatrixXs Av = MatrixXs::Random(nq, nq); |
| 253 |
2/4✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 51 times.
✗ Branch 5 not taken.
|
51 | MatrixXs B = MatrixXs::Random(nq, nu); |
| 254 |
2/4✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 51 times.
✗ Branch 5 not taken.
|
51 | MatrixXs L_tmp = MatrixXs::Random(2 * nq + nu, 2 * nq + nu); |
| 255 |
3/6✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 51 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 51 times.
✗ Branch 8 not taken.
|
51 | MatrixXs L = L_tmp.transpose() * L_tmp; |
| 256 |
1/2✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
|
51 | const Eigen::Block<MatrixXs> Q = L.topLeftCorner(2 * nq, 2 * nq); |
| 257 |
1/2✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
|
51 | const Eigen::Block<MatrixXs> R = L.bottomRightCorner(nu, nu); |
| 258 |
1/2✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
|
51 | const Eigen::Block<MatrixXs> N = L.topRightCorner(2 * nq, nu); |
| 259 |
2/4✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 51 times.
✗ Branch 5 not taken.
|
51 | MatrixXs G = MatrixXs::Random(ng, 2 * nq + nu); |
| 260 |
2/4✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 51 times.
✗ Branch 5 not taken.
|
51 | MatrixXs H = MatrixXs::Random(nh, 2 * nq + nu); |
| 261 |
2/4✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 51 times.
✗ Branch 5 not taken.
|
51 | VectorXs f = VectorXs::Random(nq); |
| 262 |
2/4✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 51 times.
✗ Branch 5 not taken.
|
51 | VectorXs q = VectorXs::Random(2 * nq); |
| 263 |
2/4✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 51 times.
✗ Branch 5 not taken.
|
51 | VectorXs r = VectorXs::Random(nu); |
| 264 |
2/4✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 51 times.
✗ Branch 5 not taken.
|
51 | VectorXs g = VectorXs::Random(ng); |
| 265 |
2/4✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 51 times.
✗ Branch 5 not taken.
|
51 | VectorXs h = VectorXs::Random(nh); |
| 266 | return DifferentialActionModelLQRTpl<Scalar>(Aq, Av, B, Q, R, N, G, H, f, q, | ||
| 267 |
4/8✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 51 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 51 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 51 times.
✗ Branch 11 not taken.
|
102 | r, g, h); |
| 268 | 51 | } | |
| 269 | |||
| 270 | template <typename Scalar> | ||
| 271 | 30 | void DifferentialActionModelLQRTpl<Scalar>::print(std::ostream& os) const { | |
| 272 | 30 | os << "DifferentialActionModelLQR {nq=" << state_->get_nq() << ", nu=" << nu_ | |
| 273 | 30 | << ", ng=" << ng_ << ", nh=" << nh_ << ", drift_free=" << drift_free_ | |
| 274 | 30 | << "}"; | |
| 275 | 30 | } | |
| 276 | |||
| 277 | template <typename Scalar> | ||
| 278 | const typename MathBaseTpl<Scalar>::MatrixXs& | ||
| 279 | 20429 | DifferentialActionModelLQRTpl<Scalar>::get_Aq() const { | |
| 280 | 20429 | return Aq_; | |
| 281 | } | ||
| 282 | |||
| 283 | template <typename Scalar> | ||
| 284 | const typename MathBaseTpl<Scalar>::MatrixXs& | ||
| 285 | 20376 | DifferentialActionModelLQRTpl<Scalar>::get_Av() const { | |
| 286 | 20376 | return Av_; | |
| 287 | } | ||
| 288 | |||
| 289 | template <typename Scalar> | ||
| 290 | const typename MathBaseTpl<Scalar>::MatrixXs& | ||
| 291 | 20429 | DifferentialActionModelLQRTpl<Scalar>::get_B() const { | |
| 292 | 20429 | return B_; | |
| 293 | } | ||
| 294 | |||
| 295 | template <typename Scalar> | ||
| 296 | const typename MathBaseTpl<Scalar>::VectorXs& | ||
| 297 | 54 | DifferentialActionModelLQRTpl<Scalar>::get_f() const { | |
| 298 | 54 | return f_; | |
| 299 | } | ||
| 300 | |||
| 301 | template <typename Scalar> | ||
| 302 | const typename MathBaseTpl<Scalar>::MatrixXs& | ||
| 303 | 20376 | DifferentialActionModelLQRTpl<Scalar>::get_Q() const { | |
| 304 | 20376 | return Q_; | |
| 305 | } | ||
| 306 | |||
| 307 | template <typename Scalar> | ||
| 308 | const typename MathBaseTpl<Scalar>::MatrixXs& | ||
| 309 | 20376 | DifferentialActionModelLQRTpl<Scalar>::get_R() const { | |
| 310 | 20376 | return R_; | |
| 311 | } | ||
| 312 | |||
| 313 | template <typename Scalar> | ||
| 314 | const typename MathBaseTpl<Scalar>::MatrixXs& | ||
| 315 | 20376 | DifferentialActionModelLQRTpl<Scalar>::get_N() const { | |
| 316 | 20376 | return N_; | |
| 317 | } | ||
| 318 | |||
| 319 | template <typename Scalar> | ||
| 320 | const typename MathBaseTpl<Scalar>::MatrixXs& | ||
| 321 | 40803 | DifferentialActionModelLQRTpl<Scalar>::get_G() const { | |
| 322 | 40803 | return G_; | |
| 323 | } | ||
| 324 | |||
| 325 | template <typename Scalar> | ||
| 326 | const typename MathBaseTpl<Scalar>::MatrixXs& | ||
| 327 | 40803 | DifferentialActionModelLQRTpl<Scalar>::get_H() const { | |
| 328 | 40803 | return H_; | |
| 329 | } | ||
| 330 | |||
| 331 | template <typename Scalar> | ||
| 332 | const typename MathBaseTpl<Scalar>::VectorXs& | ||
| 333 | 54 | DifferentialActionModelLQRTpl<Scalar>::get_q() const { | |
| 334 | 54 | return q_; | |
| 335 | } | ||
| 336 | |||
| 337 | template <typename Scalar> | ||
| 338 | const typename MathBaseTpl<Scalar>::VectorXs& | ||
| 339 | 54 | DifferentialActionModelLQRTpl<Scalar>::get_r() const { | |
| 340 | 54 | return r_; | |
| 341 | } | ||
| 342 | |||
| 343 | template <typename Scalar> | ||
| 344 | const typename MathBaseTpl<Scalar>::VectorXs& | ||
| 345 | 53 | DifferentialActionModelLQRTpl<Scalar>::get_g() const { | |
| 346 | 53 | return g_; | |
| 347 | } | ||
| 348 | |||
| 349 | template <typename Scalar> | ||
| 350 | const typename MathBaseTpl<Scalar>::VectorXs& | ||
| 351 | 53 | DifferentialActionModelLQRTpl<Scalar>::get_h() const { | |
| 352 | 53 | return h_; | |
| 353 | } | ||
| 354 | |||
| 355 | template <typename Scalar> | ||
| 356 | 104 | void DifferentialActionModelLQRTpl<Scalar>::set_LQR( | |
| 357 | const MatrixXs& Aq, const MatrixXs& Av, const MatrixXs& B, | ||
| 358 | const MatrixXs& Q, const MatrixXs& R, const MatrixXs& N, const MatrixXs& G, | ||
| 359 | const MatrixXs& H, const VectorXs& f, const VectorXs& q, const VectorXs& r, | ||
| 360 | const VectorXs& g, const VectorXs& h) { | ||
| 361 | 104 | const std::size_t nq = state_->get_nq(); | |
| 362 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 104 times.
|
104 | if (static_cast<std::size_t>(Aq.rows()) != nq) { |
| 363 | ✗ | throw_pretty( | |
| 364 | "Invalid argument: " << "Aq should be a squared matrix with size " + | ||
| 365 | std::to_string(nq)); | ||
| 366 | } | ||
| 367 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 104 times.
|
104 | if (static_cast<std::size_t>(Av.rows()) != nq) { |
| 368 | ✗ | throw_pretty( | |
| 369 | "Invalid argument: " << "Av should be a squared matrix with size " + | ||
| 370 | std::to_string(nq)); | ||
| 371 | } | ||
| 372 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 104 times.
|
104 | if (static_cast<std::size_t>(B.rows()) != nq) { |
| 373 | ✗ | throw_pretty( | |
| 374 | "Invalid argument: " << "B has wrong dimension (it should have " + | ||
| 375 | std::to_string(nq) + " rows)"); | ||
| 376 | } | ||
| 377 |
2/4✓ Branch 1 taken 104 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 104 times.
|
208 | if (static_cast<std::size_t>(Q.rows()) != 2 * nq || |
| 378 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 104 times.
|
104 | static_cast<std::size_t>(Q.cols()) != 2 * nq) { |
| 379 | ✗ | throw_pretty( | |
| 380 | "Invalid argument: " << "Q has wrong dimension (it should be " + | ||
| 381 | std::to_string(2 * nq) + " x " + | ||
| 382 | std::to_string(2 * nq) + ")"); | ||
| 383 | } | ||
| 384 |
2/4✓ Branch 1 taken 104 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 104 times.
|
208 | if (static_cast<std::size_t>(R.rows()) != nu_ || |
| 385 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 104 times.
|
104 | static_cast<std::size_t>(R.cols()) != nu_) { |
| 386 | ✗ | throw_pretty( | |
| 387 | "Invalid argument: " << "R has wrong dimension (it should be " + | ||
| 388 | std::to_string(nu_) + " x " + | ||
| 389 | std::to_string(nu_) + ")"); | ||
| 390 | } | ||
| 391 |
2/4✓ Branch 1 taken 104 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 104 times.
|
208 | if (static_cast<std::size_t>(N.rows()) != 2 * nq || |
| 392 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 104 times.
|
104 | static_cast<std::size_t>(N.cols()) != nu_) { |
| 393 | ✗ | throw_pretty( | |
| 394 | "Invalid argument: " << "N has wrong dimension (it should be " + | ||
| 395 | std::to_string(2 * nq) + " x " + | ||
| 396 | std::to_string(nu_) + ")"); | ||
| 397 | } | ||
| 398 |
2/4✓ Branch 1 taken 104 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 104 times.
|
208 | if (static_cast<std::size_t>(G.rows()) != ng_ || |
| 399 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 104 times.
|
104 | static_cast<std::size_t>(G.cols()) != 2 * nq + nu_) { |
| 400 | ✗ | throw_pretty( | |
| 401 | "Invalid argument: " << "G has wrong dimension (it should be " + | ||
| 402 | std::to_string(ng_) + " x " + | ||
| 403 | std::to_string(2 * nq + nu_) + ")"); | ||
| 404 | } | ||
| 405 |
2/4✓ Branch 1 taken 104 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 104 times.
|
208 | if (static_cast<std::size_t>(H.rows()) != nh_ || |
| 406 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 104 times.
|
104 | static_cast<std::size_t>(H.cols()) != 2 * nq + nu_) { |
| 407 | ✗ | throw_pretty( | |
| 408 | "Invalid argument: " << "H has wrong dimension (it should be " + | ||
| 409 | std::to_string(nh_) + " x " + | ||
| 410 | std::to_string(2 * nq + nu_) + ")"); | ||
| 411 | } | ||
| 412 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 104 times.
|
104 | if (static_cast<std::size_t>(f.size()) != nq) { |
| 413 | ✗ | throw_pretty( | |
| 414 | "Invalid argument: " << "f has wrong dimension (it should be " + | ||
| 415 | std::to_string(nq) + ")"); | ||
| 416 | } | ||
| 417 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 104 times.
|
104 | if (static_cast<std::size_t>(q.size()) != 2 * nq) { |
| 418 | ✗ | throw_pretty( | |
| 419 | "Invalid argument: " << "q has wrong dimension (it should be " + | ||
| 420 | std::to_string(2 * nq) + ")"); | ||
| 421 | } | ||
| 422 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 104 times.
|
104 | if (static_cast<std::size_t>(r.size()) != nu_) { |
| 423 | ✗ | throw_pretty( | |
| 424 | "Invalid argument: " << "r has wrong dimension (it should be " + | ||
| 425 | std::to_string(nu_) + ")"); | ||
| 426 | } | ||
| 427 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 104 times.
|
104 | if (static_cast<std::size_t>(g.size()) != ng_) { |
| 428 | ✗ | throw_pretty( | |
| 429 | "Invalid argument: " << "g has wrong dimension (it should be " + | ||
| 430 | std::to_string(ng_) + ")"); | ||
| 431 | } | ||
| 432 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 104 times.
|
104 | if (static_cast<std::size_t>(h.size()) != nh_) { |
| 433 | ✗ | throw_pretty( | |
| 434 | "Invalid argument: " << "h has wrong dimension (it should be " + | ||
| 435 | std::to_string(nh_) + ")"); | ||
| 436 | } | ||
| 437 |
2/4✓ Branch 1 taken 104 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 104 times.
✗ Branch 5 not taken.
|
104 | L_ = MatrixXs::Zero(2 * nq + nu_, 2 * nq + nu_); |
| 438 |
5/10✓ Branch 1 taken 104 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 104 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 104 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 104 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 104 times.
✗ Branch 14 not taken.
|
104 | L_ << Q, N, N.transpose(), R; |
| 439 |
1/2✓ Branch 1 taken 104 times.
✗ Branch 2 not taken.
|
104 | Eigen::LLT<MatrixXs> L_llt(H_); |
| 440 |
5/10✓ Branch 2 taken 104 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 104 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 104 times.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✓ Branch 11 taken 104 times.
✗ Branch 12 not taken.
✓ Branch 13 taken 104 times.
|
104 | if (!L_.isApprox(L_.transpose()) || L_llt.info() == Eigen::NumericalIssue) { |
| 441 | ✗ | throw_pretty("Invalid argument " | |
| 442 | << "[Q, N; N.T, R] is not semi-positive definite"); | ||
| 443 | } | ||
| 444 | |||
| 445 |
1/2✓ Branch 1 taken 104 times.
✗ Branch 2 not taken.
|
104 | Aq_ = Aq; |
| 446 |
1/2✓ Branch 1 taken 104 times.
✗ Branch 2 not taken.
|
104 | Av_ = Av; |
| 447 |
1/2✓ Branch 1 taken 104 times.
✗ Branch 2 not taken.
|
104 | B_ = B; |
| 448 |
1/2✓ Branch 1 taken 104 times.
✗ Branch 2 not taken.
|
104 | f_ = f; |
| 449 |
1/2✓ Branch 1 taken 104 times.
✗ Branch 2 not taken.
|
104 | Q_ = Q; |
| 450 |
1/2✓ Branch 1 taken 104 times.
✗ Branch 2 not taken.
|
104 | R_ = R; |
| 451 |
1/2✓ Branch 1 taken 104 times.
✗ Branch 2 not taken.
|
104 | N_ = N; |
| 452 |
1/2✓ Branch 1 taken 104 times.
✗ Branch 2 not taken.
|
104 | G_ = G; |
| 453 |
1/2✓ Branch 1 taken 104 times.
✗ Branch 2 not taken.
|
104 | H_ = H; |
| 454 |
1/2✓ Branch 1 taken 104 times.
✗ Branch 2 not taken.
|
104 | q_ = q; |
| 455 |
1/2✓ Branch 1 taken 104 times.
✗ Branch 2 not taken.
|
104 | r_ = r; |
| 456 |
1/2✓ Branch 1 taken 104 times.
✗ Branch 2 not taken.
|
104 | g_ = g; |
| 457 |
1/2✓ Branch 1 taken 104 times.
✗ Branch 2 not taken.
|
104 | h_ = h; |
| 458 | 104 | updated_lqr_ = false; | |
| 459 | 104 | } | |
| 460 | |||
| 461 | } // namespace crocoddyl | ||
| 462 |