|
| 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 | +void exposeActuationFloatingBaseThruster() { |
| 20 | + bp::enum_<ThrusterType>("ThrusterType") |
| 21 | + .value("CW", CW) |
| 22 | + .value("CCW", CCW) |
| 23 | + .export_values(); |
| 24 | + |
| 25 | + bp::class_<Thruster>( |
| 26 | + "Thruster", "Model for thrusters", |
| 27 | + bp::init<pinocchio::SE3, double, |
| 28 | + bp::optional<ThrusterType, double, double>>( |
| 29 | + 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 | + .def(bp::init<double, bp::optional<ThrusterType, double, double>>( |
| 38 | + 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 | + ":param max_thrust: maximum thrust (default np.inf)")) |
| 45 | + .def_readwrite("pose", &Thruster::pose, |
| 46 | + "thruster pose (traslation, rotation)") |
| 47 | + .def_readwrite("ctorque", &Thruster::ctorque, |
| 48 | + "coefficient of generated torque per thrust") |
| 49 | + .def_readwrite("type", &Thruster::type, |
| 50 | + "type of thruster (clockwise or counterclockwise)") |
| 51 | + .def_readwrite("min_thrust", &Thruster::min_thrust, "minimum thrust") |
| 52 | + .def_readwrite("max_thrust", &Thruster::min_thrust, "maximum thrust") |
| 53 | + .def(PrintableVisitor<Thruster>()) |
| 54 | + .def(CopyableVisitor<Thruster>()); |
| 55 | + |
| 56 | + StdVectorPythonVisitor<std::vector<Thruster>, true>::expose( |
| 57 | + "StdVec_Thruster"); |
| 58 | + |
| 59 | + bp::register_ptr_to_python< |
| 60 | + boost::shared_ptr<crocoddyl::ActuationModelFloatingBaseThrusters>>(); |
| 61 | + |
| 62 | + 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 | + bp::init<boost::shared_ptr<StateMultibody>, std::vector<Thruster>>( |
| 69 | + 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 | + 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 | + ":param u: joint torque input (dim. nu)") |
| 85 | + .def("calcDiff", &ActuationModelFloatingBaseThrusters::calcDiff, |
| 86 | + 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 | + ":param u: joint torque input (dim. nu)") |
| 95 | + .def("commands", &ActuationModelFloatingBaseThrusters::commands, |
| 96 | + 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 | + ":param tau: generalized torques (dim state.nv)") |
| 103 | + .def("torqueTransform", |
| 104 | + &ActuationModelFloatingBaseThrusters::torqueTransform, |
| 105 | + 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 | + ":param tau: generalized torques (dim state.nv)") |
| 112 | + .def("createData", &ActuationModelFloatingBaseThrusters::createData, |
| 113 | + bp::args("self"), |
| 114 | + "Create the floating base thrusters actuation data.") |
| 115 | + .add_property( |
| 116 | + "thrusters", |
| 117 | + bp::make_function(&ActuationModelFloatingBaseThrusters::get_thrusters, |
| 118 | + bp::return_value_policy<bp::return_by_value>()), |
| 119 | + bp::make_function( |
| 120 | + &ActuationModelFloatingBaseThrusters::set_thrusters), |
| 121 | + "vector of thrusters") |
| 122 | + .add_property("nthrusters", |
| 123 | + bp::make_function( |
| 124 | + &ActuationModelFloatingBaseThrusters::get_nthrusters), |
| 125 | + "number of thrusters") |
| 126 | + .add_property( |
| 127 | + "Wthrust", |
| 128 | + bp::make_function(&ActuationModelFloatingBaseThrusters::get_Wthrust, |
| 129 | + bp::return_value_policy<bp::return_by_value>()), |
| 130 | + "matrix mapping from thrusts to thruster wrenches") |
| 131 | + .add_property( |
| 132 | + "S", |
| 133 | + bp::make_function(&ActuationModelFloatingBaseThrusters::get_S, |
| 134 | + bp::return_value_policy<bp::return_by_value>()), |
| 135 | + "selection matrix for under-actuation part") |
| 136 | + . def(PrintableVisitor<ActuationModelFloatingBaseThrusters>()) |
| 137 | + . def(CopyableVisitor<ActuationModelFloatingBaseThrusters>()); |
| 138 | +} |
| 139 | + |
| 140 | +} // namespace python |
| 141 | +} // namespace crocoddyl |
0 commit comments