Verified Commit d85fdfb7 authored by Justin Carpentier's avatar Justin Carpentier
Browse files

python: expose shared data member for Spatial classes

parent b79510be
......@@ -3,10 +3,11 @@
// Copyright (c) 2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//
#ifndef __pinocchio_python_force_hpp__
#define __pinocchio_python_force_hpp__
#ifndef __pinocchio_python_spatial_force_hpp__
#define __pinocchio_python_spatial_force_hpp__
#include <eigenpy/memory.hpp>
#include <eigenpy/eigen-to-python.hpp>
#include <boost/python/tuple.hpp>
#include "pinocchio/spatial/se3.hpp"
......@@ -55,6 +56,9 @@ namespace pinocchio
typedef typename Force::Vector3 Vector3;
typedef typename Force::Scalar Scalar;
typedef typename Eigen::Map<Vector3> MapVector3;
typedef typename Eigen::Ref<Vector3> RefVector3;
template<class PyClass>
void visit(PyClass& cl) const
{
......@@ -67,18 +71,23 @@ namespace pinocchio
.def(bp::init<Force>((bp::arg("other")),"Copy constructor."))
.add_property("linear",
&ForcePythonVisitor::getLinear,
bp::make_function(&ForcePythonVisitor::getLinear,
bp::with_custodian_and_ward_postcall<0,1>()),
&ForcePythonVisitor::setLinear,
"Linear part of a *this, corresponding to the linear velocity in case of a Spatial velocity.")
.add_property("angular",
&ForcePythonVisitor::getAngular,
bp::make_function(&ForcePythonVisitor::getAngular,
bp::with_custodian_and_ward_postcall<0,1>()),
&ForcePythonVisitor::setAngular,
"Angular part of a *this, corresponding to the angular velocity in case of a Spatial velocity.")
.add_property("vector",
&ForcePythonVisitor::getVector,
bp::make_function((typename Force::ToVectorReturnType (Force::*)())&Force::toVector,
bp::return_internal_reference<>()),
&ForcePythonVisitor::setVector,
"Returns the components of *this as a 6d vector.")
.add_property("np",&ForcePythonVisitor::getVector)
.add_property("np",
bp::make_function((typename Force::ToVectorReturnType (Force::*)())&Force::toVector,
bp::return_internal_reference<>()))
.def("se3Action",&Force::template se3Action<Scalar,Options>,
bp::args("self","M"),"Returns the result of the dual action of M on *this.")
......@@ -118,7 +127,8 @@ namespace pinocchio
.def("Zero",&Force::Zero,"Returns a zero Force.")
.staticmethod("Zero")
.def("__array__",&ForcePythonVisitor::getVector)
.def("__array__",bp::make_function((typename Force::ToVectorReturnType (Force::*)())&Force::toVector,
bp::return_internal_reference<>()))
.def_pickle(Pickle())
;
......@@ -147,20 +157,18 @@ namespace pinocchio
{ return bp::make_tuple((Vector3)f.linear(),(Vector3)f.angular()); }
};
static Vector3 getLinear(const Force & self ) { return self.linear(); }
static RefVector3 getLinear(Force & self ) { return self.linear(); }
static void setLinear(Force & self, const Vector3 & f) { self.linear(f); }
static Vector3 getAngular(const Force & self) { return self.angular(); }
static RefVector3 getAngular(Force & self) { return self.angular(); }
static void setAngular(Force & self, const Vector3 & n) { self.angular(n); }
static void setZero(Force & self) { self.setZero(); }
static void setRandom(Force & self) { self.setRandom(); }
static Vector6 getVector(const Force & self) { return self.toVector(); }
static void setVector(Force & self, const Vector6 & f) { self = f; }
};
} // namespace python
} // namespace pinocchio
#endif // ifndef __pinocchio_python_se3_hpp__
#endif // ifndef __pinocchio_python_spatial_force_hpp__
......@@ -3,11 +3,12 @@
// Copyright (c) 2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//
#ifndef __pinocchio_python_inertia_hpp__
#define __pinocchio_python_inertia_hpp__
#ifndef __pinocchio_python_spatial_inertia_hpp__
#define __pinocchio_python_spatial_inertia_hpp__
#include <eigenpy/exception.hpp>
#include <eigenpy/memory.hpp>
#include <eigenpy/eigen-to-python.hpp>
#include <boost/python/tuple.hpp>
#include "pinocchio/spatial/inertia.hpp"
......@@ -74,7 +75,8 @@ namespace pinocchio
&InertiaPythonVisitor::setMass,
"Mass of the Spatial Inertia.")
.add_property("lever",
&InertiaPythonVisitor::getLever,
bp::make_function((typename Inertia::Vector3 & (Inertia::*)())&Inertia::lever,
bp::return_internal_reference<>()),
&InertiaPythonVisitor::setLever,
"Center of mass location of the Spatial Inertia. It corresponds to the location of the center of mass regarding to the frame where the Spatial Inertia is expressed.")
.add_property("inertia",
......@@ -169,7 +171,6 @@ namespace pinocchio
static Scalar getMass( const Inertia & self ) { return self.mass(); }
static void setMass( Inertia & self, Scalar mass ) { self.mass() = mass; }
static Vector3 getLever( const Inertia & self ) { return self.lever(); }
static void setLever( Inertia & self, const Vector3 & lever ) { self.lever() = lever; }
static Matrix3 getInertia( const Inertia & self ) { return self.inertia().matrix(); }
......@@ -233,4 +234,4 @@ namespace pinocchio
} // namespace python
} // namespace pinocchio
#endif // ifndef __pinocchio_python_se3_hpp__
#endif // ifndef __pinocchio_python_spatial_inertia_hpp__
......@@ -3,10 +3,11 @@
// Copyright (c) 2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//
#ifndef __pinocchio_python_motion_hpp__
#define __pinocchio_python_motion_hpp__
#ifndef __pinocchio_python_spatial_motion_hpp__
#define __pinocchio_python_spatial_motion_hpp__
#include <eigenpy/memory.hpp>
#include <eigenpy/eigen-to-python.hpp>
#include <boost/python/tuple.hpp>
#include "pinocchio/spatial/se3.hpp"
......@@ -56,6 +57,9 @@ namespace pinocchio
typedef ForceTpl<Scalar,traits<Motion>::Options> Force;
typedef typename Motion::Vector6 Vector6;
typedef typename Motion::Vector3 Vector3;
typedef typename Eigen::Map<Vector3> MapVector3;
typedef typename Eigen::Ref<Vector3> RefVector3;
public:
......@@ -66,23 +70,28 @@ namespace pinocchio
.def(bp::init<>("Default constructor"))
.def(bp::init<Vector3,Vector3>
((bp::arg("linear"),bp::arg("angular")),
"Initialize from linear and angular components of a Motion vector (don(t mix the order)."))
"Initialize from linear and angular components of a Motion vector (don't mix the order)."))
.def(bp::init<Vector6>((bp::arg("vec")),"Init from a vector 6 [linear velocity, angular velocity]"))
.def(bp::init<Motion>((bp::arg("other")),"Copy constructor."))
.add_property("linear",
&MotionPythonVisitor::getLinear,
bp::make_function(&MotionPythonVisitor::getLinear,
bp::with_custodian_and_ward_postcall<0,1>()),
&MotionPythonVisitor::setLinear,
"Linear part of a *this, corresponding to the linear velocity in case of a Spatial velocity.")
.add_property("angular",
&MotionPythonVisitor::getAngular,
bp::make_function(&MotionPythonVisitor::getAngular,
bp::with_custodian_and_ward_postcall<0,1>()),
&MotionPythonVisitor::setAngular,
"Angular part of a *this, corresponding to the angular velocity in case of a Spatial velocity.")
.add_property("vector",
&MotionPythonVisitor::getVector,
bp::make_function((typename Motion::ToVectorReturnType (Motion::*)())&Motion::toVector,
bp::return_internal_reference<>()),
&MotionPythonVisitor::setVector,
"Returns the components of *this as a 6d vector.")
.add_property("np",&MotionPythonVisitor::getVector)
.add_property("np",
bp::make_function((typename Motion::ToVectorReturnType (Motion::*)())&Motion::toVector,
bp::return_internal_reference<>()))
.def("se3Action",&Motion::template se3Action<Scalar,Options>,
bp::args("self","M"),"Returns the result of the action of M on *this.")
......@@ -132,7 +141,8 @@ namespace pinocchio
.def("Zero",&Motion::Zero,"Returns a zero Motion.")
.staticmethod("Zero")
.def("__array__",&MotionPythonVisitor::getVector)
.def("__array__",bp::make_function((typename Motion::ToVectorReturnType (Motion::*)())&Motion::toVector,
bp::return_internal_reference<>()))
.def_pickle(Pickle())
;
......@@ -160,12 +170,11 @@ namespace pinocchio
{ return bp::make_tuple((Vector3)m.linear(),(Vector3)m.angular()); }
};
static Vector3 getLinear(const Motion & self) { return self.linear(); }
static RefVector3 getLinear(Motion & self) { return self.linear(); }
static void setLinear (Motion & self, const Vector3 & v) { self.linear(v); }
static Vector3 getAngular(const Motion & self) { return self.angular(); }
static RefVector3 getAngular(Motion & self) { return self.angular(); }
static void setAngular(Motion & self, const Vector3 & w) { self.angular(w); }
static Vector6 getVector(const Motion & self) { return self.toVector(); }
static void setVector(Motion & self, const Vector6 & v) { self = v; }
static void setZero(Motion & self) { self.setZero(); }
......@@ -173,9 +182,6 @@ namespace pinocchio
};
}} // namespace pinocchio::python
#endif // ifndef __pinocchio_python_se3_hpp__
#endif // ifndef __pinocchio_python_spatial_motion_hpp__
......@@ -3,10 +3,11 @@
// Copyright (c) 2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//
#ifndef __pinocchio_python_se3_hpp__
#define __pinocchio_python_se3_hpp__
#ifndef __pinocchio_python_spatial_se3_hpp__
#define __pinocchio_python_spatial_se3_hpp__
#include <eigenpy/memory.hpp>
#include <eigenpy/eigen-to-python.hpp>
#include <boost/python/tuple.hpp>
#include "pinocchio/spatial/se3.hpp"
......@@ -73,15 +74,13 @@ namespace pinocchio
"Initialize from an homogeneous matrix."))
.add_property("rotation",
&getRotation,
bp::make_function((typename SE3::AngularRef (SE3::*)()) &SE3::rotation,bp::return_internal_reference<>()),
(void (SE3::*)(const Matrix3 &)) &SE3::rotation,
"The rotation part of the transformation."
)
"The rotation part of the transformation.")
.add_property("translation",
&getTranslation,
bp::make_function((typename SE3::LinearRef (SE3::*)()) &SE3::translation,bp::return_internal_reference<>()),
(void (SE3::*)(const Vector3 &)) &SE3::translation,
"The translation part of the transformation."
)
"The translation part of the transformation.")
.add_property("homogeneous",&SE3::toHomogeneousMatrix,
"Returns the equivalent homegeneous matrix (acting on SE3).")
......@@ -198,9 +197,6 @@ namespace pinocchio
{ return bp::make_tuple((Matrix3)M.rotation(),(Vector3)M.translation()); }
};
static Vector3 getTranslation(const SE3 & self) { return self.translation(); }
static Matrix3 getRotation(const SE3 & self) { return self.rotation(); }
static void setIdentity(SE3 & self) { self.setIdentity(); }
static void setRandom(SE3 & self) { self.setRandom(); }
......@@ -212,4 +208,4 @@ namespace pinocchio
} // namespace python
} // namespace pinocchio
#endif // ifndef __pinocchio_python_se3_hpp__
#endif // ifndef __pinocchio_python_spatial_se3_hpp__
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment