[Python] Remove unaligned conversion

bindings/python/algorithm/algorithms.hpp

@@ -29,10 +29,6 @@ namespace se3
 {
   namespace python
   {
-   
-    typedef eigenpy::UnalignedEquivalent<Eigen::VectorXd>::type VectorXd_fx;
-    typedef eigenpy::UnalignedEquivalent<Eigen::MatrixXd>::type MatrixXd_fx;
-
     void exposeJointsAlgo();
     void exposeABA();
     void exposeCRBA();

bindings/python/algorithm/expose-aba.cpp

@@ -25,9 +25,9 @@ namespace se3
     
     static Eigen::MatrixXd aba_proxy(const ModelHandler & model,
                               DataHandler & data,
-                              const VectorXd_fx & q,
-                              const VectorXd_fx & v,
-                              const VectorXd_fx & tau)
+                              const Eigen::VectorXd & q,
+                              const Eigen::VectorXd & v,
+                              const Eigen::VectorXd & tau)
     {
       aba(*model,*data,q,v,tau);
       return data->ddq;

bindings/python/algorithm/expose-cat.cpp

@@ -24,8 +24,8 @@ namespace se3
   {
     static void computeAllTerms_proxy(const ModelHandler & model,
                                       DataHandler & data,
-                                      const VectorXd_fx & q,
-                                      const VectorXd_fx & v)
+                                      const Eigen::VectorXd & q,
+                                      const Eigen::VectorXd & v)
     {
       data->M.fill(0);
       computeAllTerms(*model,*data,q,v);

bindings/python/algorithm/expose-com.cpp

@@ -26,7 +26,7 @@ namespace se3
     static SE3::Vector3
     com_0_proxy(const ModelHandler& model,
                 DataHandler & data,
-                const VectorXd_fx & q,
+                const Eigen::VectorXd & q,
                 const bool updateKinematics = true)
     {
       return centerOfMass(*model,*data,q,
@@ -37,8 +37,8 @@ namespace se3
     static SE3::Vector3
     com_1_proxy(const ModelHandler& model,
                 DataHandler & data,
-                const VectorXd_fx & q,
-                const VectorXd_fx & v,
+                const Eigen::VectorXd & q,
+                const Eigen::VectorXd & v,
                 const bool updateKinematics = true)
     {
       return centerOfMass(*model,*data,q,v,
@@ -49,9 +49,9 @@ namespace se3
     static SE3::Vector3
     com_2_proxy(const ModelHandler & model,
                 DataHandler & data,
-                const VectorXd_fx & q,
-                const VectorXd_fx & v,
-                const VectorXd_fx & a,
+                const Eigen::VectorXd & q,
+                const Eigen::VectorXd & v,
+                const Eigen::VectorXd & a,
                 const bool updateKinematics = true)
     {
       return centerOfMass(*model,*data,q,v,a,
@@ -62,7 +62,7 @@ namespace se3
     static Data::Matrix3x
     Jcom_proxy(const ModelHandler& model,
                DataHandler & data,
-               const VectorXd_fx & q)
+               const Eigen::VectorXd & q)
     {
       return jacobianCenterOfMass(*model,*data,q);
     }

bindings/python/algorithm/expose-crba.cpp

@@ -24,7 +24,7 @@ namespace se3
   {
     static Eigen::MatrixXd crba_proxy(const ModelHandler& model,
                                       DataHandler & data,
-                                      const VectorXd_fx & q)
+                                      const Eigen::VectorXd & q)
     {
       data->M.fill(0);
       crba(*model,*data,q);
@@ -35,8 +35,8 @@ namespace se3
     
     static Data::Matrix6x ccrba_proxy(const ModelHandler& model,
                                       DataHandler & data,
-                                      const VectorXd_fx & q,
-                                      const VectorXd_fx & v)
+                                      const Eigen::VectorXd & q,
+                                      const Eigen::VectorXd & v)
     {
       ccrba(*model,*data,q,v);
       return data->Ag;

bindings/python/algorithm/expose-dynamics.cpp

@@ -24,11 +24,11 @@ namespace se3
   {
     static Eigen::MatrixXd fd_llt_proxy(const ModelHandler & model,
                                         DataHandler & data,
-                                        const VectorXd_fx & q,
-                                        const VectorXd_fx & v,
-                                        const VectorXd_fx & tau,
-                                        const eigenpy::MatrixXd_fx & J,
-                                        const VectorXd_fx & gamma,
+                                        const Eigen::VectorXd & q,
+                                        const Eigen::VectorXd & v,
+                                        const Eigen::VectorXd & tau,
+                                        const Eigen::MatrixXd & J,
+                                        const Eigen::VectorXd & gamma,
                                         const bool update_kinematics = true)
     {
       forwardDynamics(*model,*data,q,v,tau,J,gamma,update_kinematics);
@@ -37,9 +37,9 @@ namespace se3
     
     static Eigen::MatrixXd id_llt_proxy(const ModelHandler & model,
                                         DataHandler & data,
-                                        const VectorXd_fx & q,
-                                        const VectorXd_fx & v_before,
-                                        const eigenpy::MatrixXd_fx & J,
+                                        const Eigen::VectorXd & q,
+                                        const Eigen::VectorXd & v_before,
+                                        const Eigen::MatrixXd & J,
                                         const double r_coeff,
                                         const bool update_kinematics = true)
     {

bindings/python/algorithm/expose-energy.cpp

@@ -24,8 +24,8 @@ namespace se3
   {
     static double kineticEnergy_proxy(const ModelHandler & model,
                                       DataHandler & data,
-                                      const VectorXd_fx & q,
-                                      const VectorXd_fx & v,
+                                      const Eigen::VectorXd & q,
+                                      const Eigen::VectorXd & v,
                                       const bool update_kinematics = true)
     {
       return kineticEnergy(*model,*data,q,v,update_kinematics);
@@ -33,7 +33,7 @@ namespace se3
     
     static double potentialEnergy_proxy(const ModelHandler & model,
                                         DataHandler & data,
-                                        const VectorXd_fx & q,
+                                        const Eigen::VectorXd & q,
                                         const bool update_kinematics = true)
     {
       return potentialEnergy(*model,*data,q,update_kinematics);

bindings/python/algorithm/expose-frames.cpp

@@ -25,7 +25,7 @@ namespace se3
     
     static Data::Matrix6x frame_jacobian_proxy(const ModelHandler & model,
                                                DataHandler & data,
-                                               const VectorXd_fx & q,
+                                               const Eigen::VectorXd & q,
                                                Model::FrameIndex frame_id,
                                                bool local,
                                                bool update_geometry
@@ -44,7 +44,7 @@ namespace se3
     
     static void frames_fk_0_proxy(const ModelHandler& model,
                                   DataHandler & data,
-                                  const VectorXd_fx & q
+                                  const Eigen::VectorXd & q
                                   )
     {
       framesForwardKinematics( *model,*data,q );

bindings/python/algorithm/expose-geometry.cpp

@@ -30,7 +30,7 @@ namespace se3
                                                DataHandler & data,
                                                const GeometryModelHandler & geom_model,
                                                GeometryDataHandler & geom_data,
-                                               const VectorXd_fx & q
+                                               const Eigen::VectorXd & q
                                                )
     {
       return updateGeometryPlacements(*model, *data, *geom_model, *geom_data, q);
@@ -56,7 +56,7 @@ namespace se3
                                                    DataHandler & data,
                                                    const GeometryModelHandler & model_geom,
                                                    GeometryDataHandler & data_geom,
-                                                   const VectorXd_fx & q,
+                                                   const Eigen::VectorXd & q,
                                                    const bool stopAtFirstCollision)
     {
       return computeCollisions(*model,*data,*model_geom, *data_geom, q, stopAtFirstCollision);

bindings/python/algorithm/expose-jacobian.cpp

@@ -25,7 +25,7 @@ namespace se3
     
     static void compute_jacobians_proxy(const ModelHandler& model,
                                         DataHandler & data,
-                                        const VectorXd_fx & q)
+                                        const Eigen::VectorXd & q)
     {
       computeJacobians(*model,*data,q);
     }
@@ -34,7 +34,7 @@ namespace se3
     static Data::Matrix6x
     jacobian_proxy(const ModelHandler & model,
                    DataHandler & data,
-                   const VectorXd_fx & q,
+                   const Eigen::VectorXd & q,
                    Model::JointIndex jointId,
                    bool local,
                    bool update_geometry)

bindings/python/algorithm/expose-joints.cpp

@@ -23,43 +23,43 @@ namespace se3
   namespace python
   {
     static Eigen::VectorXd integrate_proxy(const ModelHandler & model,
-                                           const VectorXd_fx & q,
-                                           const VectorXd_fx & v)
+                                           const Eigen::VectorXd & q,
+                                           const Eigen::VectorXd & v)
     {
       return integrate(*model,q,v);
     }
     
     static Eigen::VectorXd interpolate_proxy(const ModelHandler & model,
-                                             const VectorXd_fx & q1,
-                                             const VectorXd_fx & q2,
+                                             const Eigen::VectorXd & q1,
+                                             const Eigen::VectorXd & q2,
                                              const double u)
     {
       return interpolate(*model,q1,q2,u);
     }
     
     static Eigen::VectorXd differentiate_proxy(const ModelHandler & model,
-                                               const VectorXd_fx & q1,
-                                               const VectorXd_fx & q2)
+                                               const Eigen::VectorXd & q1,
+                                               const Eigen::VectorXd & q2)
     {
       return differentiate(*model,q1,q2);
     }
     
     static Eigen::VectorXd distance_proxy(const ModelHandler & model,
-                                          const VectorXd_fx & q1,
-                                          const VectorXd_fx & q2)
+                                          const Eigen::VectorXd & q1,
+                                          const Eigen::VectorXd & q2)
     {
       return distance(*model,q1,q2);
     }
     
     static Eigen::VectorXd randomConfiguration_proxy(const ModelHandler & model,
-                                                     const VectorXd_fx & lowerPosLimit,
-                                                     const VectorXd_fx & upperPosLimit)
+                                                     const Eigen::VectorXd & lowerPosLimit,
+                                                     const Eigen::VectorXd & upperPosLimit)
     {
       return randomConfiguration(*model, lowerPosLimit, upperPosLimit);
     }
 
     static void normalize_proxy(const ModelHandler & model,
-                                VectorXd_fx & config)
+                                Eigen::VectorXd & config)
     {
       Eigen::VectorXd q(config);
       normalize(*model, q);
@@ -67,8 +67,8 @@ namespace se3
     }
 
     static bool isSameConfiguration_proxy(const ModelHandler & model,
-                                          const VectorXd_fx & q1,
-                                          const VectorXd_fx & q2)
+                                          const Eigen::VectorXd & q1,
+                                          const Eigen::VectorXd & q2)
     {
       return isSameConfiguration(*model, q1, q2);
     }

bindings/python/algorithm/expose-kinematics.cpp

@@ -25,15 +25,15 @@ namespace se3
     
     static void fk_0_proxy(const ModelHandler & model,
                            DataHandler & data,
-                           const VectorXd_fx & q)
+                           const Eigen::VectorXd & q)
     {
       forwardKinematics(*model,*data,q);
     }
     
     static void fk_1_proxy(const ModelHandler& model,
                            DataHandler & data,
-                           const VectorXd_fx & q,
-                           const VectorXd_fx & qdot )
+                           const Eigen::VectorXd & q,
+                           const Eigen::VectorXd & qdot )
     {
       forwardKinematics(*model,*data,q,qdot);
     }
@@ -41,9 +41,9 @@ namespace se3
     
     static void fk_2_proxy(const ModelHandler& model,
                            DataHandler & data,
-                           const VectorXd_fx & q,
-                           const VectorXd_fx & v,
-                           const VectorXd_fx & a)
+                           const Eigen::VectorXd & q,
+                           const Eigen::VectorXd & v,
+                           const Eigen::VectorXd & a)
     {
       forwardKinematics(*model,*data,q,v,a);
     }

bindings/python/algorithm/expose-rnea.cpp

@@ -24,18 +24,18 @@ namespace se3
   {
     static Eigen::VectorXd rnea_proxy(const ModelHandler& model,
                                       DataHandler & data,
-                                      const VectorXd_fx & q,
-                                      const VectorXd_fx & v,
-                                      const VectorXd_fx & a)
+                                      const Eigen::VectorXd & q,
+                                      const Eigen::VectorXd & v,
+                                      const Eigen::VectorXd & a)
     {
       return rnea(*model,*data,q,v,a);
     }
 
     static Eigen::VectorXd rnea_fext_proxy(const ModelHandler& model,
                                            DataHandler & data,
-                                           const VectorXd_fx & q,
-                                           const VectorXd_fx & v,
-                                           const VectorXd_fx & a,
+                                           const Eigen::VectorXd & q,
+                                           const Eigen::VectorXd & v,
+                                           const Eigen::VectorXd & a,
                                            const std::vector<Force> & fext)
     {
       return rnea(*model,*data,q,v,a,fext);
@@ -43,8 +43,8 @@ namespace se3
     
     static Eigen::VectorXd nle_proxy(const ModelHandler& model,
                                      DataHandler & data,
-                                     const VectorXd_fx & q,
-                                     const VectorXd_fx & v)
+                                     const Eigen::VectorXd & q,
+                                     const Eigen::VectorXd & v)
     {
       return nonLinearEffects(*model,*data,q,v);
     }

bindings/python/eigen_container.hpp

@@ -39,7 +39,6 @@ namespace se3
     struct PyWraperForAlignedStdVector
       : public boost::python::def_visitor< PyWraperForAlignedStdVector<EigenObject> >
     {
-      typedef typename eigenpy::UnalignedEquivalent<EigenObject>::type EigenObject_fx;
       typedef std::vector<EigenObject> stdVectorAligned;
       
       template<class PyClass>
@@ -61,7 +60,7 @@ namespace se3
       }
 
       static void setItem( stdVectorAligned & Ys,
-			   int i,const EigenObject_fx & Y)
+			   int i,const EigenObject & Y)
       { 
 	assert( Ys.size()<INT_MAX );
 	if( i<0 ) i = int(Ys.size())+i;

bindings/python/explog.hpp

 //
-// Copyright (c) 2015 CNRS
+// Copyright (c) 2015-2016 CNRS
 // Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
 //
 // This file is part of Pinocchio
@@ -22,52 +22,46 @@
 # include <eigenpy/eigenpy.hpp>
 
 # include "pinocchio/spatial/explog.hpp"
-# include "pinocchio/bindings/python/se3.hpp"
-# include "pinocchio/bindings/python/motion.hpp"
 
 namespace se3
 {
   namespace python
   {
+    namespace bp = boost::python;
+    
     struct ExplogPythonVisitor
     {
       typedef Eigen::Matrix3d Matrix3d;
       typedef Eigen::Vector3d Vector3d;
       typedef Eigen::Matrix4d Matrix4d;
       typedef Eigen::Matrix<double,6,1> Vector6d;
-      typedef eigenpy::Matrix3d_fx Matrix3d_fx;
-      typedef eigenpy::Vector3d_fx Vector3d_fx;
-      typedef eigenpy::Matrix4d_fx Matrix4d_fx;
-      typedef eigenpy::UnalignedEquivalent<Vector6d>::type Vector6d_fx;
-      typedef SE3PythonVisitor<SE3>::SE3_fx SE3_fx;
-      typedef MotionPythonVisitor<Motion>::Motion_fx Motion_fx;
 
-      static Matrix3d exp3_proxy(const Vector3d_fx & v)
+      static Matrix3d exp3_proxy(const Vector3d & v)
       {
         return exp3(v);
       }
 
-      static Vector3d log3_proxy(const Matrix3d_fx & R)
+      static Vector3d log3_proxy(const Matrix3d & R)
       {
         return log3(R);
       }
 
-      static SE3_fx exp6FromMotion_proxy(const Motion_fx & nu)
+      static SE3 exp6FromMotion_proxy(const Motion & nu)
       {
         return exp6(nu);
       }
 
-      static SE3_fx exp6FromVector_proxy(const Vector6d_fx & v)
+      static SE3 exp6FromVector_proxy(const Vector6d & v)
       {
         return exp6(v);
       }
 
-      static Motion_fx log6FromSE3_proxy(const SE3_fx & m)
+      static Motion log6FromSE3_proxy(const SE3 & m)
       {
         return log6(m);
       }
 
-      static Motion_fx log6FromMatrix_proxy(const Matrix4d_fx & M)
+      static Motion log6FromMatrix_proxy(const Matrix4d & M)
       {
         return log6(M);
       }

bindings/python/force.hpp

@@ -21,17 +21,11 @@
 
 #include <eigenpy/exception.hpp>
 #include <eigenpy/eigenpy.hpp>
-#include "pinocchio/spatial/force.hpp"
+#include <eigenpy/memory.hpp>
 
+#include "pinocchio/spatial/force.hpp"
 
-namespace eigenpy
-{
-  template<>
-  struct UnalignedEquivalent<se3::Force>
-  {
-    typedef se3::ForceTpl<se3::Force::Scalar,Eigen::DontAlign> type;
-  };
-} // namespace eigenpy
+EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(se3::Force)
 
 namespace se3
 {
@@ -43,42 +37,28 @@ namespace se3
     struct ForcePythonVisitor
       : public boost::python::def_visitor< ForcePythonVisitor<Force> >
     {
-      typedef typename eigenpy::UnalignedEquivalent<Force>::type Force_fx;
       typedef typename Force::Matrix3 Matrix3;
       typedef typename Force::Matrix6 Matrix6;
       typedef typename Force::Vector6 Vector6;
       typedef typename Force::Vector3 Vector3;
-
-      typedef typename Force_fx::Matrix3 Matrix3_fx;
-      typedef typename Force_fx::Matrix6 Matrix6_fx;
-      typedef typename Force_fx::Vector6 Vector6_fx;
-      typedef typename Force_fx::Vector3 Vector3_fx;
-
-    public:
-
-      static PyObject* convert(Force const& f)
-      {
-        Force_fx f_fx (f);
-        return boost::python::incref(boost::python::object(f_fx).ptr());
-      }
-
+      
       template<class PyClass>
       void visit(PyClass& cl) const 
       {
         cl
-        .def(bp::init<Vector3_fx,Vector3_fx>
+        .def(bp::init<Vector3,Vector3>
              ((bp::arg("linear"),bp::arg("angular")),
               "Initialize from linear and angular components (dont mix the order)."))
-        .def(bp::init<Vector6_fx>((bp::arg("Vector 6d")),"Init from vector 6 [f,n]"))
-        .def(bp::init<Force_fx>((bp::arg("other")),"Copy constructor."))
+        .def(bp::init<Vector6>((bp::arg("Vector 6d")),"Init from vector 6 [f,n]"))
+        .def(bp::init<Force>((bp::arg("other")),"Copy constructor."))
         
         .add_property("linear",&ForcePythonVisitor::getLinear,&ForcePythonVisitor::setLinear)
         .add_property("angular",&ForcePythonVisitor::getAngular,&ForcePythonVisitor::setAngular)
         .add_property("vector",&ForcePythonVisitor::getVector,&ForcePythonVisitor::setVector)
         .add_property("np",&ForcePythonVisitor::getVector)
         
-        .def("se3Action",&Force_fx::se3Action)
-        .def("se3ActionInverse",&Force_fx::se3ActionInverse)
+        .def("se3Action",&Force::se3Action)
+        .def("se3ActionInverse",&Force::se3ActionInverse)
         
         .def("setZero",&ForcePythonVisitor::setZero)
         .def("setRandom",&ForcePythonVisitor::setRandom)
@@ -89,38 +69,37 @@ namespace se3
         .def(bp::self_ns::str(bp::self_ns::self))
         .def(bp::self_ns::repr(bp::self_ns::self))
         
-        .def("Random",&Force_fx::Random)
+        .def("Random",&Force::Random)
         .staticmethod("Random")
-        .def("Zero",&Force_fx::Zero)
+        .def("Zero",&Force::Zero)
         .staticmethod("Zero")
         ;
       }
 
-      static Vector3_fx getLinear(const Force_fx & self ) { return self.linear(); }
-      static void setLinear(Force_fx & self, const Vector3_fx & f) { self.linear(f); }
-      static Vector3_fx getAngular(const Force_fx & self) { return self.angular(); }
-      static void setAngular(Force_fx & self, const Vector3_fx & n) { self.angular(n); }
+      static Vector3 getLinear(const 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 void setAngular(Force & self, const Vector3 & n) { self.angular(n); }
       
-      static void setZero(Force_fx & self) { self.setZero(); }
-      static void setRandom(Force_fx & self) { self.setRandom(); }
+      static void setZero(Force & self) { self.setZero(); }
+      static void setRandom(Force & self) { self.setRandom(); }
       
-      static Vector6_fx getVector(const Force_fx & self) { return self.toVector(); }
-      static void setVector(Force_fx & self, const Vector6_fx & f) { self = f; }
+      static Vector6 getVector(const Force & self) { return self.toVector(); }
+      static void setVector(Force & self, const Vector6 & f) { self = f; }
       
-      static Force_fx add(const Force_fx & f1, const Force_fx & f2) { return f1+f2; }
-      static Force_fx subst(const Force_fx & f1, const Force_fx & f2) { return f1-f2; }
-      static Force_fx neg(const Force_fx & f1) { return -f1; }
+      static Force add(const Force & f1, const Force & f2) { return f1+f2; }
+      static Force subst(const Force & f1, const Force & f2) { return f1-f2; }
+      static Force neg(const Force & f1) { return -f1; }
 
       static void expose()
       {
-	bp::class_<Force_fx>("Force",
+	bp::class_<Force>("Force",
 			     "Force vectors, in se3* == F^6.\n\n"
 			     "Supported operations ...",
 			     bp::init<>())
 	  .def(ForcePythonVisitor<Force>())
 	;
     
-	bp::to_python_converter< Force,ForcePythonVisitor<Force> >();
     }
 
 

bindings/python/frame.hpp

@@ -23,7 +23,6 @@
 
 #include <boost/python/suite/indexing/vector_indexing_suite.hpp>
 
-#include "pinocchio/bindings/python/se3.hpp"
 #include "pinocchio/multibody/frame.hpp"
 #include "pinocchio/multibody/model.hpp"
 #include "pinocchio/container/aligned-vector.hpp"
@@ -37,23 +36,17 @@ namespace se3
     struct FramePythonVisitor
       : public boost::python::def_visitor< FramePythonVisitor >
     {
-      typedef eigenpy::UnalignedEquivalent<SE3>::type SE3_fx;
       typedef Model::Index Index;
       typedef Model::JointIndex JointIndex;
       typedef Model::FrameIndex FrameIndex;
 
     public:
 
-      static PyObject* convert(Frame const& f)
-      {
-        return boost::python::incref(boost::python::object(f).ptr());
-      }
-
       template<class PyClass>
       void visit(PyClass& cl) const 
       {
         cl
-          .def(bp::init< const std::string&,const JointIndex, const FrameIndex, const SE3_fx&,FrameType> ((bp::arg("name (string)"),bp::arg("index of parent joint"), bp::args("index of parent frame"), bp::arg("SE3 placement"), bp::arg("type (FrameType)")),
+          .def(bp::init< const std::string&,const JointIndex, const FrameIndex, const SE3&,FrameType> ((bp::arg("name (string)"),bp::arg("index of parent joint"), bp::args("index of parent frame"), bp::arg("SE3 placement"), bp::arg("type (FrameType)")),
                 "Initialize from name, parent joint id, parent frame id and placement wrt parent joint."))
 
           .def_readwrite("name", &Frame::name, "name  of the frame")
@@ -71,8 +64,8 @@ namespace se3
       }
 
 
-      static SE3_fx getPlacementWrtParentJoint(const Frame & self) { return self.placement; }
-      static void setPlacementWrtParentJoint(Frame & self, const SE3_fx & placement) { self.placement = placement; }
+      static SE3 getPlacementWrtParentJoint(const Frame & self) { return self.placement; }
+      static void setPlacementWrtParentJoint(Frame & self, const SE3 & placement) { self.placement = placement; }
 
       static void expose()