Commit 745369d3 authored by jcarpent's avatar jcarpent
Browse files

[Python] Remove unaligned conversion

parent b7ef42c5
......@@ -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();
......
......@@ -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;
......
......@@ -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);
......
......@@ -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);
}
......
......@@ -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;
......
......@@ -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)
{
......
......@@ -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);
......
......@@ -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 );
......
......@@ -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);
......
......@@ -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)
......
......@@ -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);
}
......
......@@ -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);
}
......
......@@ -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);
}
......
......@@ -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;
......
//
// 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);
}
......
......@@ -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> >();
}
......
......@@ -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()