| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | /////////////////////////////////////////////////////////////////////////////// | ||
| 2 | // BSD 3-Clause License | ||
| 3 | // | ||
| 4 | // Copyright (C) 2019-2025, University of Edinburgh, LAAS-CNRS, | ||
| 5 | // Heriot-Watt University | ||
| 6 | // Copyright note valid unless otherwise stated in individual files. | ||
| 7 | // All rights reserved. | ||
| 8 | /////////////////////////////////////////////////////////////////////////////// | ||
| 9 | |||
| 10 | #include "impulse.hpp" | ||
| 11 | |||
| 12 | #include "crocoddyl/multibody/impulses/impulse-3d.hpp" | ||
| 13 | #include "crocoddyl/multibody/impulses/impulse-6d.hpp" | ||
| 14 | |||
| 15 | namespace crocoddyl { | ||
| 16 | namespace unittest { | ||
| 17 | |||
| 18 | const std::vector<ImpulseModelTypes::Type> ImpulseModelTypes::all( | ||
| 19 | ImpulseModelTypes::init_all()); | ||
| 20 | |||
| 21 | ✗ | std::ostream& operator<<(std::ostream& os, | |
| 22 | const ImpulseModelTypes::Type& type) { | ||
| 23 | ✗ | switch (type) { | |
| 24 | ✗ | case ImpulseModelTypes::ImpulseModel3D_LOCAL: | |
| 25 | ✗ | os << "ImpulseModel3D_LOCAL"; | |
| 26 | ✗ | break; | |
| 27 | ✗ | case ImpulseModelTypes::ImpulseModel3D_WORLD: | |
| 28 | ✗ | os << "ImpulseModel3D_WORLD"; | |
| 29 | ✗ | break; | |
| 30 | ✗ | case ImpulseModelTypes::ImpulseModel3D_LWA: | |
| 31 | ✗ | os << "ImpulseModel3D_LWA"; | |
| 32 | ✗ | break; | |
| 33 | ✗ | case ImpulseModelTypes::ImpulseModel6D_LOCAL: | |
| 34 | ✗ | os << "ImpulseModel6D_LOCAL"; | |
| 35 | ✗ | break; | |
| 36 | ✗ | case ImpulseModelTypes::ImpulseModel6D_WORLD: | |
| 37 | ✗ | os << "ImpulseModel6D_WORLD"; | |
| 38 | ✗ | break; | |
| 39 | ✗ | case ImpulseModelTypes::ImpulseModel6D_LWA: | |
| 40 | ✗ | os << "ImpulseModel6D_LWA"; | |
| 41 | ✗ | break; | |
| 42 | ✗ | case ImpulseModelTypes::NbImpulseModelTypes: | |
| 43 | ✗ | os << "NbImpulseModelTypes"; | |
| 44 | ✗ | break; | |
| 45 | ✗ | default: | |
| 46 | ✗ | os << "Unknown type"; | |
| 47 | ✗ | break; | |
| 48 | } | ||
| 49 | ✗ | return os; | |
| 50 | } | ||
| 51 | |||
| 52 | ✗ | ImpulseModelFactory::ImpulseModelFactory() {} | |
| 53 | ✗ | ImpulseModelFactory::~ImpulseModelFactory() {} | |
| 54 | |||
| 55 | ✗ | std::shared_ptr<crocoddyl::ImpulseModelAbstract> ImpulseModelFactory::create( | |
| 56 | ImpulseModelTypes::Type impulse_type, PinocchioModelTypes::Type model_type, | ||
| 57 | const std::string frame_name) const { | ||
| 58 | ✗ | std::shared_ptr<crocoddyl::ImpulseModelAbstract> impulse; | |
| 59 | ✗ | PinocchioModelFactory model_factory(model_type); | |
| 60 | std::shared_ptr<crocoddyl::StateMultibody> state = | ||
| 61 | ✗ | std::make_shared<crocoddyl::StateMultibody>(model_factory.create()); | |
| 62 | ✗ | std::shared_ptr<crocoddyl::ContactModelAbstract> contact; | |
| 63 | ✗ | std::size_t frame_id = 0; | |
| 64 | ✗ | if (frame_name == "") { | |
| 65 | ✗ | frame_id = model_factory.get_frame_ids()[0]; | |
| 66 | } else { | ||
| 67 | ✗ | frame_id = state->get_pinocchio()->getFrameId(frame_name); | |
| 68 | } | ||
| 69 | ✗ | switch (impulse_type) { | |
| 70 | ✗ | case ImpulseModelTypes::ImpulseModel3D_LOCAL: | |
| 71 | ✗ | impulse = std::make_shared<crocoddyl::ImpulseModel3D>( | |
| 72 | ✗ | state, frame_id, pinocchio::ReferenceFrame::LOCAL); | |
| 73 | ✗ | break; | |
| 74 | ✗ | case ImpulseModelTypes::ImpulseModel3D_WORLD: | |
| 75 | ✗ | impulse = std::make_shared<crocoddyl::ImpulseModel3D>( | |
| 76 | ✗ | state, frame_id, pinocchio::ReferenceFrame::WORLD); | |
| 77 | ✗ | break; | |
| 78 | ✗ | case ImpulseModelTypes::ImpulseModel3D_LWA: | |
| 79 | ✗ | impulse = std::make_shared<crocoddyl::ImpulseModel3D>( | |
| 80 | ✗ | state, frame_id, pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED); | |
| 81 | ✗ | break; | |
| 82 | ✗ | case ImpulseModelTypes::ImpulseModel6D_LOCAL: | |
| 83 | ✗ | impulse = std::make_shared<crocoddyl::ImpulseModel6D>( | |
| 84 | ✗ | state, frame_id, pinocchio::ReferenceFrame::LOCAL); | |
| 85 | ✗ | break; | |
| 86 | ✗ | case ImpulseModelTypes::ImpulseModel6D_WORLD: | |
| 87 | ✗ | impulse = std::make_shared<crocoddyl::ImpulseModel6D>( | |
| 88 | ✗ | state, frame_id, pinocchio::ReferenceFrame::WORLD); | |
| 89 | ✗ | break; | |
| 90 | ✗ | case ImpulseModelTypes::ImpulseModel6D_LWA: | |
| 91 | ✗ | impulse = std::make_shared<crocoddyl::ImpulseModel6D>( | |
| 92 | ✗ | state, frame_id, pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED); | |
| 93 | ✗ | break; | |
| 94 | ✗ | default: | |
| 95 | ✗ | throw_pretty(__FILE__ ": Wrong ImpulseModelTypes::Type given"); | |
| 96 | break; | ||
| 97 | } | ||
| 98 | ✗ | return impulse; | |
| 99 | ✗ | } | |
| 100 | |||
| 101 | ✗ | std::shared_ptr<crocoddyl::ImpulseModelAbstract> create_random_impulse() { | |
| 102 | static bool once = true; | ||
| 103 | ✗ | if (once) { | |
| 104 | ✗ | srand((unsigned)time(NULL)); | |
| 105 | ✗ | once = false; | |
| 106 | } | ||
| 107 | ✗ | std::shared_ptr<crocoddyl::ImpulseModelAbstract> impulse; | |
| 108 | ✗ | ImpulseModelFactory factory; | |
| 109 | ✗ | if (rand() % 2 == 0) { | |
| 110 | ✗ | impulse = factory.create(ImpulseModelTypes::ImpulseModel3D_LOCAL, | |
| 111 | ✗ | PinocchioModelTypes::RandomHumanoid); | |
| 112 | } else { | ||
| 113 | ✗ | impulse = factory.create(ImpulseModelTypes::ImpulseModel6D_LOCAL, | |
| 114 | ✗ | PinocchioModelTypes::RandomHumanoid); | |
| 115 | } | ||
| 116 | ✗ | return impulse; | |
| 117 | ✗ | } | |
| 118 | |||
| 119 | } // namespace unittest | ||
| 120 | } // namespace crocoddyl | ||
| 121 |