1 |
|
|
/////////////////////////////////////////////////////////////////////////////// |
2 |
|
|
// BSD 3-Clause License |
3 |
|
|
// |
4 |
|
|
// Copyright (C) 2024-2024, Heriot-Watt University |
5 |
|
|
// Copyright note valid unless otherwise stated in individual files. |
6 |
|
|
// All rights reserved. |
7 |
|
|
/////////////////////////////////////////////////////////////////////////////// |
8 |
|
|
|
9 |
|
|
#include "crocoddyl/multibody/actuations/floating-base-thrusters.hpp" |
10 |
|
|
#include "python/crocoddyl/multibody/multibody.hpp" |
11 |
|
|
#include "python/crocoddyl/utils/copyable.hpp" |
12 |
|
|
#include "python/crocoddyl/utils/printable.hpp" |
13 |
|
|
#include "python/crocoddyl/utils/vector-converter.hpp" |
14 |
|
|
|
15 |
|
|
namespace crocoddyl { |
16 |
|
|
|
17 |
|
|
namespace python { |
18 |
|
|
|
19 |
|
10 |
void exposeActuationFloatingBaseThruster() { |
20 |
|
20 |
bp::enum_<ThrusterType>("ThrusterType") |
21 |
✓✗ |
10 |
.value("CW", CW) |
22 |
✓✗ |
10 |
.value("CCW", CCW) |
23 |
✓✗ |
10 |
.export_values(); |
24 |
|
|
|
25 |
✓✗ |
10 |
bp::class_<Thruster>( |
26 |
|
|
"Thruster", "Model for thrusters", |
27 |
✓✗ |
10 |
bp::init<pinocchio::SE3, double, |
28 |
|
|
bp::optional<ThrusterType, double, double>>( |
29 |
|
20 |
bp::args("self", "M", "ctorque", "type", "min_thrust", "max_thrust"), |
30 |
|
|
"Initialize the thruster in a give pose from the root joint.\n\n" |
31 |
|
|
":param M: pose from root joint\n" |
32 |
|
|
":param ctorque: coefficient of generated torque per thrust\n" |
33 |
|
|
":param type: type of thruster (clockwise or counterclockwise, " |
34 |
|
|
"default clockwise)\n" |
35 |
|
|
":param min_thrust: minimum thrust (default 0.)\n" |
36 |
|
|
":param max_thrust: maximum thrust (default np.inf)")) |
37 |
✓✗ |
10 |
.def(bp::init<double, bp::optional<ThrusterType, double, double>>( |
38 |
✓✗ |
20 |
bp::args("self", "ctorque", "type", "min_thrust", "max_thrust"), |
39 |
|
|
"Initialize the thruster in a give pose from the root joint.\n\n" |
40 |
|
|
":param ctorque: coefficient of generated torque per thrust\n" |
41 |
|
|
":param type: type of thruster (clockwise or counterclockwise, " |
42 |
|
|
"default clockwise)\n" |
43 |
|
|
":param min_thrust: minimum thrust (default 0.)\n" |
44 |
✓✗ |
10 |
":param max_thrust: maximum thrust (default np.inf)")) |
45 |
|
|
.def_readwrite("pose", &Thruster::pose, |
46 |
✓✗ |
10 |
"thruster pose (traslation, rotation)") |
47 |
|
|
.def_readwrite("ctorque", &Thruster::ctorque, |
48 |
✓✗ |
10 |
"coefficient of generated torque per thrust") |
49 |
|
|
.def_readwrite("type", &Thruster::type, |
50 |
✓✗ |
10 |
"type of thruster (clockwise or counterclockwise)") |
51 |
✓✗ |
10 |
.def_readwrite("min_thrust", &Thruster::min_thrust, "minimum thrust") |
52 |
✓✗ |
10 |
.def_readwrite("max_thrust", &Thruster::min_thrust, "maximum thrust") |
53 |
✓✗ |
10 |
.def(PrintableVisitor<Thruster>()) |
54 |
✓✗ |
10 |
.def(CopyableVisitor<Thruster>()); |
55 |
|
|
|
56 |
✓✗✓✗
✓✗ |
10 |
StdVectorPythonVisitor<std::vector<Thruster>, true>::expose( |
57 |
|
|
"StdVec_Thruster"); |
58 |
|
|
|
59 |
|
|
bp::register_ptr_to_python< |
60 |
|
10 |
boost::shared_ptr<crocoddyl::ActuationModelFloatingBaseThrusters>>(); |
61 |
|
|
|
62 |
✓✗ |
10 |
bp::class_<ActuationModelFloatingBaseThrusters, |
63 |
|
|
bp::bases<ActuationModelAbstract>>( |
64 |
|
|
"ActuationModelFloatingBaseThrusters", |
65 |
|
|
"Actuation models for floating base systems actuated with thrusters " |
66 |
|
|
"(e.g. aerial " |
67 |
|
|
"manipulators).", |
68 |
✓✗ |
10 |
bp::init<boost::shared_ptr<StateMultibody>, std::vector<Thruster>>( |
69 |
|
20 |
bp::args("self", "state", "thrusters"), |
70 |
|
|
"Initialize the floating base actuation model equipped with " |
71 |
|
|
"thrusters.\n\n" |
72 |
|
|
":param state: state of multibody system\n" |
73 |
|
|
":param thrusters: vector of thrusters")) |
74 |
|
|
.def<void (ActuationModelFloatingBaseThrusters::*)( |
75 |
|
|
const boost::shared_ptr<ActuationDataAbstract>&, |
76 |
|
|
const Eigen::Ref<const Eigen::VectorXd>&, |
77 |
|
|
const Eigen::Ref<const Eigen::VectorXd>&)>( |
78 |
|
|
"calc", &ActuationModelFloatingBaseThrusters::calc, |
79 |
✓✗ |
20 |
bp::args("self", "data", "x", "u"), |
80 |
|
|
"Compute the actuation signal and actuation set from the thrust\n" |
81 |
|
|
"forces and joint torque inputs u.\n\n" |
82 |
|
|
":param data: floating base thrusters actuation data\n" |
83 |
|
|
":param x: state point (dim. state.nx)\n" |
84 |
✓✗ |
10 |
":param u: joint torque input (dim. nu)") |
85 |
|
|
.def("calcDiff", &ActuationModelFloatingBaseThrusters::calcDiff, |
86 |
✓✗ |
20 |
bp::args("self", "data", "x", "u"), |
87 |
|
|
"Compute the derivatives of the actuation model.\n\n" |
88 |
|
|
"It computes the partial derivatives of the thruster actuation. It\n" |
89 |
|
|
"assumes that calc has been run first. The reason is that the\n" |
90 |
|
|
"derivatives are constant and defined in createData. The Hessian\n" |
91 |
|
|
"is constant, so we don't write again this value.\n" |
92 |
|
|
":param data: floating base thrusters actuation data\n" |
93 |
|
|
":param x: state point (dim. state.nx)\n" |
94 |
✓✗ |
10 |
":param u: joint torque input (dim. nu)") |
95 |
|
|
.def("commands", &ActuationModelFloatingBaseThrusters::commands, |
96 |
✓✗ |
20 |
bp::args("self", "data", "x", "tau"), |
97 |
|
|
"Compute the thrust and joint torque commands from the generalized " |
98 |
|
|
"torques.\n\n" |
99 |
|
|
"It stores the results in data.u.\n" |
100 |
|
|
":param data: actuation data\n" |
101 |
|
|
":param x: state point (dim. state.nx)\n" |
102 |
✓✗ |
10 |
":param tau: generalized torques (dim state.nv)") |
103 |
|
|
.def("torqueTransform", |
104 |
|
|
&ActuationModelFloatingBaseThrusters::torqueTransform, |
105 |
✓✗ |
20 |
bp::args("self", "data", "x", "tau"), |
106 |
|
|
"Compute the torque transform from generalized torques to thrust " |
107 |
|
|
"and joint torque inputs.\n\n" |
108 |
|
|
"It stores the results in data.Mtau.\n" |
109 |
|
|
":param data: floating base thrusters actuation data\n" |
110 |
|
|
":param x: state point (dim. state.nx)\n" |
111 |
✓✗ |
10 |
":param tau: generalized torques (dim state.nv)") |
112 |
|
|
.def("createData", &ActuationModelFloatingBaseThrusters::createData, |
113 |
✓✗ |
20 |
bp::args("self"), |
114 |
✓✗ |
10 |
"Create the floating base thrusters actuation data.") |
115 |
|
|
.add_property( |
116 |
|
|
"thrusters", |
117 |
✓✗ |
10 |
bp::make_function(&ActuationModelFloatingBaseThrusters::get_thrusters, |
118 |
|
|
bp::return_value_policy<bp::return_by_value>()), |
119 |
✓✗ |
20 |
bp::make_function( |
120 |
|
|
&ActuationModelFloatingBaseThrusters::set_thrusters), |
121 |
✓✗ |
10 |
"vector of thrusters") |
122 |
|
|
.add_property("nthrusters", |
123 |
✓✗ |
20 |
bp::make_function( |
124 |
|
|
&ActuationModelFloatingBaseThrusters::get_nthrusters), |
125 |
✓✗ |
10 |
"number of thrusters") |
126 |
|
|
.add_property( |
127 |
|
|
"Wthrust", |
128 |
✓✗ |
10 |
bp::make_function(&ActuationModelFloatingBaseThrusters::get_Wthrust, |
129 |
|
10 |
bp::return_value_policy<bp::return_by_value>()), |
130 |
✓✗ |
10 |
"matrix mapping from thrusts to thruster wrenches") |
131 |
|
|
.add_property( |
132 |
|
|
"S", |
133 |
✓✗ |
10 |
bp::make_function(&ActuationModelFloatingBaseThrusters::get_S, |
134 |
|
10 |
bp::return_value_policy<bp::return_by_value>()), |
135 |
✓✗ |
10 |
"selection matrix for under-actuation part") |
136 |
✓✗ |
10 |
.def(PrintableVisitor<ActuationModelFloatingBaseThrusters>()) |
137 |
✓✗ |
10 |
.def(CopyableVisitor<ActuationModelFloatingBaseThrusters>()); |
138 |
|
10 |
} |
139 |
|
|
|
140 |
|
|
} // namespace python |
141 |
|
|
} // namespace crocoddyl |