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/multibody/states/multibody.hpp" |
11 |
|
|
|
12 |
|
|
#include "python/crocoddyl/core/state-base.hpp" |
13 |
|
|
#include "python/crocoddyl/multibody/multibody.hpp" |
14 |
|
|
#include "python/crocoddyl/utils/copyable.hpp" |
15 |
|
|
|
16 |
|
|
namespace crocoddyl { |
17 |
|
|
namespace python { |
18 |
|
|
|
19 |
|
10 |
void exposeStateMultibody() { |
20 |
|
10 |
bp::register_ptr_to_python<boost::shared_ptr<crocoddyl::StateMultibody> >(); |
21 |
|
|
|
22 |
✓✗ |
10 |
bp::class_<StateMultibody, bp::bases<StateAbstract> >( |
23 |
|
|
"StateMultibody", |
24 |
|
|
"Multibody state defined using Pinocchio.\n\n" |
25 |
|
|
"Pinocchio defines operators for integrating or differentiating the " |
26 |
|
|
"robot's\n" |
27 |
|
|
"configuration space. And here we assume that the state is defined by " |
28 |
|
|
"the\n" |
29 |
|
|
"robot's configuration and its joint velocities (x=[q,v]). Generally " |
30 |
|
|
"speaking,\n" |
31 |
|
|
"q lies on the manifold configuration manifold (M) and v in its tangent " |
32 |
|
|
"space\n" |
33 |
|
|
"(Tx M). Additionally the Pinocchio allows us to compute analytically " |
34 |
|
|
"the\n" |
35 |
|
|
"Jacobians for the differentiate and integrate operators. Note that this " |
36 |
|
|
"code\n" |
37 |
|
|
"can be reused in any robot that is described through its Pinocchio " |
38 |
|
|
"model.", |
39 |
✓✗ |
10 |
bp::init<boost::shared_ptr<pinocchio::Model> >( |
40 |
✓✗ |
10 |
bp::args("self", "pinocchioModel"), |
41 |
|
|
"Initialize the multibody state given a Pinocchio model.\n\n" |
42 |
|
|
":param pinocchioModel: pinocchio model (i.e. multibody model)") |
43 |
✓✗ |
20 |
[bp::with_custodian_and_ward<1, 2>()]) |
44 |
✓✗ |
20 |
.def("zero", &StateMultibody::zero, bp::args("self"), |
45 |
|
|
"Return the neutral robot configuration with zero velocity.\n\n" |
46 |
✓✗ |
10 |
":return neutral robot configuration with zero velocity") |
47 |
✓✗ |
20 |
.def("rand", &StateMultibody::rand, bp::args("self"), |
48 |
|
|
"Return a random reference state.\n\n" |
49 |
✓✗ |
10 |
":return random reference state") |
50 |
✓✗ |
20 |
.def("diff", &StateMultibody::diff_dx, bp::args("self", "x0", "x1"), |
51 |
|
|
"Operator that differentiates the two robot states.\n\n" |
52 |
|
|
"It returns the value of x1 [-] x0 operation. This operator uses " |
53 |
|
|
"the Lie\n" |
54 |
|
|
"algebra since the robot's root could lie in the SE(3) manifold.\n" |
55 |
|
|
":param x0: current state (dim state.nx()).\n" |
56 |
|
|
":param x1: next state (dim state.nx()).\n" |
57 |
✓✗ |
10 |
":return x1 - x0 value (dim state.nx()).") |
58 |
|
|
.def("integrate", &StateMultibody::integrate_x, |
59 |
✓✗ |
20 |
bp::args("self", "x", "dx"), |
60 |
|
|
"Operator that integrates the current robot state.\n\n" |
61 |
|
|
"It returns the value of x [+] dx operation. This operator uses the " |
62 |
|
|
"Lie\n" |
63 |
|
|
"algebra since the robot's root could lie in the SE(3) manifold.\n" |
64 |
|
|
"Futhermore there is no timestep here (i.e. dx = v*dt), note this " |
65 |
|
|
"if you're\n" |
66 |
|
|
"integrating a velocity v during an interval dt.\n" |
67 |
|
|
":param x: current state (dim state.nx()).\n" |
68 |
|
|
":param dx: displacement of the state (dim state.ndx()).\n" |
69 |
✓✗ |
10 |
":return x + dx value (dim state.nx()).") |
70 |
|
|
.def( |
71 |
|
|
"Jdiff", &StateMultibody::Jdiff_Js, |
72 |
✓✗ |
10 |
Jdiffs( |
73 |
✓✗ |
20 |
bp::args("self", "x0", "x1", "firstsecond"), |
74 |
|
|
"Compute the partial derivatives of the diff operator.\n\n" |
75 |
|
|
"Both Jacobian matrices are represented throught an identity " |
76 |
|
|
"matrix, with the exception\n" |
77 |
|
|
"that the robot's root is defined as free-flying joint (SE(3)). " |
78 |
|
|
"By default, this\n" |
79 |
|
|
"function returns the derivatives of the first and second " |
80 |
|
|
"argument (i.e.\n" |
81 |
|
|
"firstsecond='both'). However we ask for a specific partial " |
82 |
|
|
"derivative by setting\n" |
83 |
|
|
"firstsecond='first' or firstsecond='second'.\n" |
84 |
|
|
":param x0: current state (dim state.nx()).\n" |
85 |
|
|
":param x1: next state (dim state.nx()).\n" |
86 |
|
|
":param firstsecond: derivative w.r.t x0 or x1 or both\n" |
87 |
✓✗ |
10 |
":return the partial derivative(s) of the diff(x0, x1) function")) |
88 |
|
|
.def("Jintegrate", &StateMultibody::Jintegrate_Js, |
89 |
✓✗ |
10 |
Jintegrates( |
90 |
✓✗ |
20 |
bp::args("self", "x", "dx", "firstsecond"), |
91 |
|
|
"Compute the partial derivatives of arithmetic addition.\n\n" |
92 |
|
|
"Both Jacobian matrices are represented throught an identity " |
93 |
|
|
"matrix. with the exception\n" |
94 |
|
|
"that the robot's root is defined as free-flying joint (SE(3)). " |
95 |
|
|
"By default, this\n" |
96 |
|
|
"function returns the derivatives of the first and second " |
97 |
|
|
"argument (i.e.\n" |
98 |
|
|
"firstsecond='both'). However we ask for a specific partial " |
99 |
|
|
"derivative by setting\n" |
100 |
|
|
"firstsecond='first' or firstsecond='second'.\n" |
101 |
|
|
":param x: current state (dim state.nx()).\n" |
102 |
|
|
":param dx: displacement of the state (dim state.ndx()).\n" |
103 |
|
|
":param firstsecond: derivative w.r.t x or dx or both\n" |
104 |
|
|
":return the partial derivative(s) of the integrate(x, dx) " |
105 |
✓✗ |
10 |
"function")) |
106 |
|
|
.def("JintegrateTransport", &StateMultibody::JintegrateTransport, |
107 |
✓✗ |
20 |
bp::args("self", "x", "dx", "Jin", "firstsecond"), |
108 |
|
|
"Parallel transport from integrate(x, dx) to x.\n\n" |
109 |
|
|
"This function performs the parallel transportation of an input " |
110 |
|
|
"matrix whose columns\n" |
111 |
|
|
"are expressed in the tangent space at integrate(x, dx) to the " |
112 |
|
|
"tangent space at x point\n" |
113 |
|
|
":param x: state point (dim. state.nx).\n" |
114 |
|
|
":param dx: velocity vector (dim state.ndx).\n" |
115 |
|
|
":param Jin: input matrix (number of rows = state.nv).\n" |
116 |
✓✗ |
10 |
":param firstsecond: derivative w.r.t x or dx") |
117 |
|
|
.add_property( |
118 |
|
|
"pinocchio", |
119 |
✓✗ |
10 |
bp::make_function(&StateMultibody::get_pinocchio, |
120 |
|
10 |
bp::return_value_policy<bp::return_by_value>()), |
121 |
✓✗ |
10 |
"pinocchio model") |
122 |
✓✗ |
10 |
.def(CopyableVisitor<StateMultibody>()); |
123 |
|
10 |
} |
124 |
|
|
|
125 |
|
|
} // namespace python |
126 |
|
|
} // namespace crocoddyl |