1 |
|
|
/////////////////////////////////////////////////////////////////////////////// |
2 |
|
|
// BSD 3-Clause License |
3 |
|
|
// |
4 |
|
|
// Copyright (C) 2021-2023, University of Edinburgh, University of Pisa, |
5 |
|
|
// Heriot-Watt University |
6 |
|
|
// Copyright note valid unless otherwise stated in individual files. |
7 |
|
|
// All rights reserved. |
8 |
|
|
/////////////////////////////////////////////////////////////////////////////// |
9 |
|
|
|
10 |
|
|
#include "crocoddyl/core/numdiff/state.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 exposeStateNumDiff() { |
20 |
|
10 |
bp::register_ptr_to_python<boost::shared_ptr<StateNumDiff> >(); |
21 |
|
|
|
22 |
✓✗ |
10 |
bp::class_<StateNumDiff, bp::bases<StateAbstract> >( |
23 |
|
|
"StateNumDiff", |
24 |
|
|
"Abstract class for computing Jdiff and Jintegrate by using numerical " |
25 |
|
|
"differentiation.\n\n", |
26 |
✓✗ |
10 |
bp::init<boost::shared_ptr<StateAbstract> >( |
27 |
|
20 |
bp::args("self", "state"), |
28 |
|
|
"Initialize the state numdiff.\n\n" |
29 |
|
|
":param model: state where we compute the derivatives through " |
30 |
|
|
"numerial differentiation")) |
31 |
✓✗ |
20 |
.def("zero", &StateNumDiff::zero, bp::args("self"), |
32 |
|
|
"Return a zero reference state.\n\n" |
33 |
✓✗ |
10 |
":return zero reference state") |
34 |
✓✗ |
20 |
.def("rand", &StateNumDiff::rand, bp::args("self"), |
35 |
|
|
"Return a random reference state.\n\n" |
36 |
✓✗ |
10 |
":return random reference state") |
37 |
✓✗ |
20 |
.def("diff", &StateNumDiff::diff_dx, bp::args("self", "x0", "x1"), |
38 |
|
|
"Operator that differentiates the two state points.\n\n" |
39 |
|
|
"It returns the value of x1 [-] x0 operation. Due to a state vector " |
40 |
|
|
"lies in\n" |
41 |
|
|
"the Euclidean space, this operator is defined with arithmetic " |
42 |
|
|
"subtraction.\n" |
43 |
|
|
":param x0: current state (dim state.nx()).\n" |
44 |
|
|
":param x1: next state (dim state.nx()).\n" |
45 |
✓✗ |
10 |
":return x1 - x0 value (dim state.nx()).") |
46 |
✓✗ |
20 |
.def("integrate", &StateNumDiff::integrate_x, bp::args("self", "x", "dx"), |
47 |
|
|
"Operator that integrates the current state.\n\n" |
48 |
|
|
"It returns the value of x [+] dx operation. Due to a state vector " |
49 |
|
|
"lies in\n" |
50 |
|
|
"the Euclidean space, this operator is defined with arithmetic " |
51 |
|
|
"addition.\n" |
52 |
|
|
"Futhermore there is no timestep here (i.e. dx = v*dt), note this " |
53 |
|
|
"if you're\n" |
54 |
|
|
"integrating a velocity v during an interval dt.\n" |
55 |
|
|
":param x: current state (dim state.nx()).\n" |
56 |
|
|
":param dx: displacement of the state (dim state.nx()).\n" |
57 |
✓✗ |
10 |
":return x + dx value (dim state.nx()).") |
58 |
|
|
.def( |
59 |
|
|
"Jdiff", &StateNumDiff::Jdiff_Js, |
60 |
✓✗ |
10 |
Jdiffs( |
61 |
✓✗ |
20 |
bp::args("self", "x0", "x1", "firstsecond"), |
62 |
|
|
"Compute the partial derivatives of arithmetic substraction.\n\n" |
63 |
|
|
"Both Jacobian matrices are represented throught an identity " |
64 |
|
|
"matrix, with the exception\n" |
65 |
|
|
"that the first partial derivatives (w.r.t. x0) has negative " |
66 |
|
|
"signed. By default, this\n" |
67 |
|
|
"function returns the derivatives of the first and second " |
68 |
|
|
"argument (i.e.\n" |
69 |
|
|
"firstsecond='both'). However we ask for a specific partial " |
70 |
|
|
"derivative by setting\n" |
71 |
|
|
"firstsecond='first' or firstsecond='second'.\n" |
72 |
|
|
":param x0: current state (dim state.nx()).\n" |
73 |
|
|
":param x1: next state (dim state.nx()).\n" |
74 |
|
|
":param firstsecond: derivative w.r.t x0 or x1 or both\n" |
75 |
✓✗ |
10 |
":return the partial derivative(s) of the diff(x0, x1) function")) |
76 |
|
|
.def("Jintegrate", &StateNumDiff::Jintegrate_Js, |
77 |
✓✗ |
10 |
Jintegrates( |
78 |
✓✗ |
20 |
bp::args("self", "x", "dx", "firstsecond"), |
79 |
|
|
"Compute the partial derivatives of arithmetic addition.\n\n" |
80 |
|
|
"Both Jacobian matrices are represented throught an identity " |
81 |
|
|
"matrix. By default, this\n" |
82 |
|
|
"function returns the derivatives of the first and second " |
83 |
|
|
"argument (i.e.\n" |
84 |
|
|
"firstsecond='both'). However we ask for a specific partial " |
85 |
|
|
"derivative by setting\n" |
86 |
|
|
"firstsecond='first' or firstsecond='second'.\n" |
87 |
|
|
":param x: current state (dim state.nx()).\n" |
88 |
|
|
":param dx: displacement of the state (dim state.nx()).\n" |
89 |
|
|
":param firstsecond: derivative w.r.t x or dx or both\n" |
90 |
|
|
":return the partial derivative(s) of the integrate(x, dx) " |
91 |
✓✗ |
10 |
"function")) |
92 |
|
|
.def("JintegrateTransport", &StateNumDiff::JintegrateTransport, |
93 |
✓✗ |
20 |
bp::args("self", "x", "dx", "Jin", "firstsecond"), |
94 |
|
|
"Parallel transport from integrate(x, dx) to x.\n\n" |
95 |
|
|
"This function performs the parallel transportation of an input " |
96 |
|
|
"matrix whose columns\n" |
97 |
|
|
"are expressed in the tangent space at integrate(x, dx) to the " |
98 |
|
|
"tangent space at x point\n" |
99 |
|
|
":param x: state point (dim. state.nx).\n" |
100 |
|
|
":param dx: velocity vector (dim state.ndx).\n" |
101 |
|
|
":param Jin: input matrix (number of rows = state.nv).\n" |
102 |
✓✗ |
10 |
":param firstsecond: derivative w.r.t x or dx") |
103 |
|
|
.add_property( |
104 |
|
10 |
"disturbance", bp::make_function(&StateNumDiff::get_disturbance), |
105 |
|
|
&StateNumDiff::set_disturbance, |
106 |
✓✗✓✗
|
10 |
"disturbance constant used in the numerical differentiation") |
107 |
✓✗ |
10 |
.def(CopyableVisitor<StateNumDiff>()); |
108 |
|
10 |
} |
109 |
|
|
|
110 |
|
|
} // namespace python |
111 |
|
|
} // namespace crocoddyl |