Unverified Commit 5d5ceddc authored by Justin Carpentier's avatar Justin Carpentier Committed by GitHub
Browse files

Merge pull request #1433 from jcarpent/topic/python

Simplify Model bindings
parents cdf90b84 3e5604fe
Pipeline #14060 passed with stage
in 156 minutes and 29 seconds
......@@ -14,22 +14,13 @@ namespace pinocchio
namespace python
{
static void exposeVariants()
void exposeJoints()
{
boost::mpl::for_each<JointModelVariant::types>(JointModelExposer());
bp::to_python_converter<pinocchio::JointModelVariant,
JointVariantVisitor<pinocchio::JointModelVariant > >();
boost::mpl::for_each<JointDataVariant::types>(JointDataExposer());
bp::to_python_converter<pinocchio::JointDataVariant,
JointVariantVisitor<pinocchio::JointDataVariant > >();
}
void exposeJoints()
{
exposeVariants();
JointModelPythonVisitor::expose();
StdAlignedVectorPythonVisitor<JointModel,true>::expose("StdVec_JointModelVector");
StdAlignedVectorPythonVisitor<JointModel>::expose("StdVec_JointModelVector");
}
} // namespace python
......
//
// Copyright (c) 2015-2020 CNRS INRIA
// Copyright (c) 2015-2021 CNRS INRIA
//
#ifndef __pinocchio_python_joint_hpp__
#define __pinocchio_python_joint_hpp__
#ifndef __pinocchio_python_multibody_joint_joint_hpp__
#define __pinocchio_python_multibody_joint_joint_hpp__
#include <boost/python.hpp>
......@@ -17,22 +17,20 @@ namespace pinocchio
namespace bp = boost::python;
struct JointModelPythonVisitor
: public boost::python::def_visitor< JointModelPythonVisitor >
: public boost::python::def_visitor< JointModelPythonVisitor >
{
public:
template<class PyClass>
void visit(PyClass& cl) const
{
cl
.def(bp::init<>())
.def(bp::init<>(bp::arg("self")))
// All are add_properties cause ReadOnly
.add_property("id",&JointModelPythonVisitor::getId)
.add_property("idx_q",&JointModelPythonVisitor::getIdx_q)
.add_property("idx_v",&JointModelPythonVisitor::getIdx_v)
.add_property("nq",&JointModelPythonVisitor::getNq)
.add_property("nv",&JointModelPythonVisitor::getNv)
.add_property("id",&getId)
.add_property("idx_q",&getIdx_q)
.add_property("idx_v",&getIdx_v)
.add_property("nq",&getNq)
.add_property("nv",&getNv)
.def("setIndexes",&JointModel::setIndexes)
.def("shortname",&JointModel::shortname)
;
......@@ -49,7 +47,7 @@ namespace pinocchio
bp::class_<JointModel>("JointModel",
"Generic Joint Model",
bp::no_init)
.def(bp::init<pinocchio::JointModelVariant>())
.def(bp::init<JointModel>(bp::args("self","other")))
.def(JointModelPythonVisitor())
.def(PrintableVisitor<JointModel>())
;
......@@ -59,4 +57,4 @@ namespace pinocchio
}} // namespace pinocchio::python
#endif // ifndef __pinocchio_python_joint_hpp__
#endif // ifndef __pinocchio_python_multibody_joint_joint_hpp__
//
// Copyright (c) 2015-2020 CNRS INRIA
// Copyright (c) 2015-2021 CNRS INRIA
//
#ifndef __pinocchio_python_joints_models_hpp__
......@@ -9,6 +9,7 @@
#include "pinocchio/multibody/joint/joint-collection.hpp"
#include "pinocchio/multibody/joint/joint-composite.hpp"
#include "pinocchio/multibody/joint/joint-generic.hpp"
#include <eigenpy/eigen-to-python.hpp>
......@@ -31,8 +32,9 @@ namespace pinocchio
inline bp::class_<JointModelRevoluteUnaligned>& expose_joint_model<JointModelRevoluteUnaligned> (bp::class_<JointModelRevoluteUnaligned> & cl)
{
return cl
.def(bp::init<double, double, double> (bp::args("x", "y", "z"), "Init JointModelRevoluteUnaligned from the components x, y, z of the axis"))
.def(bp::init<Eigen::Vector3d> (bp::args("axis"),
.def(bp::init<double, double, double> (bp::args("self","x", "y", "z"),
"Init JointModelRevoluteUnaligned from the components x, y, z of the axis"))
.def(bp::init<Eigen::Vector3d> (bp::args("self","axis"),
"Init JointModelRevoluteUnaligned from an axis with x-y-z components"))
.def_readwrite("axis",&JointModelRevoluteUnaligned::axis,
"Rotation axis of the JointModelRevoluteUnaligned.")
......@@ -44,9 +46,9 @@ namespace pinocchio
inline bp::class_<JointModelPrismaticUnaligned>& expose_joint_model<JointModelPrismaticUnaligned> (bp::class_<JointModelPrismaticUnaligned> & cl)
{
return cl
.def(bp::init<double, double, double> (bp::args("x", "y", "z"),
.def(bp::init<double, double, double> (bp::args("self","x", "y", "z"),
"Init JointModelPrismaticUnaligned from the components x, y, z of the axis"))
.def(bp::init<Eigen::Vector3d> (bp::args("axis"),
.def(bp::init<Eigen::Vector3d> (bp::args("self","axis"),
"Init JointModelPrismaticUnaligned from an axis with x-y-z components"))
.def_readwrite("axis",&JointModelPrismaticUnaligned::axis,
"Translation axis of the JointModelPrismaticUnaligned.")
......@@ -74,10 +76,11 @@ namespace pinocchio
}; // struct JointModelCompositeAddJointVisitor
static JointModelComposite & addJoint_proxy(JointModelComposite & joint_composite,
const JointModelVariant & jmodel_variant,
const JointModel & jmodel,
const SE3 & joint_placement = SE3::Identity())
{
return boost::apply_visitor(JointModelCompositeAddJointVisitor(joint_composite,joint_placement), jmodel_variant);
return boost::apply_visitor(JointModelCompositeAddJointVisitor(joint_composite,joint_placement),
jmodel.toVariant());
}
BOOST_PYTHON_FUNCTION_OVERLOADS(addJoint_proxy_overloads,addJoint_proxy,2,3)
......@@ -97,22 +100,25 @@ namespace pinocchio
}
}; // struct JointModelCompositeConstructorVisitor
static JointModelComposite* init_proxy1(const JointModelVariant & jmodel_variant)
static JointModelComposite* init_proxy1(const JointModel & jmodel)
{
return boost::apply_visitor(JointModelCompositeConstructorVisitor(SE3::Identity()), jmodel_variant);
return boost::apply_visitor(JointModelCompositeConstructorVisitor(SE3::Identity()),
jmodel);
}
static JointModelComposite* init_proxy2(const JointModelVariant & jmodel_variant,
static JointModelComposite* init_proxy2(const JointModel & jmodel,
const SE3 & joint_placement)
{
return boost::apply_visitor(JointModelCompositeConstructorVisitor(joint_placement), jmodel_variant);
return boost::apply_visitor(JointModelCompositeConstructorVisitor(joint_placement),
jmodel);
}
template<>
inline bp::class_<JointModelComposite>& expose_joint_model<JointModelComposite> (bp::class_<JointModelComposite> & cl)
{
return cl
.def(bp::init<const size_t> (bp::args("size"), "Init JointModelComposite with a defined size"))
.def(bp::init<const size_t> (bp::args("self","size"),
"Init JointModelComposite with a defined size"))
.def("__init__",
bp::make_constructor(init_proxy1,
bp::default_call_policies(),
......@@ -132,7 +138,7 @@ namespace pinocchio
.add_property("njoints",&JointModelComposite::njoints)
.def("addJoint",
&addJoint_proxy,
addJoint_proxy_overloads(bp::args("joint_model","joint_placement"),
addJoint_proxy_overloads(bp::args("self","joint_model","joint_placement"),
"Add a joint to the vector of joints."
)[bp::return_internal_reference<>()]
)
......
//
// Copyright (c) 2015-2020 CNRS INRIA
// Copyright (c) 2015-2021 CNRS INRIA
//
#ifndef __pinocchio_python_joints_variant_hpp__
......@@ -45,7 +45,7 @@ namespace pinocchio
.def(JointDataDerivedPythonVisitor<T>())
.def(PrintableVisitor<T>())
);
bp::implicitly_convertible<T,pinocchio::JointDataVariant>();
bp::implicitly_convertible<T,pinocchio::JointData>();
}
};
......@@ -61,7 +61,7 @@ namespace pinocchio
.def(JointModelDerivedPythonVisitor<T>())
.def(PrintableVisitor<T>())
);
bp::implicitly_convertible<T,pinocchio::JointModelVariant>();
bp::implicitly_convertible<T,pinocchio::JointModel>();
}
};
......
//
// Copyright (c) 2015-2020 CNRS INRIA
// Copyright (c) 2015-2021 CNRS INRIA
// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//
......@@ -95,106 +95,6 @@ namespace pinocchio
typedef typename Model::VectorXs VectorXs;
protected:
struct addJointVisitor0
: public boost::static_visitor<Index>
{
Model & m_model;
const JointIndex m_parent_id;
const SE3 & m_joint_placement;
const std::string & m_joint_name;
addJointVisitor0(Model & model,
const JointIndex parent_id,
const SE3 & joint_placement,
const std::string & joint_name)
: m_model(model)
, m_parent_id(parent_id)
, m_joint_placement(joint_placement)
, m_joint_name(joint_name)
{}
template <typename JointModelDerived>
JointIndex operator()(JointModelDerived & jmodel) const
{
return m_model.addJoint(m_parent_id,jmodel,m_joint_placement,m_joint_name);
}
}; // struct addJointVisitor0
struct addJointVisitor1
: public addJointVisitor0
{
using addJointVisitor0::m_model;
using addJointVisitor0::m_parent_id;
using addJointVisitor0::m_joint_placement;
using addJointVisitor0::m_joint_name;
const VectorXs & m_max_effort;
const VectorXs & m_max_velocity;
const VectorXs & m_min_config;
const VectorXs & m_max_config;
addJointVisitor1(Model & model,
const JointIndex parent_id,
const SE3 & joint_placement,
const std::string & joint_name,
const VectorXs & max_effort,
const VectorXs & max_velocity,
const VectorXs & min_config,
const VectorXs & max_config)
: addJointVisitor0(model,parent_id,joint_placement,joint_name)
, m_max_effort(max_effort)
, m_max_velocity(max_velocity)
, m_min_config(min_config)
, m_max_config(max_config)
{}
template <typename JointModelDerived>
JointIndex operator()(JointModelDerived & jmodel) const
{
return m_model.addJoint(m_parent_id,jmodel,m_joint_placement,m_joint_name,m_max_effort,m_max_velocity,m_min_config,m_max_config);
}
}; // struct addJointVisitor1
struct addJointVisitor2
: public addJointVisitor1
{
using addJointVisitor1::m_model;
using addJointVisitor1::m_parent_id;
using addJointVisitor1::m_joint_placement;
using addJointVisitor1::m_joint_name;
using addJointVisitor1::m_max_effort;
using addJointVisitor1::m_max_velocity;
using addJointVisitor1::m_min_config;
using addJointVisitor1::m_max_config;
const VectorXs & m_friction;
const VectorXs & m_damping;
addJointVisitor2(Model & model,
const JointIndex parent_id,
const SE3 & joint_placement,
const std::string & joint_name,
const VectorXs & max_effort,
const VectorXs & max_velocity,
const VectorXs & min_config,
const VectorXs & max_config,
const VectorXs & friction,
const VectorXs & damping)
: addJointVisitor1(model,parent_id,joint_placement,joint_name,
max_effort,max_velocity,min_config,max_config)
, m_friction(friction)
, m_damping(damping)
{}
template <typename JointModelDerived>
JointIndex operator()(JointModelDerived & jmodel) const
{
return m_model.addJoint(m_parent_id,jmodel,m_joint_placement,m_joint_name,m_max_effort,m_max_velocity,m_min_config,m_max_config,m_friction,m_damping);
}
}; // struct addJointVisitor1
public:
/* --- Exposing C++ API to python through the handler ----------------- */
......@@ -305,17 +205,16 @@ namespace pinocchio
static JointIndex addJoint0(Model & model,
JointIndex parent_id,
bp::object jmodel,
const JointModel & jmodel,
const SE3 & joint_placement,
const std::string & joint_name)
{
JointModelVariant jmodel_variant = bp::extract<JointModelVariant> (jmodel)();
return boost::apply_visitor(addJointVisitor0(model,parent_id,joint_placement,joint_name), jmodel_variant);
return model.addJoint(parent_id,jmodel,joint_placement,joint_name);
}
static JointIndex addJoint1(Model & model,
JointIndex parent_id,
bp::object jmodel,
const JointModel & jmodel,
const SE3 & joint_placement,
const std::string & joint_name,
const VectorXs & max_effort,
......@@ -323,13 +222,13 @@ namespace pinocchio
const VectorXs & min_config,
const VectorXs & max_config)
{
JointModelVariant jmodel_variant = bp::extract<JointModelVariant> (jmodel)();
return boost::apply_visitor(addJointVisitor1(model,parent_id,joint_placement,joint_name,max_effort,max_velocity,min_config,max_config), jmodel_variant);
return model.addJoint(parent_id,jmodel,joint_placement,joint_name,
max_effort,max_velocity,min_config,max_config);
}
static JointIndex addJoint2(Model & model,
JointIndex parent_id,
bp::object jmodel,
const JointModel & jmodel,
const SE3 & joint_placement,
const std::string & joint_name,
const VectorXs & max_effort,
......@@ -339,8 +238,9 @@ namespace pinocchio
const VectorXs & friction,
const VectorXs & damping)
{
JointModelVariant jmodel_variant = bp::extract<JointModelVariant> (jmodel)();
return boost::apply_visitor(addJointVisitor2(model,parent_id,joint_placement,joint_name,max_effort,max_velocity,min_config,max_config,friction,damping), jmodel_variant);
return model.addJoint(parent_id,jmodel,joint_placement,joint_name,
max_effort,max_velocity,min_config,max_config,
friction,damping);
}
static Data createData(const Model & model) { return Data(model); }
......
......@@ -30,36 +30,32 @@ namespace pinocchio
}
Model buildModelFromUrdf(const std::string & filename,
bp::object & root_joint_object)
const JointModel & root_joint)
{
JointModelVariant root_joint = bp::extract<JointModelVariant>(root_joint_object)();
Model model;
pinocchio::urdf::buildModel(filename, root_joint, model);
return model;
}
Model & buildModelFromUrdf(const std::string & filename,
bp::object & root_joint_object,
const JointModel & root_joint,
Model & model)
{
JointModelVariant root_joint = bp::extract<JointModelVariant>(root_joint_object)();
return pinocchio::urdf::buildModel(filename, root_joint, model);
}
Model buildModelFromXML(const std::string & XMLstream,
bp::object & root_joint_object)
const JointModel & root_joint)
{
JointModelVariant root_joint = bp::extract<JointModelVariant> (root_joint_object)();
Model model;
pinocchio::urdf::buildModelFromXML(XMLstream, root_joint, model);
return model;
}
Model & buildModelFromXML(const std::string & XMLstream,
bp::object & root_joint_object,
const JointModel & root_joint,
Model & model)
{
JointModelVariant root_joint = bp::extract<JointModelVariant> (root_joint_object)();
pinocchio::urdf::buildModelFromXML(XMLstream, root_joint, model);
return model;
}
......@@ -86,7 +82,7 @@ namespace pinocchio
#ifdef PINOCCHIO_WITH_URDFDOM
bp::def("buildModelFromUrdf",
static_cast <Model (*) (const std::string &, bp::object &)> (pinocchio::python::buildModelFromUrdf),
static_cast <Model (*) (const std::string &, const JointModel &)> (pinocchio::python::buildModelFromUrdf),
bp::args("urdf_filename","root_joint"),
"Parse the URDF file given in input and return a pinocchio Model starting with the given root joint."
);
......@@ -105,20 +101,20 @@ namespace pinocchio
);
bp::def("buildModelFromUrdf",
static_cast <Model & (*) (const std::string &, bp::object &, Model &)> (pinocchio::python::buildModelFromUrdf),
static_cast <Model & (*) (const std::string &, const JointModel &, Model &)> (pinocchio::python::buildModelFromUrdf),
bp::args("urdf_filename","root_joint","model"),
"Append to a given model a URDF structure given by its filename and the root joint.",
bp::return_internal_reference<3>()
);
bp::def("buildModelFromXML",
static_cast <Model (*) (const std::string &, bp::object &)> (pinocchio::python::buildModelFromXML),
static_cast <Model (*) (const std::string &, const JointModel &)> (pinocchio::python::buildModelFromXML),
bp::args("urdf_xml_stream","root_joint"),
"Parse the URDF XML stream given in input and return a pinocchio Model starting with the given root joint."
);
bp::def("buildModelFromXML",
static_cast <Model & (*) (const std::string &, bp::object &, Model &)> (pinocchio::python::buildModelFromXML),
static_cast <Model & (*) (const std::string &, const JointModel &, Model &)> (pinocchio::python::buildModelFromXML),
bp::args("urdf_xml_stream","root_joint","model"),
"Parse the URDF XML stream given in input and append it to the input model with the given interfacing joint.",
bp::return_internal_reference<3>()
......
......@@ -301,7 +301,7 @@ namespace pinocchio
Vector10 v;
v[0] = mass();
v.template segment<3>(1) = mass() * lever();
v.template segment<3>(1).noalias() = mass() * lever();
v.template segment<6>(4) = (inertia() - AlphaSkewSquare(mass(),lever())).data();
return v;
......
......@@ -178,10 +178,12 @@ BOOST_AUTO_TEST_CASE ( build_model_with_joint_from_UDRFTree )
{
const std::string filename = PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots/romeo_description/urdf/romeo_small.urdf");
std::cout << "parseURDFFile" << std::endl;
::urdf::ModelInterfaceSharedPtr urdfTree = ::urdf::parseURDFFile(filename);
pinocchio::Model model;
pinocchio::urdf::buildModel(urdfTree, pinocchio::JointModelFreeFlyer(), model);
std::cout << "buildModel" << std::endl;
pinocchio::urdf::buildModel(urdfTree, pinocchio::JointModelFreeFlyer(), model, true);
BOOST_CHECK(model.nq == 38);
}
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment