diff --git a/src/spatial/motion-ref.hpp b/src/spatial/motion-ref.hpp index 9f891e5293bb114995fa37f075598cbe55209f0c..929ec7adc600885f3dd5be0e9f3fceb7ff4ad094 100644 --- a/src/spatial/motion-ref.hpp +++ b/src/spatial/motion-ref.hpp @@ -1,6 +1,6 @@ // -// Copyright (c) 2017 CNRS +// Copyright (c) 2017-2018 CNRS // // This file is part of Pinocchio // Pinocchio is free software: you can redistribute it @@ -71,6 +71,14 @@ namespace se3 MOTION_TYPEDEF_TPL(MotionRef); using Base::operator=; + using Base::linear; + using Base::angular; + + using Base::__plus__; + using Base::__minus__; + using Base::__pequ__; + using Base::__mequ__; + using Base::__mult__; MotionRef(const Eigen::MatrixBase<Vector6ArgType> & v_like) : ref(const_cast<Vector6ArgType &>(v_like.derived())) @@ -103,6 +111,43 @@ namespace se3 linear_impl()=v; } + // Specific operators for MotionTpl and MotionRef + template<typename S1, int O1> + MotionPlain __plus__(const MotionTpl<S1,O1> & v) const + { return MotionPlain(ref+v.toVector()); } + + template<typename Vector6Like> + MotionPlain __plus__(const MotionRef<Vector6ArgType> & v) const + { return MotionPlain(ref+v.toVector()); } + + template<typename S1, int O1> + MotionPlain __minus__(const MotionTpl<S1,O1> & v) const + { return MotionPlain(ref-v.toVector()); } + + template<typename Vector6Like> + MotionPlain __minus__(const MotionRef<Vector6ArgType> & v) const + { return MotionPlain(ref-v.toVector()); } + + template<typename S1, int O1> + MotionRef & __pequ__(const MotionTpl<S1,O1> & v) + { ref += v.toVector(); return *this; } + + template<typename Vector6Like> + MotionRef & __pequ__(const MotionRef<Vector6ArgType> & v) + { ref += v.toVector(); return *this; } + + template<typename S1, int O1> + MotionRef & __mequ__(const MotionTpl<S1,O1> & v) + { ref -= v.toVector(); return *this; } + + template<typename Vector6Like> + MotionRef & __mequ__(const MotionRef<Vector6ArgType> & v) + { ref -= v.toVector(); return *this; } + + template<typename OtherScalar> + MotionPlain __mult__(const OtherScalar & alpha) const + { return MotionPlain(alpha*ref); } + protected: DataRefType ref; diff --git a/src/spatial/motion-tpl.hpp b/src/spatial/motion-tpl.hpp index ba168d8b3399b18c0ef598c9ba2c2043558f91db..0ba70db646de30be74d821d424348eb43d56c56b 100644 --- a/src/spatial/motion-tpl.hpp +++ b/src/spatial/motion-tpl.hpp @@ -1,5 +1,5 @@ // -// Copyright (c) 2015-2017 CNRS +// Copyright (c) 2015-2018 CNRS // Copyright (c) 2015-2016 Wandercraft, 86 rue de Paris 91400 Orsay, France. // // This file is part of Pinocchio @@ -55,6 +55,12 @@ namespace se3 using Base::linear; using Base::angular; + using Base::__plus__; + using Base::__minus__; + using Base::__pequ__; + using Base::__mequ__; + using Base::__mult__; + // Constructors MotionTpl() : data() {} @@ -109,6 +115,43 @@ namespace se3 linear_impl()=v; } + // Specific operators for MotionTpl and MotionRef + template<typename S2, int O2> + MotionPlain __plus__(const MotionTpl<S2,O2> & v) const + { return MotionPlain(data+v.toVector()); } + + template<typename Vector6ArgType> + MotionPlain __plus__(const MotionRef<Vector6ArgType> & v) const + { return MotionPlain(data+v.toVector()); } + + template<typename S2, int O2> + MotionPlain __minus__(const MotionTpl<S2,O2> & v) const + { return MotionPlain(data-v.toVector()); } + + template<typename Vector6ArgType> + MotionPlain __minus__(const MotionRef<Vector6ArgType> & v) const + { return MotionPlain(data-v.toVector()); } + + template<typename S2, int O2> + MotionTpl & __pequ__(const MotionTpl<S2,O2> & v) + { data += v.toVector(); return *this; } + + template<typename Vector6ArgType> + MotionTpl & __pequ__(const MotionRef<Vector6ArgType> & v) + { data += v.toVector(); return *this; } + + template<typename S2, int O2> + MotionTpl & __mequ__(const MotionTpl<S2,O2> & v) + { data -= v.toVector(); return *this; } + + template<typename Vector6ArgType> + MotionTpl & __mequ__(const MotionRef<Vector6ArgType> & v) + { data -= v.toVector(); return *this; } + + template<typename OtherScalar> + MotionPlain __mult__(const OtherScalar & alpha) const + { return MotionPlain(alpha*data); } + protected: Vector6 data;