1 |
|
|
/////////////////////////////////////////////////////////////////////////////// |
2 |
|
|
// BSD 3-Clause License |
3 |
|
|
// |
4 |
|
|
// Copyright (C) 2021, University of Edinburgh, LAAS-CNRS, University of Trento |
5 |
|
|
// Copyright note valid unless otherwise stated in individual files. |
6 |
|
|
// All rights reserved. |
7 |
|
|
/////////////////////////////////////////////////////////////////////////////// |
8 |
|
|
|
9 |
|
|
#include "integrator.hpp" |
10 |
|
|
|
11 |
|
|
#include "crocoddyl/core/integrator/euler.hpp" |
12 |
|
|
#include "crocoddyl/core/integrator/rk.hpp" |
13 |
|
|
#include "crocoddyl/core/utils/exception.hpp" |
14 |
|
|
|
15 |
|
|
namespace crocoddyl { |
16 |
|
|
namespace unittest { |
17 |
|
|
|
18 |
|
|
const std::vector<IntegratorTypes::Type> IntegratorTypes::all( |
19 |
|
|
IntegratorTypes::init_all()); |
20 |
|
|
|
21 |
|
330 |
std::ostream& operator<<(std::ostream& os, IntegratorTypes::Type type) { |
22 |
✓✓✓✓
✗✗ |
330 |
switch (type) { |
23 |
|
88 |
case IntegratorTypes::IntegratorEuler: |
24 |
|
88 |
os << "IntegratorEuler"; |
25 |
|
88 |
break; |
26 |
|
66 |
case IntegratorTypes::IntegratorRK2: |
27 |
|
66 |
os << "IntegratorRK2"; |
28 |
|
66 |
break; |
29 |
|
88 |
case IntegratorTypes::IntegratorRK3: |
30 |
|
88 |
os << "IntegratorRK3"; |
31 |
|
88 |
break; |
32 |
|
88 |
case IntegratorTypes::IntegratorRK4: |
33 |
|
88 |
os << "IntegratorRK4"; |
34 |
|
88 |
break; |
35 |
|
|
case IntegratorTypes::NbIntegratorTypes: |
36 |
|
|
os << "NbIntegratorTypes"; |
37 |
|
|
break; |
38 |
|
|
default: |
39 |
|
|
break; |
40 |
|
|
} |
41 |
|
330 |
return os; |
42 |
|
|
} |
43 |
|
|
|
44 |
|
990 |
IntegratorFactory::IntegratorFactory() {} |
45 |
|
990 |
IntegratorFactory::~IntegratorFactory() {} |
46 |
|
|
|
47 |
|
|
boost::shared_ptr<crocoddyl::IntegratedActionModelAbstract> |
48 |
|
352 |
IntegratorFactory::create( |
49 |
|
|
IntegratorTypes::Type type, |
50 |
|
|
boost::shared_ptr<DifferentialActionModelAbstract> model) const { |
51 |
|
352 |
boost::shared_ptr<crocoddyl::IntegratedActionModelAbstract> action; |
52 |
✓✓✓✓
✗ |
352 |
switch (type) { |
53 |
|
88 |
case IntegratorTypes::IntegratorEuler: |
54 |
✓✗ |
88 |
action = boost::make_shared<crocoddyl::IntegratedActionModelEuler>(model); |
55 |
|
88 |
break; |
56 |
|
88 |
case IntegratorTypes::IntegratorRK2: |
57 |
|
88 |
action = boost::make_shared<crocoddyl::IntegratedActionModelRK>( |
58 |
✓✗ |
88 |
model, RKType::two); |
59 |
|
88 |
break; |
60 |
|
88 |
case IntegratorTypes::IntegratorRK3: |
61 |
|
88 |
action = boost::make_shared<crocoddyl::IntegratedActionModelRK>( |
62 |
✓✗ |
88 |
model, RKType::three); |
63 |
|
88 |
break; |
64 |
|
88 |
case IntegratorTypes::IntegratorRK4: |
65 |
|
88 |
action = boost::make_shared<crocoddyl::IntegratedActionModelRK>( |
66 |
✓✗ |
88 |
model, RKType::four); |
67 |
|
88 |
break; |
68 |
|
|
default: |
69 |
|
|
throw_pretty(__FILE__ ": Wrong IntegratorTypes::Type given"); |
70 |
|
|
break; |
71 |
|
|
} |
72 |
|
352 |
return action; |
73 |
|
|
} |
74 |
|
|
|
75 |
|
|
boost::shared_ptr<crocoddyl::IntegratedActionModelAbstract> |
76 |
|
682 |
IntegratorFactory::create( |
77 |
|
|
IntegratorTypes::Type type, |
78 |
|
|
boost::shared_ptr<DifferentialActionModelAbstract> model, |
79 |
|
|
boost::shared_ptr<ControlParametrizationModelAbstract> control) const { |
80 |
|
682 |
boost::shared_ptr<crocoddyl::IntegratedActionModelAbstract> action; |
81 |
✓✓✓✓
✗ |
682 |
switch (type) { |
82 |
|
154 |
case IntegratorTypes::IntegratorEuler: |
83 |
✓✗ |
308 |
action = boost::make_shared<crocoddyl::IntegratedActionModelEuler>( |
84 |
|
154 |
model, control); |
85 |
|
154 |
break; |
86 |
|
132 |
case IntegratorTypes::IntegratorRK2: |
87 |
|
132 |
action = boost::make_shared<crocoddyl::IntegratedActionModelRK>( |
88 |
✓✗ |
132 |
model, control, RKType::two); |
89 |
|
132 |
break; |
90 |
|
198 |
case IntegratorTypes::IntegratorRK3: |
91 |
|
198 |
action = boost::make_shared<crocoddyl::IntegratedActionModelRK>( |
92 |
✓✗ |
198 |
model, control, RKType::three); |
93 |
|
198 |
break; |
94 |
|
198 |
case IntegratorTypes::IntegratorRK4: |
95 |
|
198 |
action = boost::make_shared<crocoddyl::IntegratedActionModelRK>( |
96 |
✓✗ |
198 |
model, control, RKType::four); |
97 |
|
198 |
break; |
98 |
|
|
default: |
99 |
|
|
throw_pretty(__FILE__ ": Wrong IntegratorTypes::Type given"); |
100 |
|
|
break; |
101 |
|
|
} |
102 |
|
682 |
return action; |
103 |
|
|
} |
104 |
|
|
|
105 |
|
|
} // namespace unittest |
106 |
|
|
} // namespace crocoddyl |