6 #ifndef __pinocchio_python_spatial_motion_hpp__ 7 #define __pinocchio_python_spatial_motion_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" 28 template<
typename T>
struct call;
30 template<
typename Scalar,
int Options>
35 static bool isApprox(
const Motion &
self,
const Motion & other,
36 const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
38 return self.isApprox(other,prec);
41 static bool isZero(
const Motion &
self,
42 const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
44 return self.isZero(prec);
51 template<
typename Motion>
53 :
public boost::python::def_visitor< MotionPythonVisitor<Motion> >
67 template<
class PyClass>
71 .def(bp::init<>(bp::arg(
"self"),
"Default constructor"))
72 .def(bp::init<Vector3,Vector3>
73 (bp::args(
"self",
"linear",
"angular"),
74 "Initialize from linear and angular components of a Motion vector (don't mix the order)."))
75 .def(bp::init<Vector6>((bp::arg(
"self"),bp::arg(
"array")),
"Init from a vector 6 [linear velocity, angular velocity]"))
76 .def(bp::init<Motion>((bp::arg(
"self"),bp::arg(
"other")),
"Copy constructor."))
78 .add_property(
"linear",
80 bp::with_custodian_and_ward_postcall<0,1>()),
82 "Linear part of a *this, corresponding to the linear velocity in case of a Spatial velocity.")
83 .add_property(
"angular",
85 bp::with_custodian_and_ward_postcall<0,1>()),
87 "Angular part of a *this, corresponding to the angular velocity in case of a Spatial velocity.")
88 .add_property(
"vector",
89 bp::make_function((
typename Motion::ToVectorReturnType (Motion::*)())&
Motion::toVector,
90 bp::return_internal_reference<>()),
92 "Returns the components of *this as a 6d vector.")
94 bp::make_function((
typename Motion::ToVectorReturnType (Motion::*)())&
Motion::toVector,
95 bp::return_internal_reference<>()))
97 .def(
"se3Action",&Motion::template se3Action<Scalar,Options>,
98 bp::args(
"self",
"M"),
"Returns the result of the action of M on *this.")
99 .def(
"se3ActionInverse",&Motion::template se3ActionInverse<Scalar,Options>,
100 bp::args(
"self",
"M"),
"Returns the result of the action of the inverse of M on *this.")
107 "Set the linear and angular components of *this to zero.")
109 "Set the linear and angular components of *this to random values.")
111 .def(
"cross",(Motion (Motion::*)(
const Motion &)
const) &
Motion::cross,
112 bp::args(
"self",
"m"),
"Action of *this onto another Motion m. Returns ยจ*this x m.")
113 .def(
"cross",(Force (Motion::*)(
const Force &)
const) &Motion::cross,
114 bp::args(
"self",
"f"),
"Dual action of *this onto a Force f. Returns *this x* f.")
116 .def(bp::self + bp::self)
117 .def(bp::self += bp::self)
118 .def(bp::self - bp::self)
119 .def(bp::self -= bp::self)
121 .def(bp::self ^ bp::self)
122 .def(bp::self ^
Force())
124 .def(bp::self == bp::self)
125 .def(bp::self != bp::self)
133 isApproxMotion_overload(bp::args(
"self",
"other",
"prec"),
134 "Returns true if *this is approximately equal to other, within the precision given by prec."))
138 isZero_overload(bp::args(
"self",
"prec"),
139 "Returns true if *this is approximately equal to the zero Motion, within the precision given by prec."))
142 .staticmethod(
"Random")
144 .staticmethod(
"Zero")
146 .def(
"__array__",bp::make_function((
typename Motion::ToVectorReturnType (Motion::*)())&
Motion::toVector,
147 bp::return_internal_reference<>()))
155 #if PY_MAJOR_VERSION == 3 && PY_MINOR_VERSION == 6 && EIGENPY_VERSION_AT_LEAST(2,9,0) 158 typedef ::boost::python::detail::not_specified HolderType;
160 bp::class_<Motion,HolderType>(
"Motion",
161 "Motion vectors, in se3 == M^6.\n\n" 162 "Supported operations ...",
177 {
return bp::make_tuple((Vector3)m.
linear(),(Vector3)m.
angular()); }
182 static RefVector3
getLinear(Motion &
self) {
return self.linear(); }
183 static void setLinear (Motion &
self,
const Vector3 &
v) {
self.linear(v); }
184 static RefVector3
getAngular(Motion &
self) {
return self.angular(); }
185 static void setAngular(Motion &
self,
const Vector3 &
w) {
self.angular(w); }
187 static void setVector(Motion &
self,
const Vector6 &
v) {
self =
v; }
189 static void setZero(Motion &
self) {
self.setZero(); }
190 static void setRandom(Motion &
self) {
self.setRandom(); }
196 #endif // ifndef __pinocchio_python_spatial_motion_hpp__
HomogeneousMatrixType toHomogeneousMatrix() const
The homogeneous representation of the motion vector .
ForceTpl< double, 0 > Force
static bool isZero(const Motion &self, const Scalar &prec=Eigen::NumTraits< Scalar >::dummy_precision())
Set the Python method str and repr to use the overloading operator<<.
void def(const char *name, Func func)
static void setZero(Motion &self)
BOOST_PYTHON_FUNCTION_OVERLOADS(computeKKTContactDynamicMatrixInverse_overload, computeKKTContactDynamicMatrixInverse_proxy, 4, 5) static const Eigen
Eigen::Ref< Vector3 > RefVector3
static void setLinear(Motion &self, const Vector3 &v)
static void setAngular(Motion &self, const Vector3 &w)
ConstLinearType linear() const
MotionTpl< Scalar, Options > Motion
ConstAngularType angular() const
MotionAlgebraAction< OtherSpatialType, MotionTpl< _Scalar, _Options > >::ReturnType cross(const OtherSpatialType &d) const
static RefVector3 getLinear(Motion &self)
static boost::python::tuple getinitargs(const Motion &m)
#define PINOCCHIO_SHARED_PTR_HOLDER_TYPE(T)
Add the Python method copy to allow a copy of this by calling the copy constructor.
Main pinocchio namespace.
void visit(PyClass &cl) const
ToVectorConstReturnType toVector() const
static void setVector(Motion &self, const Vector6 &v)
auto call(R(*f)(Args...), typename convert_type< Args >::type... args)
static RefVector3 getAngular(Motion &self)
Eigen::Map< Vector3 > MapVector3
static bool getstate_manages_dict()
Common traits structure to fully define base classes for CRTP.
static bool isApprox(const Motion &self, const Motion &other, const Scalar &prec=Eigen::NumTraits< Scalar >::dummy_precision())
#define EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(...)
static MotionTpl Random()
ForceTpl< Scalar, traits< Motion >::Options > Force
ActionMatrixType toActionMatrix() const
static void setRandom(Motion &self)
ActionMatrixType toDualActionMatrix() const