diff --git a/CMakeLists.txt b/CMakeLists.txt
index b6b6904522424895fe6aa1649390584fa62ad243..74b1ad4712566f2a7373afc90b56db6e8aee80cb 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -135,6 +135,9 @@ SET(${PROJECT_NAME}_PYTHON_HEADERS
python/force.hpp
python/motion.hpp
python/inertia.hpp
+ python/joint-dense.hpp
+ python/joints-models.hpp
+ python/joints-variant.hpp
python/model.hpp
python/data.hpp
python/algorithms.hpp
diff --git a/src/multibody/joint.hpp b/src/multibody/joint.hpp
index e4d20d71e27c3c564b0c06ca9e2a10eb35bf6bf6..30cb0018b32db930dfb9c15913f983e2636a864e 100644
--- a/src/multibody/joint.hpp
+++ b/src/multibody/joint.hpp
@@ -20,13 +20,15 @@
#include "pinocchio/multibody/joint/joint-base.hpp"
#include "pinocchio/multibody/joint/joint-dense.hpp"
-#include "pinocchio/multibody/joint/joint-revolute.hpp"
+#include "pinocchio/multibody/joint/joint-free-flyer.hpp"
+#include "pinocchio/multibody/joint/joint-planar.hpp"
+#include "pinocchio/multibody/joint/joint-prismatic.hpp"
#include "pinocchio/multibody/joint/joint-revolute-unaligned.hpp"
-#include "pinocchio/multibody/joint/joint-spherical.hpp"
+#include "pinocchio/multibody/joint/joint-revolute.hpp"
#include "pinocchio/multibody/joint/joint-spherical-ZYX.hpp"
-#include "pinocchio/multibody/joint/joint-prismatic.hpp"
-#include "pinocchio/multibody/joint/joint-free-flyer.hpp"
-#include "pinocchio/multibody/joint/joint-variant.hpp"
+#include "pinocchio/multibody/joint/joint-spherical.hpp"
+#include "pinocchio/multibody/joint/joint-translation.hpp"
+// #include "pinocchio/multibody/joint/joint-variant.hpp"
namespace se3
{
diff --git a/src/multibody/joint/joint-dense.hpp b/src/multibody/joint/joint-dense.hpp
index b9cae0db5aaaa65df47b6844609d48ada134521a..70f4817f12853f27a3c597cd33ddea30b76c68da 100644
--- a/src/multibody/joint/joint-dense.hpp
+++ b/src/multibody/joint/joint-dense.hpp
@@ -92,8 +92,13 @@ namespace se3
typedef JointDense<_NQ, _NV > Joint;
SE3_JOINT_TYPEDEF_TEMPLATE;
+ using JointModelBase<JointModelDense<_NQ, _NV > >::id;
using JointModelBase<JointModelDense<_NQ, _NV > >::idx_q;
using JointModelBase<JointModelDense<_NQ, _NV > >::idx_v;
+ using JointModelBase<JointModelDense<_NQ, _NV > >::lowerPosLimit;
+ using JointModelBase<JointModelDense<_NQ, _NV > >::upperPosLimit;
+ using JointModelBase<JointModelDense<_NQ, _NV > >::maxEffortLimit;
+ using JointModelBase<JointModelDense<_NQ, _NV > >::maxVelocityLimit;
using JointModelBase<JointModelDense<_NQ, _NV > >::setLowerPositionLimit;
using JointModelBase<JointModelDense<_NQ, _NV > >::setUpperPositionLimit;
using JointModelBase<JointModelDense<_NQ, _NV > >::setMaxEffortLimit;
@@ -175,6 +180,28 @@ namespace se3
return *this;
}
+ static const std::string shortname()
+ {
+ return std::string("JointModelDense");
+ }
+
+ template <class D>
+ bool operator == (const JointModelBase<D> &) const
+ {
+ return false;
+ }
+
+ bool operator == (const JointModelBase<JointModelDense> & jmodel) const
+ {
+ return jmodel.id() == id()
+ && jmodel.idx_q() == idx_q()
+ && jmodel.idx_v() == idx_v()
+ && jmodel.lowerPosLimit() == lowerPosLimit()
+ && jmodel.upperPosLimit() == upperPosLimit()
+ && jmodel.maxEffortLimit() == maxEffortLimit()
+ && jmodel.maxVelocityLimit() == maxVelocityLimit();
+ }
+
}; // struct JointModelDense
template<>
diff --git a/src/multibody/joint/joint-free-flyer.hpp b/src/multibody/joint/joint-free-flyer.hpp
index 62e436b6aa8e990196f24413a85aaa40a6b1ac93..a654834cc0cddc6d2f6a4adc005ba9313d746d64 100644
--- a/src/multibody/joint/joint-free-flyer.hpp
+++ b/src/multibody/joint/joint-free-flyer.hpp
@@ -203,9 +203,26 @@ namespace se3
);
}
- bool operator == (const JointModelFreeFlyer& /*Ohter*/) const
+ static const std::string shortname()
{
- return true; // TODO ?? used to bind variant in python
+ return std::string("JointModelFreeFlyer");
+ }
+
+ template <class D>
+ bool operator == (const JointModelBase<D> &) const
+ {
+ return false;
+ }
+
+ bool operator == (const JointModelBase<JointModelFreeFlyer> & jmodel) const
+ {
+ return jmodel.id() == id()
+ && jmodel.idx_q() == idx_q()
+ && jmodel.idx_v() == idx_v()
+ && jmodel.lowerPosLimit() == lowerPosLimit()
+ && jmodel.upperPosLimit() == upperPosLimit()
+ && jmodel.maxEffortLimit() == maxEffortLimit()
+ && jmodel.maxVelocityLimit() == maxVelocityLimit();
}
}; // struct JointModelFreeFlyer
diff --git a/src/multibody/joint/joint-planar.hpp b/src/multibody/joint/joint-planar.hpp
index 244223df7ed5a757583b3655c2568ff002692e33..01ae91aa3e42318282e31aedc119a04555708f67 100644
--- a/src/multibody/joint/joint-planar.hpp
+++ b/src/multibody/joint/joint-planar.hpp
@@ -335,9 +335,26 @@ namespace se3
);
}
- bool operator == (const JointModelPlanar& /*Ohter*/) const
+ static const std::string shortname()
{
- return true; // TODO ?? used to bind variant in python
+ return std::string("JointModelPlanar");
+ }
+
+ template <class D>
+ bool operator == (const JointModelBase<D> &) const
+ {
+ return false;
+ }
+
+ bool operator == (const JointModelBase<JointModelPlanar> & jmodel) const
+ {
+ return jmodel.id() == id()
+ && jmodel.idx_q() == idx_q()
+ && jmodel.idx_v() == idx_v()
+ && jmodel.lowerPosLimit() == lowerPosLimit()
+ && jmodel.upperPosLimit() == upperPosLimit()
+ && jmodel.maxEffortLimit() == maxEffortLimit()
+ && jmodel.maxVelocityLimit() == maxVelocityLimit();
}
}; // struct JointModelPlanar
diff --git a/src/multibody/joint/joint-prismatic.hpp b/src/multibody/joint/joint-prismatic.hpp
index 40e23fb7f2bdd233d8667c041bf9be6fc4970838..ddb9924448d2514a588bb26fd1dab6cbd08c99de 100644
--- a/src/multibody/joint/joint-prismatic.hpp
+++ b/src/multibody/joint/joint-prismatic.hpp
@@ -403,9 +403,23 @@ namespace se3
);
}
- bool operator == (const JointModelPrismatic& /*Ohter*/) const
+ static const std::string shortname();
+
+ template <class D>
+ bool operator == (const JointModelBase<D> &) const
+ {
+ return false;
+ }
+
+ bool operator == (const JointModelBase<JointModelPrismatic> & jmodel) const
{
- return true; // TODO ?? used to bind variant in python
+ return jmodel.id() == id()
+ && jmodel.idx_q() == idx_q()
+ && jmodel.idx_v() == idx_v()
+ && jmodel.lowerPosLimit() == lowerPosLimit()
+ && jmodel.upperPosLimit() == upperPosLimit()
+ && jmodel.maxEffortLimit() == maxEffortLimit()
+ && jmodel.maxVelocityLimit() == maxVelocityLimit();
}
}; // struct JointModelPrismatic
@@ -413,14 +427,32 @@ namespace se3
typedef JointDataPrismatic<0> JointDataPX;
typedef JointModelPrismatic<0> JointModelPX;
+ template<>
+ const std::string JointModelPrismatic<0>::shortname()
+ {
+ return std::string("JointModelPX");
+ }
+
typedef JointPrismatic<1> JointPY;
typedef JointDataPrismatic<1> JointDataPY;
typedef JointModelPrismatic<1> JointModelPY;
+ template<>
+ const std::string JointModelPrismatic<1>::shortname()
+ {
+ return std::string("JointModelPY");
+ }
+
typedef JointPrismatic<2> JointPZ;
typedef JointDataPrismatic<2> JointDataPZ;
typedef JointModelPrismatic<2> JointModelPZ;
+ template<>
+ const std::string JointModelPrismatic<2>::shortname()
+ {
+ return std::string("JointModelPZ");
+ }
+
} //namespace se3
#endif // ifndef __se3_joint_prismatic_hpp__
diff --git a/src/multibody/joint/joint-revolute-unaligned.hpp b/src/multibody/joint/joint-revolute-unaligned.hpp
index 3b18f608bb47ea31da43fb0bd0db9817145d9ff6..2cefb439f2a9c12c95190cd9239f8749c0e5560b 100644
--- a/src/multibody/joint/joint-revolute-unaligned.hpp
+++ b/src/multibody/joint/joint-revolute-unaligned.hpp
@@ -279,6 +279,12 @@ namespace se3
using JointModelBase<JointModelRevoluteUnaligned>::setIndexes;
JointModelRevoluteUnaligned() : axis(Eigen::Vector3d::Constant(NAN)) {}
+ JointModelRevoluteUnaligned(double x, double y, double z)
+ {
+ axis << x, y, z ;
+ axis.normalize();
+ assert(axis.isUnitary() && "Rotation axis is not unitary");
+ }
JointModelRevoluteUnaligned( const Motion::Vector3 & axis ) : axis(axis)
{
assert(axis.isUnitary() && "Rotation axis is not unitary");
@@ -326,9 +332,26 @@ namespace se3
);
}
- bool operator == (const JointModelRevoluteUnaligned& /*Ohter*/) const
+ static const std::string shortname()
+ {
+ return std::string("JointModelRevoluteUnaligned");
+ }
+
+ template <class D>
+ bool operator == (const JointModelBase<D> &) const
+ {
+ return false;
+ }
+
+ bool operator == (const JointModelBase<JointModelRevoluteUnaligned> & jmodel) const
{
- return true; // TODO ?? used to bind variant in python
+ return jmodel.id() == id()
+ && jmodel.idx_q() == idx_q()
+ && jmodel.idx_v() == idx_v()
+ && jmodel.lowerPosLimit() == lowerPosLimit()
+ && jmodel.upperPosLimit() == upperPosLimit()
+ && jmodel.maxEffortLimit() == maxEffortLimit()
+ && jmodel.maxVelocityLimit() == maxVelocityLimit();
}
}; // struct JointModelRevoluteUnaligned
diff --git a/src/multibody/joint/joint-revolute.hpp b/src/multibody/joint/joint-revolute.hpp
index b71ef48c14b6cd2a7c7d2f99f2760a16742e16b2..f33bd23755fb1d98c12cebf57f8137626b31cbf5 100644
--- a/src/multibody/joint/joint-revolute.hpp
+++ b/src/multibody/joint/joint-revolute.hpp
@@ -429,9 +429,23 @@ namespace se3
);
}
- bool operator == (const JointModelRevolute<axis>& /*Other*/) const
+ static const std::string shortname();
+
+ template <class D>
+ bool operator == (const JointModelBase<D> &) const
+ {
+ return false;
+ }
+
+ bool operator == (const JointModelBase<JointModelRevolute <axis> > & jmodel) const
{
- return true; // TODO ??
+ return jmodel.id() == id()
+ && jmodel.idx_q() == idx_q()
+ && jmodel.idx_v() == idx_v()
+ && jmodel.lowerPosLimit() == lowerPosLimit()
+ && jmodel.upperPosLimit() == upperPosLimit()
+ && jmodel.maxEffortLimit() == maxEffortLimit()
+ && jmodel.maxVelocityLimit() == maxVelocityLimit();
}
}; // struct JointModelRevolute
@@ -440,14 +454,32 @@ namespace se3
typedef JointDataRevolute<0> JointDataRX;
typedef JointModelRevolute<0> JointModelRX;
+ template<>
+ const std::string JointModelRevolute<0>::shortname()
+ {
+ return std::string("JointModelRX") ;
+ }
+
typedef JointRevolute<1> JointRY;
typedef JointDataRevolute<1> JointDataRY;
typedef JointModelRevolute<1> JointModelRY;
+ template<>
+ const std::string JointModelRevolute<1>::shortname()
+ {
+ return std::string("JointModelRY") ;
+ }
+
typedef JointRevolute<2> JointRZ;
typedef JointDataRevolute<2> JointDataRZ;
typedef JointModelRevolute<2> JointModelRZ;
+ template<>
+ const std::string JointModelRevolute<2>::shortname()
+ {
+ return std::string("JointModelRZ") ;
+ }
+
} //namespace se3
#endif // ifndef __se3_joint_revolute_hpp__
diff --git a/src/multibody/joint/joint-spherical-ZYX.hpp b/src/multibody/joint/joint-spherical-ZYX.hpp
index bb8a50b083f1aa4a32d3b9b18537eb949b620b0e..6c38a109cfbaa2d674e205f41570ca4a107317a7 100644
--- a/src/multibody/joint/joint-spherical-ZYX.hpp
+++ b/src/multibody/joint/joint-spherical-ZYX.hpp
@@ -340,9 +340,26 @@ namespace se3
);
}
- bool operator == (const JointModelSphericalZYX& /*Ohter*/) const
+ static const std::string shortname()
{
- return true; // TODO ?? used to bind variant in python
+ return std::string("JointModelSphericalZYX");
+ }
+
+ template <class D>
+ bool operator == (const JointModelBase<D> &) const
+ {
+ return false;
+ }
+
+ bool operator == (const JointModelBase<JointModelSphericalZYX> & jmodel) const
+ {
+ return jmodel.id() == id()
+ && jmodel.idx_q() == idx_q()
+ && jmodel.idx_v() == idx_v()
+ && jmodel.lowerPosLimit() == lowerPosLimit()
+ && jmodel.upperPosLimit() == upperPosLimit()
+ && jmodel.maxEffortLimit() == maxEffortLimit()
+ && jmodel.maxVelocityLimit() == maxVelocityLimit();
}
}; // struct JointModelSphericalZYX
diff --git a/src/multibody/joint/joint-spherical.hpp b/src/multibody/joint/joint-spherical.hpp
index c079a8253a58bf0f9ad3aec039e9ac5fa43856bc..3112bcd415c43cb48c0e7a9a1ae7894f7bfbd030 100644
--- a/src/multibody/joint/joint-spherical.hpp
+++ b/src/multibody/joint/joint-spherical.hpp
@@ -282,9 +282,26 @@ namespace se3
);
}
- bool operator == (const JointModelSpherical& /*Ohter*/) const
+ static const std::string shortname()
{
- return true; // TODO ?? used to bind variant in python
+ return std::string("JointModelSpherical");
+ }
+
+ template <class D>
+ bool operator == (const JointModelBase<D> &) const
+ {
+ return false;
+ }
+
+ bool operator == (const JointModelBase<JointModelSpherical> & jmodel) const
+ {
+ return jmodel.id() == id()
+ && jmodel.idx_q() == idx_q()
+ && jmodel.idx_v() == idx_v()
+ && jmodel.lowerPosLimit() == lowerPosLimit()
+ && jmodel.upperPosLimit() == upperPosLimit()
+ && jmodel.maxEffortLimit() == maxEffortLimit()
+ && jmodel.maxVelocityLimit() == maxVelocityLimit();
}
}; // struct JointModelSpherical
diff --git a/src/multibody/joint/joint-translation.hpp b/src/multibody/joint/joint-translation.hpp
index 8ee94c1e08636dedeac70af1bdcb4d34c750cc84..ea7c0afa73672a5ea68fbb821719797c04c10038 100644
--- a/src/multibody/joint/joint-translation.hpp
+++ b/src/multibody/joint/joint-translation.hpp
@@ -289,9 +289,26 @@ namespace se3
);
}
- bool operator == (const JointModelTranslation& /*Ohter*/) const
+ static const std::string shortname()
{
- return true; // TODO ?? used to bind variant in python
+ return std::string("JointModelTranslation");
+ }
+
+ template <class D>
+ bool operator == (const JointModelBase<D> &) const
+ {
+ return false;
+ }
+
+ bool operator == (const JointModelBase<JointModelTranslation> & jmodel) const
+ {
+ return jmodel.id() == id()
+ && jmodel.idx_q() == idx_q()
+ && jmodel.idx_v() == idx_v()
+ && jmodel.lowerPosLimit() == lowerPosLimit()
+ && jmodel.upperPosLimit() == upperPosLimit()
+ && jmodel.maxEffortLimit() == maxEffortLimit()
+ && jmodel.maxVelocityLimit() == maxVelocityLimit();
}
}; // struct JointModelTranslation
diff --git a/src/multibody/joint/joint-variant.hpp b/src/multibody/joint/joint-variant.hpp
index 8d4a21b8fe7cbbe8529d4522fca15abfb6862d63..d72f71b2ca5314dcc5f353617b6e896358eaafba 100644
--- a/src/multibody/joint/joint-variant.hpp
+++ b/src/multibody/joint/joint-variant.hpp
@@ -18,14 +18,7 @@
#ifndef __se3_joint_variant_hpp__
#define __se3_joint_variant_hpp__
-#include "pinocchio/multibody/joint/joint-revolute.hpp"
-#include "pinocchio/multibody/joint/joint-revolute-unaligned.hpp"
-#include "pinocchio/multibody/joint/joint-spherical.hpp"
-#include "pinocchio/multibody/joint/joint-spherical-ZYX.hpp"
-#include "pinocchio/multibody/joint/joint-prismatic.hpp"
-#include "pinocchio/multibody/joint/joint-planar.hpp"
-#include "pinocchio/multibody/joint/joint-translation.hpp"
-#include "pinocchio/multibody/joint/joint-free-flyer.hpp"
+#include "pinocchio/multibody/joint.hpp"
namespace se3
{
diff --git a/src/multibody/model.hpp b/src/multibody/model.hpp
index 9d3e3024086fdc4f73cee95310bb9aa92d70b90a..3e8809aa6ecbb91a1ce57e59c9a5bd10148af519 100644
--- a/src/multibody/model.hpp
+++ b/src/multibody/model.hpp
@@ -21,10 +21,10 @@
#include "pinocchio/spatial/fwd.hpp"
#include "pinocchio/spatial/se3.hpp"
-#include "pinocchio/spatial/inertia.hpp"
-#include "pinocchio/spatial/motion.hpp"
#include "pinocchio/spatial/force.hpp"
-#include "pinocchio/multibody/joint.hpp"
+#include "pinocchio/spatial/motion.hpp"
+#include "pinocchio/spatial/inertia.hpp"
+#include "pinocchio/multibody/joint/joint-variant.hpp"
#include <iostream>
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(se3::SE3)
diff --git a/src/multibody/visitor.hpp b/src/multibody/visitor.hpp
index a8f1f2ad77cc5ed111f6982a63f62d5b4aa236a1..f2141d9e329cb004e21795ee29e81a33b38f1095 100644
--- a/src/multibody/visitor.hpp
+++ b/src/multibody/visitor.hpp
@@ -21,6 +21,7 @@
#define BOOST_FUSION_INVOKE_MAX_ARITY 10
#include <boost/fusion/include/invoke.hpp>
#include <boost/fusion/include/algorithm.hpp>
+#include "pinocchio/multibody/joint/joint-variant.hpp"
namespace boost {
diff --git a/src/python/joint-dense.hpp b/src/python/joint-dense.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..df5f794cfd71afa69419c303ee329efa3673bff0
--- /dev/null
+++ b/src/python/joint-dense.hpp
@@ -0,0 +1,93 @@
+//
+// Copyright (c) 2015 CNRS
+//
+// This file is part of Pinocchio
+// Pinocchio is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+//
+// Pinocchio is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// Pinocchio If not, see
+// <http://www.gnu.org/licenses/>.
+
+#ifndef __se3_python_joint_dense_hpp__
+#define __se3_python_joint_dense_hpp__
+
+#include <eigenpy/exception.hpp>
+#include <eigenpy/eigenpy.hpp>
+#include "pinocchio/multibody/joint/joint-variant.hpp"
+
+
+namespace eigenpy
+{
+ template<class JointModelDense>
+ struct UnalignedEquivalentTypes
+ {
+ typedef Eigen::Matrix<double, JointModelDense::NQ, 1, Eigen::DontAlign> MatrixNQd_fx;
+ typedef Eigen::Matrix<double, JointModelDense::NV, 1, Eigen::DontAlign> MatrixNVd_fx;
+ };
+} // namespace eigenpy
+
+namespace se3
+{
+ namespace python
+ {
+ namespace bp = boost::python;
+
+ template<class JointModelDense>
+ struct JointPythonVisitor
+ : public boost::python::def_visitor< JointPythonVisitor<JointModelDense> >
+ {
+ typedef typename eigenpy::UnalignedEquivalentTypes<JointModelDense>::MatrixNQd_fx MatrixNQd_fx;
+ typedef typename eigenpy::UnalignedEquivalentTypes<JointModelDense>::MatrixNVd_fx MatrixNVd_fx;
+
+ public:
+
+ static PyObject* convert(JointModelDense const& jm)
+ {
+ return boost::python::incref(boost::python::object(jm).ptr());
+ }
+
+ template<class PyClass>
+ void visit(PyClass& cl) const
+ {
+ cl
+ .def(bp::init<>())
+
+ // All are add_properties cause ReadOnly
+ .add_property("id",&JointPythonVisitor::getId)
+ .add_property("idx_q",&JointPythonVisitor::getIdx_q)
+ .add_property("idx_v",&JointPythonVisitor::getIdx_v)
+ .add_property("nq",&JointPythonVisitor::getNq)
+ .add_property("nv",&JointPythonVisitor::getNv)
+
+
+ ;
+ }
+
+ static int getId( const JointModelDense & self ) { return self.id(); }
+ static int getIdx_q(const JointModelDense & self) {return self.idx_q();}
+ static int getIdx_v(const JointModelDense & self) {return self.idx_v();}
+ static int getNq(const JointModelDense & self) {return self.nq();}
+ static int getNv(const JointModelDense & self) {return self.nv();}
+
+
+
+ static void expose()
+ {
+
+ }
+
+ };
+
+
+
+ }} // namespace se3::python
+
+#endif // ifndef __se3_python_joint_dense_hpp__
+
diff --git a/src/python/joints-models.hpp b/src/python/joints-models.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..9412d6471d8cbcba177672da5e104eba136221c9
--- /dev/null
+++ b/src/python/joints-models.hpp
@@ -0,0 +1,49 @@
+//
+// Copyright (c) 2015 CNRS
+//
+// This file is part of Pinocchio
+// Pinocchio is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+//
+// Pinocchio is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// Pinocchio If not, see
+// <http://www.gnu.org/licenses/>.
+
+#ifndef __se3_python_joints_models_hpp__
+#define __se3_python_joints_models_hpp__
+
+#include <eigenpy/exception.hpp>
+#include <eigenpy/eigenpy.hpp>
+#include "pinocchio/multibody/joint/joint-variant.hpp"
+
+
+
+namespace se3
+{
+ namespace python
+ {
+ namespace bp = boost::python;
+
+
+ // generic expose_constructor : do nothing special
+ template <class T>
+ inline bp::class_<T>& expose_constructors(bp::class_<T>& cl)
+ {
+ return cl;
+ }
+
+ template<>
+ inline bp::class_<JointModelRevoluteUnaligned>& expose_constructors<JointModelRevoluteUnaligned> (bp::class_<JointModelRevoluteUnaligned> & cl)
+ {
+ return cl.def(bp::init<double, double, double> ((bp::args("x"), bp::args("y"), bp::args("z")), "Init JointRevoluteUnaligned from the components x, y, z of the axis"));
+ }
+ } // namespace python
+} // namespace se3
+
+#endif // ifndef __se3_python_joint_models_hpp__
diff --git a/src/python/joints-variant.hpp b/src/python/joints-variant.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..21f9a54b4b7f482a90ed27525015ed42a403241d
--- /dev/null
+++ b/src/python/joints-variant.hpp
@@ -0,0 +1,78 @@
+//
+// Copyright (c) 2015 CNRS
+//
+// This file is part of Pinocchio
+// Pinocchio is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+//
+// Pinocchio is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// Pinocchio If not, see
+// <http://www.gnu.org/licenses/>.
+
+#ifndef __se3_python_joints_variant_hpp__
+#define __se3_python_joints_variant_hpp__
+
+#include <eigenpy/exception.hpp>
+#include <eigenpy/eigenpy.hpp>
+#include "pinocchio/multibody/joint/joint-variant.hpp"
+#include "pinocchio/python/joints-models.hpp"
+
+
+
+namespace se3
+{
+ namespace python
+ {
+ namespace bp = boost::python;
+
+ struct jointModelVariantVisitor : boost::static_visitor<PyObject *>
+ {
+ static result_type convert(JointModelVariant const &jv)
+ {
+ return apply_visitor(jointModelVariantVisitor(), jv);
+ }
+
+ template<typename T>
+ result_type operator()(T const &t) const
+ {
+ return boost::python::incref(boost::python::object(t).ptr());
+ }
+ };
+
+
+ struct exposer {
+ template<class T> inline void operator()(T)
+ {
+ expose_constructors<T>(bp::class_<T>(T::shortname().c_str(),bp::init<>()).def(JointPythonVisitor<T >())
+ );
+ bp::implicitly_convertible<T, se3::JointModelVariant>();
+ }
+
+
+ };
+
+ // For the moment, only expose models of joint. Not data ( to do it, split exposer into exposerModels & exposer_Datas and do another for_each)
+ static void exposeVariants()
+ {
+
+ boost::mpl::for_each<JointModelVariant::types>(exposer());
+
+ bp::to_python_converter<se3::JointModelVariant, jointModelVariantVisitor>();
+
+ // bp::def("make_variant", se3::make_variant);
+
+ }
+
+
+
+
+ }} // namespace se3::python
+
+#endif // ifndef __se3_python_joints_variant_hpp__
+
diff --git a/src/python/model.hpp b/src/python/model.hpp
index dd26c7ea34f9478d6f646d15ed13df45bafd14bc..eb89ed4a2e5632c9c90f06bd773df49a73ba7058 100644
--- a/src/python/model.hpp
+++ b/src/python/model.hpp
@@ -94,11 +94,6 @@ namespace se3
.add_property("fix_hasVisual", bp::make_function(&ModelPythonVisitor::fix_hasVisual, bp::return_internal_reference<>()) )
.add_property("fix_bodyNames", bp::make_function(&ModelPythonVisitor::fix_bodyNames, bp::return_internal_reference<>()) )
- // Add here some access of joint data (to be modified when the joints will be binded).
- .def("joint_nq", bp::make_function(&ModelPythonVisitor::joint_nq) )
- .def("joint_nv", bp::make_function(&ModelPythonVisitor::joint_nv) )
- .def("joint_idx_q", bp::make_function(&ModelPythonVisitor::joint_idx_q) )
- .def("joint_idx_v", bp::make_function(&ModelPythonVisitor::joint_idx_v) )
.add_property("gravity",&ModelPythonVisitor::gravity,&ModelPythonVisitor::setGravity)
.def("BuildEmptyModel",&ModelPythonVisitor::maker_empty)
@@ -118,6 +113,7 @@ namespace se3
static int nbody( ModelHandler & m ) { return m->nbody; }
static std::vector<Inertia> & inertias( ModelHandler & m ) { return m->inertias; }
static std::vector<SE3> & jointPlacements( ModelHandler & m ) { return m->jointPlacements; }
+ static JointModelVector & joints( ModelHandler & m ) { return m->joints; }
static std::vector<Model::Index> & parents( ModelHandler & m ) { return m->parents; }
static std::vector<std::string> & names ( ModelHandler & m ) { return m->names; }
static std::vector<std::string> & bodyNames ( ModelHandler & m ) { return m->bodyNames; }
@@ -129,11 +125,6 @@ namespace se3
static std::vector<bool> & fix_hasVisual ( ModelHandler & m ) { return m->fix_hasVisual; }
static std::vector<std::string> & fix_bodyNames ( ModelHandler & m ) { return m->fix_bodyNames; }
- static int joint_nq( ModelHandler & m,const Model::Index & idx ) { return se3::nq( m->joints[idx] ); }
- static int joint_nv( ModelHandler & m,const Model::Index & idx ) { return se3::nv( m->joints[idx] ); }
- static int joint_idx_q( ModelHandler & m,const Model::Index & idx ) { return se3::idx_q( m->joints[idx] ); }
- static int joint_idx_v( ModelHandler & m,const Model::Index & idx ) { return se3::idx_v( m->joints[idx] ); }
-
static Motion gravity( ModelHandler & m ) { return m->gravity; }
static void setGravity( ModelHandler & m,const Motion_fx & g ) { m->gravity = g; }
@@ -163,12 +154,14 @@ namespace se3
.def(bp::vector_indexing_suite< std::vector<bool> >());
bp::class_< std::vector<double> >("StdVec_double")
.def(bp::vector_indexing_suite< std::vector<double> >());
-
- bp::class_<ModelHandler>("Model",
- "Articulated rigid body model (const)",
- bp::no_init)
- .def(ModelPythonVisitor());
-
+ bp::class_< JointModelVector >("StdVec_JointModelVector")
+ .def(bp::vector_indexing_suite< JointModelVector, true >());
+
+ 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 >();
diff --git a/src/python/module.cpp b/src/python/module.cpp
index 866fc07a43e98670ae27cf80c405875c7cb484e5..9fadbffc5718884d981d393760d4737832681305 100644
--- a/src/python/module.cpp
+++ b/src/python/module.cpp
@@ -39,6 +39,7 @@ BOOST_PYTHON_MODULE(libpinocchio_pywrap)
se3::python::exposeForce();
se3::python::exposeMotion();
se3::python::exposeInertia();
+ se3::python::exposeJoints();
se3::python::exposeExplog();
se3::python::exposeModel();
diff --git a/src/python/python.cpp b/src/python/python.cpp
index 951a2f99269fbb26626c1d24a2d2edf46a65f445..e25a26a4639d2a9b54ce50cf403fd80d55fd9d6a 100644
--- a/src/python/python.cpp
+++ b/src/python/python.cpp
@@ -20,6 +20,8 @@
#include "pinocchio/python/force.hpp"
#include "pinocchio/python/motion.hpp"
#include "pinocchio/python/inertia.hpp"
+#include "pinocchio/python/joint-dense.hpp"
+#include "pinocchio/python/joints-variant.hpp"
#include "pinocchio/python/model.hpp"
#include "pinocchio/python/data.hpp"
@@ -51,6 +53,10 @@ namespace se3
InertiaPythonVisitor<Inertia>::expose();
PyWraperForAlignedStdVector<Inertia>::expose("StdVec_Inertia");
}
+ void exposeJoints()
+ {
+ exposeVariants();
+ }
void exposeModel()
{
ModelPythonVisitor::expose();
diff --git a/src/python/python.hpp b/src/python/python.hpp
index f939c0d2e4e6ffaf07487d86f0ea49250614b2b0..76361b565195c16c53e127b2b4c3dfbccd387ae3 100644
--- a/src/python/python.hpp
+++ b/src/python/python.hpp
@@ -26,6 +26,7 @@ namespace se3
void exposeForce();
void exposeMotion();
void exposeInertia();
+ void exposeJoints();
void exposeExplog();
void exposeModel();