Commit 5b8822ce authored by jcarpent's avatar jcarpent
Browse files

[Python] Clean Data bindings

parent 34cfb19b
......@@ -18,16 +18,15 @@
#ifndef __se3_python_algorithm_hpp__
#define __se3_python_algorithm_hpp__
#include <eigenpy/exception.hpp>
#include <eigenpy/eigenpy.hpp>
#include <boost/python.hpp>
#include "pinocchio/bindings/python/fwd.hpp"
#include "pinocchio/bindings/python/multibody/data.hpp"
namespace se3
{
namespace python
{
namespace bp = boost::python;
void exposeJointsAlgo();
void exposeABA();
void exposeCRBA();
......
......@@ -29,8 +29,8 @@ namespace se3
const Eigen::VectorXd & v,
const Eigen::VectorXd & tau)
{
aba(model,*data,q,v,tau);
return data->ddq;
aba(model,data,q,v,tau);
return data.ddq;
}
void exposeABA()
......
......@@ -23,14 +23,14 @@ namespace se3
namespace python
{
static void computeAllTerms_proxy(const Model & model,
DataHandler & data,
Data & data,
const Eigen::VectorXd & q,
const Eigen::VectorXd & v)
{
data->M.fill(0);
computeAllTerms(model,*data,q,v);
data->M.triangularView<Eigen::StrictlyLower>()
= data->M.transpose().triangularView<Eigen::StrictlyLower>();
data.M.fill(0);
computeAllTerms(model,data,q,v);
data.M.triangularView<Eigen::StrictlyLower>()
= data.M.transpose().triangularView<Eigen::StrictlyLower>();
}
void exposeCAT()
......
......@@ -25,46 +25,46 @@ namespace se3
static SE3::Vector3
com_0_proxy(const Model& model,
DataHandler & data,
Data & data,
const Eigen::VectorXd & q,
const bool updateKinematics = true)
{
return centerOfMass(model,*data,q,
return centerOfMass(model,data,q,
true,
updateKinematics);
}
static SE3::Vector3
com_1_proxy(const Model& model,
DataHandler & data,
Data & data,
const Eigen::VectorXd & q,
const Eigen::VectorXd & v,
const bool updateKinematics = true)
{
return centerOfMass(model,*data,q,v,
return centerOfMass(model,data,q,v,
true,
updateKinematics);
}
static SE3::Vector3
com_2_proxy(const Model & model,
DataHandler & data,
Data & data,
const Eigen::VectorXd & q,
const Eigen::VectorXd & v,
const Eigen::VectorXd & a,
const bool updateKinematics = true)
{
return centerOfMass(model,*data,q,v,a,
return centerOfMass(model,data,q,v,a,
true,
updateKinematics);
}
static Data::Matrix3x
Jcom_proxy(const Model& model,
DataHandler & data,
Data & data,
const Eigen::VectorXd & q)
{
return jacobianCenterOfMass(model,*data,q);
return jacobianCenterOfMass(model,data,q);
}
void exposeCOM()
......
......@@ -23,23 +23,23 @@ namespace se3
namespace python
{
static Eigen::MatrixXd crba_proxy(const Model & model,
DataHandler & data,
Data & data,
const Eigen::VectorXd & q)
{
data->M.fill(0);
crba(model,*data,q);
data->M.triangularView<Eigen::StrictlyLower>()
= data->M.transpose().triangularView<Eigen::StrictlyLower>();
return data->M;
data.M.fill(0);
crba(model,data,q);
data.M.triangularView<Eigen::StrictlyLower>()
= data.M.transpose().triangularView<Eigen::StrictlyLower>();
return data.M;
}
static Data::Matrix6x ccrba_proxy(const Model & model,
DataHandler & data,
Data & data,
const Eigen::VectorXd & q,
const Eigen::VectorXd & v)
{
ccrba(model,*data,q,v);
return data->Ag;
ccrba(model,data,q,v);
return data.Ag;
}
void exposeCRBA()
......
......@@ -23,7 +23,7 @@ namespace se3
namespace python
{
static Eigen::MatrixXd fd_llt_proxy(const Model & model,
DataHandler & data,
Data & data,
const Eigen::VectorXd & q,
const Eigen::VectorXd & v,
const Eigen::VectorXd & tau,
......@@ -31,20 +31,20 @@ namespace se3
const Eigen::VectorXd & gamma,
const bool update_kinematics = true)
{
forwardDynamics(model,*data,q,v,tau,J,gamma,update_kinematics);
return data->ddq;
forwardDynamics(model,data,q,v,tau,J,gamma,update_kinematics);
return data.ddq;
}
static Eigen::MatrixXd id_llt_proxy(const Model & model,
DataHandler & data,
Data & data,
const Eigen::VectorXd & q,
const Eigen::VectorXd & v_before,
const Eigen::MatrixXd & J,
const double r_coeff,
const bool update_kinematics = true)
{
impulseDynamics(model,*data,q,v_before,J,r_coeff,update_kinematics);
return data->dq_after;
impulseDynamics(model,data,q,v_before,J,r_coeff,update_kinematics);
return data.dq_after;
}
void exposeDynamics()
......
......@@ -23,20 +23,20 @@ namespace se3
namespace python
{
static double kineticEnergy_proxy(const Model & model,
DataHandler & data,
Data & data,
const Eigen::VectorXd & q,
const Eigen::VectorXd & v,
const bool update_kinematics = true)
{
return kineticEnergy(model,*data,q,v,update_kinematics);
return kineticEnergy(model,data,q,v,update_kinematics);
}
static double potentialEnergy_proxy(const Model & model,
DataHandler & data,
Data & data,
const Eigen::VectorXd & q,
const bool update_kinematics = true)
{
return potentialEnergy(model,*data,q,update_kinematics);
return potentialEnergy(model,data,q,update_kinematics);
}
void exposeEnergy()
......
......@@ -24,7 +24,7 @@ namespace se3
{
static Data::Matrix6x frame_jacobian_proxy(const Model & model,
DataHandler & data,
Data & data,
const Eigen::VectorXd & q,
Model::FrameIndex frame_id,
bool local,
......@@ -34,20 +34,20 @@ namespace se3
Data::Matrix6x J(6,model.nv); J.setZero();
if (update_geometry)
computeJacobians( model,*data,q );
computeJacobians(model,data,q);
if(local) getFrameJacobian<true> (model, *data, frame_id, J);
else getFrameJacobian<false> (model, *data, frame_id, J);
if(local) getFrameJacobian<true> (model, data, frame_id, J);
else getFrameJacobian<false> (model, data, frame_id, J);
return J;
}
static void frames_fk_0_proxy(const Model & model,
DataHandler & data,
Data & data,
const Eigen::VectorXd & q
)
{
framesForwardKinematics( model,*data,q );
framesForwardKinematics( model,data,q );
}
void exposeFramesAlgo()
......
......@@ -27,13 +27,13 @@ namespace se3
{
static void updateGeometryPlacements_proxy(const Model & model,
DataHandler & data,
Data & data,
const GeometryModelHandler & geom_model,
GeometryDataHandler & geom_data,
const Eigen::VectorXd & q
)
{
return updateGeometryPlacements(model, *data, *geom_model, *geom_data, q);
return updateGeometryPlacements(model, data, *geom_model, *geom_data, q);
}
#ifdef WITH_HPP_FCL
......@@ -53,13 +53,13 @@ namespace se3
}
static bool computeGeometryAndCollisions_proxy(const Model & model,
DataHandler & data,
Data & data,
const GeometryModelHandler & model_geom,
GeometryDataHandler & data_geom,
const Eigen::VectorXd & q,
const bool stopAtFirstCollision)
{
return computeCollisions(model,*data,*model_geom, *data_geom, q, stopAtFirstCollision);
return computeCollisions(model,data,*model_geom, *data_geom, q, stopAtFirstCollision);
}
static fcl::DistanceResult computeDistance_proxy(const GeometryModelHandler & model_geom,
......@@ -76,13 +76,13 @@ namespace se3
}
static std::size_t computeGeometryAndDistances_proxy(const Model & model,
DataHandler & data,
Data & data,
const GeometryModelHandler & model_geom,
GeometryDataHandler & data_geom,
const Eigen::VectorXd & q
)
{
return computeDistances<true>(model, *data, *model_geom, *data_geom, q);
return computeDistances<true>(model, data, *model_geom, *data_geom, q);
}
#endif // WITH_HPP_FCL
......
......@@ -24,16 +24,16 @@ namespace se3
{
static void compute_jacobians_proxy(const Model & model,
DataHandler & data,
Data & data,
const Eigen::VectorXd & q)
{
computeJacobians(model,*data,q);
computeJacobians(model,data,q);
}
static Data::Matrix6x
jacobian_proxy(const Model & model,
DataHandler & data,
Data & data,
const Eigen::VectorXd & q,
Model::JointIndex jointId,
bool local,
......@@ -42,10 +42,10 @@ namespace se3
Data::Matrix6x J(6,model.nv); J.setZero();
if (update_geometry)
computeJacobians(model,*data,q);
computeJacobians(model,data,q);
if(local) getJacobian<true> (model,*data,jointId,J);
else getJacobian<false> (model,*data,jointId,J);
if(local) getJacobian<true> (model,data,jointId,J);
else getJacobian<false> (model,data,jointId,J);
return J;
}
......
......@@ -24,28 +24,28 @@ namespace se3
{
static void fk_0_proxy(const Model & model,
DataHandler & data,
Data & data,
const Eigen::VectorXd & q)
{
forwardKinematics(model,*data,q);
forwardKinematics(model,data,q);
}
static void fk_1_proxy(const Model & model,
DataHandler & data,
Data & data,
const Eigen::VectorXd & q,
const Eigen::VectorXd & qdot )
{
forwardKinematics(model,*data,q,qdot);
forwardKinematics(model,data,q,qdot);
}
static void fk_2_proxy(const Model & model,
DataHandler & data,
Data & data,
const Eigen::VectorXd & q,
const Eigen::VectorXd & v,
const Eigen::VectorXd & a)
{
forwardKinematics(model,*data,q,v,a);
forwardKinematics(model,data,q,v,a);
}
void exposeKinematics()
......
......@@ -23,30 +23,30 @@ namespace se3
namespace python
{
static Eigen::VectorXd rnea_proxy(const Model & model,
DataHandler & data,
Data & data,
const Eigen::VectorXd & q,
const Eigen::VectorXd & v,
const Eigen::VectorXd & a)
{
return rnea(model,*data,q,v,a);
return rnea(model,data,q,v,a);
}
static Eigen::VectorXd rnea_fext_proxy(const Model & model,
DataHandler & data,
Data & data,
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);
return rnea(model,data,q,v,a,fext);
}
static Eigen::VectorXd nle_proxy(const Model & model,
DataHandler & data,
Data & data,
const Eigen::VectorXd & q,
const Eigen::VectorXd & v)
{
return nonLinearEffects(model,*data,q,v);
return nonLinearEffects(model,data,q,v);
}
void exposeRNEA()
......
......@@ -19,20 +19,18 @@
#define __se3_python_data_hpp__
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/bindings/python/utils/handler.hpp"
#include <eigenpy/exception.hpp>
#include <eigenpy/eigenpy.hpp>
#include <eigenpy/memory.hpp>
#include <boost/python/suite/indexing/vector_indexing_suite.hpp>
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(se3::Data)
namespace se3
{
namespace python
{
namespace bp = boost::python;
typedef Handler<Data> DataHandler;
struct DataPythonVisitor
: public boost::python::def_visitor< DataPythonVisitor >
{
......@@ -42,33 +40,28 @@ namespace se3
public:
/* --- Convert From C++ to Python ------------------------------------- */
static PyObject* convert(DataHandler::SmartPtr_t const& ptr)
{
return boost::python::incref(boost::python::object(DataHandler(ptr)).ptr());
}
#define ADD_DATA_PROPERTY(TYPE,NAME,DOC) \
def_readwrite(#NAME, \
&Data::NAME, \
DOC)
#define ADD_DATA_PROPERTY_READONLY(TYPE,NAME,DOC) \
def_readonly(#NAME, \
&Data::NAME, \
DOC)
#define ADD_DATA_PROPERTY_READONLY_BYVALUE(TYPE,NAME,DOC) \
add_property(#NAME, \
bp::make_function(&DataPythonVisitor::NAME, \
bp::return_internal_reference<>()), \
DOC)
#define ADD_DATA_PROPERTY_CONST(TYPE,NAME,DOC) \
add_property(#NAME, \
bp::make_function(&DataPythonVisitor::NAME), \
DOC)
#define IMPL_DATA_PROPERTY(TYPE,NAME,DOC) \
static TYPE & NAME( DataHandler & d ) { return d->NAME; }
#define IMPL_DATA_PROPERTY_CONST(TYPE,NAME,DOC) \
static TYPE NAME( DataHandler & d ) { return d->NAME; }
make_getter(&Data::NAME,bp::return_value_policy<bp::return_by_value>()), \
DOC)
/* --- Exposing C++ API to python through the handler ----------------- */
template<class PyClass>
void visit(PyClass& cl) const
{
cl
.def(bp::init<Model>(bp::arg("Molde"),"Constructs a data structure from a given model."))
.ADD_DATA_PROPERTY(container::aligned_vector<Motion>,a,"Body acceleration")
.ADD_DATA_PROPERTY(container::aligned_vector<Motion>,a_gf,"Body acceleration containing also the gravity acceleration")
......@@ -77,97 +70,53 @@ namespace se3
.ADD_DATA_PROPERTY(container::aligned_vector<SE3>,oMi,"Body absolute placement (wrt world)")
.ADD_DATA_PROPERTY(container::aligned_vector<SE3>,oMf,"frames absolute placement (wrt world)")
.ADD_DATA_PROPERTY(container::aligned_vector<SE3>,liMi,"Body relative placement (wrt parent)")
.ADD_DATA_PROPERTY_CONST(Eigen::VectorXd,tau,"Joint forces")
.ADD_DATA_PROPERTY_CONST(Eigen::VectorXd,nle,"Non Linear Effects")
.ADD_DATA_PROPERTY_CONST(Eigen::VectorXd,ddq,"Joint accelerations")
.ADD_DATA_PROPERTY_READONLY_BYVALUE(Eigen::VectorXd,tau,"Joint forces")
.ADD_DATA_PROPERTY_READONLY_BYVALUE(Eigen::VectorXd,nle,"Non Linear Effects")
.ADD_DATA_PROPERTY_READONLY_BYVALUE(Eigen::VectorXd,ddq,"Joint accelerations")
.ADD_DATA_PROPERTY(container::aligned_vector<Inertia>,Ycrb,"Inertia of the sub-tree composit rigid body")
.ADD_DATA_PROPERTY_CONST(Eigen::MatrixXd,M,"Joint Inertia matrix")
.ADD_DATA_PROPERTY_CONST(container::aligned_vector<Matrix6x>,Fcrb,"Spatial forces set, used in CRBA")
.ADD_DATA_PROPERTY_READONLY_BYVALUE(Eigen::MatrixXd,M,"Joint Inertia matrix")
.ADD_DATA_PROPERTY(container::aligned_vector<Matrix6x>,Fcrb,"Spatial forces set, used in CRBA")
.ADD_DATA_PROPERTY(std::vector<int>,lastChild,"Index of the last child (for CRBA)")
.ADD_DATA_PROPERTY(std::vector<int>,nvSubtree,"Dimension of the subtree motion space (for CRBA)")
.ADD_DATA_PROPERTY_CONST(Eigen::MatrixXd,U,"Joint Inertia square root (upper triangle)")
.ADD_DATA_PROPERTY_CONST(Eigen::VectorXd,D,"Diagonal of UDUT inertia decomposition")
.ADD_DATA_PROPERTY_READONLY_BYVALUE(Eigen::MatrixXd,U,"Joint Inertia square root (upper triangle)")
.ADD_DATA_PROPERTY_READONLY_BYVALUE(Eigen::VectorXd,D,"Diagonal of UDUT inertia decomposition")
.ADD_DATA_PROPERTY(std::vector<int>,parents_fromRow,"First previous non-zero row in M (used in Cholesky)")
.ADD_DATA_PROPERTY(std::vector<int>,nvSubtree_fromRow,"")
.ADD_DATA_PROPERTY_CONST(Matrix6x,J,"Jacobian of joint placement")
.ADD_DATA_PROPERTY_READONLY_BYVALUE(Matrix6x,J,"Jacobian of joint placement")
.ADD_DATA_PROPERTY(container::aligned_vector<SE3>,iMf,"Body placement wrt to algorithm end effector.")
.ADD_DATA_PROPERTY_CONST(Matrix6x,Ag,
.ADD_DATA_PROPERTY_READONLY_BYVALUE(Matrix6x,Ag,
"Centroidal matrix which maps from joint velocity to the centroidal momentum.")
.ADD_DATA_PROPERTY_CONST(Force,hg,
"Centroidal momentum (expressed in the frame centered at the CoM and aligned with the inertial frame).")
.ADD_DATA_PROPERTY_CONST(Inertia,Ig,
"Centroidal Composite Rigid Body Inertia.")
.ADD_DATA_PROPERTY_READONLY(Force,hg,
"Centroidal momentum (expressed in the frame centered at the CoM and aligned with the inertial frame).")
.ADD_DATA_PROPERTY_READONLY(Inertia,Ig,
"Centroidal Composite Rigid Body Inertia.")
.ADD_DATA_PROPERTY(container::aligned_vector<Vector3>,com,"Subtree com position.")
.ADD_DATA_PROPERTY(container::aligned_vector<Vector3>,vcom,"Subtree com velocity.")
.ADD_DATA_PROPERTY(container::aligned_vector<Vector3>,acom,"Subtree com acceleration.")
.ADD_DATA_PROPERTY(std::vector<double>,mass,"Subtree total mass.")
.ADD_DATA_PROPERTY_CONST(Matrix3x,Jcom,"Jacobian of center of mass.")
.ADD_DATA_PROPERTY_READONLY_BYVALUE(Matrix3x,Jcom,"Jacobian of center of mass.")
.ADD_DATA_PROPERTY_CONST(double,kinetic_energy,"Kinetic energy in [J] computed by kineticEnergy(model,data,q,v,True/False)")
.ADD_DATA_PROPERTY_CONST(double,potential_energy,"Potential energy in [J] computed by potentialEnergy(model,data,q,True/False)")
.ADD_DATA_PROPERTY_READONLY_BYVALUE(double,kinetic_energy,"Kinetic energy in [J] computed by kineticEnergy(model,data,q,v,True/False)")
.ADD_DATA_PROPERTY_READONLY_BYVALUE(double,potential_energy,"Potential energy in [J] computed by potentialEnergy(model,data,q,True/False)")
.ADD_DATA_PROPERTY_CONST(Eigen::VectorXd,lambda_c,"Lagrange Multipliers linked to contact forces")
.ADD_DATA_PROPERTY_CONST(Eigen::VectorXd,impulse_c,"Lagrange Multipliers linked to contact impulses")
.ADD_DATA_PROPERTY_READONLY_BYVALUE(Eigen::VectorXd,lambda_c,"Lagrange Multipliers linked to contact forces")
.ADD_DATA_PROPERTY_READONLY_BYVALUE(Eigen::VectorXd,impulse_c,"Lagrange Multipliers linked to contact impulses")
.ADD_DATA_PROPERTY_CONST(Eigen::VectorXd,dq_after,"Generalized velocity after the impact.")
.ADD_DATA_PROPERTY_READONLY_BYVALUE(Eigen::VectorXd,dq_after,"Generalized velocity after the impact.")
;
}
IMPL_DATA_PROPERTY(container::aligned_vector<Motion>,a,"Body acceleration")
IMPL_DATA_PROPERTY(container::aligned_vector<Motion>,a_gf,"Body acceleration containing also the gravity acceleration")
IMPL_DATA_PROPERTY(container::aligned_vector<Motion>,v,"Body velocity")
IMPL_DATA_PROPERTY(container::aligned_vector<Force>,f,"Body force")
IMPL_DATA_PROPERTY(container::aligned_vector<SE3>,oMi,"Body absolute placement (wrt world)")
IMPL_DATA_PROPERTY(container::aligned_vector<SE3>,oMf,"frames absolute placement (wrt world)")
IMPL_DATA_PROPERTY(container::aligned_vector<SE3>,liMi,"Body relative placement (wrt parent)")
IMPL_DATA_PROPERTY_CONST(Eigen::VectorXd,tau,"Joint forces")
IMPL_DATA_PROPERTY_CONST(Eigen::VectorXd,nle,"Non Linear Effects")
IMPL_DATA_PROPERTY_CONST(Eigen::VectorXd,ddq,"Joint acceleration")
IMPL_DATA_PROPERTY(container::aligned_vector<Inertia>,Ycrb,"Inertia of the sub-tree composit rigid body")
IMPL_DATA_PROPERTY_CONST(Eigen::MatrixXd,M,"Joint Inertia")
IMPL_DATA_PROPERTY_CONST(container::aligned_vector<Matrix6x>,Fcrb,"Spatial forces set, used in CRBA")
IMPL_DATA_PROPERTY(std::vector<int>,lastChild,"Index of the last child (for CRBA)")
IMPL_DATA_PROPERTY(std::vector<int>,nvSubtree,"Dimension of the subtree motion space (for CRBA)")
IMPL_DATA_PROPERTY_CONST(Eigen::MatrixXd,U,"Joint Inertia square root (upper triangle)")
IMPL_DATA_PROPERTY_CONST(Eigen::VectorXd,D,"Diagonal of UDUT inertia decomposition")
IMPL_DATA_PROPERTY(std::vector<int>,parents_fromRow,"First previous non-zero row in M (used in Cholesky)")
IMPL_DATA_PROPERTY(std::vector<int>,nvSubtree_fromRow,"")
IMPL_DATA_PROPERTY_CONST(Matrix6x,J,"Jacobian of joint placement")
IMPL_DATA_PROPERTY(container::aligned_vector<SE3>,iMf,"Body placement wrt to algorithm end effector.")
IMPL_DATA_PROPERTY_CONST(Matrix6x,Ag,
"Centroidal matrix which maps from joint velocity to the centroidal momentum.")
IMPL_DATA_PROPERTY_CONST(Force,hg,
"Centroidal momentum (expressed in the frame centered at the CoM and aligned with the inertial frame).")
IMPL_DATA_PROPERTY_CONST(Inertia,Ig,
"Centroidal Composite Rigid Body Inertia.")
IMPL_DATA_PROPERTY(container::aligned_vector<Vector3>,com,"Subtree com position.")
IMPL_DATA_PROPERTY(container::aligned_vector<Vector3>,vcom,"Subtree com velocity.")
IMPL_DATA_PROPERTY(container::aligned_vector<Vector3>,acom,"Subtree com acceleration.")
IMPL_DATA_PROPERTY(std::vector<double>,mass,"Subtree total mass.")
IMPL_DATA_PROPERTY_CONST(Matrix3x,Jcom,"Jacobian of center of mass.")
IMPL_DATA_PROPERTY_CONST(double,kinetic_energy,"Kinetic energy in [J] computed by kineticEnergy(model,data,q,v,True/False)")
IMPL_DATA_PROPERTY_CONST(double,potential_energy,"Potential energy in [J] computed by potentialEnergy(model,data,q,True/False)")
IMPL_DATA_PROPERTY_CONST(Eigen::VectorXd,lambda_c,"Lagrange Multipliers linked to contact forces")
IMPL_DATA_PROPERTY_CONST(Eigen::VectorXd,impulse_c,"Lagrange Multipliers linked to contact impulses")
IMPL_DATA_PROPERTY_CONST(Eigen::VectorXd,dq_after,"Generalized velocity after the impact.")
/* --- Expose --------------------------------------------------------- */
static void expose()
{
bp::class_<DataHandler>("Data",
"Articulated rigid body data (const)",
bp::no_init)
bp::class_<Data>("Data",
"Articulated rigid body data (const)",
bp::no_init)
.def(DataPythonVisitor());
bp::to_python_converter< DataHandler::SmartPtr_t,DataPythonVisitor >();
bp::class_< container::aligned_vector<Vector3> >("StdVec_vec3d")
.def(bp::vector_indexing_suite< container::aligned_vector<Vector3>, true >());
}
......
......@@ -152,13 +152,9 @@ namespace se3
return boost::apply_visitor(addJointVisitor(