1 |
|
|
/////////////////////////////////////////////////////////////////////////////// |
2 |
|
|
// BSD 3-Clause License |
3 |
|
|
// |
4 |
|
|
// Copyright (C) 2019-2023, LAAS-CNRS, University of Edinburgh, IRI: CSIC-UPC, |
5 |
|
|
// University of Trento, Heriot-Watt University |
6 |
|
|
// Copyright note valid unless otherwise stated in individual files. |
7 |
|
|
// All rights reserved. |
8 |
|
|
/////////////////////////////////////////////////////////////////////////////// |
9 |
|
|
|
10 |
|
|
#include "crocoddyl/core/integrator/rk4.hpp" |
11 |
|
|
|
12 |
|
|
#include "python/crocoddyl/core/core.hpp" |
13 |
|
|
#include "python/crocoddyl/core/integ-action-base.hpp" |
14 |
|
|
#include "python/crocoddyl/utils/copyable.hpp" |
15 |
|
|
|
16 |
|
|
#pragma GCC diagnostic push |
17 |
|
|
#pragma GCC diagnostic ignored "-Wdeprecated-declarations" |
18 |
|
|
|
19 |
|
|
namespace crocoddyl { |
20 |
|
|
namespace python { |
21 |
|
|
|
22 |
|
10 |
void exposeIntegratedActionRK4() { |
23 |
|
10 |
bp::register_ptr_to_python<boost::shared_ptr<IntegratedActionModelRK4> >(); |
24 |
|
|
|
25 |
✓✗ |
10 |
bp::class_<IntegratedActionModelRK4, |
26 |
|
|
bp::bases<IntegratedActionModelAbstract, ActionModelAbstract> >( |
27 |
|
|
"IntegratedActionModelRK4", |
28 |
|
|
"RK4 integrator for differential action models.\n\n" |
29 |
|
|
"This class implements an RK4 integrator\n" |
30 |
|
|
"given a differential action model, i.e.:\n" |
31 |
|
|
" [q+, v+] = State.integrate([q, v], dt / 6 (k0 + k1 + k2 + k3)) with\n" |
32 |
|
|
"k0 = f(x, u)\n" |
33 |
|
|
"k1 = f(x + dt / 2 * k0, u)\n" |
34 |
|
|
"k2 = f(x + dt / 2 * k1, u)\n" |
35 |
|
|
"k3 = f(x + dt * k2, u)", |
36 |
✓✗ |
10 |
bp::init<boost::shared_ptr<DifferentialActionModelAbstract>, |
37 |
|
|
bp::optional<double, bool> >( |
38 |
|
20 |
bp::args("self", "diffModel", "stepTime", "withCostResidual"), |
39 |
|
|
"Initialize the RK4 integrator.\n\n" |
40 |
|
|
":param diffModel: differential action model\n" |
41 |
|
|
":param stepTime: step time (default 1e-3)\n" |
42 |
|
|
":param withCostResidual: includes the cost residuals and " |
43 |
|
|
"derivatives (default True).")) |
44 |
✓✗ |
10 |
.def(bp::init<boost::shared_ptr<DifferentialActionModelAbstract>, |
45 |
|
|
boost::shared_ptr<ControlParametrizationModelAbstract>, |
46 |
|
|
bp::optional<double, bool> >( |
47 |
✓✗ |
20 |
bp::args("self", "diffModel", "control", "stepTime", |
48 |
|
|
"withCostResidual"), |
49 |
|
|
"Initialize the RK4 integrator.\n\n" |
50 |
|
|
":param diffModel: differential action model\n" |
51 |
|
|
":param control: the control parametrization\n" |
52 |
|
|
":param stepTime: step time (default 1e-3)\n" |
53 |
|
|
":param withCostResidual: includes the cost residuals and " |
54 |
✓✗ |
10 |
"derivatives (default True).")) |
55 |
|
|
.def<void (IntegratedActionModelRK4::*)( |
56 |
|
|
const boost::shared_ptr<ActionDataAbstract>&, |
57 |
|
|
const Eigen::Ref<const Eigen::VectorXd>&, |
58 |
|
|
const Eigen::Ref<const Eigen::VectorXd>&)>( |
59 |
|
|
"calc", &IntegratedActionModelRK4::calc, |
60 |
✓✗ |
20 |
bp::args("self", "data", "x", "u"), |
61 |
|
|
"Compute the time-discrete evolution of a differential action " |
62 |
|
|
"model.\n\n" |
63 |
|
|
"It describes the time-discrete evolution of action model.\n" |
64 |
|
|
":param data: action data\n" |
65 |
|
|
":param x: state point (dim. state.nx)\n" |
66 |
|
20 |
":param u: control input (dim. nu)") |
67 |
|
|
.def<void (IntegratedActionModelRK4::*)( |
68 |
|
|
const boost::shared_ptr<ActionDataAbstract>&, |
69 |
|
|
const Eigen::Ref<const Eigen::VectorXd>&)>( |
70 |
✓✗✓✗
|
20 |
"calc", &ActionModelAbstract::calc, bp::args("self", "data", "x")) |
71 |
|
|
.def<void (IntegratedActionModelRK4::*)( |
72 |
|
|
const boost::shared_ptr<ActionDataAbstract>&, |
73 |
|
|
const Eigen::Ref<const Eigen::VectorXd>&, |
74 |
|
|
const Eigen::Ref<const Eigen::VectorXd>&)>( |
75 |
|
|
"calcDiff", &IntegratedActionModelRK4::calcDiff, |
76 |
✓✗ |
20 |
bp::args("self", "data", "x", "u"), |
77 |
|
|
"Computes the derivatives of the integrated action model wrt state " |
78 |
|
|
"and control. \n\n" |
79 |
|
|
"This function builds a quadratic approximation of the\n" |
80 |
|
|
"action model (i.e. dynamical system and cost function).\n" |
81 |
|
|
"It assumes that calc has been run first.\n" |
82 |
|
|
":param data: action data\n" |
83 |
|
|
":param x: state point (dim. state.nx)\n" |
84 |
✓✗ |
10 |
":param u: control input (dim. nu)") |
85 |
|
|
.def<void (IntegratedActionModelRK4::*)( |
86 |
|
|
const boost::shared_ptr<ActionDataAbstract>&, |
87 |
|
|
const Eigen::Ref<const Eigen::VectorXd>&)>( |
88 |
|
|
"calcDiff", &ActionModelAbstract::calcDiff, |
89 |
✓✗✓✗
|
20 |
bp::args("self", "data", "x")) |
90 |
|
|
.def("createData", &IntegratedActionModelRK4::createData, |
91 |
✓✗✓✗
✓✗ |
20 |
bp::args("self"), "Create the RK4 integrator data.") |
92 |
✓✗ |
10 |
.def(CopyableVisitor<IntegratedActionModelRK4>()); |
93 |
|
|
|
94 |
|
10 |
bp::register_ptr_to_python<boost::shared_ptr<IntegratedActionDataRK4> >(); |
95 |
|
|
|
96 |
✓✗ |
10 |
bp::class_<IntegratedActionDataRK4, bp::bases<IntegratedActionDataAbstract> >( |
97 |
|
|
"IntegratedActionDataRK4", "RK4 integrator data.", |
98 |
✓✗ |
20 |
bp::init<IntegratedActionModelRK4*>(bp::args("self", "model"), |
99 |
|
|
"Create RK4 integrator data.\n\n" |
100 |
|
|
":param model: RK4 integrator model")) |
101 |
|
|
.add_property( |
102 |
|
|
"differential", |
103 |
✓✗ |
10 |
bp::make_getter(&IntegratedActionDataRK4::differential, |
104 |
|
10 |
bp::return_value_policy<bp::return_by_value>()), |
105 |
✓✗ |
10 |
"list of differential action data") |
106 |
|
|
.add_property( |
107 |
|
|
"control", |
108 |
✓✗ |
10 |
bp::make_getter(&IntegratedActionDataRK4::control, |
109 |
|
10 |
bp::return_value_policy<bp::return_by_value>()), |
110 |
✓✗ |
10 |
"list of control parametrization data") |
111 |
|
|
.add_property( |
112 |
|
|
"integral", |
113 |
✓✗ |
10 |
bp::make_getter(&IntegratedActionDataRK4::integral, |
114 |
|
10 |
bp::return_value_policy<bp::return_by_value>()), |
115 |
✓✗ |
10 |
"list of RK4 terms related to the cost") |
116 |
|
|
.add_property("dx", |
117 |
✓✗ |
10 |
bp::make_getter(&IntegratedActionDataRK4::dx, |
118 |
|
10 |
bp::return_internal_reference<>()), |
119 |
✓✗ |
10 |
"state rate.") |
120 |
|
|
.add_property("ki", |
121 |
✓✗ |
10 |
bp::make_getter(&IntegratedActionDataRK4::ki, |
122 |
|
10 |
bp::return_internal_reference<>()), |
123 |
✓✗ |
10 |
"list of RK4 terms related to system dynamics") |
124 |
|
|
.add_property( |
125 |
|
|
"y", |
126 |
✓✗ |
10 |
bp::make_getter(&IntegratedActionDataRK4::y, |
127 |
|
10 |
bp::return_internal_reference<>()), |
128 |
✓✗ |
10 |
"list of states where f is evaluated in the RK4 integration") |
129 |
|
|
.add_property("ws", |
130 |
✓✗ |
10 |
bp::make_getter(&IntegratedActionDataRK4::ws, |
131 |
|
10 |
bp::return_internal_reference<>()), |
132 |
✓✗ |
10 |
"control inputs evaluated in the RK4 integration") |
133 |
|
|
.add_property("dki_dx", |
134 |
✓✗ |
10 |
bp::make_getter(&IntegratedActionDataRK4::dki_dx, |
135 |
|
10 |
bp::return_internal_reference<>()), |
136 |
|
|
"list of partial derivatives of RK4 nodes with respect to " |
137 |
|
|
"the state of the RK4 " |
138 |
✓✗ |
10 |
"integration. dki/dx") |
139 |
|
|
.add_property("dki_du", |
140 |
✓✗ |
10 |
bp::make_getter(&IntegratedActionDataRK4::dki_du, |
141 |
|
10 |
bp::return_internal_reference<>()), |
142 |
|
|
"list of partial derivatives of RK4 nodes with respect to " |
143 |
|
|
"the control parameters of the RK4 " |
144 |
✓✗ |
10 |
"integration. dki/du") |
145 |
|
|
.add_property("dyi_dx", |
146 |
✓✗ |
10 |
bp::make_getter(&IntegratedActionDataRK4::dyi_dx, |
147 |
|
10 |
bp::return_internal_reference<>()), |
148 |
|
|
"list of partial derivatives of RK4 dynamics with respect " |
149 |
✓✗ |
10 |
"to the state of the RK4 integrator. dyi/dx") |
150 |
|
|
.add_property("dyi_du", |
151 |
✓✗ |
10 |
bp::make_getter(&IntegratedActionDataRK4::dyi_du, |
152 |
|
10 |
bp::return_internal_reference<>()), |
153 |
|
|
"list of partial derivatives of RK4 dynamics with respect " |
154 |
|
|
"to the control parameters of the " |
155 |
✓✗ |
10 |
"RK4 integrator. dyi/du") |
156 |
|
|
.add_property("dli_dx", |
157 |
✓✗ |
10 |
bp::make_getter(&IntegratedActionDataRK4::dli_dx, |
158 |
|
10 |
bp::return_internal_reference<>()), |
159 |
|
|
"list of partial derivatives of the cost with respect to " |
160 |
|
|
"the state of the RK4 " |
161 |
✓✗ |
10 |
"integration. dli_dx") |
162 |
|
|
.add_property("dli_du", |
163 |
✓✗ |
10 |
bp::make_getter(&IntegratedActionDataRK4::dli_du, |
164 |
|
10 |
bp::return_internal_reference<>()), |
165 |
|
|
"list of partial derivatives of the cost with respect to " |
166 |
|
|
"the control input of the RK4 " |
167 |
✓✗ |
10 |
"integration. dli_du") |
168 |
|
|
.add_property("ddli_ddx", |
169 |
✓✗ |
10 |
bp::make_getter(&IntegratedActionDataRK4::ddli_ddx, |
170 |
|
10 |
bp::return_internal_reference<>()), |
171 |
|
|
"list of second partial derivatives of the cost with " |
172 |
|
|
"respect to the state of the RK4 " |
173 |
✓✗ |
10 |
"integration. ddli_ddx") |
174 |
|
|
.add_property("ddli_ddw", |
175 |
✓✗ |
10 |
bp::make_getter(&IntegratedActionDataRK4::ddli_ddw, |
176 |
|
10 |
bp::return_internal_reference<>()), |
177 |
|
|
"list of second partial derivatives of the cost with " |
178 |
|
|
"respect to the control parameters of " |
179 |
✓✗ |
10 |
"the RK4 integration. ddli_ddw") |
180 |
|
|
.add_property("ddli_ddu", |
181 |
✓✗ |
10 |
bp::make_getter(&IntegratedActionDataRK4::ddli_ddu, |
182 |
|
10 |
bp::return_internal_reference<>()), |
183 |
|
|
"list of second partial derivatives of the cost with " |
184 |
|
|
"respect to the control input of the RK4 " |
185 |
✓✗ |
10 |
"integration. ddli_ddu") |
186 |
|
|
.add_property("ddli_dxdw", |
187 |
✓✗ |
10 |
bp::make_getter(&IntegratedActionDataRK4::ddli_dxdw, |
188 |
|
10 |
bp::return_internal_reference<>()), |
189 |
|
|
"list of second partial derivatives of the cost with " |
190 |
|
|
"respect to the state and control parameters of " |
191 |
✓✗ |
10 |
"the RK4 integration. ddli_dxdw") |
192 |
|
|
.add_property("ddli_dxdu", |
193 |
✓✗ |
10 |
bp::make_getter(&IntegratedActionDataRK4::ddli_dxdu, |
194 |
|
10 |
bp::return_internal_reference<>()), |
195 |
|
|
"list of second partial derivatives of the cost with " |
196 |
|
|
"respect to the state and control input " |
197 |
✓✗ |
10 |
"of the RK4 integration. ddli_dxdu") |
198 |
|
|
.add_property("ddli_dwdu", |
199 |
✓✗ |
10 |
bp::make_getter(&IntegratedActionDataRK4::ddli_dwdu, |
200 |
|
10 |
bp::return_internal_reference<>()), |
201 |
|
|
"list of second partial derivatives of the cost with " |
202 |
|
|
"respect to the control parameters and " |
203 |
✓✗ |
10 |
"inputs control of the RK4 integration. ddli_dxdu") |
204 |
✓✗ |
10 |
.def(CopyableVisitor<IntegratedActionDataRK4>()); |
205 |
|
10 |
} |
206 |
|
|
|
207 |
|
|
} // namespace python |
208 |
|
|
} // namespace crocoddyl |
209 |
|
|
|
210 |
|
|
#pragma GCC diagnostic pop |