Commit 65b9bf19 authored by jcarpent's avatar jcarpent
Browse files

Minor clean of the code

parent 77955e24
......@@ -379,12 +379,9 @@ namespace se3
q.segment<4>(idx_q()+3).normalize();
}
JointModelDense<NQ, NV> toDense_impl() const
JointModelDense<NQ,NV> toDense_impl() const
{
return JointModelDense<NQ, NV>( id(),
idx_q(),
idx_v()
);
return JointModelDense<NQ,NV>(id(),idx_q(),idx_v());
}
static std::string classname() { return std::string("JointModelFreeFlyer"); }
......
......@@ -434,10 +434,7 @@ namespace se3
JointModelDense<NQ, NV> toDense_impl() const
{
return JointModelDense<NQ, NV>( id(),
idx_q(),
idx_v()
);
return JointModelDense<NQ, NV>(id(),idx_q(),idx_v());
}
static std::string classname() { return std::string("JointModelPlanar");}
......
......@@ -438,12 +438,9 @@ namespace se3
return q;
}
JointModelDense<NQ, NV> toDense_impl() const
JointModelDense<NQ,NV> toDense_impl() const
{
return JointModelDense<NQ, NV>( id(),
idx_q(),
idx_v()
);
return JointModelDense<NQ,NV>(id(),idx_q(),idx_v());
}
static std::string classname() { return std::string("JointModelPrismaticUnaligned"); }
......
......@@ -493,12 +493,9 @@ namespace se3
return q;
}
JointModelDense<NQ, NV> toDense_impl() const
JointModelDense<NQ,NV> toDense_impl() const
{
return JointModelDense<NQ, NV>( id(),
idx_q(),
idx_v()
);
return JointModelDense<NQ,NV>(id(),idx_q(),idx_v());
}
static std::string classname();
......
......@@ -442,12 +442,9 @@ namespace se3
return q;
}
JointModelDense<NQ, NV> toDense_impl() const
JointModelDense<NQ,NV> toDense_impl() const
{
return JointModelDense<NQ, NV>( id(),
idx_q(),
idx_v()
);
return JointModelDense<NQ,NV>(id(),idx_q(),idx_v());
}
static std::string classname() { return std::string("JointModelRevoluteUnaligned"); }
......
......@@ -243,12 +243,9 @@ namespace se3
q.segment<NQ>(idx_q()).normalize();
}
JointModelDense<NQ, NV> toDense_impl() const
JointModelDense<NQ,NV> toDense_impl() const
{
return JointModelDense<NQ, NV>( id(),
idx_q(),
idx_v()
);
return JointModelDense<NQ,NV>(id(),idx_q(),idx_v());
}
static std::string classname();
......
......@@ -527,12 +527,9 @@ namespace se3
return q;
}
JointModelDense<NQ, NV> toDense_impl() const
JointModelDense<NQ,NV> toDense_impl() const
{
return JointModelDense<NQ, NV>( id(),
idx_q(),
idx_v()
);
return JointModelDense<NQ,NV>(id(),idx_q(),idx_v());
}
static std::string classname();
......
......@@ -459,12 +459,9 @@ namespace se3
return q;
}
JointModelDense<NQ, NV> toDense_impl() const
JointModelDense<NQ,NV> toDense_impl() const
{
return JointModelDense<NQ, NV>( id(),
idx_q(),
idx_v()
);
return JointModelDense<NQ,NV>(id(),idx_q(),idx_v());
}
static std::string classname() { return std::string("JointModelSphericalZYX"); }
......
......@@ -404,12 +404,9 @@ namespace se3
q.segment<NQ>(idx_q()).normalize();
}
JointModelDense<NQ, NV> toDense_impl() const
JointModelDense<NQ,NV> toDense_impl() const
{
return JointModelDense<NQ, NV>( id(),
idx_q(),
idx_v()
);
return JointModelDense<NQ,NV>(id(),idx_q(),idx_v());
}
static std::string classname() { return std::string("JointModelSpherical"); }
......
......@@ -356,12 +356,12 @@ namespace se3
Eigen::VectorXd::ConstFixedSegmentReturnType<NQ>::Type & q_0 = q0.segment<NQ> (idx_q ());
Eigen::VectorXd::ConstFixedSegmentReturnType<NQ>::Type & q_1 = q1.segment<NQ> (idx_q ());
return ( q_1 - q_0);
return q_1-q_0;
}
double distance_impl(const Eigen::VectorXd & q0,const Eigen::VectorXd & q1) const
{
return difference_impl(q0, q1).norm();
return difference_impl(q0,q1).norm();
}
ConfigVector_t neutralConfiguration_impl() const
......@@ -371,12 +371,9 @@ namespace se3
return q;
}
JointModelDense<NQ, NV> toDense_impl() const
JointModelDense<NQ,NV> toDense_impl() const
{
return JointModelDense<NQ, NV>( id(),
idx_q(),
idx_v()
);
return JointModelDense<NQ,NV>(id(),idx_q(),idx_v());
}
static std::string classname() { return std::string("JointModelTranslation"); }
......
......@@ -32,7 +32,6 @@ namespace se3
template<>
struct traits<Joint>
{
enum {
NQ = -1, // Dynamic because unknown at compilation
NV = -1
......@@ -77,9 +76,8 @@ namespace se3
const D_t Dinv() const { return dinv_inertia(*this); }
const UD_t UDinv() const { return udinv_inertia(*this); }
JointData() : JointDataBoostVariant() {}
JointData(const JointDataVariant & jdata ) : JointDataBoostVariant(jdata){}
JointData(const JointDataVariant & jdata) : JointDataBoostVariant(jdata) {}
};
......@@ -94,62 +92,55 @@ namespace se3
using Base::setIndexes;
using Base::operator==;
JointModel() : JointModelBoostVariant() {}
JointModel(const JointModelVariant & model_variant) : JointModelBoostVariant(model_variant)
{}
JointModelVariant& toVariant() { return *static_cast<JointModelVariant*>(this); }
const JointModelVariant& toVariant() const { return *static_cast<const JointModelVariant*>(this); }
JointDataVariant createData()
{
return ::se3::createData(*this);
}
JointDataVariant createData() { return ::se3::createData(*this); }
void calc(JointData & data,const Eigen::VectorXd & q) const { calc_zero_order(*this,data,q); }
void calc (JointData & data,
const Eigen::VectorXd & qs) const
void calc (JointData & data, const Eigen::VectorXd & q, const Eigen::VectorXd & v) const
{
calc_zero_order(*this, data, qs);
}
void calc (JointData & data,
const Eigen::VectorXd & qs,
const Eigen::VectorXd & vs ) const
{
calc_first_order(*this, data, qs, vs);
calc_first_order(*this,data,q,v);
}
void calc_aba(JointData & data, Inertia::Matrix6 & I, const bool update_I) const
{
::se3::calc_aba(*this, data, I, update_I);
::se3::calc_aba(*this,data,I,update_I);
}
ConfigVector_t integrate_impl(const Eigen::VectorXd & qs,const Eigen::VectorXd & vs) const
ConfigVector_t integrate_impl(const Eigen::VectorXd & q,const Eigen::VectorXd & v) const
{
return ::se3::integrate(*this, qs, vs);
return ::se3::integrate(*this,q,v);
}
ConfigVector_t interpolate_impl(const Eigen::VectorXd & q0,const Eigen::VectorXd & q1, const double u) const
{
return ::se3::interpolate(*this, q0, q1, u);
return ::se3::interpolate(*this,q0,q1,u);
}
ConfigVector_t randomConfiguration_impl(const ConfigVector_t & lower_pos_limit, const ConfigVector_t & upper_pos_limit ) const throw (std::runtime_error)
ConfigVector_t randomConfiguration_impl(const ConfigVector_t & lower_pos_limit, const ConfigVector_t & upper_pos_limit) const throw (std::runtime_error)
{
return ::se3::randomConfiguration(*this, lower_pos_limit, upper_pos_limit);
return ::se3::randomConfiguration(*this,lower_pos_limit,upper_pos_limit);
}
TangentVector_t difference_impl(const Eigen::VectorXd & q0,const Eigen::VectorXd & q1) const
{
return ::se3::difference(*this, q0, q1);
return ::se3::difference(*this,q0,q1);
}
double distance_impl(const Eigen::VectorXd & q0,const Eigen::VectorXd & q1) const
{
return ::se3::distance(*this, q0, q1);
return ::se3::distance(*this,q0,q1);
}
void normalize_impl(Eigen::VectorXd & q) const
{
return ::se3::normalize(*this, q);
return ::se3::normalize(*this,q);
}
ConfigVector_t neutralConfiguration_impl() const
......@@ -168,11 +159,7 @@ namespace se3
JointIndex id() const { return ::se3::id(*this); }
// void setIndexes(JointIndex ,int ,int ) { SE3_STATIC_ASSERT(false, THIS_METHOD_SHOULD_NOT_BE_CALLED_ON_DERIVED_CLASS); }
void setIndexes(JointIndex id,int nq,int nv)
{
::se3::setIndexes(*this,id, nq, nv);
}
void setIndexes(JointIndex id,int nq,int nv) { ::se3::setIndexes(*this,id, nq, nv); }
};
typedef std::vector<JointData> JointDataVector;
......
//
// Copyright (c) 2015 CNRS
// Copyright (c) 2015-2016 CNRS
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
......@@ -23,8 +23,6 @@
#include "pinocchio/multibody/joint/joint-variant.hpp"
#include "pinocchio/python/joints-models.hpp"
namespace se3
{
namespace python
......@@ -33,45 +31,37 @@ namespace se3
struct jointModelVariantVisitor : boost::static_visitor<PyObject *>
{
static result_type convert(JointModelVariant const &jv)
static result_type convert(JointModelVariant const & jv)
{
return apply_visitor(jointModelVariantVisitor(), jv);
}
template<typename T>
result_type operator()(T const &t) const
result_type operator()(T const & t) const
{
return boost::python::incref(boost::python::object(t).ptr());
}
};
struct exposer {
template<class T> inline void operator()(T)
struct exposer
{
template<class T>
void operator()(T)
{
expose_constructors<T>(bp::class_<T>(T::classname().c_str(),bp::init<>()).def(JointPythonVisitor<T>()));
bp::implicitly_convertible<T,se3::JointModelVariant>();
}
};
// For the moment, only expose models of joint. Not data ( to do it, split exposer into exposerModels & exposer_Datas and do another for_each)
static void exposeVariants()
{
boost::mpl::for_each<JointModelVariant::types>(exposer());
bp::to_python_converter<se3::JointModelVariant, jointModelVariantVisitor>();
// bp::def("make_variant", se3::make_variant);
}
}} // namespace se3::python
} // namespace python
} // namespace se3
#endif // ifndef __se3_python_joints_variant_hpp__
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