| Directory: | ./ |
|---|---|
| File: | include/crocoddyl/multibody/impulse-base.hxx |
| Date: | 2025-03-26 19:23:43 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 23 | 41 | 56.1% |
| Branches: | 3 | 22 | 13.6% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | /////////////////////////////////////////////////////////////////////////////// | ||
| 2 | // BSD 3-Clause License | ||
| 3 | // | ||
| 4 | // Copyright (C) 2019-2025, 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/multibody/impulse-base.hpp" | ||
| 11 | |||
| 12 | namespace crocoddyl { | ||
| 13 | |||
| 14 | template <typename Scalar> | ||
| 15 | 421 | ImpulseModelAbstractTpl<Scalar>::ImpulseModelAbstractTpl( | |
| 16 | std::shared_ptr<StateMultibody> state, const pinocchio::ReferenceFrame type, | ||
| 17 | const std::size_t nc) | ||
| 18 | 421 | : state_(state), nc_(nc), id_(0), type_(type) {} | |
| 19 | |||
| 20 | template <typename Scalar> | ||
| 21 | ✗ | ImpulseModelAbstractTpl<Scalar>::ImpulseModelAbstractTpl( | |
| 22 | std::shared_ptr<StateMultibody> state, const std::size_t nc) | ||
| 23 | ✗ | : state_(state), nc_(nc), id_(0), type_(pinocchio::ReferenceFrame::LOCAL) { | |
| 24 | ✗ | std::cerr << "Deprecated: Use constructor that passes the type of contact, " | |
| 25 | "this assumes is pinocchio::LOCAL." | ||
| 26 | ✗ | << std::endl; | |
| 27 | } | ||
| 28 | |||
| 29 | template <typename Scalar> | ||
| 30 | 730 | void ImpulseModelAbstractTpl<Scalar>::updateForceDiff( | |
| 31 | const std::shared_ptr<ImpulseDataAbstract>& data, | ||
| 32 | const MatrixXs& df_dx) const { | ||
| 33 |
2/4✓ Branch 1 taken 730 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 730 times.
|
1460 | if (static_cast<std::size_t>(df_dx.rows()) != nc_ || |
| 34 |
1/2✗ Branch 3 not taken.
✓ Branch 4 taken 730 times.
|
730 | static_cast<std::size_t>(df_dx.cols()) != state_->get_ndx()) |
| 35 | ✗ | throw_pretty("df_dq has wrong dimension"); | |
| 36 | |||
| 37 | 730 | data->df_dx = df_dx; | |
| 38 | 730 | } | |
| 39 | |||
| 40 | template <typename Scalar> | ||
| 41 | ✗ | void ImpulseModelAbstractTpl<Scalar>::setZeroForce( | |
| 42 | const std::shared_ptr<ImpulseDataAbstract>& data) const { | ||
| 43 | ✗ | data->f.setZero(); | |
| 44 | ✗ | data->fext.setZero(); | |
| 45 | } | ||
| 46 | |||
| 47 | template <typename Scalar> | ||
| 48 | ✗ | void ImpulseModelAbstractTpl<Scalar>::setZeroForceDiff( | |
| 49 | const std::shared_ptr<ImpulseDataAbstract>& data) const { | ||
| 50 | ✗ | data->df_dx.setZero(); | |
| 51 | } | ||
| 52 | |||
| 53 | template <typename Scalar> | ||
| 54 | std::shared_ptr<ImpulseDataAbstractTpl<Scalar> > | ||
| 55 | ✗ | ImpulseModelAbstractTpl<Scalar>::createData( | |
| 56 | pinocchio::DataTpl<Scalar>* const data) { | ||
| 57 | return std::allocate_shared<ImpulseDataAbstract>( | ||
| 58 | ✗ | Eigen::aligned_allocator<ImpulseDataAbstract>(), this, data); | |
| 59 | } | ||
| 60 | |||
| 61 | template <typename Scalar> | ||
| 62 | ✗ | void ImpulseModelAbstractTpl<Scalar>::print(std::ostream& os) const { | |
| 63 | ✗ | os << boost::core::demangle(typeid(*this).name()); | |
| 64 | } | ||
| 65 | |||
| 66 | template <typename Scalar> | ||
| 67 | const std::shared_ptr<StateMultibodyTpl<Scalar> >& | ||
| 68 | 115280 | ImpulseModelAbstractTpl<Scalar>::get_state() const { | |
| 69 | 115280 | return state_; | |
| 70 | } | ||
| 71 | |||
| 72 | template <typename Scalar> | ||
| 73 | 76794 | std::size_t ImpulseModelAbstractTpl<Scalar>::get_nc() const { | |
| 74 | 76794 | return nc_; | |
| 75 | } | ||
| 76 | |||
| 77 | template <typename Scalar> | ||
| 78 | ✗ | std::size_t ImpulseModelAbstractTpl<Scalar>::get_ni() const { | |
| 79 | ✗ | return nc_; | |
| 80 | } | ||
| 81 | |||
| 82 | template <typename Scalar> | ||
| 83 | 10460 | std::size_t ImpulseModelAbstractTpl<Scalar>::get_nu() const { | |
| 84 | 10460 | return 0; | |
| 85 | } | ||
| 86 | |||
| 87 | template <typename Scalar> | ||
| 88 | 20960 | pinocchio::FrameIndex ImpulseModelAbstractTpl<Scalar>::get_id() const { | |
| 89 | 20960 | return id_; | |
| 90 | } | ||
| 91 | |||
| 92 | template <typename Scalar> | ||
| 93 | 11242 | pinocchio::ReferenceFrame ImpulseModelAbstractTpl<Scalar>::get_type() const { | |
| 94 | 11242 | return type_; | |
| 95 | } | ||
| 96 | |||
| 97 | template <typename Scalar> | ||
| 98 | 6 | void ImpulseModelAbstractTpl<Scalar>::set_id(const pinocchio::FrameIndex id) { | |
| 99 | 6 | id_ = id; | |
| 100 | 6 | } | |
| 101 | |||
| 102 | template <typename Scalar> | ||
| 103 | ✗ | void ImpulseModelAbstractTpl<Scalar>::set_type( | |
| 104 | const pinocchio::ReferenceFrame type) { | ||
| 105 | ✗ | type_ = type; | |
| 106 | } | ||
| 107 | |||
| 108 | template <class Scalar> | ||
| 109 | 30 | std::ostream& operator<<(std::ostream& os, | |
| 110 | const ImpulseModelAbstractTpl<Scalar>& model) { | ||
| 111 | 30 | model.print(os); | |
| 112 | 30 | return os; | |
| 113 | } | ||
| 114 | |||
| 115 | } // namespace crocoddyl | ||
| 116 |