special-orthogonal.hh 1.9 KB
 Joseph Mirabel committed Jan 05, 2017 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 ``````// Copyright (c) 2017, Joseph Mirabel // Authors: Joseph Mirabel (joseph.mirabel@laas.fr) // // This file is part of hpp-pinocchio. // hpp-pinocchio is free software: you can redistribute it // and/or modify it under the terms of the GNU Lesser General Public // License as published by the Free Software Foundation, either version // 3 of the License, or (at your option) any later version. // // hpp-pinocchio is distributed in the hope that it will be // useful, but WITHOUT ANY WARRANTY; without even the implied warranty // of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU // General Lesser Public License for more details. You should have // received a copy of the GNU Lesser General Public License along with // hpp-pinocchio. If not, see . #ifndef HPP_PINOCCHIO_LIEGROUP_SPECIAL_ORTHOGONAL_OPERATION_HH #define HPP_PINOCCHIO_LIEGROUP_SPECIAL_ORTHOGONAL_OPERATION_HH #include namespace hpp { namespace pinocchio { namespace liegroup { template struct SpecialOrthogonalOperation : public se3::SpecialOrthogonalOperation { template static double squaredDistance( const Eigen::MatrixBase & q0, const Eigen::MatrixBase & q1) { return se3::SpecialOrthogonalOperation::squaredDistance(q0, q1); } template static double squaredDistance( const Eigen::MatrixBase & q0, const Eigen::MatrixBase & q1, const typename ConfigL_t::Scalar& w) { `````` Joseph Mirabel committed Jan 05, 2017 42 `````` return w * squaredDistance(q0, q1); `````` Joseph Mirabel committed Jan 05, 2017 43 44 45 46 47 48 49 `````` } }; } // namespace liegroup } // namespace pinocchio } // namespace hpp #endif // HPP_PINOCCHIO_LIEGROUP_SPECIAL_ORTHOGONAL_OPERATION_HH``````