diff --git a/include/sot/core/device.hh b/include/sot/core/device.hh index 0c9f1c5623bde2bbb690d7f0817cc5ca427755aa..eef2d4b8c597e275bb21745af5c206d077b6d93e 100644 --- a/include/sot/core/device.hh +++ b/include/sot/core/device.hh @@ -97,8 +97,8 @@ public: void setTorqueBounds(const Vector &lower, const Vector &upper); /// \} - PeriodicCall& periodicCallBefore() { return periodicCallBefore_; } - PeriodicCall& periodicCallAfter() { return periodicCallAfter_; } + PeriodicCall &periodicCallBefore() { return periodicCallBefore_; } + PeriodicCall &periodicCallAfter() { return periodicCallAfter_; } public: /* --- DISPLAY --- */ virtual void display(std::ostream &os) const; diff --git a/include/sot/core/flags.hh b/include/sot/core/flags.hh index e233969d488e56145f6a59a1ee63dc45c63d4748..2874f405c140e7f48fb2895710ffa9e304a6591c 100644 --- a/include/sot/core/flags.hh +++ b/include/sot/core/flags.hh @@ -19,8 +19,8 @@ #include <vector> /* SOT */ -#include <dynamic-graph/signal-caster.h> #include "sot/core/api.hh" +#include <dynamic-graph/signal-caster.h> /* --------------------------------------------------------------------- */ /* --- CLASS ----------------------------------------------------------- */ diff --git a/include/sot/core/integrator-abstract.hh b/include/sot/core/integrator-abstract.hh index 7af240312bc01d3743f445d3bb3483dc820ca460..332eacf709e50dd90ab9f363117b5c67765720d2 100644 --- a/include/sot/core/integrator-abstract.hh +++ b/include/sot/core/integrator-abstract.hh @@ -90,21 +90,19 @@ public: void popNumCoef() { numerator.pop_back(); } void popDenomCoef() { denominator.pop_back(); } - const std::vector<coefT>& numCoeffs() const { return numerator; } - void numCoeffs(const std::vector<coefT>& coeffs) { numerator = coeffs; } + const std::vector<coefT> &numCoeffs() const { return numerator; } + void numCoeffs(const std::vector<coefT> &coeffs) { numerator = coeffs; } - const std::vector<coefT>& denomCoeffs() const { return denominator; } - void denomCoeffs(const std::vector<coefT>& coeffs) { denominator = coeffs; } + const std::vector<coefT> &denomCoeffs() const { return denominator; } + void denomCoeffs(const std::vector<coefT> &coeffs) { denominator = coeffs; } public: dynamicgraph::SignalPtr<sigT, int> SIN; dynamicgraph::SignalTimeDependent<sigT, int> SOUT; - virtual void display(std::ostream &os) const - { - os << this->getClassName() << ": " << getName() << '\n' - << " "; + virtual void display(std::ostream &os) const { + os << this->getClassName() << ": " << getName() << '\n' << " "; if (numerator.empty() || denominator.empty()) { os << "ill-formed."; return; diff --git a/include/sot/core/mailbox.hh b/include/sot/core/mailbox.hh index 53600f5feed30123f1d8980437307c2ba2571446..af87346b23a0cd580b1d56b121eefb0362a1b65e 100644 --- a/include/sot/core/mailbox.hh +++ b/include/sot/core/mailbox.hh @@ -33,8 +33,7 @@ namespace sot { namespace dg = dynamicgraph; -template <class Object> struct MailboxTimestampedObject -{ +template <class Object> struct MailboxTimestampedObject { Object obj; struct timeval timestamp; }; @@ -73,10 +72,11 @@ public: /* --- SIGNALS --- */ } /* namespace sot */ -template <class Object> struct signal_io<sot::MailboxTimestampedObject<Object> > -: signal_io_unimplemented<sot::MailboxTimestampedObject<Object> > {}; +template <class Object> +struct signal_io<sot::MailboxTimestampedObject<Object> > + : signal_io_unimplemented<sot::MailboxTimestampedObject<Object> > {}; -template <> struct signal_io<timeval> : signal_io_unimplemented<timeval > {}; +template <> struct signal_io<timeval> : signal_io_unimplemented<timeval> {}; } /* namespace dynamicgraph */ #endif // #ifndef __SOT_MAILBOX_HH diff --git a/include/sot/core/multi-bound.hh b/include/sot/core/multi-bound.hh index fd5ba3ac077a16e968e435ff7841963e737cbc89..755758b7b711a788f016ec1b50c49565798b36d7 100644 --- a/include/sot/core/multi-bound.hh +++ b/include/sot/core/multi-bound.hh @@ -19,8 +19,8 @@ #include <vector> /* SOT */ -#include <dynamic-graph/signal-caster.h> #include "sot/core/api.hh" +#include <dynamic-graph/signal-caster.h> #include <sot/core/exception-task.hh> /* --------------------------------------------------------------------- */ @@ -73,7 +73,8 @@ SOT_CORE_EXPORT std::istream &operator>>(std::istream &os, VectorMultiBound &v); } /* namespace sot */ -template <> struct signal_io<sot::MultiBound> : signal_io_unimplemented<sot::MultiBound> {}; +template <> +struct signal_io<sot::MultiBound> : signal_io_unimplemented<sot::MultiBound> {}; } /* namespace dynamicgraph */ #endif // #ifndef __SOT_MultiBound_H__ diff --git a/include/sot/core/trajectory.hh b/include/sot/core/trajectory.hh index 85d15cb4a268f7072ba9c76d3b91b4177a8a623c..b5bb1e4f8bec36d1d13bc50ff22edd4cb63d4b0e 100644 --- a/include/sot/core/trajectory.hh +++ b/include/sot/core/trajectory.hh @@ -195,7 +195,8 @@ public: }; } // namespace sot -template <> struct signal_io<sot::Trajectory> : signal_io_unimplemented<sot::Trajectory> {}; +template <> +struct signal_io<sot::Trajectory> : signal_io_unimplemented<sot::Trajectory> {}; } // namespace dynamicgraph diff --git a/include/sot/core/variadic-op.hh b/include/sot/core/variadic-op.hh index 83e4bf8844bfc90c592842103f829f2d42aac2d5..c03018f5a60161bd9c16640c3a9a5c4314c7eead 100644 --- a/include/sot/core/variadic-op.hh +++ b/include/sot/core/variadic-op.hh @@ -108,8 +108,7 @@ public: /* --- SIGNAL --- */ int getSignalNumber() const { return (int)signalsIN.size(); } - signal_t* getSignalIn(int i) - { + signal_t *getSignalIn(int i) { if (i < 0 || i >= (int)signalsIN.size()) throw std::out_of_range("Wrong signal index"); return signalsIN[i]; diff --git a/include/sot/core/vector-to-rotation.hh b/include/sot/core/vector-to-rotation.hh index a6c5bb8d8b2d9c8b41cee461e34c672f3503fc97..7d60cc73d5e048ca31899a4fd745a6e74d16a6ff 100644 --- a/include/sot/core/vector-to-rotation.hh +++ b/include/sot/core/vector-to-rotation.hh @@ -40,7 +40,8 @@ namespace dynamicgraph { namespace sot { -class SOTVECTORTOROTATION_EXPORT VectorToRotation : public dynamicgraph::Entity { +class SOTVECTORTOROTATION_EXPORT VectorToRotation + : public dynamicgraph::Entity { enum sotAxis { AXIS_X, AXIS_Y, AXIS_Z }; unsigned int size; diff --git a/src/control/admittance-control-op-point-python.h b/src/control/admittance-control-op-point-python.h index 8c65fbee0d50e9b3c020115ea579063828ffc279..321bd05eb8be53da9306536565262255ddf5fe8e 100644 --- a/src/control/admittance-control-op-point-python.h +++ b/src/control/admittance-control-op-point-python.h @@ -1,3 +1,4 @@ #include <sot/core/admittance-control-op-point.hh> -typedef boost::mpl::vector<dynamicgraph::sot::core::AdmittanceControlOpPoint> entities_t; +typedef boost::mpl::vector<dynamicgraph::sot::core::AdmittanceControlOpPoint> + entities_t; diff --git a/src/feature/feature-point6d-relative-python.h b/src/feature/feature-point6d-relative-python.h index 2066f4c1b3361419398bf40178c4831a2c90493c..d18d6a3b725f084816b849588ae03d29733cdd36 100644 --- a/src/feature/feature-point6d-relative-python.h +++ b/src/feature/feature-point6d-relative-python.h @@ -1,3 +1,4 @@ #include <sot/core/feature-point6d-relative.hh> -typedef boost::mpl::vector<dynamicgraph::sot::FeaturePoint6dRelative> entities_t; +typedef boost::mpl::vector<dynamicgraph::sot::FeaturePoint6dRelative> + entities_t; diff --git a/src/feature/feature-pose-python.h b/src/feature/feature-pose-python.h index fecbdf9e0088b9863857facf1e20b70a7bef85ee..55a7b901d1f8065c38bee6fdc1eaf8a5cb55d756 100644 --- a/src/feature/feature-pose-python.h +++ b/src/feature/feature-pose-python.h @@ -2,7 +2,6 @@ namespace dgs = dynamicgraph::sot; -typedef boost::mpl::vector< - dgs::FeaturePose<dgs::SE3Representation> -, dgs::FeaturePose<dgs::R3xSO3Representation> - > entities_t; +typedef boost::mpl::vector<dgs::FeaturePose<dgs::SE3Representation>, + dgs::FeaturePose<dgs::R3xSO3Representation> > + entities_t; diff --git a/src/matrix/derivator-python.h b/src/matrix/derivator-python.h index b1645bce1c7f40200065813cd979a47d6892aa51..e08c43edef39e76e63df2196977ee5d6041f71c3 100644 --- a/src/matrix/derivator-python.h +++ b/src/matrix/derivator-python.h @@ -1,9 +1,8 @@ #include <sot/core/derivator.hh> namespace dg = ::dynamicgraph; -typedef boost::mpl::vector< - dg::sot::Derivator<double> - , dg::sot::Derivator<dg::Vector> - , dg::sot::Derivator<dg::Matrix> - , dg::sot::Derivator<dg::sot::VectorQuaternion> -> entities_t; +typedef boost::mpl::vector<dg::sot::Derivator<double>, + dg::sot::Derivator<dg::Vector>, + dg::sot::Derivator<dg::Matrix>, + dg::sot::Derivator<dg::sot::VectorQuaternion> > + entities_t; diff --git a/src/matrix/fir-filter-python.h b/src/matrix/fir-filter-python.h index e22ac891d0f9918424cde5cb94b53bf7acd75405..1266eb6bb4f6bc9890192a8a3deb11f729b0009e 100644 --- a/src/matrix/fir-filter-python.h +++ b/src/matrix/fir-filter-python.h @@ -1,8 +1,7 @@ #include <sot/core/fir-filter.hh> namespace dg = ::dynamicgraph; -typedef boost::mpl::vector< - dg::sot::FIRFilter<double, double> - , dg::sot::FIRFilter<dg::Vector, double> - , dg::sot::FIRFilter<dg::Vector, dg::Matrix> -> entities_t; +typedef boost::mpl::vector<dg::sot::FIRFilter<double, double>, + dg::sot::FIRFilter<dg::Vector, double>, + dg::sot::FIRFilter<dg::Vector, dg::Matrix> > + entities_t; diff --git a/src/matrix/integrator-euler-python-module-py.cc b/src/matrix/integrator-euler-python-module-py.cc index 670a401610db6f4ab600c593d30efaedeb9cc340..e921d35ec7aa472e73525a2cf831d5e663fda29b 100644 --- a/src/matrix/integrator-euler-python-module-py.cc +++ b/src/matrix/integrator-euler-python-module-py.cc @@ -10,28 +10,34 @@ namespace dg = dynamicgraph; namespace dgc = dynamicgraph::command; namespace dgs = dynamicgraph::sot; -using dg::Vector; using dg::Matrix; +using dg::Vector; -template<typename S, typename C> void exposeIntegratorEuler() -{ +template <typename S, typename C> void exposeIntegratorEuler() { typedef dgs::IntegratorEuler<S, C> IE_t; - const std::string cName = - dgc::Value::typeName(dgc::ValueHelper<C>::TypeID); + const std::string cName = dgc::Value::typeName(dgc::ValueHelper<C>::TypeID); dg::python::exposeEntity<IE_t>() - .add_property("numerators", - +[](const IE_t& e) { return dg::python::to_py_list(e.numCoeffs().begin(), e.numCoeffs().end()); }, - +[](IE_t& e, bp::object iterable) { e.numCoeffs(dg::python::to_std_vector<C>(iterable)); }) - .add_property("denominators", - +[](const IE_t& e) { return dg::python::to_py_list(e.denomCoeffs().begin(), e.denomCoeffs().end()); }, - +[](IE_t& e, bp::object iterable) { e.denomCoeffs(dg::python::to_std_vector<C>(iterable)); }) - ; + .add_property("numerators", + +[](const IE_t &e) { + return dg::python::to_py_list(e.numCoeffs().begin(), + e.numCoeffs().end()); + }, + +[](IE_t &e, bp::object iterable) { + e.numCoeffs(dg::python::to_std_vector<C>(iterable)); + }) + .add_property("denominators", + +[](const IE_t &e) { + return dg::python::to_py_list(e.denomCoeffs().begin(), + e.denomCoeffs().end()); + }, + +[](IE_t &e, bp::object iterable) { + e.denomCoeffs(dg::python::to_std_vector<C>(iterable)); + }); } -BOOST_PYTHON_MODULE(wrap) -{ +BOOST_PYTHON_MODULE(wrap) { bp::import("dynamic_graph"); exposeIntegratorEuler<double, double>(); diff --git a/src/matrix/operator-python-module-py.cc b/src/matrix/operator-python-module-py.cc index b53f8ec7dae137eb4354874c3133e7a1d5162c6a..d9e3a7533af93416001f90866a1f348a40f0f90d 100644 --- a/src/matrix/operator-python-module-py.cc +++ b/src/matrix/operator-python-module-py.cc @@ -6,170 +6,157 @@ namespace dg = dynamicgraph; namespace dgs = dynamicgraph::sot; namespace bp = boost::python; -typedef bp::return_value_policy<bp::reference_existing_object> reference_existing_object; +typedef bp::return_value_policy<bp::reference_existing_object> + reference_existing_object; -template<typename Operator> -void exposeUnaryOp() -{ +template <typename Operator> void exposeUnaryOp() { typedef dgs::UnaryOp<Operator> O_t; - dg::python::exposeEntity<O_t, bp::bases<dg::Entity>, dg::python::AddCommands>() + dg::python::exposeEntity<O_t, bp::bases<dg::Entity>, + dg::python::AddCommands>() .def_readonly("sin", &O_t::SIN) - .def_readonly("sout", &O_t::SOUT) - ; - + .def_readonly("sout", &O_t::SOUT); } -template<typename Operator> -void exposeBinaryOp() -{ +template <typename Operator> void exposeBinaryOp() { typedef dgs::BinaryOp<Operator> O_t; - dg::python::exposeEntity<O_t, bp::bases<dg::Entity>, dg::python::AddCommands>() + dg::python::exposeEntity<O_t, bp::bases<dg::Entity>, + dg::python::AddCommands>() .def_readonly("sin1", &O_t::SIN1) .def_readonly("sin2", &O_t::SIN2) - .def_readonly("sout", &O_t::SOUT) - ; - + .def_readonly("sout", &O_t::SOUT); } -template<typename Operator> -auto exposeVariadicOpBase() -{ +template <typename Operator> auto exposeVariadicOpBase() { typedef dgs::VariadicOp<Operator> O_t; typedef typename O_t::Base B_t; - return - dg::python::exposeEntity<O_t, bp::bases<dg::Entity>, dg::python::AddCommands>() + return dg::python::exposeEntity<O_t, bp::bases<dg::Entity>, + dg::python::AddCommands>() .def_readonly("sout", &O_t::SOUT) .def("sin", &B_t::getSignalIn, reference_existing_object()) .add_property("n_sin", &B_t::getSignalNumber, &B_t::setSignalNumber, - "the number of input signal.") + "the number of input signal.") .def("setSignalNumber", &B_t::setSignalNumber, - "set the number of input signal.", bp::arg("size")) + "set the number of input signal.", bp::arg("size")) .def("getSignalNumber", &B_t::getSignalNumber, - "get the number of input signal.", bp::arg("size")) - ; + "get the number of input signal.", bp::arg("size")); } -template<typename Operator> -struct exposeVariadicOpImpl { - static void run () { exposeVariadicOpBase<Operator>(); } +template <typename Operator> struct exposeVariadicOpImpl { + static void run() { exposeVariadicOpBase<Operator>(); } }; -template<typename T> -struct exposeVariadicOpImpl<dgs::AdderVariadic<T> > { - static void run () { +template <typename T> struct exposeVariadicOpImpl<dgs::AdderVariadic<T> > { + static void run() { typedef dgs::VariadicOp<dgs::AdderVariadic<T> > E_t; - exposeVariadicOpBase<dgs::AdderVariadic<T> >() - .add_property("coeffs", +[](E_t& e) { return e.op.coeffs; }, - +[](E_t& e, const dg::Vector& c) { e.op.setCoeffs(c); }, - "the multipliers.") - ; + exposeVariadicOpBase<dgs::AdderVariadic<T> >().add_property( + "coeffs", +[](E_t &e) { return e.op.coeffs; }, + +[](E_t &e, const dg::Vector &c) { e.op.setCoeffs(c); }, + "the multipliers."); } }; -template<typename Operator> -void exposeVariadicOp() -{ +template <typename Operator> void exposeVariadicOp() { exposeVariadicOpImpl<Operator>::run(); } -BOOST_PYTHON_MODULE(wrap) -{ +BOOST_PYTHON_MODULE(wrap) { using namespace dynamicgraph; using namespace dynamicgraph::sot; - exposeUnaryOp < VectorSelecter> (); - exposeUnaryOp < VectorComponent> (); - exposeUnaryOp < MatrixSelector> (); - exposeUnaryOp < MatrixColumnSelector> (); - exposeUnaryOp < MatrixTranspose> (); - exposeUnaryOp < Diagonalizer> (); - - /* ---------------------------------------------------------------------- */ - /* --- INVERSION -------------------------------------------------------- */ - /* ---------------------------------------------------------------------- */ - exposeUnaryOp < Inverser<Matrix> > (); - exposeUnaryOp < Inverser<MatrixHomogeneous> > (); - exposeUnaryOp < Inverser<MatrixTwist> > (); - exposeUnaryOp < Normalize> (); - exposeUnaryOp < InverserRotation> (); - exposeUnaryOp < InverserQuaternion> (); - - /* ----------------------------------------------------------------------- */ - /* --- SE3/SO3 conversions ----------------------------------------------- */ - /* ----------------------------------------------------------------------- */ - - exposeUnaryOp < SkewSymToVector> (); - exposeUnaryOp < PoseUThetaToMatrixHomo> (); - exposeUnaryOp < MatrixHomoToPoseUTheta> (); - exposeUnaryOp < MatrixHomoToSE3Vector> (); - exposeUnaryOp < SE3VectorToMatrixHomo> (); - exposeUnaryOp < PoseQuaternionToMatrixHomo> (); - exposeUnaryOp < MatrixHomoToPoseQuaternion> (); - exposeUnaryOp < MatrixHomoToPoseRollPitchYaw> (); - exposeUnaryOp < PoseRollPitchYawToMatrixHomo> (); - exposeUnaryOp < PoseRollPitchYawToPoseUTheta> (); - exposeUnaryOp < HomoToMatrix> (); - exposeUnaryOp < MatrixToHomo> (); - exposeUnaryOp < HomoToTwist> (); - exposeUnaryOp < HomoToRotation> (); - exposeUnaryOp < MatrixHomoToPose> (); - exposeUnaryOp < RPYToMatrix> (); - exposeUnaryOp < MatrixToRPY> (); - exposeUnaryOp < RPYToQuaternion> (); - exposeUnaryOp < QuaternionToRPY> (); - exposeUnaryOp < QuaternionToMatrix> (); - exposeUnaryOp < MatrixToQuaternion> (); - exposeUnaryOp < MatrixToUTheta> (); - exposeUnaryOp < UThetaToQuaternion> (); - - /* --- MULTIPLICATION --------------------------------------------------- */ - - exposeBinaryOp < Multiplier_double_vector > (); - exposeBinaryOp < Multiplier_matrix_vector > (); - exposeBinaryOp < Multiplier_matrixHomo_vector > (); - exposeBinaryOp < Multiplier_matrixTwist_vector > (); - - /* --- SUBSTRACTION ----------------------------------------------------- */ - exposeBinaryOp < Substraction<dynamicgraph::Matrix> > (); - exposeBinaryOp < Substraction<dynamicgraph::Vector> > (); - exposeBinaryOp < Substraction<double> > (); - - /* --- STACK ------------------------------------------------------------ */ - exposeBinaryOp < VectorStack > (); - - /* ---------------------------------------------------------------------- */ - exposeBinaryOp < Composer > (); - - /* --- CONVOLUTION PRODUCT ---------------------------------------------- */ - exposeBinaryOp < ConvolutionTemporal > (); - - /* --- BOOLEAN REDUCTION ------------------------------------------------ */ - exposeBinaryOp < Comparison<double> > (); - exposeBinaryOp < MatrixComparison<Vector> > (); - - exposeBinaryOp < WeightedAdder<dynamicgraph::Matrix> > (); - exposeBinaryOp < WeightedAdder<dynamicgraph::Vector> > (); - exposeBinaryOp < WeightedAdder<double> > (); - - /* --- VectorMix ------------------------------------------------------------ */ - exposeVariadicOp < VectorMix > (); - - /* --- ADDITION --------------------------------------------------------- */ - exposeVariadicOp < AdderVariadic<Matrix> > (); - exposeVariadicOp < AdderVariadic<Vector> > (); - exposeVariadicOp < AdderVariadic<double> > (); - - /* --- MULTIPLICATION --------------------------------------------------- */ - exposeVariadicOp < Multiplier<Matrix> > (); - exposeVariadicOp < Multiplier<Vector> > (); - exposeVariadicOp < Multiplier<MatrixRotation> > (); - exposeVariadicOp < Multiplier<MatrixHomogeneous> > (); - exposeVariadicOp < Multiplier<MatrixTwist> > (); - exposeVariadicOp < Multiplier<VectorQuaternion> > (); - exposeVariadicOp < Multiplier<double> > (); - - /* --- BOOLEAN --------------------------------------------------------- */ - exposeVariadicOp < BoolOp<0> > (); - exposeVariadicOp < BoolOp<1> > (); + exposeUnaryOp<VectorSelecter>(); + exposeUnaryOp<VectorComponent>(); + exposeUnaryOp<MatrixSelector>(); + exposeUnaryOp<MatrixColumnSelector>(); + exposeUnaryOp<MatrixTranspose>(); + exposeUnaryOp<Diagonalizer>(); + + /* ---------------------------------------------------------------------- */ + /* --- INVERSION -------------------------------------------------------- */ + /* ---------------------------------------------------------------------- */ + exposeUnaryOp<Inverser<Matrix> >(); + exposeUnaryOp<Inverser<MatrixHomogeneous> >(); + exposeUnaryOp<Inverser<MatrixTwist> >(); + exposeUnaryOp<Normalize>(); + exposeUnaryOp<InverserRotation>(); + exposeUnaryOp<InverserQuaternion>(); + + /* ----------------------------------------------------------------------- */ + /* --- SE3/SO3 conversions ----------------------------------------------- */ + /* ----------------------------------------------------------------------- */ + + exposeUnaryOp<SkewSymToVector>(); + exposeUnaryOp<PoseUThetaToMatrixHomo>(); + exposeUnaryOp<MatrixHomoToPoseUTheta>(); + exposeUnaryOp<MatrixHomoToSE3Vector>(); + exposeUnaryOp<SE3VectorToMatrixHomo>(); + exposeUnaryOp<PoseQuaternionToMatrixHomo>(); + exposeUnaryOp<MatrixHomoToPoseQuaternion>(); + exposeUnaryOp<MatrixHomoToPoseRollPitchYaw>(); + exposeUnaryOp<PoseRollPitchYawToMatrixHomo>(); + exposeUnaryOp<PoseRollPitchYawToPoseUTheta>(); + exposeUnaryOp<HomoToMatrix>(); + exposeUnaryOp<MatrixToHomo>(); + exposeUnaryOp<HomoToTwist>(); + exposeUnaryOp<HomoToRotation>(); + exposeUnaryOp<MatrixHomoToPose>(); + exposeUnaryOp<RPYToMatrix>(); + exposeUnaryOp<MatrixToRPY>(); + exposeUnaryOp<RPYToQuaternion>(); + exposeUnaryOp<QuaternionToRPY>(); + exposeUnaryOp<QuaternionToMatrix>(); + exposeUnaryOp<MatrixToQuaternion>(); + exposeUnaryOp<MatrixToUTheta>(); + exposeUnaryOp<UThetaToQuaternion>(); + + /* --- MULTIPLICATION --------------------------------------------------- */ + + exposeBinaryOp<Multiplier_double_vector>(); + exposeBinaryOp<Multiplier_matrix_vector>(); + exposeBinaryOp<Multiplier_matrixHomo_vector>(); + exposeBinaryOp<Multiplier_matrixTwist_vector>(); + + /* --- SUBSTRACTION ----------------------------------------------------- */ + exposeBinaryOp<Substraction<dynamicgraph::Matrix> >(); + exposeBinaryOp<Substraction<dynamicgraph::Vector> >(); + exposeBinaryOp<Substraction<double> >(); + + /* --- STACK ------------------------------------------------------------ */ + exposeBinaryOp<VectorStack>(); + + /* ---------------------------------------------------------------------- */ + exposeBinaryOp<Composer>(); + + /* --- CONVOLUTION PRODUCT ---------------------------------------------- */ + exposeBinaryOp<ConvolutionTemporal>(); + + /* --- BOOLEAN REDUCTION ------------------------------------------------ */ + exposeBinaryOp<Comparison<double> >(); + exposeBinaryOp<MatrixComparison<Vector> >(); + + exposeBinaryOp<WeightedAdder<dynamicgraph::Matrix> >(); + exposeBinaryOp<WeightedAdder<dynamicgraph::Vector> >(); + exposeBinaryOp<WeightedAdder<double> >(); + + /* --- VectorMix ------------------------------------------------------------ + */ + exposeVariadicOp<VectorMix>(); + + /* --- ADDITION --------------------------------------------------------- */ + exposeVariadicOp<AdderVariadic<Matrix> >(); + exposeVariadicOp<AdderVariadic<Vector> >(); + exposeVariadicOp<AdderVariadic<double> >(); + + /* --- MULTIPLICATION --------------------------------------------------- */ + exposeVariadicOp<Multiplier<Matrix> >(); + exposeVariadicOp<Multiplier<Vector> >(); + exposeVariadicOp<Multiplier<MatrixRotation> >(); + exposeVariadicOp<Multiplier<MatrixHomogeneous> >(); + exposeVariadicOp<Multiplier<MatrixTwist> >(); + exposeVariadicOp<Multiplier<VectorQuaternion> >(); + exposeVariadicOp<Multiplier<double> >(); + + /* --- BOOLEAN --------------------------------------------------------- */ + exposeVariadicOp<BoolOp<0> >(); + exposeVariadicOp<BoolOp<1> >(); } diff --git a/src/matrix/operator.hh b/src/matrix/operator.hh index 9724e716c781dfd0cc1403ec7bb3ed812a8e1b86..1b62bff6d93014305e3fad5934d73bc81702d901 100644 --- a/src/matrix/operator.hh +++ b/src/matrix/operator.hh @@ -89,7 +89,8 @@ struct VectorSelecter : public UnaryOpHeader<dg::Vector, dg::Vector> { size += M - m; } - inline void addSpecificCommands(Entity &ent, Entity::CommandMap_t &commandMap) { + inline void addSpecificCommands(Entity &ent, + Entity::CommandMap_t &commandMap) { using namespace dynamicgraph::command; std::string doc; @@ -117,7 +118,8 @@ struct VectorComponent : public UnaryOpHeader<dg::Vector, double> { int index; inline void setIndex(const int &m) { index = m; } - inline void addSpecificCommands(Entity &ent, Entity::CommandMap_t &commandMap) { + inline void addSpecificCommands(Entity &ent, + Entity::CommandMap_t &commandMap) { std::string doc; boost::function<void(const int &)> callback = @@ -158,7 +160,8 @@ public: jmax = M; } - inline void addSpecificCommands(Entity &ent, Entity::CommandMap_t &commandMap) { + inline void addSpecificCommands(Entity &ent, + Entity::CommandMap_t &commandMap) { using namespace dynamicgraph::command; std::string doc; @@ -196,7 +199,8 @@ public: imax = M; } - inline void addSpecificCommands(Entity &ent, Entity::CommandMap_t &commandMap) { + inline void addSpecificCommands(Entity &ent, + Entity::CommandMap_t &commandMap) { using namespace dynamicgraph::command; std::string doc; @@ -231,7 +235,8 @@ public: nbr = r; nbc = c; } - inline void addSpecificCommands(Entity &ent, Entity::CommandMap_t &commandMap) { + inline void addSpecificCommands(Entity &ent, + Entity::CommandMap_t &commandMap) { using namespace dynamicgraph::command; std::string doc; @@ -255,7 +260,9 @@ struct Inverser : public UnaryOpHeader<matrixgen, matrixgen> { }; struct Normalize : public UnaryOpHeader<dg::Vector, double> { - inline void operator()(const dg::Vector &m, double &res) const { res = m.norm(); } + inline void operator()(const dg::Vector &m, double &res) const { + res = m.norm(); + } inline std::string getDocString() const { std::string docString("Computes the norm of a vector\n" @@ -412,7 +419,7 @@ struct HomoToMatrix : public UnaryOpHeader<MatrixHomogeneous, Matrix> { struct MatrixToHomo : public UnaryOpHeader<Matrix, MatrixHomogeneous> { inline void operator()(const Eigen::Matrix<double, 4, 4> &M, - MatrixHomogeneous &res) { + MatrixHomogeneous &res) { res = M; } }; @@ -440,7 +447,6 @@ struct HomoToRotation } }; - struct MatrixHomoToPose : public UnaryOpHeader<MatrixHomogeneous, Vector> { inline void operator()(const MatrixHomogeneous &M, Vector &res) { res.resize(3); @@ -489,16 +495,22 @@ struct QuaternionToMatrix struct MatrixToQuaternion : public UnaryOpHeader<MatrixRotation, VectorQuaternion> { - inline void operator()(const MatrixRotation &r, VectorQuaternion &res) { res = r; } + inline void operator()(const MatrixRotation &r, VectorQuaternion &res) { + res = r; + } }; struct MatrixToUTheta : public UnaryOpHeader<MatrixRotation, VectorUTheta> { - inline void operator()(const MatrixRotation &r, VectorUTheta &res) { res = r; } + inline void operator()(const MatrixRotation &r, VectorUTheta &res) { + res = r; + } }; struct UThetaToQuaternion : public UnaryOpHeader<VectorUTheta, VectorQuaternion> { - inline void operator()(const VectorUTheta &r, VectorQuaternion &res) { res = r; } + inline void operator()(const VectorUTheta &r, VectorQuaternion &res) { + res = r; + } }; template <typename TypeIn1, typename TypeIn2, typename TypeOut> @@ -547,8 +559,8 @@ struct Multiplier_FxE__E : public BinaryOpHeader<F, E, E> { }; template <> -inline void Multiplier_FxE__E<dynamicgraph::sot::MatrixHomogeneous, - dynamicgraph::Vector>:: +inline void +Multiplier_FxE__E<dynamicgraph::sot::MatrixHomogeneous, dynamicgraph::Vector>:: operator()(const dynamicgraph::sot::MatrixHomogeneous &f, const dynamicgraph::Vector &e, dynamicgraph::Vector &res) const { res = f.matrix() * e; @@ -579,7 +591,6 @@ template <typename T> struct Substraction : public BinaryOpHeader<T, T, T> { } }; - /* --- STACK ------------------------------------------------------------ */ struct VectorStack : public BinaryOpHeader<dynamicgraph::Vector, dynamicgraph::Vector, @@ -588,8 +599,8 @@ public: int v1min, v1max; int v2min, v2max; inline void operator()(const dynamicgraph::Vector &v1, - const dynamicgraph::Vector &v2, - dynamicgraph::Vector &res) const { + const dynamicgraph::Vector &v2, + dynamicgraph::Vector &res) const { assert((v1max >= v1min) && (v1.size() >= v1max)); assert((v2max >= v2min) && (v2.size() >= v2max)); @@ -612,7 +623,8 @@ public: v2max = M; } - inline void addSpecificCommands(Entity &ent, Entity::CommandMap_t &commandMap) { + inline void addSpecificCommands(Entity &ent, + Entity::CommandMap_t &commandMap) { using namespace dynamicgraph::command; std::string doc; @@ -639,8 +651,9 @@ public: struct Composer : public BinaryOpHeader<dynamicgraph::Matrix, dynamicgraph::Vector, MatrixHomogeneous> { - inline void operator()(const dynamicgraph::Matrix &R, const dynamicgraph::Vector &t, - MatrixHomogeneous &H) const { + inline void operator()(const dynamicgraph::Matrix &R, + const dynamicgraph::Vector &t, + MatrixHomogeneous &H) const { H.linear() = R; H.translation() = t; } @@ -654,7 +667,7 @@ struct ConvolutionTemporal MemoryType memory; inline void convolution(const MemoryType &f1, const dynamicgraph::Matrix &f2, - dynamicgraph::Vector &res) { + dynamicgraph::Vector &res) { const Vector::Index nconv = (Vector::Index)f1.size(), nsig = f2.rows(); sotDEBUG(15) << "Size: " << nconv << "x" << nsig << std::endl; if (nconv > f2.cols()) @@ -676,7 +689,8 @@ struct ConvolutionTemporal } } inline void operator()(const dynamicgraph::Vector &v1, - const dynamicgraph::Matrix &m2, dynamicgraph::Vector &res) { + const dynamicgraph::Matrix &m2, + dynamicgraph::Vector &res) { memory.push_front(v1); while ((Vector::Index)memory.size() > m2.cols()) memory.pop_back(); @@ -687,7 +701,9 @@ struct ConvolutionTemporal /* --- BOOLEAN REDUCTION ------------------------------------------------ */ template <typename T> struct Comparison : public BinaryOpHeader<T, T, bool> { - inline void operator()(const T &a, const T &b, bool &res) const { res = (a < b); } + inline void operator()(const T &a, const T &b, bool &res) const { + res = (a < b); + } inline std::string getDocString() const { typedef BinaryOpHeader<T, T, bool> Base; return std::string("Comparison of inputs:\n" @@ -735,7 +751,8 @@ struct MatrixComparison : public BinaryOpHeader<T1, T2, bool> { "comparison can be made <=.\n"); } MatrixComparison() : any(true), equal(false) {} - inline void addSpecificCommands(Entity &ent, Entity::CommandMap_t &commandMap) { + inline void addSpecificCommands(Entity &ent, + Entity::CommandMap_t &commandMap) { using namespace dynamicgraph::command; ADD_COMMAND( "setTrueIfAny", @@ -766,7 +783,8 @@ public: res += gain2 * v2; } - inline void addSpecificCommands(Entity &ent, Entity::CommandMap_t &commandMap) { + inline void addSpecificCommands(Entity &ent, + Entity::CommandMap_t &commandMap) { using namespace dynamicgraph::command; std::string doc; @@ -838,7 +856,8 @@ public: typedef std::vector<segment_t> segments_t; Base *entity; segments_t idxs; - inline void operator()(const std::vector<const Vector *> &vs, Vector &res) const { + inline void operator()(const std::vector<const Vector *> &vs, + Vector &res) const { res = *vs[0]; for (std::size_t i = 0; i < idxs.size(); ++i) { const segment_t &s = idxs[i]; @@ -932,7 +951,9 @@ template <typename T> struct Multiplier : public VariadicOpHeader<T, T> { ent->setSignalNumber(2); } }; -template <> inline void Multiplier<double>::setIdentity(double &res) const { res = 1; } +template <> inline void Multiplier<double>::setIdentity(double &res) const { + res = 1; +} template <> inline void Multiplier<MatrixHomogeneous>:: operator()(const std::vector<const MatrixHomogeneous *> &vs, @@ -946,8 +967,8 @@ operator()(const std::vector<const MatrixHomogeneous *> &vs, } } template <> -inline void Multiplier<Vector>::operator()(const std::vector<const Vector *> &vs, - Vector &res) const { +inline void Multiplier<Vector>:: +operator()(const std::vector<const Vector *> &vs, Vector &res) const { if (vs.size() == 0) res.resize(0); else { @@ -957,7 +978,6 @@ inline void Multiplier<Vector>::operator()(const std::vector<const Vector *> &vs } } - /* --- BOOLEAN --------------------------------------------------------- */ template <int operation> struct BoolOp : public VariadicOpHeader<bool, bool> { typedef VariadicOp<BoolOp> Base; diff --git a/src/python-module.cc b/src/python-module.cc index baabaaa40af6da4fcbda3326e094c0bcef7092c9..731db0d93fbeb9e136a30ce7a1aebe4bc0f58b33 100644 --- a/src/python-module.cc +++ b/src/python-module.cc @@ -7,82 +7,96 @@ namespace dg = dynamicgraph; namespace dgs = dynamicgraph::sot; -typedef bp::return_value_policy<bp::reference_existing_object> reference_existing_object; +typedef bp::return_value_policy<bp::reference_existing_object> + reference_existing_object; -BOOST_PYTHON_MODULE(wrap) -{ +BOOST_PYTHON_MODULE(wrap) { bp::import("dynamic_graph"); using dgs::PeriodicCall; bp::class_<PeriodicCall, boost::noncopyable>("PeriodicCall", bp::no_init) - .def("addSignal", static_cast<void (PeriodicCall::*)(const std::string &, dg::SignalBase<int> &)> (&PeriodicCall::addSignal), - "Add the signal to the refresh list", (bp::arg("name"), "signal")) - .def("addSignal", static_cast<void (PeriodicCall::*)(const std::string &)> (&PeriodicCall::addSignal), - "Add the signal to the refresh list", (bp::arg("signal_name"))) + .def("addSignal", + static_cast<void (PeriodicCall::*)(const std::string &, + dg::SignalBase<int> &)>( + &PeriodicCall::addSignal), + "Add the signal to the refresh list", (bp::arg("name"), "signal")) + .def("addSignal", + static_cast<void (PeriodicCall::*)(const std::string &)>( + &PeriodicCall::addSignal), + "Add the signal to the refresh list", (bp::arg("signal_name"))) - .def("addDownsampledSignal", static_cast<void (PeriodicCall::*)(const std::string &, dg::SignalBase<int> &, const unsigned int &)> (&PeriodicCall::addDownsampledSignal), - "Add the signal to the refresh list\n" - "The downsampling factor: 1 means every time, " - "2 means every other time, etc...", - (bp::arg("name"), "signal", "factor")) - .def("addDownsampledSignal", static_cast<void (PeriodicCall::*)(const std::string &, const unsigned int &)> (&PeriodicCall::addDownsampledSignal), - "Add the signal to the refresh list\n" - "The downsampling factor: 1 means every time, " - "2 means every other time, etc...", - (bp::arg("signal_name"), "factor")) + .def("addDownsampledSignal", + static_cast<void (PeriodicCall::*)( + const std::string &, dg::SignalBase<int> &, + const unsigned int &)>(&PeriodicCall::addDownsampledSignal), + "Add the signal to the refresh list\n" + "The downsampling factor: 1 means every time, " + "2 means every other time, etc...", + (bp::arg("name"), "signal", "factor")) + .def("addDownsampledSignal", + static_cast<void (PeriodicCall::*)(const std::string &, + const unsigned int &)>( + &PeriodicCall::addDownsampledSignal), + "Add the signal to the refresh list\n" + "The downsampling factor: 1 means every time, " + "2 means every other time, etc...", + (bp::arg("signal_name"), "factor")) - .def("rmSignal", &PeriodicCall::rmSignal, "Remove the signal to the refresh list", bp::arg("name")) - .def("clear", &PeriodicCall::clear, "Clear all signals and commands from the refresh list.") - .def("__str__", +[](const PeriodicCall& e) { - std::ostringstream os; - e.display(os); - return os.str(); - }) - ; + .def("rmSignal", &PeriodicCall::rmSignal, + "Remove the signal to the refresh list", bp::arg("name")) + .def("clear", &PeriodicCall::clear, + "Clear all signals and commands from the refresh list.") + .def("__str__", +[](const PeriodicCall &e) { + std::ostringstream os; + e.display(os); + return os.str(); + }); dynamicgraph::python::exposeEntity<dgs::Device>() - .add_property("after", - bp::make_function(&dgs::Device::periodicCallAfter, reference_existing_object())) - .def_readonly("before", - bp::make_function(&dgs::Device::periodicCallBefore, reference_existing_object())) - ; + .add_property("after", bp::make_function(&dgs::Device::periodicCallAfter, + reference_existing_object())) + .def_readonly("before", + bp::make_function(&dgs::Device::periodicCallBefore, + reference_existing_object())); using dgs::Flags; bp::class_<Flags>("Flags", bp::init<>()) - .def(bp::init<const char*>()) - .def("__init__", bp::make_constructor(+[](bp::list bools) { - std::vector<bool> flags (bp::len(bools)); - for (std::size_t i = 0; i < flags.size(); ++i) flags[i] = bp::extract<bool>(bools[i]); - return new Flags(flags); - })) - .def("__init__", bp::make_constructor(+[](bp::tuple bools) { - std::vector<bool> flags (bp::len(bools)); - for (std::size_t i = 0; i < flags.size(); ++i) flags[i] = bp::extract<bool>(bools[i]); - return new Flags(flags); - })) - .def("add", &Flags::add) - .def("set", &Flags::set) - .def("unset", &Flags::unset) + .def(bp::init<const char *>()) + .def("__init__", bp::make_constructor(+[](bp::list bools) { + std::vector<bool> flags(bp::len(bools)); + for (std::size_t i = 0; i < flags.size(); ++i) + flags[i] = bp::extract<bool>(bools[i]); + return new Flags(flags); + })) + .def("__init__", bp::make_constructor(+[](bp::tuple bools) { + std::vector<bool> flags(bp::len(bools)); + for (std::size_t i = 0; i < flags.size(); ++i) + flags[i] = bp::extract<bool>(bools[i]); + return new Flags(flags); + })) + .def("add", &Flags::add) + .def("set", &Flags::set) + .def("unset", &Flags::unset) - .def(bp::self & bp::self) - .def(bp::self | bp::self) - .def(bp::self &= bp::self) - .def(bp::self |= bp::self) + .def(bp::self & bp::self) + .def(bp::self | bp::self) + .def(bp::self &= bp::self) + .def(bp::self |= bp::self) - .def("__call__", &Flags::operator()) - .def("__bool__", &Flags::operator bool) - .def("reversed", &Flags::operator!) + .def("__call__", &Flags::operator()) + .def("__bool__", &Flags::operator bool) + .def("reversed", &Flags::operator!) - .def("set", +[](Flags& f, const std::string& s) { - std::istringstream is (s); - is >> f; - }) - .def("__str__", +[](const Flags& f) { - std::ostringstream os; - os << f; - return os.str(); - }) - ; + .def("set", + +[](Flags &f, const std::string &s) { + std::istringstream is(s); + is >> f; + }) + .def("__str__", +[](const Flags &f) { + std::ostringstream os; + os << f; + return os.str(); + }); dg::python::exposeSignalsOfType<Flags, int>("Flags"); } diff --git a/src/signal/signal-cast.cpp b/src/signal/signal-cast.cpp index d24340e7e1fd7c11c0f0232e16d7ba5457b231f2..c20bf06e5972a4161112557bf012cc33adf62cc8 100644 --- a/src/signal/signal-cast.cpp +++ b/src/signal/signal-cast.cpp @@ -36,8 +36,8 @@ namespace dgsot = dynamicgraph::sot; /* --- CASTER IMPLEMENTATION ------------------------------------------------ */ /* --- CASTER IMPLEMENTATION ------------------------------------------------ */ -//DG_SIGNAL_CAST_DEFINITION(sot::Flags); -//DG_ADD_CASTER(sot::Flags, flags); +// DG_SIGNAL_CAST_DEFINITION(sot::Flags); +// DG_ADD_CASTER(sot::Flags, flags); /* --- TIMEVAL -------------------------------------------------------------- */ /* --- TIMEVAL -------------------------------------------------------------- */ diff --git a/src/tools/joint-trajectory-entity-python.h b/src/tools/joint-trajectory-entity-python.h index 26ee4a939bd8575bfd87775038979bd0c8f1b825..f4307f8cb5410830420dba5365c7aaba1fe310c7 100644 --- a/src/tools/joint-trajectory-entity-python.h +++ b/src/tools/joint-trajectory-entity-python.h @@ -1,3 +1,4 @@ #include <sot/core/joint-trajectory-entity.hh> -typedef boost::mpl::vector<dynamicgraph::sot::SotJointTrajectoryEntity> entities_t; +typedef boost::mpl::vector<dynamicgraph::sot::SotJointTrajectoryEntity> + entities_t; diff --git a/src/tools/switch-python-module-py.cc b/src/tools/switch-python-module-py.cc index 444352458079ffe17cbd8336cad81bf1dfd430e2..0c9f87c8d52db9380939234bffb30dc0916d8fe7 100644 --- a/src/tools/switch-python-module-py.cc +++ b/src/tools/switch-python-module-py.cc @@ -2,32 +2,29 @@ #include <sot/core/switch.hh> namespace dg = dynamicgraph; -typedef bp::return_value_policy<bp::reference_existing_object> reference_existing_object; +typedef bp::return_value_policy<bp::reference_existing_object> + reference_existing_object; -template<typename T, typename Time> -void exposeSwitch() -{ +template <typename T, typename Time> void exposeSwitch() { typedef dg::sot::Switch<T, Time> E_t; typedef typename E_t::Base B_t; - dg::python::exposeEntity<E_t, bp::bases<dg::Entity>, dg::python::AddCommands>() + dg::python::exposeEntity<E_t, bp::bases<dg::Entity>, + dg::python::AddCommands>() .def_readonly("sout", &E_t::SOUT) .def("sin", &B_t::getSignalIn, reference_existing_object()) .add_property("n_sin", &B_t::getSignalNumber, &B_t::setSignalNumber, - "the number of input signal.") + "the number of input signal.") .def_readonly("selection", &E_t::selectionSIN) .def_readonly("boolSelection", &E_t::boolSelectionSIN) .def("setSignalNumber", &B_t::setSignalNumber, - "set the number of input signal.", bp::arg("size")) + "set the number of input signal.", bp::arg("size")) .def("getSignalNumber", &B_t::getSignalNumber, - "get the number of input signal.", bp::arg("size")) - ; + "get the number of input signal.", bp::arg("size")); } -BOOST_PYTHON_MODULE(wrap) -{ - exposeSwitch <bool, int> (); - exposeSwitch <dg::Vector, int> (); - exposeSwitch <dg::sot::MatrixHomogeneous, int> (); - +BOOST_PYTHON_MODULE(wrap) { + exposeSwitch<bool, int>(); + exposeSwitch<dg::Vector, int>(); + exposeSwitch<dg::sot::MatrixHomogeneous, int>(); } diff --git a/src/tools/timer-python.h b/src/tools/timer-python.h index be2cbe57583663bbcc13d42409bcaace04f505e5..2e57bf6ee4b28e7ec24fb6e4b87a2f9efbc7bc4e 100644 --- a/src/tools/timer-python.h +++ b/src/tools/timer-python.h @@ -1,9 +1,7 @@ -#include <sot/core/timer.hh> #include <sot/core/matrix-geometry.hh> +#include <sot/core/timer.hh> typedef boost::mpl::vector< - Timer<dynamicgraph::Vector> -, Timer<dynamicgraph::Matrix> -, Timer<dynamicgraph::sot::MatrixHomogeneous> -, Timer<double> -> entities_t; + Timer<dynamicgraph::Vector>, Timer<dynamicgraph::Matrix>, + Timer<dynamicgraph::sot::MatrixHomogeneous>, Timer<double> > + entities_t; diff --git a/src/tools/type-name-helper.hh b/src/tools/type-name-helper.hh index f7c92eba1e2cc01225601881bf4f051059792074..61c6308ff9f7c98f0e2cfe36515a3b29e2d3c231 100644 --- a/src/tools/type-name-helper.hh +++ b/src/tools/type-name-helper.hh @@ -17,10 +17,14 @@ template <typename TypeRef> struct TypeNameHelper { static inline std::string typeName(); }; template <typename TypeRef> -inline std::string TypeNameHelper<TypeRef>::typeName() { return "unspecified"; } +inline std::string TypeNameHelper<TypeRef>::typeName() { + return "unspecified"; +} #define ADD_KNOWN_TYPE(typeid) \ - template <> inline std::string TypeNameHelper<typeid>::typeName () { return #typeid; } + template <> inline std::string TypeNameHelper<typeid>::typeName() { \ + return #typeid; \ + } ADD_KNOWN_TYPE(bool) ADD_KNOWN_TYPE(double)