From fefc641a4ac560173e341a1f259791ae7d1c2cd7 Mon Sep 17 00:00:00 2001
From: Mansard <nmansard@laas.fr>
Date: Tue, 23 Sep 2014 15:06:27 +0200
Subject: [PATCH] Partial commit while building model interface.
---
CMakeLists.txt | 2 +
python/bindings.py | 4 +-
python/test_model.py | 12 ++
src/multibody/joint/joint-variant.hpp | 4 +-
src/multibody/model.hpp | 12 +-
src/python/__init__.py | 4 +-
src/python/data.hpp | 95 +++++++++++
src/python/force.hpp | 2 +
src/python/inertia.hpp | 1 +
src/python/model.hpp | 229 ++++++++++++++++++++++++++
src/python/module.cpp | 2 +
src/python/motion.hpp | 1 +
src/python/python.cpp | 11 +-
src/python/python.hpp | 2 +
src/python/se3.hpp | 3 +
src/spatial/inertia.hpp | 3 +
src/spatial/symmetric3.hpp | 3 +
17 files changed, 376 insertions(+), 14 deletions(-)
create mode 100644 python/test_model.py
create mode 100644 src/python/data.hpp
create mode 100644 src/python/model.hpp
diff --git a/CMakeLists.txt b/CMakeLists.txt
index ef510c34c..edd3d06a1 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -72,6 +72,8 @@ SET(HEADERS
python/force.hpp
python/motion.hpp
python/inertia.hpp
+ python/model.hpp
+ python/data.hpp
)
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio")
diff --git a/python/bindings.py b/python/bindings.py
index 8f37b9e4a..4580368c2 100644
--- a/python/bindings.py
+++ b/python/bindings.py
@@ -9,7 +9,9 @@ X = np.vstack( [ np.hstack([ R, skew(p)*R ]), np.hstack([ zero([3,3]), R ]) ])
assert( isapprox(m.action(),X))
M = np.vstack( [ np.hstack([R,p]), np.matrix('0 0 0 1',np.double) ] )
assert( isapprox(m.homogeneous(),M) )
-
+m2 = se3.SE3.Random()
+assert(isapprox( (m*m2).homogeneous(),m.homogeneous()*m2.homogeneous() ))
+assert(isapprox( (~m).homogeneous(),npl.inv(m.homogeneous()) ))
p = rand(3)
assert(isapprox(m*p,m.rotation*p+m.translation))
diff --git a/python/test_model.py b/python/test_model.py
new file mode 100644
index 000000000..36cf3e675
--- /dev/null
+++ b/python/test_model.py
@@ -0,0 +1,12 @@
+import pinocchio as se3
+from pinocchio.utils import *
+
+model = se3.Model.BuildEmptyModel()
+print model
+model = se3.Model.BuildHumanoidSimple()
+print model
+print "Bye bye"
+
+data = model.createData()
+
+print model.inertias()
diff --git a/src/multibody/joint/joint-variant.hpp b/src/multibody/joint/joint-variant.hpp
index f41945ad4..2ce0f8cee 100644
--- a/src/multibody/joint/joint-variant.hpp
+++ b/src/multibody/joint/joint-variant.hpp
@@ -74,7 +74,7 @@ namespace se3
} // namespace se3
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(se3::JointModelVariant);
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(se3::JointDataVariant);
+EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(se3::JointModelVariant)
+EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(se3::JointDataVariant)
#endif // ifndef __se3_joint_variant_hpp__
diff --git a/src/multibody/model.hpp b/src/multibody/model.hpp
index ea09740b8..5196c4f8e 100644
--- a/src/multibody/model.hpp
+++ b/src/multibody/model.hpp
@@ -11,11 +11,11 @@
#include "pinocchio/multibody/force-set.hpp"
#include <iostream>
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(se3::SE3);
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(se3::Inertia);
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(se3::Force);
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(se3::Motion);
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix<double,6,Eigen::Dynamic>);
+EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(se3::SE3)
+EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(se3::Inertia)
+EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(se3::Force)
+EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(se3::Motion)
+EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix<double,6,Eigen::Dynamic>)
namespace se3
{
@@ -53,6 +53,7 @@ namespace se3
{
names[0] = "universe";
}
+ ~Model() { std::cout << "Destroy model" << std::endl; }
template<typename D>
Index addBody( Index parent,const JointModelBase<D> & j,const SE3 & placement,
const Inertia & Y,const std::string & name = "" );
@@ -98,6 +99,7 @@ namespace se3
Data( const Model& ref );
+ private:
void computeLastChild(const Model& model);
void computeParents_fromRow(const Model& model);
};
diff --git a/src/python/__init__.py b/src/python/__init__.py
index 03bbca1e6..2facd72d7 100644
--- a/src/python/__init__.py
+++ b/src/python/__init__.py
@@ -16,7 +16,9 @@ def SE3act(m,x):
elif 'se3Action' in x.__class__.__dict__:
return x.se3Action(m)
else:
- print 'Error: SE3 cannot act on the given object'
+ #print 'Error: SE3 cannot act on the given object'
+ return m.oldmult(x)
+setattr(se3.SE3,'oldmult',se3.SE3.__mul__)
setattr(se3.SE3,'__mul__',SE3act)
setattr(se3.SE3,'act',SE3act)
diff --git a/src/python/data.hpp b/src/python/data.hpp
new file mode 100644
index 000000000..f553cd9f7
--- /dev/null
+++ b/src/python/data.hpp
@@ -0,0 +1,95 @@
+#ifndef __se3_python_data_hpp__
+#define __se3_python_data_hpp__
+
+#include <eigenpy/exception.hpp>
+#include <eigenpy/eigenpy.hpp>
+#include "pinocchio/multibody/model.hpp"
+
+#include <boost/shared_ptr.hpp>
+
+namespace se3
+{
+ namespace python
+ {
+
+ /* See the strategy applied for handling the Model object. The same applies
+ * here. */
+ struct DataHandler
+ {
+ typedef boost::shared_ptr<Data> SmartPtr_t;
+ typedef Data * Ptr_t;
+
+ SmartPtr_t smptr;
+ Ptr_t rawptr;
+ bool smart;
+
+ DataHandler(Data * data,bool transmitOwnership=false)
+ : smptr( transmitOwnership ? data : NULL )
+ , rawptr( data )
+ , smart( transmitOwnership ) {}
+ DataHandler( SmartPtr_t data )
+ : smptr(data), rawptr(NULL), smart(true) {}
+ ~DataHandler()
+ {
+ std::cout << "Destroy data handler " << std::endl;
+ if( (!smart) && (rawptr!=NULL) ) delete rawptr;
+ }
+
+ Data * ptr() { return smart ? smptr.get() : rawptr; }
+ const Data * ptr() const { return smart ? smptr.get() : rawptr; }
+ Data * operator->() { return ptr(); }
+ const Data * operator->() const { return ptr(); }
+
+ Data & get() { return smart ? *smptr : *rawptr; }
+ const Data & get() const { return smart ? *smptr : *rawptr; }
+ Data & operator*() { return get(); }
+ const Data& operator*() const { return get(); }
+ };
+
+
+ namespace bp = boost::python;
+
+ struct DataPythonVisitor
+ : public boost::python::def_visitor< DataPythonVisitor >
+ {
+
+ public:
+
+ /* --- Convert From C++ to Python ------------------------------------- */
+ static PyObject* convert(DataHandler::SmartPtr_t const& ptr)
+ {
+ return boost::python::incref(boost::python::object(DataHandler(ptr)).ptr());
+ }
+
+ /* --- Exposing C++ API to python through the handler ----------------- */
+ template<class PyClass>
+ void visit(PyClass& cl) const
+ {
+ cl
+ ;
+ }
+
+
+ /* --- Expose --------------------------------------------------------- */
+ static void expose()
+ {
+ bp::class_<DataHandler>("Data",
+ "Articulated rigid body data (const)",
+ bp::no_init)
+ .def(DataPythonVisitor());
+
+ /* Not sure if it is a good idea to enable automatic
+ * conversion. Prevent it for now */
+ //bp::to_python_converter< Data,DataPythonVisitor >();
+ bp::to_python_converter< DataHandler::SmartPtr_t,DataPythonVisitor >();
+ }
+
+
+ };
+
+
+
+ }} // namespace se3::python
+
+#endif // ifndef __se3_python_data_hpp__
+
diff --git a/src/python/force.hpp b/src/python/force.hpp
index 13eb0b61f..94b8533d0 100644
--- a/src/python/force.hpp
+++ b/src/python/force.hpp
@@ -58,7 +58,9 @@ namespace se3
.def("vector",&Force_fx::toVector)
.def("se3Action",&Force_fx::se3Action)
.def("se3ActionInverse",&Force_fx::se3ActionInverse)
+
.def("__str__",&ForcePythonVisitor::toString)
+ .add_property("np",&Force_fx::toVector)
.def("Random",&Force_fx::Random)
.staticmethod("Random")
diff --git a/src/python/inertia.hpp b/src/python/inertia.hpp
index f0d96275d..d16542698 100644
--- a/src/python/inertia.hpp
+++ b/src/python/inertia.hpp
@@ -66,6 +66,7 @@ namespace se3
.def("__str__",&InertiaPythonVisitor::toString)
.def( bp::self + bp::self)
.def( bp::self * bp::other<Motion_fx>() )
+ .add_property("np",&Inertia_fx::matrix)
.def("Identity",&Inertia_fx::Identity)
.staticmethod("Identity")
diff --git a/src/python/model.hpp b/src/python/model.hpp
new file mode 100644
index 000000000..6d8a689c9
--- /dev/null
+++ b/src/python/model.hpp
@@ -0,0 +1,229 @@
+#ifndef __se3_python_model_hpp__
+#define __se3_python_model_hpp__
+
+#include <eigenpy/exception.hpp>
+#include <eigenpy/eigenpy.hpp>
+#include "pinocchio/multibody/model.hpp"
+#include "pinocchio/multibody/parser/sample-models.hpp"
+
+#include <boost/shared_ptr.hpp>
+#include <boost/python/suite/indexing/vector_indexing_suite.hpp>
+#include <boost/python/return_internal_reference.hpp>
+
+namespace se3
+{
+ namespace python
+ {
+
+ /* There might be several way of building and owning a model object:
+
+ * - build from C++ (as static or dynamic, wathever), and owned by
+ * C++. In that case, use a ModelHandler with no ownership ModelHandler(
+ * Model*,false).
+ * - build from C++ but owned by python. Python would be in charge of
+ * destroying it when it is done. In that case, use
+ * ModelHandler(Model*,true). Take care not to destroy the object by
+ * yourself, otherwise there might have access to deallocated memory and
+ * double destruction.
+ * - build and managed by shared_ptr. It is the best way, in the sense
+ * that the object is manage both by C++ and python. It will be released
+ * only when both are done with it. In that case, simply give Python the
+ * shared_ptr ModelHandler( shared_ptr<Model> ).
+ *
+ * It is not possible to construct a Model from python, because of Eigen
+ * memory alignement. The python visitor does not define any
+ * constructor. Instead, some static makers (function allocating memory and
+ * returning a pointer Model *) are implemented. When Python is making
+ * such a Model, it is stored in a shared_ptr and can be access directly
+ * from C++. By default, when passing a Model to python, it is kept as a
+ * raw pointer. Access is then done from python to the C++ memory without
+ * any guarantee. If you want to enforce memory access, prefer transmitting
+ * a shared_ptr to python.
+ */
+ struct ModelHandler
+ {
+ typedef boost::shared_ptr<Model> SmartPtr_t;
+ typedef Model * Ptr_t;
+
+ SmartPtr_t smptr;
+ Ptr_t rawptr;
+ bool smart;
+
+ ModelHandler(Model * model,bool transmitOwnership=false)
+ : smptr( transmitOwnership ? model : NULL )
+ , rawptr( model )
+ , smart( transmitOwnership ) {}
+ ModelHandler( SmartPtr_t model )
+ : smptr(model), rawptr(NULL), smart(true) {}
+ ~ModelHandler()
+ {
+ std::cout << "Destroy model handler " << std::endl;
+ if( (!smart) && (rawptr!=NULL) ) delete rawptr;
+ }
+
+ Model * ptr() { return smart ? smptr.get() : rawptr; }
+ const Model * ptr() const { return smart ? smptr.get() : rawptr; }
+ Model * operator->() { return ptr(); }
+ const Model * operator->() const { return ptr(); }
+
+ Model & get() { return smart ? *smptr : *rawptr; }
+ const Model & get() const { return smart ? *smptr : *rawptr; }
+ Model & operator*() { return get(); }
+ const Model& operator*() const { return get(); }
+ };
+
+
+ namespace bp = boost::python;
+
+ struct InertiasVisitor
+ : public boost::python::def_visitor< InertiasVisitor >
+ {
+ typedef typename eigenpy::UnalignedEquivalent<Inertia>::type Inertia_fx;
+ typedef std::vector<Inertia> Inertias;
+
+ template<class PyClass>
+ void visit(PyClass& cl) const
+ {
+ cl
+ .def("__getitem__", &InertiasVisitor::getItem)
+ .def("__setitem__", &InertiasVisitor::setItem)
+ .def("__len__",&InertiasVisitor::length)
+ ;
+ }
+
+ static Inertia getItem( const Inertias & Ys,int i) { return Ys[i]; }
+ static void setItem( Inertias & Ys,int i,const Inertia_fx & Y)
+ {
+ std::cout << "Y = " << Y << std::endl;
+ Ys[i] = Y;
+ }
+ static int length( const Inertias & Ys ) { return Ys.size(); }
+
+ };
+
+
+ struct ParentsVisitor
+ : public boost::python::def_visitor< ParentsVisitor >
+ {
+ typedef Model::Index Index;
+ typedef std::vector<Index> Parents;
+
+ template<class PyClass>
+ void visit(PyClass& cl) const
+ {
+ cl
+ .def("__getitem__", &ParentsVisitor::getItem)
+ .def("__setitem__", &ParentsVisitor::setItem)
+ .def("__len__",&ParentsVisitor::length)
+ ;
+ }
+
+ static Index getItem( const Parents & Ys,int i) { return Ys[i]; }
+ static void setItem( Parents & Ys,int i,const Index & Y)
+ {
+ std::cout << "p = " << Y << std::endl;
+ Ys[i] = Y;
+ }
+ static int length( const Parents & Ys ) { return Ys.size(); }
+ };
+
+
+ struct ModelPythonVisitor
+ : public boost::python::def_visitor< ModelPythonVisitor >
+ {
+
+ public:
+
+ /* --- Convert From C++ to Python ------------------------------------- */
+ // static PyObject* convert(Model const& modelConstRef)
+ // {
+ // Model * ptr = const_cast<Model*>(&modelConstRef);
+ // return boost::python::incref(boost::python::object(ModelHandler(ptr)).ptr());
+ // }
+ static PyObject* convert(ModelHandler::SmartPtr_t const& ptr)
+ {
+ return boost::python::incref(boost::python::object(ModelHandler(ptr)).ptr());
+ }
+
+ /* --- Exposing C++ API to python through the handler ----------------- */
+ template<class PyClass>
+ void visit(PyClass& cl) const
+ {
+ cl
+ .def("getBodyId",&ModelPythonVisitor::getBodyId)
+ .def("createData",&ModelPythonVisitor::createData)
+
+ .def("__str__",&ModelPythonVisitor::toString)
+
+ .add_property("inertias",
+ bp::make_function(&ModelPythonVisitor::inertias,
+ bp::return_internal_reference<>()) )
+
+ .add_property("parents",
+ bp::make_function(&ModelPythonVisitor::parents,
+ bp::return_internal_reference<>()) )
+
+ .def("BuildEmptyModel",&ModelPythonVisitor::maker_empty)
+ .staticmethod("BuildEmptyModel")
+ .def("BuildHumanoidSimple",&ModelPythonVisitor::maker_humanoidSimple)
+ .staticmethod("BuildHumanoidSimple")
+ ;
+ }
+
+ static Model::Index getBodyId( const ModelHandler & modelPtr, const std::string & name )
+ { return modelPtr->getBodyId(name); }
+ static boost::shared_ptr<Data> createData(const ModelHandler& m )
+ { return boost::shared_ptr<Data>( new Data(*m) ); }
+
+ typedef std::vector<Inertia> Inertias_t;
+ static Inertias_t & inertias( ModelHandler & m ) { return m->inertias; }
+ static void set_inertias( ModelHandler & m,const Inertias_t & Ys )
+ { m->inertias = Ys; }
+ static ParentsVisitor::Parents & parents( ModelHandler & m ) { return m->parents; }
+
+
+
+ static ModelHandler maker_empty()
+ {
+ return ModelHandler( new Model(),true );
+ }
+ static ModelHandler maker_humanoidSimple()
+ {
+ Model * model = new Model();
+ buildModels::humanoidSimple(*model);
+ return ModelHandler( model,true );
+ }
+
+ static std::string toString(const ModelHandler& m)
+ { std::ostringstream s; s << *m; return s.str(); }
+
+ /* --- Expose --------------------------------------------------------- */
+ static void expose()
+ {
+ bp::class_<InertiasVisitor::Inertias>("ListInertia")
+ .def(InertiasVisitor());
+
+ bp::class_<ParentsVisitor::Parents>("ListInertia")
+ .def(ParentsVisitor());
+ //.def(bp::vector_indexing_suite<Inertias_t>());
+
+ bp::class_<ModelHandler>("Model",
+ "Articulated rigid body model (const)",
+ bp::no_init)
+ .def(ModelPythonVisitor());
+
+ /* Not sure if it is a good idea to enable automatic
+ * conversion. Prevent it for now */
+ //bp::to_python_converter< Model,ModelPythonVisitor >();
+ bp::to_python_converter< ModelHandler::SmartPtr_t,ModelPythonVisitor >();
+ }
+
+
+ };
+
+
+
+ }} // namespace se3::python
+
+#endif // ifndef __se3_python_model_hpp__
+
diff --git a/src/python/module.cpp b/src/python/module.cpp
index 2e089477a..b621f2a60 100644
--- a/src/python/module.cpp
+++ b/src/python/module.cpp
@@ -22,5 +22,7 @@ BOOST_PYTHON_MODULE(libpinocchio_pywrap)
se3::python::exposeForce();
se3::python::exposeMotion();
se3::python::exposeInertia();
+
+ se3::python::exposeModel();
}
diff --git a/src/python/motion.hpp b/src/python/motion.hpp
index 425e42fbe..4835e8665 100644
--- a/src/python/motion.hpp
+++ b/src/python/motion.hpp
@@ -66,6 +66,7 @@ namespace se3
.def("cross_force",&MotionPythonVisitor::cross_force)
.def("__str__",&MotionPythonVisitor::toString)
+ .add_property("np",&Motion_fx::toVector)
.def("Random",&Motion_fx::Random)
.staticmethod("Random")
diff --git a/src/python/python.cpp b/src/python/python.cpp
index 0b33429e5..58337655b 100644
--- a/src/python/python.cpp
+++ b/src/python/python.cpp
@@ -4,6 +4,9 @@
#include "pinocchio/python/motion.hpp"
#include "pinocchio/python/inertia.hpp"
+#include "pinocchio/python/model.hpp"
+#include "pinocchio/python/data.hpp"
+
namespace se3
{
namespace python
@@ -24,12 +27,10 @@ namespace se3
{
InertiaPythonVisitor<Inertia>::expose();
}
- void exposeStatics()
+ void exposeModel()
{
-
-
+ ModelPythonVisitor::expose();
+ DataPythonVisitor::expose();
}
-
-
}} // namespace se3::python
diff --git a/src/python/python.hpp b/src/python/python.hpp
index e82dec694..dc00aa812 100644
--- a/src/python/python.hpp
+++ b/src/python/python.hpp
@@ -10,6 +10,8 @@ namespace se3
void exposeMotion();
void exposeInertia();
+ void exposeModel();
+
}} // namespace se3::python
#endif // ifndef __se3_python_python_hpp__
diff --git a/src/python/se3.hpp b/src/python/se3.hpp
index 08660c838..10e80d29d 100644
--- a/src/python/se3.hpp
+++ b/src/python/se3.hpp
@@ -69,6 +69,9 @@ namespace se3
.def("actInv_se3", &SE3PythonVisitor::actInv_se3)
.def("__str__",&SE3PythonVisitor::toString)
+ .def("__invert__",&SE3_fx::inverse)
+ .def(bp::self * bp::self)
+ .add_property("np",&SE3_fx::toActionMatrix)
.def("Identity",&SE3_fx::Identity)
.staticmethod("Identity")
diff --git a/src/spatial/inertia.hpp b/src/spatial/inertia.hpp
index 4b5976abf..5d6ae4510 100644
--- a/src/spatial/inertia.hpp
+++ b/src/spatial/inertia.hpp
@@ -49,6 +49,9 @@ namespace se3
m=clone.m; c=clone.c; I=clone.I;
return *this;
}
+ /* Requiered by std::vector boost::python bindings. */
+ bool operator==( const InertiaTpl& Y2 )
+ { return (m==Y2.m) && (c==Y2.c) && (I==Y2.I); }
template<typename S2,int O2>
InertiaTpl( const InertiaTpl<S2,O2> & clone )
: m(clone.mass()),
diff --git a/src/spatial/symmetric3.hpp b/src/spatial/symmetric3.hpp
index 80a65d789..5bbcd0b5a 100644
--- a/src/spatial/symmetric3.hpp
+++ b/src/spatial/symmetric3.hpp
@@ -58,6 +58,9 @@ namespace se3
static Symmetric3Tpl Random() { return Symmetric3Tpl(Vector6::Random().eval()); }
static Symmetric3Tpl Identity() { return Symmetric3Tpl( 1, 0, 1, 0, 0, 1); }
+ /* Requiered by Inertia::operator== */
+ bool operator== (const Symmetric3Tpl & S2 ) { return data == S2.data; }
+
struct SkewSquare
{
const Vector3 & v;
--
GitLab