1 |
|
|
/////////////////////////////////////////////////////////////////////////////// |
2 |
|
|
// BSD 3-Clause License |
3 |
|
|
// |
4 |
|
|
// Copyright (C) 2020-2023, University of Edinburgh, Heriot-Watt University |
5 |
|
|
// Copyright note valid unless otherwise stated in individual files. |
6 |
|
|
// All rights reserved. |
7 |
|
|
/////////////////////////////////////////////////////////////////////////////// |
8 |
|
|
|
9 |
|
|
#include "python/crocoddyl/core/constraint-base.hpp" |
10 |
|
|
|
11 |
|
|
#include "python/crocoddyl/utils/copyable.hpp" |
12 |
|
|
#include "python/crocoddyl/utils/printable.hpp" |
13 |
|
|
|
14 |
|
|
namespace crocoddyl { |
15 |
|
|
namespace python { |
16 |
|
|
|
17 |
|
10 |
void exposeConstraintAbstract() { |
18 |
|
10 |
bp::register_ptr_to_python<boost::shared_ptr<ConstraintModelAbstract> >(); |
19 |
|
|
|
20 |
|
20 |
bp::enum_<ConstraintType>("ConstraintType") |
21 |
✓✗ |
10 |
.value("Inequality", Inequality) |
22 |
✓✗ |
10 |
.value("Equality", Equality) |
23 |
✓✗ |
10 |
.value("Both", Both) |
24 |
✓✗ |
10 |
.export_values(); |
25 |
|
|
|
26 |
✓✗ |
10 |
bp::class_<ConstraintModelAbstract_wrap, boost::noncopyable>( |
27 |
|
|
"ConstraintModelAbstract", |
28 |
|
|
"Abstract multibody constraint models.\n\n" |
29 |
|
|
"A constraint model defines both: inequality g(x,u) and equality h(x, u) " |
30 |
|
|
"constraints.\n" |
31 |
|
|
"The constraint function depends on the state point x, which lies in the " |
32 |
|
|
"state manifold\n" |
33 |
|
|
"described with a nx-tuple, its velocity xd that belongs to the tangent " |
34 |
|
|
"space with ndx dimension,\n" |
35 |
|
|
"and the control input u.", |
36 |
✓✗ |
10 |
bp::init<boost::shared_ptr<StateAbstract>, |
37 |
|
|
boost::shared_ptr<ResidualModelAbstract>, std::size_t, |
38 |
|
20 |
std::size_t>(bp::args("self", "state", "residual", "ng", "nh"), |
39 |
|
|
"Initialize the constraint model.\n\n" |
40 |
|
|
":param state: state description\n" |
41 |
|
|
":param residual: residual model\n" |
42 |
|
|
":param ng: number of inequality constraints\n" |
43 |
|
|
":param nh: number of equality constraints")) |
44 |
✓✗ |
10 |
.def(bp::init<boost::shared_ptr<StateAbstract>, std::size_t, std::size_t, |
45 |
|
|
std::size_t>( |
46 |
✓✗ |
20 |
bp::args("self", "state", "nu", "ng", "nh"), |
47 |
|
|
"Initialize the constraint model.\n\n" |
48 |
|
|
":param state: state description\n" |
49 |
|
|
":param nu: dimension of control vector (default state.nv)\n" |
50 |
|
|
":param ng: number of inequality constraints\n" |
51 |
✓✗ |
10 |
":param nh: number of equality constraints")) |
52 |
✓✗ |
10 |
.def(bp::init<boost::shared_ptr<StateAbstract>, std::size_t, std::size_t>( |
53 |
✓✗ |
20 |
bp::args("self", "state", "ng", "nh"), |
54 |
|
|
"Initialize the constraint model.\n\n" |
55 |
|
|
":param state: state description\n" |
56 |
|
|
":param ng: number of inequality constraints\n" |
57 |
✓✗ |
10 |
":param nh: number of equality constraints")) |
58 |
|
|
.def("calc", pure_virtual(&ConstraintModelAbstract_wrap::calc), |
59 |
✓✗ |
20 |
bp::args("self", "data", "x", "u"), |
60 |
|
|
"Compute the constraint value.\n\n" |
61 |
|
|
":param data: constraint data\n" |
62 |
|
|
":param x: state point (dim. state.nx)\n" |
63 |
✓✗✓✗
|
10 |
":param u: control input (dim. nu)") |
64 |
|
|
.def<void (ConstraintModelAbstract::*)( |
65 |
|
|
const boost::shared_ptr<ConstraintDataAbstract>&, |
66 |
|
|
const Eigen::Ref<const Eigen::VectorXd>&)>( |
67 |
✓✗ |
20 |
"calc", &ConstraintModelAbstract::calc, bp::args("self", "data", "x"), |
68 |
|
|
"Compute the constraint value for nodes that depends only on the " |
69 |
|
|
"state.\n\n" |
70 |
|
|
"It updates the constraint based on the state only.\n" |
71 |
|
|
"This function is commonly used in the terminal nodes of an optimal " |
72 |
|
|
"control problem.\n" |
73 |
|
|
":param data: constraint data\n" |
74 |
|
20 |
":param x: state point (dim. state.nx)") |
75 |
|
|
.def("calcDiff", pure_virtual(&ConstraintModelAbstract_wrap::calcDiff), |
76 |
✓✗ |
20 |
bp::args("self", "data", "x", "u"), |
77 |
|
|
"Compute the Jacobians of the constraint function.\n\n" |
78 |
|
|
"It computes the Jacobians of the constraint function.\n" |
79 |
|
|
"It assumes that calc has been run first.\n" |
80 |
|
|
":param data: constraint data\n" |
81 |
|
|
":param x: state point (dim. state.nx)\n" |
82 |
✓✗✓✗ ✓✗ |
20 |
":param u: control input (dim. nu)\n") |
83 |
|
|
.def<void (ConstraintModelAbstract::*)( |
84 |
|
|
const boost::shared_ptr<ConstraintDataAbstract>&, |
85 |
|
|
const Eigen::Ref<const Eigen::VectorXd>&)>( |
86 |
|
|
"calcDiff", &ConstraintModelAbstract::calcDiff, |
87 |
✓✗ |
20 |
bp::args("self", "data", "x"), |
88 |
|
|
"Compute the Jacobian of the constraint with respect to the state " |
89 |
|
|
"only.\n\n" |
90 |
|
|
"It computes the Jacobian of the constraint function based on the " |
91 |
|
|
"state only.\n" |
92 |
|
|
"This function is commonly used in the terminal nodes of an optimal " |
93 |
|
|
"control problem.\n" |
94 |
|
|
":param data: constraint data\n" |
95 |
|
20 |
":param x: state point (dim. state.nx)") |
96 |
|
|
.def("createData", &ConstraintModelAbstract_wrap::createData, |
97 |
|
|
bp::with_custodian_and_ward_postcall<0, 2>(), |
98 |
✓✗ |
20 |
bp::args("self", "data"), |
99 |
|
|
"Create the constraint data.\n\n" |
100 |
|
|
"Each constraint model has its own data that needs to be allocated. " |
101 |
|
|
"This function\n" |
102 |
|
|
"returns the allocated data for a predefined constraint.\n" |
103 |
|
|
":param data: shared data\n" |
104 |
✓✗✓✗
|
20 |
":return constraint data.") |
105 |
|
|
.def("createData", &ConstraintModelAbstract_wrap::default_createData, |
106 |
✓✗ |
10 |
bp::with_custodian_and_ward_postcall<0, 2>()) |
107 |
|
|
.def("updateBounds", &ConstraintModelAbstract_wrap::update_bounds, |
108 |
✓✗ |
20 |
bp::args("self", "lower", "upper"), |
109 |
|
|
"Update the lower and upper bounds.\n\n" |
110 |
|
|
":param lower: lower bound\n" |
111 |
✓✗ |
10 |
":param upper: upper bound") |
112 |
|
|
.def("removeBounds", &ConstraintModelAbstract_wrap::remove_bounds, |
113 |
✓✗✓✗
|
20 |
bp::args("self"), "Remove the bounds.") |
114 |
|
|
.add_property( |
115 |
|
|
"state", |
116 |
✓✗ |
10 |
bp::make_function(&ConstraintModelAbstract_wrap::get_state, |
117 |
|
10 |
bp::return_value_policy<bp::return_by_value>()), |
118 |
✓✗ |
10 |
"state description") |
119 |
|
|
.add_property( |
120 |
|
|
"residual", |
121 |
✓✗ |
10 |
bp::make_function(&ConstraintModelAbstract_wrap::get_residual, |
122 |
|
10 |
bp::return_value_policy<bp::return_by_value>()), |
123 |
✓✗ |
10 |
"residual model") |
124 |
|
|
.add_property("type", |
125 |
✓✗ |
20 |
bp::make_function(&ConstraintModelAbstract_wrap::get_type), |
126 |
✓✗ |
10 |
"type of constraint") |
127 |
|
|
.add_property("lb", |
128 |
✓✗ |
10 |
bp::make_function(&ConstraintModelAbstract_wrap::get_lb, |
129 |
|
10 |
bp::return_internal_reference<>()), |
130 |
✓✗ |
10 |
"lower bound of constraint") |
131 |
|
|
.add_property("ub", |
132 |
✓✗ |
10 |
bp::make_function(&ConstraintModelAbstract_wrap::get_ub, |
133 |
|
10 |
bp::return_internal_reference<>()), |
134 |
✓✗ |
10 |
"upper bound of constraint") |
135 |
|
|
.add_property("nu", |
136 |
✓✗ |
20 |
bp::make_function(&ConstraintModelAbstract_wrap::get_nu), |
137 |
✓✗ |
10 |
"dimension of control vector") |
138 |
|
|
.add_property("ng", |
139 |
✓✗ |
20 |
bp::make_function(&ConstraintModelAbstract_wrap::get_ng), |
140 |
✓✗ |
10 |
"number of inequality constraints") |
141 |
|
|
.add_property("nh", |
142 |
✓✗ |
20 |
bp::make_function(&ConstraintModelAbstract_wrap::get_nh), |
143 |
✓✗ |
10 |
"number of equality constraints") |
144 |
✓✗ |
10 |
.def(PrintableVisitor<ConstraintModelAbstract>()); |
145 |
|
|
|
146 |
|
10 |
bp::register_ptr_to_python<boost::shared_ptr<ConstraintDataAbstract> >(); |
147 |
|
|
|
148 |
✓✗ |
10 |
bp::class_<ConstraintDataAbstract>( |
149 |
|
|
"ConstraintDataAbstract", "Abstract class for constraint data.\n\n", |
150 |
✓✗ |
10 |
bp::init<ConstraintModelAbstract*, DataCollectorAbstract*>( |
151 |
✓✗ |
10 |
bp::args("self", "model", "data"), |
152 |
|
|
"Create common data shared between constraint models.\n\n" |
153 |
|
|
":param model: constraint model\n" |
154 |
|
10 |
":param data: shared data")[bp::with_custodian_and_ward< |
155 |
✓✗ |
20 |
1, 2, bp::with_custodian_and_ward<1, 3> >()]) |
156 |
|
|
.add_property("shared", |
157 |
✓✗ |
10 |
bp::make_getter(&ConstraintDataAbstract::shared, |
158 |
|
10 |
bp::return_internal_reference<>()), |
159 |
✓✗ |
10 |
"shared data") |
160 |
|
|
.add_property( |
161 |
|
|
"residual", |
162 |
✓✗ |
10 |
bp::make_getter(&ConstraintDataAbstract::residual, |
163 |
|
10 |
bp::return_value_policy<bp::return_by_value>()), |
164 |
✓✗ |
10 |
"residual data") |
165 |
|
|
.add_property("g", |
166 |
✓✗ |
10 |
bp::make_getter(&ConstraintDataAbstract::g, |
167 |
|
|
bp::return_internal_reference<>()), |
168 |
✓✗ |
20 |
bp::make_setter(&ConstraintDataAbstract::g), |
169 |
✓✗ |
10 |
"inequality constraint residual") |
170 |
|
|
.add_property("Gx", |
171 |
✓✗ |
10 |
bp::make_getter(&ConstraintDataAbstract::Gx, |
172 |
|
|
bp::return_internal_reference<>()), |
173 |
✓✗ |
20 |
bp::make_setter(&ConstraintDataAbstract::Gx), |
174 |
✓✗ |
10 |
"Jacobian of the inequality constraint") |
175 |
|
|
.add_property("Gu", |
176 |
✓✗ |
10 |
bp::make_getter(&ConstraintDataAbstract::Gu, |
177 |
|
|
bp::return_internal_reference<>()), |
178 |
✓✗ |
20 |
bp::make_setter(&ConstraintDataAbstract::Gu), |
179 |
✓✗ |
10 |
"Jacobian of the inequality constraint") |
180 |
|
|
.add_property("h", |
181 |
✓✗ |
10 |
bp::make_getter(&ConstraintDataAbstract::h, |
182 |
|
|
bp::return_internal_reference<>()), |
183 |
✓✗ |
20 |
bp::make_setter(&ConstraintDataAbstract::h), |
184 |
✓✗ |
10 |
"equality constraint residual") |
185 |
|
|
.add_property("Hx", |
186 |
✓✗ |
10 |
bp::make_getter(&ConstraintDataAbstract::Hx, |
187 |
|
|
bp::return_internal_reference<>()), |
188 |
✓✗ |
20 |
bp::make_setter(&ConstraintDataAbstract::Hx), |
189 |
✓✗ |
10 |
"Jacobian of the equality constraint") |
190 |
|
|
.add_property("Hu", |
191 |
✓✗ |
10 |
bp::make_getter(&ConstraintDataAbstract::Hu, |
192 |
|
|
bp::return_internal_reference<>()), |
193 |
✓✗ |
20 |
bp::make_setter(&ConstraintDataAbstract::Hu), |
194 |
✓✗ |
10 |
"Jacobian of the equality constraint") |
195 |
✓✗ |
10 |
.def(CopyableVisitor<ConstraintDataAbstract>()); |
196 |
|
10 |
} |
197 |
|
|
|
198 |
|
|
} // namespace python |
199 |
|
|
} // namespace crocoddyl |