6 #ifndef __pinocchio_python_spatial_se3_hpp__ 7 #define __pinocchio_python_spatial_se3_hpp__ 11 #include <boost/python/tuple.hpp> 13 #include "pinocchio/spatial/se3.hpp" 14 #include "pinocchio/spatial/motion.hpp" 15 #include "pinocchio/spatial/force.hpp" 16 #include "pinocchio/spatial/inertia.hpp" 17 #include "pinocchio/spatial/explog.hpp" 30 BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(isIdentity_overload,SE3::isIdentity,0,1)
32 template<typename
T> struct call;
39 static bool isApprox(
const SE3 &
self,
const SE3 & other,
40 const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
42 return self.isApprox(other,prec);
48 template<
typename SE3>
50 :
public boost::python::def_visitor< SE3PythonVisitor<SE3> >
60 template<
class PyClass>
64 .def(bp::init<Matrix3,Vector3>
65 ((bp::arg(
"self"),bp::arg(
"rotation"),bp::arg(
"translation")),
66 "Initialize from a rotation matrix and a translation vector."))
67 .def(bp::init<Quaternion,Vector3>
68 ((bp::arg(
"self"),bp::arg(
"quat"),bp::arg(
"translation")),
69 "Initialize from a quaternion and a translation vector."))
70 .def(bp::init<int>((bp::arg(
"self"),bp::arg(
"int")),
"Init to identity."))
71 .def(bp::init<SE3>((bp::arg(
"self"),bp::arg(
"other")),
"Copy constructor."))
72 .def(bp::init<Matrix4>
73 ((bp::arg(
"self"),bp::arg(
"array")),
74 "Initialize from an homogeneous matrix."))
76 .add_property(
"rotation",
77 bp::make_function((
typename SE3::AngularRef (SE3::*)()) &
SE3::rotation,bp::return_internal_reference<>()),
79 "The rotation part of the transformation.")
80 .add_property(
"translation",
81 bp::make_function((
typename SE3::LinearRef (SE3::*)()) &SE3::translation,bp::return_internal_reference<>()),
82 (
void (SE3::*)(
const Vector3 &)) &SE3::translation,
83 "The translation part of the transformation.")
85 .add_property(
"homogeneous",&SE3::toHomogeneousMatrix,
86 "Returns the equivalent homegeneous matrix (acting on SE3).")
87 .add_property(
"action",&SE3::toActionMatrix,
88 "Returns the related action matrix (acting on Motion).")
89 .def(
"toActionMatrix",&SE3::toActionMatrix,bp::arg(
"self"),
90 "Returns the related action matrix (acting on Motion).")
91 .add_property(
"actionInverse",&SE3::toActionMatrixInverse,
92 "Returns the inverse of the action matrix (acting on Motion).\n" 93 "This is equivalent to do m.inverse().action")
94 .def(
"toActionMatrixInverse",&SE3::toActionMatrixInverse,bp::arg(
"self"),
95 "Returns the inverse of the action matrix (acting on Motion).\n" 96 "This is equivalent to do m.inverse().toActionMatrix()")
97 .add_property(
"dualAction",&SE3::toDualActionMatrix,
98 "Returns the related dual action matrix (acting on Force).")
99 .def(
"toDualActionMatrix",&SE3::toDualActionMatrix,bp::arg(
"self"),
100 "Returns the related dual action matrix (acting on Force).")
102 .def(
"setIdentity",&SE3PythonVisitor::setIdentity,bp::arg(
"self"),
103 "Set *this to the identity placement.")
104 .def(
"setRandom",&SE3PythonVisitor::setRandom,bp::arg(
"self"),
105 "Set *this to a random placement.")
108 "Returns the inverse transform")
110 .def(
"act", (Vector3 (SE3::*)(
const Vector3 &)
const) &
SE3::act,
112 "Returns a point which is the result of the entry point transforms by *this.")
113 .def(
"actInv", (Vector3 (SE3::*)(
const Vector3 &)
const) &SE3::actInv,
115 "Returns a point which is the result of the entry point by the inverse of *this.")
117 .def(
"act", (SE3 (SE3::*)(
const SE3 & other)
const) &SE3::act,
118 bp::args(
"self",
"M"),
"Returns the result of *this * M.")
119 .def(
"actInv", (SE3 (SE3::*)(
const SE3 & other)
const) &SE3::actInv,
120 bp::args(
"self",
"M"),
"Returns the result of the inverse of *this times M.")
122 .def(
"act", (
Motion (SE3::*)(
const Motion &)
const) &SE3::act,
123 bp::args(
"self",
"motion"),
"Returns the result action of *this onto a Motion.")
124 .def(
"actInv", (
Motion (SE3::*)(
const Motion &)
const) &SE3::actInv,
125 bp::args(
"self",
"motion"),
"Returns the result of the inverse of *this onto a Motion.")
127 .def(
"act", (
Force (SE3::*)(
const Force &)
const) &SE3::act,
128 bp::args(
"self",
"force"),
"Returns the result of *this onto a Force.")
129 .def(
"actInv", (
Force (SE3::*)(
const Force &)
const) &SE3::actInv,
130 bp::args(
"self",
"force"),
"Returns the result of the inverse of *this onto an Inertia.")
133 bp::args(
"self",
"inertia"),
"Returns the result of *this onto a Force.")
134 .def(
"actInv", (
Inertia (SE3::*)(
const Inertia &)
const) &SE3::actInv,
135 bp::args(
"self",
"inertia"),
"Returns the result of the inverse of *this onto an Inertia.")
139 isApproxSE3_overload(
bp::args(
"self",
"other",
"prec"),
140 "Returns true if *this is approximately equal to other, within the precision given by prec."))
144 isIdentity_overload(
bp::args(
"self",
"prec"),
145 "Returns true if *this is approximately equal to the identity placement, within the precision given by prec."))
147 .def(
"__invert__",&
SE3::inverse,
"Returns the inverse of *this.")
148 .def(bp::self * bp::self)
149 .def(
"__mul__",&__mul__<Motion>)
150 .def(
"__mul__",&__mul__<Force>)
151 .def(
"__mul__",&__mul__<Inertia>)
152 .def(
"__mul__",&__mul__<Vector3>)
153 .add_property(
"np",&SE3::toHomogeneousMatrix)
155 .def(bp::self == bp::self)
156 .def(bp::self != bp::self)
158 .def(
"Identity",&SE3::Identity,
"Returns the identity transformation.")
159 .staticmethod(
"Identity")
160 .def(
"Random",&SE3::Random,
"Returns a random transformation.")
161 .staticmethod(
"Random")
162 .def(
"Interpolate",&SE3::template Interpolate<double>,
164 "Linear interpolation on the SE3 manifold.\n\n" 165 "This method computes the linear interpolation between A and B, such that the result C = A + (B-A)*t if it would be applied on classic Euclidian space.\n" 166 "This operation is very similar to the SLERP operation on Rotations.\n" 168 "\tA: Initial transformation\n" 169 "\tB: Target transformation\n" 170 "\talpha: Interpolation factor")
171 .staticmethod(
"Interpolate")
173 .def(
"__array__",&SE3::toHomogeneousMatrix)
181 bp::class_<SE3>(
"SE3",
182 "SE3 transformation defined by a 3d vector and a rotation matrix.",
183 bp::init<>(bp::arg(
"self"),
"Default constructor."))
203 template<
typename Spatial>
204 static Spatial
__mul__(
const SE3 &
self,
const Spatial & other)
205 {
return self.act(other); }
211 #endif // ifndef __pinocchio_python_spatial_se3_hpp__
static boost::python::tuple getinitargs(const SE3 &M)
Set the Python method str and repr to use the overloading operator<<.
void visit(PyClass &cl) const
BOOST_PYTHON_FUNCTION_OVERLOADS(computeKKTContactDynamicMatrixInverse_overload, computeKKTContactDynamicMatrixInverse_proxy, 4, 5) static const Eigen
static Spatial __mul__(const SE3 &self, const Spatial &other)
SE3::Quaternion Quaternion
static void setRandom(SE3 &self)
static void act(const Eigen::MatrixBase< Mat > &iV, const ForceDense< ForceDerived > &f, Eigen::MatrixBase< MatRet > const &jF)
Action of a motion set on a force object. The input motion set is represented by a 6xN matrix whose e...
static bool isApprox(const SE3 &self, const SE3 &other, const Scalar &prec=Eigen::NumTraits< Scalar >::dummy_precision())
void inverse(const Eigen::MatrixBase< MatrixIn > &m_in, const Eigen::MatrixBase< MatrixOut > &dest)
Eigen::Quaternion< Scalar, Options > Quaternion
SE3Tpl< Scalar, Options > SE3
traits< SE3Tpl >::Matrix4 Matrix4
ConstLinearRef translation() const
Add the Python method copy to allow a copy of this by calling the copy constructor.
traits< SE3Tpl >::Vector3 Vector3
Main pinocchio namespace.
#define EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(...)
traits< SE3Tpl >::Matrix3 Matrix3
static void setIdentity(SE3 &self)
ConstAngularRef rotation() const