| Line |
Branch |
Exec |
Source |
| 1 |
|
|
/////////////////////////////////////////////////////////////////////////////// |
| 2 |
|
|
// BSD 3-Clause License |
| 3 |
|
|
// |
| 4 |
|
|
// Copyright (C) 2019-2025, 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 |
|
|
// Auto-generated file for float |
| 11 |
|
|
#include "crocoddyl/multibody/states/multibody.hpp" |
| 12 |
|
|
|
| 13 |
|
|
#include "python/crocoddyl/core/state-base.hpp" |
| 14 |
|
|
#include "python/crocoddyl/multibody/multibody.hpp" |
| 15 |
|
|
|
| 16 |
|
|
namespace crocoddyl { |
| 17 |
|
|
namespace python { |
| 18 |
|
|
|
| 19 |
|
|
#if PINOCCHIO_VERSION_AT_LEAST(3, 4, 1) |
| 20 |
|
|
#define _CROCODDYL_CONDITIONAL_PINOCCHIO_MODEL_REGISTER |
| 21 |
|
|
#else |
| 22 |
|
|
#define _CROCODDYL_CONDITIONAL_PINOCCHIO_MODEL_REGISTER \ |
| 23 |
|
|
bp::register_ptr_to_python<std::shared_ptr<Model>>(); |
| 24 |
|
|
#endif |
| 25 |
|
|
|
| 26 |
|
|
template <typename State> |
| 27 |
|
|
struct StateMultibodyVisitor |
| 28 |
|
|
: public bp::def_visitor<StateMultibodyVisitor<State>> { |
| 29 |
|
|
typedef typename State::Scalar Scalar; |
| 30 |
|
✗ |
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(Jdiffs, |
| 31 |
|
|
StateAbstractTpl<Scalar>::Jdiff_Js, 2, |
| 32 |
|
|
3) |
| 33 |
|
✗ |
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS( |
| 34 |
|
|
Jintegrates, StateAbstractTpl<Scalar>::Jintegrate_Js, 2, 3) |
| 35 |
|
|
template <class PyClass> |
| 36 |
|
✗ |
void visit(PyClass& cl) const { |
| 37 |
|
✗ |
cl.def("zero", &State::zero, bp::args("self"), |
| 38 |
|
|
"Return the neutral robot configuration with zero velocity.\n\n" |
| 39 |
|
|
":return neutral robot configuration with zero velocity") |
| 40 |
|
✗ |
.def("rand", &State::rand, bp::args("self"), |
| 41 |
|
|
"Return a random reference state.\n\n" |
| 42 |
|
|
":return random reference state") |
| 43 |
|
✗ |
.def("diff", &State::diff_dx, bp::args("self", "x0", "x1"), |
| 44 |
|
|
"Operator that differentiates the two robot states.\n\n" |
| 45 |
|
|
"It returns the value of x1 [-] x0 operation. This operator uses " |
| 46 |
|
|
"the Lie algebra since the robot's root could lie in the SE(3) " |
| 47 |
|
|
"manifold.\n" |
| 48 |
|
|
":param x0: current state (dim state.nx()).\n" |
| 49 |
|
|
":param x1: next state (dim state.nx()).\n" |
| 50 |
|
|
":return x1 - x0 value (dim state.nx()).") |
| 51 |
|
✗ |
.def("integrate", &State::integrate_x, bp::args("self", "x", "dx"), |
| 52 |
|
|
"Operator that integrates the current robot state.\n\n" |
| 53 |
|
|
"It returns the value of x [+] dx operation. This operator uses " |
| 54 |
|
|
"the Lie algebra since the robot's root could lie in the SE(3) " |
| 55 |
|
|
"manifold. Futhermore there is no timestep here (i.e. dx = v*dt), " |
| 56 |
|
|
"note this if you're integrating a velocity v during an interval " |
| 57 |
|
|
"dt.\n" |
| 58 |
|
|
":param x: current state (dim state.nx()).\n" |
| 59 |
|
|
":param dx: displacement of the state (dim state.ndx()).\n" |
| 60 |
|
|
":return x + dx value (dim state.nx()).") |
| 61 |
|
✗ |
.def("Jdiff", &State::Jdiff_Js, |
| 62 |
|
✗ |
Jdiffs(bp::args("self", "x0", "x1", "firstsecond"), |
| 63 |
|
|
"Compute the partial derivatives of the diff operator.\n\n" |
| 64 |
|
|
"Both Jacobian matrices are represented throught an " |
| 65 |
|
|
"identity matrix, with the exception that the robot's root " |
| 66 |
|
|
"is defined as free-flying joint (SE(3)). By default, this " |
| 67 |
|
|
"function returns the derivatives of the first and second " |
| 68 |
|
|
"argument (i.e. firstsecond='both'). However we ask for a " |
| 69 |
|
|
"specific partial derivative by setting " |
| 70 |
|
|
"firstsecond='first' or firstsecond='second'.\n" |
| 71 |
|
|
":param x0: current state (dim state.nx()).\n" |
| 72 |
|
|
":param x1: next state (dim state.nx()).\n" |
| 73 |
|
|
":param firstsecond: derivative w.r.t x0 or x1 or both\n" |
| 74 |
|
|
":return the partial derivative(s) of the diff(x0, x1) " |
| 75 |
|
|
"function")) |
| 76 |
|
✗ |
.def("Jintegrate", &State::Jintegrate_Js, |
| 77 |
|
✗ |
Jintegrates( |
| 78 |
|
|
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. with the exception that the robot's root is defined " |
| 82 |
|
|
"as free-flying joint (SE(3)). By default, this function " |
| 83 |
|
|
"returns the derivatives of the first and second argument " |
| 84 |
|
|
"(i.e. firstsecond='both'). However we ask for a specific " |
| 85 |
|
|
"partial derivative by setting firstsecond='first' or " |
| 86 |
|
|
"firstsecond='second'.\n" |
| 87 |
|
|
":param x: current state (dim state.nx()).\n" |
| 88 |
|
|
":param dx: displacement of the state (dim state.ndx()).\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 |
|
|
"function")) |
| 92 |
|
✗ |
.def("JintegrateTransport", &State::JintegrateTransport, |
| 93 |
|
|
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 are expressed in the tangent space at " |
| 97 |
|
|
"integrate(x, dx) to the tangent space at x point\n" |
| 98 |
|
|
":param x: state point (dim. state.nx).\n" |
| 99 |
|
|
":param dx: velocity vector (dim state.ndx).\n" |
| 100 |
|
|
":param Jin: input matrix (number of rows = state.nv).\n" |
| 101 |
|
|
":param firstsecond: derivative w.r.t x or dx") |
| 102 |
|
✗ |
.add_property( |
| 103 |
|
|
"pinocchio", |
| 104 |
|
|
bp::make_function(&State::get_pinocchio, |
| 105 |
|
✗ |
bp::return_value_policy<bp::return_by_value>()), |
| 106 |
|
|
"pinocchio model"); |
| 107 |
|
|
} |
| 108 |
|
|
}; |
| 109 |
|
|
|
| 110 |
|
|
#define CROCODDYL_STATE_MULTIBODY_PYTHON_BINDINGS(Scalar) \ |
| 111 |
|
|
typedef StateMultibodyTpl<Scalar> State; \ |
| 112 |
|
|
typedef StateAbstractTpl<Scalar> StateBase; \ |
| 113 |
|
|
typedef pinocchio::ModelTpl<Scalar> Model; \ |
| 114 |
|
|
bp::register_ptr_to_python<std::shared_ptr<State>>(); \ |
| 115 |
|
|
_CROCODDYL_CONDITIONAL_PINOCCHIO_MODEL_REGISTER \ |
| 116 |
|
|
bp::class_<State, bp::bases<StateBase>>( \ |
| 117 |
|
|
"StateMultibody", \ |
| 118 |
|
|
"Multibody state defined using Pinocchio.\n\n" \ |
| 119 |
|
|
"Pinocchio defines operators for integrating or differentiating the " \ |
| 120 |
|
|
"robot's configuration space. And here we assume that the state is " \ |
| 121 |
|
|
"defined by the robot's configuration and its joint velocities " \ |
| 122 |
|
|
"(x=[q,v]). Generally speaking, q lies on the manifold configuration " \ |
| 123 |
|
|
"manifold (M) and v in its tangent space (Tx M). Additionally the " \ |
| 124 |
|
|
"Pinocchio allows us to compute analytically the Jacobians for the " \ |
| 125 |
|
|
"differentiate and integrate operators. Note that this code can be " \ |
| 126 |
|
|
"reused in any robot that is described through its Pinocchio model.", \ |
| 127 |
|
|
bp::init<std::shared_ptr<Model>>( \ |
| 128 |
|
|
bp::args("self", "pinocchio"), \ |
| 129 |
|
|
"Initialize the multibody state given a Pinocchio model.\n\n" \ |
| 130 |
|
|
":param pinocchio: pinocchio model (i.e. multibody model)") \ |
| 131 |
|
|
[bp::with_custodian_and_ward<1, 2>()]) \ |
| 132 |
|
|
.def(StateMultibodyVisitor<State>()) \ |
| 133 |
|
|
.def(CastVisitor<State>()) \ |
| 134 |
|
|
.def(PrintableVisitor<State>()) \ |
| 135 |
|
|
.def(CopyableVisitor<State>()); |
| 136 |
|
|
|
| 137 |
|
✗ |
void exposeStateMultibody() { |
| 138 |
|
✗ |
CROCODDYL_STATE_MULTIBODY_PYTHON_BINDINGS(float) |
| 139 |
|
|
} |
| 140 |
|
|
|
| 141 |
|
|
} // namespace python |
| 142 |
|
|
} // namespace crocoddyl |
| 143 |
|
|
|