1 /* 2 * Copyright (c) 2011-2021, The DART development contributors 3 * All rights reserved. 4 * 5 * The list of contributors can be found at: 6 * https://github.com/dartsim/dart/blob/master/LICENSE 7 * 8 * This file is provided under the following "BSD-style" License: 9 * Redistribution and use in source and binary forms, with or 10 * without modification, are permitted provided that the following 11 * conditions are met: 12 * * Redistributions of source code must retain the above copyright 13 * notice, this list of conditions and the following disclaimer. 14 * * Redistributions in binary form must reproduce the above 15 * copyright notice, this list of conditions and the following 16 * disclaimer in the documentation and/or other materials provided 17 * with the distribution. 18 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND 19 * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, 20 * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF 21 * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 23 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 24 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 25 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF 26 * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 * POSSIBILITY OF SUCH DAMAGE. 31 */ 32 33 #ifndef DART_MATH_CONFIGURATIONSPACE_HPP_ 34 #define DART_MATH_CONFIGURATIONSPACE_HPP_ 35 36 #include <Eigen/Dense> 37 #include "dart/math/Geometry.hpp" 38 #include "dart/math/MathTypes.hpp" 39 40 namespace dart { 41 namespace math { 42 43 //============================================================================== 44 template <std::size_t Dimension> 45 struct RealVectorSpace 46 { 47 static constexpr std::size_t NumDofs = Dimension; 48 static constexpr int NumDofsEigen = static_cast<int>(Dimension); 49 50 using TangentSpace = RealVectorSpace<NumDofs>; 51 52 using Point = Eigen::Matrix<double, NumDofs, 1>; 53 using EuclideanPoint = Eigen::Matrix<double, NumDofs, 1>; 54 using Vector = Eigen::Matrix<double, NumDofs, 1>; 55 using Matrix = Eigen::Matrix<double, NumDofs, NumDofs>; 56 using JacobianMatrix = Eigen::Matrix<double, 6, NumDofs>; 57 }; 58 59 //============================================================================== 60 // 61 // These namespace-level definitions are required to enable ODR-use of static 62 // constexpr member variables. 63 // 64 // See this StackOverflow answer: http://stackoverflow.com/a/14396189/111426 65 // 66 template <std::size_t Dimension> 67 constexpr std::size_t RealVectorSpace<Dimension>::NumDofs; 68 template <std::size_t Dimension> 69 constexpr int RealVectorSpace<Dimension>::NumDofsEigen; 70 71 using NullSpace = RealVectorSpace<0u>; 72 using R1Space = RealVectorSpace<1u>; 73 using R2Space = RealVectorSpace<2u>; 74 using R3Space = RealVectorSpace<3u>; 75 76 //============================================================================== 77 struct SO3Space 78 { 79 static constexpr std::size_t NumDofs = 3u; 80 static constexpr int NumDofsEigen = 3; 81 82 using TangentSpace = RealVectorSpace<NumDofs>; 83 84 using Point = Eigen::Matrix3d; 85 using EuclideanPoint = Eigen::Vector3d; 86 using Vector = Eigen::Vector3d; 87 using Matrix = Eigen::Matrix3d; 88 using JacobianMatrix = Eigen::Matrix<double, 6, NumDofs>; 89 }; 90 91 //============================================================================== 92 struct SE3Space 93 { 94 static constexpr std::size_t NumDofs = 6u; 95 static constexpr int NumDofsEigen = 6; 96 97 using TangentSpace = RealVectorSpace<NumDofs>; 98 99 using Point = Eigen::Isometry3d; 100 using EuclideanPoint = Eigen::Vector6d; 101 using Vector = Eigen::Vector6d; 102 using Matrix = Eigen::Matrix6d; 103 using JacobianMatrix = Eigen::Matrix6d; 104 }; 105 106 struct MapsToManifoldPoint 107 { 108 }; 109 110 //============================================================================== 111 template <typename SpaceT> 112 typename SpaceT::Matrix inverse(const typename SpaceT::Matrix& mat); 113 114 //============================================================================== 115 template <typename SpaceT> 116 typename SpaceT::EuclideanPoint toEuclideanPoint( 117 const typename SpaceT::Point& point); 118 119 //============================================================================== 120 template <typename SpaceT> 121 typename SpaceT::Point toManifoldPoint( 122 const typename SpaceT::EuclideanPoint& point); 123 124 //============================================================================== 125 template <typename SpaceT> 126 typename SpaceT::Point integratePosition( 127 const typename SpaceT::Point& pos, 128 const typename SpaceT::Vector& vel, 129 double dt); 130 131 //============================================================================== 132 template <typename SpaceT> 133 typename SpaceT::Vector integrateVelocity( 134 const typename SpaceT::Vector& vel, 135 const typename SpaceT::Vector& acc, 136 double dt); 137 138 } // namespace math 139 } // namespace dart 140 141 #include "dart/math/detail/ConfigurationSpace.hpp" 142 143 #endif // DART_MATH_CONFIGURATIONSPACE_HPP_ 144