| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | /////////////////////////////////////////////////////////////////////////////// | ||
| 2 | // BSD 3-Clause License | ||
| 3 | // | ||
| 4 | // Copyright (C) 2023-2025, Heriot-Watt University | ||
| 5 | // Copyright note valid unless otherwise stated in individual files. | ||
| 6 | // All rights reserved. | ||
| 7 | /////////////////////////////////////////////////////////////////////////////// | ||
| 8 | |||
| 9 | #include "crocoddyl/core/residuals/joint-acceleration.hpp" | ||
| 10 | |||
| 11 | namespace crocoddyl { | ||
| 12 | |||
| 13 | template <typename Scalar> | ||
| 14 | ✗ | ResidualModelJointAccelerationTpl<Scalar>::ResidualModelJointAccelerationTpl( | |
| 15 | std::shared_ptr<StateAbstract> state, const VectorXs& aref, | ||
| 16 | const std::size_t nu) | ||
| 17 | ✗ | : Base(state, state->get_nv(), nu, true, true, true), aref_(aref) { | |
| 18 | ✗ | if (static_cast<std::size_t>(aref_.size()) != state->get_nv()) { | |
| 19 | ✗ | throw_pretty( | |
| 20 | "Invalid argument: " << "aref has wrong dimension (it should be " + | ||
| 21 | std::to_string(state->get_nv()) + ")"); | ||
| 22 | } | ||
| 23 | ✗ | } | |
| 24 | |||
| 25 | template <typename Scalar> | ||
| 26 | ✗ | ResidualModelJointAccelerationTpl<Scalar>::ResidualModelJointAccelerationTpl( | |
| 27 | std::shared_ptr<StateAbstract> state, const VectorXs& aref) | ||
| 28 | : Base(state, state->get_nv(), state->get_nv(), true, true, true), | ||
| 29 | ✗ | aref_(aref) { | |
| 30 | ✗ | if (static_cast<std::size_t>(aref_.size()) != state->get_nv()) { | |
| 31 | ✗ | throw_pretty( | |
| 32 | "Invalid argument: " << "aref has wrong dimension (it should be " + | ||
| 33 | std::to_string(state->get_nv()) + ")"); | ||
| 34 | } | ||
| 35 | ✗ | } | |
| 36 | |||
| 37 | template <typename Scalar> | ||
| 38 | ✗ | ResidualModelJointAccelerationTpl<Scalar>::ResidualModelJointAccelerationTpl( | |
| 39 | std::shared_ptr<StateAbstract> state, const std::size_t nu) | ||
| 40 | : Base(state, state->get_nv(), nu, true, true, true), | ||
| 41 | ✗ | aref_(VectorXs::Zero(state->get_nv())) {} | |
| 42 | |||
| 43 | template <typename Scalar> | ||
| 44 | ✗ | ResidualModelJointAccelerationTpl<Scalar>::ResidualModelJointAccelerationTpl( | |
| 45 | std::shared_ptr<StateAbstract> state) | ||
| 46 | : Base(state, state->get_nv(), state->get_nv(), true, true, true), | ||
| 47 | ✗ | aref_(VectorXs::Zero(state->get_nv())) {} | |
| 48 | |||
| 49 | template <typename Scalar> | ||
| 50 | ✗ | void ResidualModelJointAccelerationTpl<Scalar>::calc( | |
| 51 | const std::shared_ptr<ResidualDataAbstract>& data, | ||
| 52 | const Eigen::Ref<const VectorXs>&, const Eigen::Ref<const VectorXs>&) { | ||
| 53 | ✗ | Data* d = static_cast<Data*>(data.get()); | |
| 54 | ✗ | data->r = d->joint->a - aref_; | |
| 55 | ✗ | } | |
| 56 | |||
| 57 | template <typename Scalar> | ||
| 58 | ✗ | void ResidualModelJointAccelerationTpl<Scalar>::calc( | |
| 59 | const std::shared_ptr<ResidualDataAbstract>& data, | ||
| 60 | const Eigen::Ref<const VectorXs>&) { | ||
| 61 | ✗ | data->r.setZero(); | |
| 62 | ✗ | } | |
| 63 | |||
| 64 | template <typename Scalar> | ||
| 65 | ✗ | void ResidualModelJointAccelerationTpl<Scalar>::calcDiff( | |
| 66 | const std::shared_ptr<ResidualDataAbstract>& data, | ||
| 67 | const Eigen::Ref<const VectorXs>&, const Eigen::Ref<const VectorXs>&) { | ||
| 68 | ✗ | Data* d = static_cast<Data*>(data.get()); | |
| 69 | ✗ | data->Rx = d->joint->da_dx; | |
| 70 | ✗ | data->Ru = d->joint->da_du; | |
| 71 | ✗ | } | |
| 72 | |||
| 73 | template <typename Scalar> | ||
| 74 | std::shared_ptr<ResidualDataAbstractTpl<Scalar> > | ||
| 75 | ✗ | ResidualModelJointAccelerationTpl<Scalar>::createData( | |
| 76 | DataCollectorAbstract* const data) { | ||
| 77 | ✗ | std::shared_ptr<ResidualDataAbstract> d = | |
| 78 | ✗ | std::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this, data); | |
| 79 | ✗ | return d; | |
| 80 | } | ||
| 81 | |||
| 82 | template <typename Scalar> | ||
| 83 | template <typename NewScalar> | ||
| 84 | ResidualModelJointAccelerationTpl<NewScalar> | ||
| 85 | ✗ | ResidualModelJointAccelerationTpl<Scalar>::cast() const { | |
| 86 | typedef ResidualModelJointAccelerationTpl<NewScalar> ReturnType; | ||
| 87 | ✗ | ReturnType ret(state_->template cast<NewScalar>(), | |
| 88 | ✗ | aref_.template cast<NewScalar>(), nu_); | |
| 89 | ✗ | return ret; | |
| 90 | } | ||
| 91 | |||
| 92 | template <typename Scalar> | ||
| 93 | ✗ | void ResidualModelJointAccelerationTpl<Scalar>::print(std::ostream& os) const { | |
| 94 | ✗ | os << "ResidualModelJointAcceleration"; | |
| 95 | ✗ | } | |
| 96 | |||
| 97 | template <typename Scalar> | ||
| 98 | const typename MathBaseTpl<Scalar>::VectorXs& | ||
| 99 | ✗ | ResidualModelJointAccelerationTpl<Scalar>::get_reference() const { | |
| 100 | ✗ | return aref_; | |
| 101 | } | ||
| 102 | |||
| 103 | template <typename Scalar> | ||
| 104 | ✗ | void ResidualModelJointAccelerationTpl<Scalar>::set_reference( | |
| 105 | const VectorXs& reference) { | ||
| 106 | ✗ | if (static_cast<std::size_t>(reference.size()) != nr_) { | |
| 107 | ✗ | throw_pretty( | |
| 108 | "Invalid argument: " | ||
| 109 | << "the generalized-acceleration reference has wrong dimension (" | ||
| 110 | << reference.size() | ||
| 111 | << " provided - it should be " + std::to_string(nr_) + ")") | ||
| 112 | } | ||
| 113 | ✗ | aref_ = reference; | |
| 114 | ✗ | } | |
| 115 | |||
| 116 | } // namespace crocoddyl | ||
| 117 |