1 |
|
|
/////////////////////////////////////////////////////////////////////////////// |
2 |
|
|
// BSD 3-Clause License |
3 |
|
|
// |
4 |
|
|
// Copyright (C) 2019-2023, 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 "crocoddyl/core/states/euclidean.hpp" |
11 |
|
|
|
12 |
|
|
#include "python/crocoddyl/core/core.hpp" |
13 |
|
|
#include "python/crocoddyl/core/state-base.hpp" |
14 |
|
|
#include "python/crocoddyl/utils/copyable.hpp" |
15 |
|
|
|
16 |
|
|
namespace crocoddyl { |
17 |
|
|
namespace python { |
18 |
|
|
|
19 |
|
10 |
void exposeStateEuclidean() { |
20 |
|
10 |
bp::register_ptr_to_python<boost::shared_ptr<StateVector> >(); |
21 |
|
|
|
22 |
✓✗ |
10 |
bp::class_<StateVector, bp::bases<StateAbstract> >( |
23 |
|
|
"StateVector", |
24 |
|
|
"Euclidean state vector.\n\n" |
25 |
|
|
"For this type of states, the difference and integrate operators are " |
26 |
|
|
"described by\n" |
27 |
|
|
"arithmetic subtraction and addition operations, respectively. Due to " |
28 |
|
|
"the Euclidean\n" |
29 |
|
|
"point and its velocity lie in the same space, all Jacobians are " |
30 |
|
|
"described throught\n" |
31 |
|
|
"the identity matrix.", |
32 |
✓✗ |
20 |
bp::init<int>(bp::args("self", "nx"), |
33 |
|
|
"Initialize the vector dimension.\n\n" |
34 |
|
|
":param nx: dimension of state")) |
35 |
✓✗ |
20 |
.def("zero", &StateVector::zero, bp::args("self"), |
36 |
|
|
"Return a zero reference state.\n\n" |
37 |
✓✗ |
10 |
":return zero reference state") |
38 |
✓✗ |
20 |
.def("rand", &StateVector::rand, bp::args("self"), |
39 |
|
|
"Return a random reference state.\n\n" |
40 |
✓✗ |
10 |
":return random reference state") |
41 |
✓✗ |
20 |
.def("diff", &StateVector::diff_dx, bp::args("self", "x0", "x1"), |
42 |
|
|
"Operator that differentiates the two state points.\n\n" |
43 |
|
|
"It returns the value of x1 [-] x0 operation. Due to a state vector " |
44 |
|
|
"lies in\n" |
45 |
|
|
"the Euclidean space, this operator is defined with arithmetic " |
46 |
|
|
"subtraction.\n" |
47 |
|
|
":param x0: current state (dim state.nx()).\n" |
48 |
|
|
":param x1: next state (dim state.nx()).\n" |
49 |
✓✗ |
10 |
":return x1 - x0 value (dim state.nx()).") |
50 |
✓✗ |
20 |
.def("integrate", &StateVector::integrate_x, bp::args("self", "x", "dx"), |
51 |
|
|
"Operator that integrates the current state.\n\n" |
52 |
|
|
"It returns the value of x [+] dx operation. Due to a state vector " |
53 |
|
|
"lies in\n" |
54 |
|
|
"the Euclidean space, this operator is defined with arithmetic " |
55 |
|
|
"addition.\n" |
56 |
|
|
"Futhermore there is no timestep here (i.e. dx = v*dt), note this " |
57 |
|
|
"if you're\n" |
58 |
|
|
"integrating a velocity v during an interval dt.\n" |
59 |
|
|
":param x: current state (dim state.nx()).\n" |
60 |
|
|
":param dx: displacement of the state (dim state.nx()).\n" |
61 |
✓✗ |
10 |
":return x + dx value (dim state.nx()).") |
62 |
|
|
.def( |
63 |
|
|
"Jdiff", &StateVector::Jdiff_Js, |
64 |
✓✗ |
10 |
Jdiffs( |
65 |
✓✗ |
20 |
bp::args("self", "x0", "x1", "firstsecond"), |
66 |
|
|
"Compute the partial derivatives of arithmetic substraction.\n\n" |
67 |
|
|
"Both Jacobian matrices are represented throught an identity " |
68 |
|
|
"matrix, with the exception\n" |
69 |
|
|
"that the first partial derivatives (w.r.t. x0) has negative " |
70 |
|
|
"signed. By default, this\n" |
71 |
|
|
"function returns the derivatives of the first and second " |
72 |
|
|
"argument (i.e.\n" |
73 |
|
|
"firstsecond='both'). However we ask for a specific partial " |
74 |
|
|
"derivative by setting\n" |
75 |
|
|
"firstsecond='first' or firstsecond='second'.\n" |
76 |
|
|
":param x0: current state (dim state.nx()).\n" |
77 |
|
|
":param x1: next state (dim state.nx()).\n" |
78 |
|
|
":param firstsecond: derivative w.r.t x0 or x1 or both\n" |
79 |
✓✗ |
10 |
":return the partial derivative(s) of the diff(x0, x1) function")) |
80 |
|
|
.def("Jintegrate", &StateVector::Jintegrate_Js, |
81 |
✓✗ |
10 |
Jintegrates( |
82 |
✓✗ |
20 |
bp::args("self", "x", "dx", "firstsecond"), |
83 |
|
|
"Compute the partial derivatives of arithmetic addition.\n\n" |
84 |
|
|
"Both Jacobian matrices are represented throught an identity " |
85 |
|
|
"matrix. By default, this\n" |
86 |
|
|
"function returns the derivatives of the first and second " |
87 |
|
|
"argument (i.e.\n" |
88 |
|
|
"firstsecond='both'). However we ask for a specific partial " |
89 |
|
|
"derivative by setting\n" |
90 |
|
|
"firstsecond='first' or firstsecond='second'.\n" |
91 |
|
|
":param x: current state (dim state.nx()).\n" |
92 |
|
|
":param dx: displacement of the state (dim state.nx()).\n" |
93 |
|
|
":param firstsecond: derivative w.r.t x or dx or both\n" |
94 |
|
|
":return the partial derivative(s) of the integrate(x, dx) " |
95 |
✓✗ |
10 |
"function")) |
96 |
|
|
.def("JintegrateTransport", &StateVector::JintegrateTransport, |
97 |
✓✗ |
20 |
bp::args("self", "x", "dx", "Jin", "firstsecond"), |
98 |
|
|
"Parallel transport from integrate(x, dx) to x.\n\n" |
99 |
|
|
"This function performs the parallel transportation of an input " |
100 |
|
|
"matrix whose columns\n" |
101 |
|
|
"are expressed in the tangent space at integrate(x, dx) to the " |
102 |
|
|
"tangent space at x point\n" |
103 |
|
|
":param x: state point (dim. state.nx).\n" |
104 |
|
|
":param dx: velocity vector (dim state.ndx).\n" |
105 |
|
|
":param Jin: input matrix (number of rows = state.nv).\n" |
106 |
✓✗ |
10 |
":param firstsecond: derivative w.r.t x or dx") |
107 |
✓✗ |
10 |
.def(CopyableVisitor<StateVector>()); |
108 |
|
10 |
} |
109 |
|
|
|
110 |
|
|
} // namespace python |
111 |
|
|
} // namespace crocoddyl |