Commit cc0133d3 authored by Valenza Florian's avatar Valenza Florian
Browse files

[Python] Binding the different joint models. The exposure is done through the...

[Python] Binding the different joint models. The exposure is done through the variant. In joints-models, you can specialise the binding for any jointModel you want.
parent 2bd39d8d
......@@ -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
......
......@@ -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
{
......
......@@ -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<>
......
......@@ -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
......
......@@ -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
......
......@@ -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__
......@@ -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
......
......@@ -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__
......@@ -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
......
......@@ -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
......
......@@ -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
......
......@@ -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
{
......
......@@ -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)
......
......@@ -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 {
......
//
// 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__
//
// 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__
//
// 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