| 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 | namespace crocoddyl { | ||
| 11 | |||
| 12 | template <typename Scalar> | ||
| 13 | ✗ | ImpulseModelMultipleTpl<Scalar>::ImpulseModelMultipleTpl( | |
| 14 | std::shared_ptr<StateMultibody> state) | ||
| 15 | ✗ | : state_(state), nc_(0), nc_total_(0) {} | |
| 16 | |||
| 17 | template <typename Scalar> | ||
| 18 | ✗ | ImpulseModelMultipleTpl<Scalar>::~ImpulseModelMultipleTpl() {} | |
| 19 | |||
| 20 | template <typename Scalar> | ||
| 21 | ✗ | void ImpulseModelMultipleTpl<Scalar>::addImpulse( | |
| 22 | const std::string& name, std::shared_ptr<ImpulseModelAbstract> impulse, | ||
| 23 | const bool active) { | ||
| 24 | std::pair<typename ImpulseModelContainer::iterator, bool> ret = | ||
| 25 | ✗ | impulses_.insert(std::make_pair( | |
| 26 | name, std::make_shared<ImpulseItem>(name, impulse, active))); | ||
| 27 | ✗ | if (ret.second == false) { | |
| 28 | std::cerr << "Warning: we couldn't add the " << name | ||
| 29 | ✗ | << " impulse item, it already existed." << std::endl; | |
| 30 | ✗ | } else if (active) { | |
| 31 | ✗ | nc_ += impulse->get_nc(); | |
| 32 | ✗ | nc_total_ += impulse->get_nc(); | |
| 33 | ✗ | active_set_.insert(name); | |
| 34 | ✗ | } else if (!active) { | |
| 35 | ✗ | nc_total_ += impulse->get_nc(); | |
| 36 | ✗ | inactive_set_.insert(name); | |
| 37 | } | ||
| 38 | ✗ | } | |
| 39 | |||
| 40 | template <typename Scalar> | ||
| 41 | ✗ | void ImpulseModelMultipleTpl<Scalar>::removeImpulse(const std::string& name) { | |
| 42 | ✗ | typename ImpulseModelContainer::iterator it = impulses_.find(name); | |
| 43 | ✗ | if (it != impulses_.end()) { | |
| 44 | ✗ | nc_ -= it->second->impulse->get_nc(); | |
| 45 | ✗ | nc_total_ -= it->second->impulse->get_nc(); | |
| 46 | ✗ | impulses_.erase(it); | |
| 47 | ✗ | active_set_.erase(name); | |
| 48 | ✗ | inactive_set_.erase(name); | |
| 49 | } else { | ||
| 50 | std::cerr << "Warning: we couldn't remove the " << name | ||
| 51 | ✗ | << " impulse item, it doesn't exist." << std::endl; | |
| 52 | } | ||
| 53 | ✗ | } | |
| 54 | |||
| 55 | template <typename Scalar> | ||
| 56 | ✗ | void ImpulseModelMultipleTpl<Scalar>::changeImpulseStatus( | |
| 57 | const std::string& name, const bool active) { | ||
| 58 | ✗ | typename ImpulseModelContainer::iterator it = impulses_.find(name); | |
| 59 | ✗ | if (it != impulses_.end()) { | |
| 60 | ✗ | if (active && !it->second->active) { | |
| 61 | ✗ | nc_ += it->second->impulse->get_nc(); | |
| 62 | ✗ | active_set_.insert(name); | |
| 63 | ✗ | inactive_set_.erase(name); | |
| 64 | ✗ | } else if (!active && it->second->active) { | |
| 65 | ✗ | nc_ -= it->second->impulse->get_nc(); | |
| 66 | ✗ | inactive_set_.insert(name); | |
| 67 | ✗ | active_set_.erase(name); | |
| 68 | } | ||
| 69 | ✗ | it->second->active = active; | |
| 70 | } else { | ||
| 71 | std::cerr << "Warning: we couldn't change the status of the " << name | ||
| 72 | ✗ | << " impulse item, it doesn't exist." << std::endl; | |
| 73 | } | ||
| 74 | ✗ | } | |
| 75 | |||
| 76 | template <typename Scalar> | ||
| 77 | ✗ | void ImpulseModelMultipleTpl<Scalar>::calc( | |
| 78 | const std::shared_ptr<ImpulseDataMultiple>& data, | ||
| 79 | const Eigen::Ref<const VectorXs>& x) { | ||
| 80 | ✗ | if (data->impulses.size() != impulses_.size()) { | |
| 81 | ✗ | throw_pretty("Invalid argument: " | |
| 82 | << "it doesn't match the number of impulse datas and models"); | ||
| 83 | } | ||
| 84 | |||
| 85 | ✗ | std::size_t nc = 0; | |
| 86 | ✗ | const std::size_t nv = state_->get_nv(); | |
| 87 | ✗ | typename ImpulseModelContainer::iterator it_m, end_m; | |
| 88 | ✗ | typename ImpulseDataContainer::iterator it_d, end_d; | |
| 89 | ✗ | for (it_m = impulses_.begin(), end_m = impulses_.end(), | |
| 90 | ✗ | it_d = data->impulses.begin(), end_d = data->impulses.end(); | |
| 91 | ✗ | it_m != end_m || it_d != end_d; ++it_m, ++it_d) { | |
| 92 | ✗ | const std::shared_ptr<ImpulseItem>& m_i = it_m->second; | |
| 93 | ✗ | if (m_i->active) { | |
| 94 | ✗ | const std::shared_ptr<ImpulseDataAbstract>& d_i = it_d->second; | |
| 95 | ✗ | assert_pretty(it_m->first == it_d->first, | |
| 96 | "it doesn't match the impulse name between model and data (" | ||
| 97 | << it_m->first << " != " << it_d->first << ")"); | ||
| 98 | |||
| 99 | ✗ | m_i->impulse->calc(d_i, x); | |
| 100 | ✗ | const std::size_t nc_i = m_i->impulse->get_nc(); | |
| 101 | ✗ | data->Jc.block(nc, 0, nc_i, nv) = d_i->Jc; | |
| 102 | ✗ | nc += nc_i; | |
| 103 | } | ||
| 104 | } | ||
| 105 | ✗ | } | |
| 106 | |||
| 107 | template <typename Scalar> | ||
| 108 | ✗ | void ImpulseModelMultipleTpl<Scalar>::calcDiff( | |
| 109 | const std::shared_ptr<ImpulseDataMultiple>& data, | ||
| 110 | const Eigen::Ref<const VectorXs>& x) { | ||
| 111 | ✗ | if (data->impulses.size() != impulses_.size()) { | |
| 112 | ✗ | throw_pretty("Invalid argument: " | |
| 113 | << "it doesn't match the number of impulse datas and models"); | ||
| 114 | } | ||
| 115 | |||
| 116 | ✗ | std::size_t nc = 0; | |
| 117 | ✗ | const std::size_t nv = state_->get_nv(); | |
| 118 | ✗ | typename ImpulseModelContainer::iterator it_m, end_m; | |
| 119 | ✗ | typename ImpulseDataContainer::iterator it_d, end_d; | |
| 120 | ✗ | for (it_m = impulses_.begin(), end_m = impulses_.end(), | |
| 121 | ✗ | it_d = data->impulses.begin(), end_d = data->impulses.end(); | |
| 122 | ✗ | it_m != end_m || it_d != end_d; ++it_m, ++it_d) { | |
| 123 | ✗ | const std::shared_ptr<ImpulseItem>& m_i = it_m->second; | |
| 124 | ✗ | if (m_i->active) { | |
| 125 | ✗ | const std::shared_ptr<ImpulseDataAbstract>& d_i = it_d->second; | |
| 126 | ✗ | assert_pretty(it_m->first == it_d->first, | |
| 127 | "it doesn't match the impulse name between model and data (" | ||
| 128 | << it_m->first << " != " << it_d->first << ")"); | ||
| 129 | |||
| 130 | ✗ | m_i->impulse->calcDiff(d_i, x); | |
| 131 | ✗ | const std::size_t nc_i = m_i->impulse->get_nc(); | |
| 132 | ✗ | data->dv0_dq.block(nc, 0, nc_i, nv) = d_i->dv0_dq; | |
| 133 | ✗ | nc += nc_i; | |
| 134 | } | ||
| 135 | } | ||
| 136 | ✗ | } | |
| 137 | |||
| 138 | template <typename Scalar> | ||
| 139 | ✗ | void ImpulseModelMultipleTpl<Scalar>::updateVelocity( | |
| 140 | const std::shared_ptr<ImpulseDataMultiple>& data, | ||
| 141 | const VectorXs& vnext) const { | ||
| 142 | ✗ | if (static_cast<std::size_t>(vnext.size()) != state_->get_nv()) { | |
| 143 | ✗ | throw_pretty( | |
| 144 | "Invalid argument: " << "vnext has wrong dimension (it should be " + | ||
| 145 | std::to_string(state_->get_nv()) + ")"); | ||
| 146 | } | ||
| 147 | ✗ | data->vnext = vnext; | |
| 148 | ✗ | } | |
| 149 | |||
| 150 | template <typename Scalar> | ||
| 151 | ✗ | void ImpulseModelMultipleTpl<Scalar>::updateForce( | |
| 152 | const std::shared_ptr<ImpulseDataMultiple>& data, const VectorXs& force) { | ||
| 153 | ✗ | if (static_cast<std::size_t>(force.size()) != nc_) { | |
| 154 | ✗ | throw_pretty( | |
| 155 | "Invalid argument: " << "force has wrong dimension (it should be " + | ||
| 156 | std::to_string(nc_) + ")"); | ||
| 157 | } | ||
| 158 | ✗ | if (static_cast<std::size_t>(data->impulses.size()) != impulses_.size()) { | |
| 159 | ✗ | throw_pretty("Invalid argument: " | |
| 160 | << "it doesn't match the number of impulse datas and models"); | ||
| 161 | } | ||
| 162 | |||
| 163 | ✗ | for (ForceIterator it = data->fext.begin(); it != data->fext.end(); ++it) { | |
| 164 | ✗ | *it = pinocchio::ForceTpl<Scalar>::Zero(); | |
| 165 | } | ||
| 166 | |||
| 167 | ✗ | std::size_t nc = 0; | |
| 168 | ✗ | typename ImpulseModelContainer::iterator it_m, end_m; | |
| 169 | ✗ | typename ImpulseDataContainer::iterator it_d, end_d; | |
| 170 | ✗ | for (it_m = impulses_.begin(), end_m = impulses_.end(), | |
| 171 | ✗ | it_d = data->impulses.begin(), end_d = data->impulses.end(); | |
| 172 | ✗ | it_m != end_m || it_d != end_d; ++it_m, ++it_d) { | |
| 173 | ✗ | const std::shared_ptr<ImpulseItem>& m_i = it_m->second; | |
| 174 | ✗ | const std::shared_ptr<ImpulseDataAbstract>& d_i = it_d->second; | |
| 175 | ✗ | assert_pretty(it_m->first == it_d->first, | |
| 176 | "it doesn't match the impulse name between data and model"); | ||
| 177 | ✗ | if (m_i->active) { | |
| 178 | ✗ | const std::size_t nc_i = m_i->impulse->get_nc(); | |
| 179 | const Eigen::VectorBlock<const VectorXs, Eigen::Dynamic> force_i = | ||
| 180 | ✗ | force.segment(nc, nc_i); | |
| 181 | ✗ | m_i->impulse->updateForce(d_i, force_i); | |
| 182 | ✗ | const pinocchio::JointIndex joint = | |
| 183 | ✗ | state_->get_pinocchio()->frames[d_i->frame].parentJoint; | |
| 184 | ✗ | data->fext[joint] = d_i->fext; | |
| 185 | ✗ | nc += nc_i; | |
| 186 | } else { | ||
| 187 | ✗ | m_i->impulse->setZeroForce(d_i); | |
| 188 | } | ||
| 189 | } | ||
| 190 | ✗ | } | |
| 191 | |||
| 192 | template <typename Scalar> | ||
| 193 | ✗ | void ImpulseModelMultipleTpl<Scalar>::updateVelocityDiff( | |
| 194 | const std::shared_ptr<ImpulseDataMultiple>& data, | ||
| 195 | const MatrixXs& dvnext_dx) const { | ||
| 196 | ✗ | if (static_cast<std::size_t>(dvnext_dx.rows()) != state_->get_nv() || | |
| 197 | ✗ | static_cast<std::size_t>(dvnext_dx.cols()) != state_->get_ndx()) { | |
| 198 | ✗ | throw_pretty( | |
| 199 | "Invalid argument: " << "dvnext_dx has wrong dimension (it should be " + | ||
| 200 | std::to_string(state_->get_nv()) + "," + | ||
| 201 | std::to_string(state_->get_ndx()) + ")"); | ||
| 202 | } | ||
| 203 | ✗ | data->dvnext_dx = dvnext_dx; | |
| 204 | ✗ | } | |
| 205 | |||
| 206 | template <typename Scalar> | ||
| 207 | ✗ | void ImpulseModelMultipleTpl<Scalar>::updateForceDiff( | |
| 208 | const std::shared_ptr<ImpulseDataMultiple>& data, | ||
| 209 | const MatrixXs& df_dx) const { | ||
| 210 | ✗ | const std::size_t ndx = state_->get_ndx(); | |
| 211 | ✗ | if (static_cast<std::size_t>(df_dx.rows()) != nc_ || | |
| 212 | ✗ | static_cast<std::size_t>(df_dx.cols()) != ndx) { | |
| 213 | ✗ | throw_pretty("Invalid argument: " | |
| 214 | << "df_dx has wrong dimension (it should be " + | ||
| 215 | std::to_string(nc_) + "," + std::to_string(ndx) + ")"); | ||
| 216 | } | ||
| 217 | ✗ | if (static_cast<std::size_t>(data->impulses.size()) != impulses_.size()) { | |
| 218 | ✗ | throw_pretty("Invalid argument: " | |
| 219 | << "it doesn't match the number of impulse datas and models"); | ||
| 220 | } | ||
| 221 | |||
| 222 | ✗ | std::size_t nc = 0; | |
| 223 | ✗ | typename ImpulseModelContainer::const_iterator it_m, end_m; | |
| 224 | ✗ | typename ImpulseDataContainer::const_iterator it_d, end_d; | |
| 225 | ✗ | for (it_m = impulses_.begin(), end_m = impulses_.end(), | |
| 226 | ✗ | it_d = data->impulses.begin(), end_d = data->impulses.end(); | |
| 227 | ✗ | it_m != end_m || it_d != end_d; ++it_m, ++it_d) { | |
| 228 | ✗ | const std::shared_ptr<ImpulseItem>& m_i = it_m->second; | |
| 229 | ✗ | const std::shared_ptr<ImpulseDataAbstract>& d_i = it_d->second; | |
| 230 | ✗ | assert_pretty(it_m->first == it_d->first, | |
| 231 | "it doesn't match the impulse name between data and model"); | ||
| 232 | ✗ | if (m_i->active) { | |
| 233 | ✗ | const std::size_t nc_i = m_i->impulse->get_nc(); | |
| 234 | const Eigen::Block<const MatrixXs> df_dx_i = | ||
| 235 | ✗ | df_dx.block(nc, 0, nc_i, ndx); | |
| 236 | ✗ | m_i->impulse->updateForceDiff(d_i, df_dx_i); | |
| 237 | ✗ | nc += nc_i; | |
| 238 | } else { | ||
| 239 | ✗ | m_i->impulse->setZeroForceDiff(d_i); | |
| 240 | } | ||
| 241 | } | ||
| 242 | ✗ | } | |
| 243 | |||
| 244 | template <typename Scalar> | ||
| 245 | ✗ | void ImpulseModelMultipleTpl<Scalar>::updateRneaDiff( | |
| 246 | const std::shared_ptr<ImpulseDataMultiple>& data, | ||
| 247 | pinocchio::DataTpl<Scalar>& pinocchio) const { | ||
| 248 | ✗ | if (static_cast<std::size_t>(data->impulses.size()) != | |
| 249 | ✗ | this->get_impulses().size()) { | |
| 250 | ✗ | throw_pretty("Invalid argument: " | |
| 251 | << "it doesn't match the number of impulse datas and models"); | ||
| 252 | } | ||
| 253 | ✗ | typename ImpulseModelContainer::const_iterator it_m, end_m; | |
| 254 | ✗ | typename ImpulseDataContainer::const_iterator it_d, end_d; | |
| 255 | ✗ | for (it_m = impulses_.begin(), end_m = impulses_.end(), | |
| 256 | ✗ | it_d = data->impulses.begin(), end_d = data->impulses.end(); | |
| 257 | ✗ | it_m != end_m || it_d != end_d; ++it_m, ++it_d) { | |
| 258 | ✗ | const std::shared_ptr<ImpulseItem>& m_i = it_m->second; | |
| 259 | ✗ | const std::shared_ptr<ImpulseDataAbstract>& d_i = it_d->second; | |
| 260 | ✗ | assert_pretty(it_m->first == it_d->first, | |
| 261 | "it doesn't match the impulse name between data and model"); | ||
| 262 | ✗ | if (m_i->active) { | |
| 263 | ✗ | switch (m_i->impulse->get_type()) { | |
| 264 | ✗ | case pinocchio::ReferenceFrame::LOCAL: | |
| 265 | ✗ | break; | |
| 266 | ✗ | case pinocchio::ReferenceFrame::WORLD: | |
| 267 | case pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED: | ||
| 268 | ✗ | pinocchio.dtau_dq += d_i->dtau_dq; | |
| 269 | ✗ | break; | |
| 270 | } | ||
| 271 | } | ||
| 272 | } | ||
| 273 | ✗ | } | |
| 274 | |||
| 275 | template <typename Scalar> | ||
| 276 | std::shared_ptr<ImpulseDataMultipleTpl<Scalar> > | ||
| 277 | ✗ | ImpulseModelMultipleTpl<Scalar>::createData( | |
| 278 | pinocchio::DataTpl<Scalar>* const data) { | ||
| 279 | return std::allocate_shared<ImpulseDataMultiple>( | ||
| 280 | ✗ | Eigen::aligned_allocator<ImpulseDataMultiple>(), this, data); | |
| 281 | } | ||
| 282 | |||
| 283 | template <typename Scalar> | ||
| 284 | template <typename NewScalar> | ||
| 285 | ✗ | ImpulseModelMultipleTpl<NewScalar> ImpulseModelMultipleTpl<Scalar>::cast() | |
| 286 | const { | ||
| 287 | typedef ImpulseModelMultipleTpl<NewScalar> ReturnType; | ||
| 288 | typedef StateMultibodyTpl<NewScalar> StateType; | ||
| 289 | typedef ImpulseItemTpl<NewScalar> ImpulseType; | ||
| 290 | ✗ | ReturnType ret( | |
| 291 | ✗ | std::make_shared<StateType>(state_->template cast<NewScalar>())); | |
| 292 | ✗ | typename ImpulseModelContainer::const_iterator it_m, end_m; | |
| 293 | ✗ | for (it_m = impulses_.begin(), end_m = impulses_.end(); it_m != end_m; | |
| 294 | ✗ | ++it_m) { | |
| 295 | ✗ | const std::string name = it_m->first; | |
| 296 | ✗ | const ImpulseType& m_i = it_m->second->template cast<NewScalar>(); | |
| 297 | ✗ | ret.addImpulse(name, m_i.impulse, m_i.active); | |
| 298 | } | ||
| 299 | ✗ | return ret; | |
| 300 | ✗ | } | |
| 301 | |||
| 302 | template <typename Scalar> | ||
| 303 | const std::shared_ptr<StateMultibodyTpl<Scalar> >& | ||
| 304 | ✗ | ImpulseModelMultipleTpl<Scalar>::get_state() const { | |
| 305 | ✗ | return state_; | |
| 306 | } | ||
| 307 | |||
| 308 | template <typename Scalar> | ||
| 309 | const typename ImpulseModelMultipleTpl<Scalar>::ImpulseModelContainer& | ||
| 310 | ✗ | ImpulseModelMultipleTpl<Scalar>::get_impulses() const { | |
| 311 | ✗ | return impulses_; | |
| 312 | } | ||
| 313 | |||
| 314 | template <typename Scalar> | ||
| 315 | ✗ | std::size_t ImpulseModelMultipleTpl<Scalar>::get_nc() const { | |
| 316 | ✗ | return nc_; | |
| 317 | } | ||
| 318 | |||
| 319 | template <typename Scalar> | ||
| 320 | ✗ | std::size_t ImpulseModelMultipleTpl<Scalar>::get_nc_total() const { | |
| 321 | ✗ | return nc_total_; | |
| 322 | } | ||
| 323 | |||
| 324 | template <typename Scalar> | ||
| 325 | ✗ | const std::set<std::string>& ImpulseModelMultipleTpl<Scalar>::get_active_set() | |
| 326 | const { | ||
| 327 | ✗ | return active_set_; | |
| 328 | } | ||
| 329 | |||
| 330 | template <typename Scalar> | ||
| 331 | ✗ | const std::set<std::string>& ImpulseModelMultipleTpl<Scalar>::get_inactive_set() | |
| 332 | const { | ||
| 333 | ✗ | return inactive_set_; | |
| 334 | } | ||
| 335 | |||
| 336 | template <typename Scalar> | ||
| 337 | ✗ | bool ImpulseModelMultipleTpl<Scalar>::getImpulseStatus( | |
| 338 | const std::string& name) const { | ||
| 339 | ✗ | typename ImpulseModelContainer::const_iterator it = impulses_.find(name); | |
| 340 | ✗ | if (it != impulses_.end()) { | |
| 341 | ✗ | return it->second->active; | |
| 342 | } else { | ||
| 343 | std::cerr << "Warning: we couldn't get the status of the " << name | ||
| 344 | ✗ | << " impulse item, it doesn't exist." << std::endl; | |
| 345 | ✗ | return false; | |
| 346 | } | ||
| 347 | } | ||
| 348 | |||
| 349 | template <class Scalar> | ||
| 350 | ✗ | std::ostream& operator<<(std::ostream& os, | |
| 351 | const ImpulseModelMultipleTpl<Scalar>& model) { | ||
| 352 | ✗ | const auto& active = model.get_active_set(); | |
| 353 | ✗ | const auto& inactive = model.get_inactive_set(); | |
| 354 | ✗ | os << "ImpulseModelMultiple:" << std::endl; | |
| 355 | ✗ | os << " Active:" << std::endl; | |
| 356 | ✗ | for (std::set<std::string>::const_iterator it = active.begin(); | |
| 357 | ✗ | it != active.end(); ++it) { | |
| 358 | const std::shared_ptr< | ||
| 359 | ✗ | typename ImpulseModelMultipleTpl<Scalar>::ImpulseItem>& impulse_item = | |
| 360 | ✗ | model.get_impulses().find(*it)->second; | |
| 361 | ✗ | if (it != --active.end()) { | |
| 362 | ✗ | os << " " << *it << ": " << *impulse_item << std::endl; | |
| 363 | } else { | ||
| 364 | ✗ | os << " " << *it << ": " << *impulse_item << std::endl; | |
| 365 | } | ||
| 366 | } | ||
| 367 | ✗ | os << " Inactive:" << std::endl; | |
| 368 | ✗ | for (std::set<std::string>::const_iterator it = inactive.begin(); | |
| 369 | ✗ | it != inactive.end(); ++it) { | |
| 370 | const std::shared_ptr< | ||
| 371 | ✗ | typename ImpulseModelMultipleTpl<Scalar>::ImpulseItem>& impulse_item = | |
| 372 | ✗ | model.get_impulses().find(*it)->second; | |
| 373 | ✗ | if (it != --inactive.end()) { | |
| 374 | ✗ | os << " " << *it << ": " << *impulse_item << std::endl; | |
| 375 | } else { | ||
| 376 | ✗ | os << " " << *it << ": " << *impulse_item; | |
| 377 | } | ||
| 378 | } | ||
| 379 | ✗ | return os; | |
| 380 | } | ||
| 381 | |||
| 382 | } // namespace crocoddyl | ||
| 383 |