Directory: | ./ |
---|---|
File: | bindings/python/crocoddyl/core/state-base.cpp |
Date: | 2025-01-30 11:01:55 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 48 | 48 | 100.0% |
Branches: | 49 | 98 | 50.0% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | /////////////////////////////////////////////////////////////////////////////// | ||
2 | // BSD 3-Clause License | ||
3 | // | ||
4 | // Copyright (C) 2019-2024, 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 | #include "python/crocoddyl/core/state-base.hpp" | ||
11 | |||
12 | #include "python/crocoddyl/core/core.hpp" | ||
13 | #include "python/crocoddyl/utils/copyable.hpp" | ||
14 | |||
15 | namespace crocoddyl { | ||
16 | namespace python { | ||
17 | |||
18 | 10 | void exposeStateAbstract() { | |
19 | 10 | bp::register_ptr_to_python<std::shared_ptr<StateAbstract> >(); | |
20 | |||
21 | 20 | bp::enum_<Jcomponent>("Jcomponent") | |
22 |
1/2✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
|
10 | .value("both", both) |
23 |
1/2✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
|
10 | .value("first", first) |
24 |
1/2✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
|
10 | .export_values() |
25 |
1/2✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
|
10 | .value("second", second); |
26 | |||
27 | 20 | bp::enum_<AssignmentOp>("AssignmentOp") | |
28 |
1/2✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
|
10 | .value("setto", setto) |
29 |
1/2✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
|
10 | .value("addto", addto) |
30 |
1/2✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
|
10 | .value("rmfrom", rmfrom) |
31 |
1/2✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
|
10 | .export_values(); |
32 | |||
33 |
1/2✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
|
10 | bp::class_<StateAbstract_wrap, boost::noncopyable>( |
34 | "StateAbstract", | ||
35 | "Abstract class for the state representation.\n\n" | ||
36 | "A state is represented by its operators: difference, integrates and " | ||
37 | "their derivatives.\n" | ||
38 | "The difference operator returns the value of x1 [-] x0 operation. " | ||
39 | "Instead the integrate\n" | ||
40 | "operator returns the value of x [+] dx. These operators are used to " | ||
41 | "compared two points\n" | ||
42 | "on the state manifold M or to advance the state given a tangential " | ||
43 | "velocity (Tx M).\n" | ||
44 | "Therefore the points x, x0 and x1 belong to the manifold M; and dx or " | ||
45 | "x1 [-] x0 lie\n" | ||
46 | "on its tangential space.", | ||
47 |
1/2✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
|
10 | bp::init<std::size_t, std::size_t>( |
48 | 20 | bp::args("self", "nx", "ndx"), | |
49 | "Initialize the state dimensions.\n\n" | ||
50 | ":param nx: dimension of state configuration tuple\n" | ||
51 | ":param ndx: dimension of state tangent vector")) | ||
52 |
3/6✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
|
20 | .def("zero", pure_virtual(&StateAbstract_wrap::zero), bp::args("self"), |
53 | "Generate a zero reference state.\n\n" | ||
54 | ":return zero reference state") | ||
55 |
3/6✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
|
20 | .def("rand", pure_virtual(&StateAbstract_wrap::rand), bp::args("self"), |
56 | "Generate a random reference state.\n\n" | ||
57 | ":return random reference state") | ||
58 |
2/4✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
|
10 | .def("diff", pure_virtual(&StateAbstract_wrap::diff_wrap), |
59 |
1/2✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
|
20 | bp::args("self", "x0", "x1"), |
60 | "Compute the state manifold differentiation.\n\n" | ||
61 | "It returns the value of x1 [-] x0 operation. Note tha x0 and x1 " | ||
62 | "are points in the state\n" | ||
63 | "manifold (in M). Instead the operator result lies in the " | ||
64 | "tangent-space of M.\n" | ||
65 | ":param x0: previous state point (dim state.nx).\n" | ||
66 | ":param x1: current state point (dim state.nx).\n" | ||
67 | ":return x1 [-] x0 value (dim state.ndx).") | ||
68 |
2/4✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
|
10 | .def("integrate", pure_virtual(&StateAbstract_wrap::integrate_wrap), |
69 |
1/2✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
|
20 | bp::args("self", "x", "dx"), |
70 | "Compute the state manifold integration.\n\n" | ||
71 | "It returns the value of x [+] dx operation. x and dx are points in " | ||
72 | "the state.diff(x0,x1) (in M)\n" | ||
73 | "and its tangent, respectively. Note that the operator result lies " | ||
74 | "on M too.\n" | ||
75 | ":param x: state point (dim. state.nx).\n" | ||
76 | ":param dx: velocity vector (dim state.ndx).\n" | ||
77 | ":return x [+] dx value (dim state.nx).") | ||
78 |
2/4✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
|
10 | .def("Jdiff", pure_virtual(&StateAbstract_wrap::Jdiff_wrap), |
79 |
1/2✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
|
20 | bp::args("self", "x0", "x1", "firstsecond"), |
80 | "Compute the partial derivatives of difference operator.\n\n" | ||
81 | "The difference operator (x1 [-] x0) is defined by diff(x0, x1). " | ||
82 | "Instead Jdiff\n" | ||
83 | "computes its partial derivatives, i.e. \\partial{diff(x0, x1)}{x0} " | ||
84 | "and\n" | ||
85 | "\\partial{diff(x0, x1)}{x1}. By default, this function returns the " | ||
86 | "derivatives of the\n" | ||
87 | "first and second argument (i.e. firstsecond='both'). However we " | ||
88 | "can also specific the\n" | ||
89 | "partial derivative for the first and second variables by setting " | ||
90 | "firstsecond='first'\n" | ||
91 | "or firstsecond='second', respectively.\n" | ||
92 | ":param x0: previous state point (dim state.nx).\n" | ||
93 | ":param x1: current state point (dim state.nx).\n" | ||
94 | ":param firstsecond: derivative w.r.t x0 or x1 or both\n" | ||
95 | ":return the partial derivative(s) of the diff(x0, x1) function") | ||
96 |
2/4✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
|
10 | .def("Jintegrate", pure_virtual(&StateAbstract_wrap::Jintegrate_wrap), |
97 |
1/2✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
|
20 | bp::args("self", "x", "dx", "firstsecond"), |
98 | "Compute the partial derivatives of integrate operator.\n\n" | ||
99 | "The integrate operator (x [+] dx) is defined by integrate(x, dx). " | ||
100 | "Instead Jintegrate\n" | ||
101 | "computes its partial derivatives, i.e. \\partial{integrate(x, " | ||
102 | "dx)}{x} and\n" | ||
103 | "\\partial{integrate(x, dx)}{dx}. By default, this function returns " | ||
104 | "the derivatives of\n" | ||
105 | "the first and second argument (i.e. firstsecond='both').\n" | ||
106 | "partial derivative by setting firstsecond='first' or " | ||
107 | "firstsecond='second'.\n" | ||
108 | ":param x: state point (dim. state.nx).\n" | ||
109 | ":param dx: velocity vector (dim state.ndx).\n" | ||
110 | ":param firstsecond: derivative w.r.t x or dx or both\n" | ||
111 | ":return the partial derivative(s) of the integrate(x, dx) function") | ||
112 |
2/4✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
|
10 | .def("JintegrateTransport", |
113 | pure_virtual(&StateAbstract_wrap::JintegrateTransport_wrap), | ||
114 |
1/2✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
|
20 | bp::args("self", "x", "dx", "Jin", "firstsecond"), |
115 | "Parallel transport from integrate(x, dx) to x.\n\n" | ||
116 | "This function performs the parallel transportation of an input " | ||
117 | "matrix whose columns\n" | ||
118 | "are expressed in the tangent space at integrate(x, dx) to the " | ||
119 | "tangent space at x point\n" | ||
120 | ":param x: state point (dim. state.nx).\n" | ||
121 | ":param dx: velocity vector (dim state.ndx).\n" | ||
122 | ":param Jin: input matrix (number of rows = state.nv).\n" | ||
123 | ":param firstsecond: derivative w.r.t x or dx") | ||
124 |
2/4✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
|
10 | .add_property("nx", bp::make_function(&StateAbstract_wrap::get_nx), |
125 |
1/2✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
|
20 | bp::make_setter(&StateAbstract_wrap::nx_, |
126 | 10 | bp::return_internal_reference<>()), | |
127 | "dimension of state tuple") | ||
128 |
2/4✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
|
10 | .add_property("ndx", bp::make_function(&StateAbstract_wrap::get_ndx), |
129 |
1/2✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
|
20 | bp::make_setter(&StateAbstract_wrap::ndx_, |
130 | 10 | bp::return_internal_reference<>()), | |
131 | "dimension of the tangent space of the state manifold") | ||
132 |
2/4✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
|
10 | .add_property("nq", bp::make_function(&StateAbstract_wrap::get_nq), |
133 |
1/2✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
|
20 | bp::make_setter(&StateAbstract_wrap::nq_, |
134 | 10 | bp::return_internal_reference<>()), | |
135 | "dimension of the configuration tuple") | ||
136 |
2/4✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
|
10 | .add_property("nv", bp::make_function(&StateAbstract_wrap::get_nv), |
137 |
1/2✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
|
20 | bp::make_setter(&StateAbstract_wrap::nv_, |
138 | 10 | bp::return_internal_reference<>()), | |
139 | "dimension of tangent space of the configuration manifold") | ||
140 |
1/2✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
|
10 | .add_property("has_limits", |
141 |
1/2✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
|
20 | bp::make_function(&StateAbstract_wrap::get_has_limits), |
142 | "indicates whether problem has finite state limits") | ||
143 |
1/2✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
|
10 | .add_property("lb", |
144 |
1/2✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
|
10 | bp::make_getter(&StateAbstract_wrap::lb_, |
145 | 10 | bp::return_internal_reference<>()), | |
146 | &StateAbstract_wrap::set_lb, "lower state limits") | ||
147 |
1/2✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
|
10 | .add_property("ub", |
148 |
1/2✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
|
20 | bp::make_getter(&StateAbstract_wrap::ub_, |
149 | 10 | bp::return_internal_reference<>()), | |
150 | &StateAbstract_wrap::set_ub, "upper state limits"); | ||
151 | 10 | } | |
152 | |||
153 | } // namespace python | ||
154 | } // namespace crocoddyl | ||
155 |