33 #ifndef DART_MATH_CONFIGURATIONSPACE_HPP_
34 #define DART_MATH_CONFIGURATIONSPACE_HPP_
36 #include <Eigen/Dense>
44 template <std::
size_t Dimension>
47 static constexpr std::size_t
NumDofs = Dimension;
52 using Point = Eigen::Matrix<double, NumDofs, 1>;
54 using Vector = Eigen::Matrix<double, NumDofs, 1>;
55 using Matrix = Eigen::Matrix<double, NumDofs, NumDofs>;
66 template <std::
size_t Dimension>
68 template <std::
size_t Dimension>
79 static constexpr std::size_t
NumDofs = 3u;
94 static constexpr std::size_t
NumDofs = 6u;
99 using Point = Eigen::Isometry3d;
111 template <
typename SpaceT>
112 typename SpaceT::Matrix
inverse(
const typename SpaceT::Matrix& mat);
115 template <
typename SpaceT>
117 const typename SpaceT::Point& point);
120 template <
typename SpaceT>
122 const typename SpaceT::EuclideanPoint& point);
125 template <
typename SpaceT>
127 const typename SpaceT::Point& pos,
128 const typename SpaceT::Vector& vel,
132 template <
typename SpaceT>
134 const typename SpaceT::Vector& vel,
135 const typename SpaceT::Vector& acc,
Matrix< double, 6, 1 > Vector6d
Definition: MathTypes.hpp:49
Matrix< double, 6, 6 > Matrix6d
Definition: MathTypes.hpp:50
SpaceT::Matrix inverse(const typename SpaceT::Matrix &mat)
Definition: ConfigurationSpace.hpp:187
SpaceT::EuclideanPoint toEuclideanPoint(const typename SpaceT::Point &point)
Definition: ConfigurationSpace.hpp:199
SpaceT::Point toManifoldPoint(const typename SpaceT::EuclideanPoint &point)
Definition: ConfigurationSpace.hpp:207
SpaceT::Vector integrateVelocity(const typename SpaceT::Vector &vel, const typename SpaceT::Vector &acc, double dt)
Definition: ConfigurationSpace.hpp:225
SpaceT::Point integratePosition(const typename SpaceT::Point &pos, const typename SpaceT::Vector &vel, double dt)
Definition: ConfigurationSpace.hpp:215
Definition: BulletCollisionDetector.cpp:65
Definition: ConfigurationSpace.hpp:107
Definition: ConfigurationSpace.hpp:46
Eigen::Matrix< double, NumDofs, NumDofs > Matrix
Definition: ConfigurationSpace.hpp:55
Eigen::Matrix< double, NumDofs, 1 > Point
Definition: ConfigurationSpace.hpp:52
Eigen::Matrix< double, NumDofs, 1 > Vector
Definition: ConfigurationSpace.hpp:54
static constexpr int NumDofsEigen
Definition: ConfigurationSpace.hpp:48
Eigen::Matrix< double, NumDofs, 1 > EuclideanPoint
Definition: ConfigurationSpace.hpp:53
Eigen::Matrix< double, 6, NumDofs > JacobianMatrix
Definition: ConfigurationSpace.hpp:56
static constexpr std::size_t NumDofs
Definition: ConfigurationSpace.hpp:47
Definition: ConfigurationSpace.hpp:93
Eigen::Vector6d EuclideanPoint
Definition: ConfigurationSpace.hpp:100
static constexpr std::size_t NumDofs
Definition: ConfigurationSpace.hpp:94
Eigen::Isometry3d Point
Definition: ConfigurationSpace.hpp:99
static constexpr int NumDofsEigen
Definition: ConfigurationSpace.hpp:95
Eigen::Matrix6d JacobianMatrix
Definition: ConfigurationSpace.hpp:103
Eigen::Matrix6d Matrix
Definition: ConfigurationSpace.hpp:102
Eigen::Vector6d Vector
Definition: ConfigurationSpace.hpp:101
Definition: ConfigurationSpace.hpp:78
static constexpr std::size_t NumDofs
Definition: ConfigurationSpace.hpp:79
Eigen::Vector3d EuclideanPoint
Definition: ConfigurationSpace.hpp:85
Eigen::Vector3d Vector
Definition: ConfigurationSpace.hpp:86
static constexpr int NumDofsEigen
Definition: ConfigurationSpace.hpp:80
Eigen::Matrix< double, 6, NumDofs > JacobianMatrix
Definition: ConfigurationSpace.hpp:88
Eigen::Matrix3d Matrix
Definition: ConfigurationSpace.hpp:87
Eigen::Matrix3d Point
Definition: ConfigurationSpace.hpp:84