33 #ifndef DART_MATH_DETAIL_CONFIGURATIONSPACE_H_
34 #define DART_MATH_DETAIL_CONFIGURATIONSPACE_H_
49 template <
typename MatrixType,
int Size,
typename Enable = Range<true>>
52 static void run(
const MatrixType& matrix, MatrixType&
result)
54 result = matrix.ldlt().solve(MatrixType::Identity());
59 template <
typename MatrixType,
int Size>
62 static void run(
const MatrixType& matrix, MatrixType&
result)
69 template <
typename SpaceT>
72 static typename SpaceT::EuclideanPoint
run(
73 const typename SpaceT::Point& point)
100 x.tail<3>() = point.translation();
107 template <
typename SpaceT>
110 static typename SpaceT::Point
run(
111 const typename SpaceT::EuclideanPoint& point)
135 Eigen::Isometry3d tf(Eigen::Isometry3d::Identity());
138 tf.translation() = point.tail<3>();
145 template <
typename SpaceT>
148 static typename SpaceT::Point
run(
149 const typename SpaceT::Point& pos,
150 const typename SpaceT::Vector& vel,
153 return pos + dt * vel;
166 return pos * toManifoldPoint<SO3Space>(vel * dt);
179 return pos * toManifoldPoint<SE3Space>(vel * dt);
186 template <
typename SpaceT>
187 typename SpaceT::Matrix
inverse(
const typename SpaceT::Matrix& mat)
189 typename SpaceT::Matrix res;
198 template <
typename SpaceT>
200 const typename SpaceT::Point& point)
206 template <
typename SpaceT>
208 const typename SpaceT::EuclideanPoint& point)
214 template <
typename SpaceT>
216 const typename SpaceT::Point& pos,
217 const typename SpaceT::Vector& vel,
224 template <
typename SpaceT>
226 const typename SpaceT::Vector& vel,
227 const typename SpaceT::Vector& acc,
230 return vel + acc * dt;
CollisionResult * result
Collision result of DART.
Definition: FCLCollisionDetector.cpp:160
Matrix< double, 6, 1 > Vector6d
Definition: MathTypes.hpp:49
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
Eigen::Vector3d logMap(const Eigen::Matrix3d &_R)
Log mapping.
Definition: Geometry.cpp:565
SpaceT::Point integratePosition(const typename SpaceT::Point &pos, const typename SpaceT::Vector &vel, double dt)
Definition: ConfigurationSpace.hpp:215
Eigen::Matrix3d expMapRot(const Eigen::Vector3d &_q)
Computes the Rotation matrix from a given expmap vector.
Definition: Geometry.cpp:471
Definition: BulletCollisionDetector.cpp:65
Definition: ConfigurationSpace.hpp:93
Eigen::Vector6d EuclideanPoint
Definition: ConfigurationSpace.hpp:100
Eigen::Isometry3d Point
Definition: ConfigurationSpace.hpp:99
Eigen::Vector6d Vector
Definition: ConfigurationSpace.hpp:101
Definition: ConfigurationSpace.hpp:78
Eigen::Vector3d EuclideanPoint
Definition: ConfigurationSpace.hpp:85
Eigen::Vector3d Vector
Definition: ConfigurationSpace.hpp:86
Eigen::Matrix3d Point
Definition: ConfigurationSpace.hpp:84
Definition: ConfigurationSpace.hpp:46
static SE3Space::Point run(const typename SE3Space::Point &pos, const typename SE3Space::Vector &vel, double dt)
Definition: ConfigurationSpace.hpp:174
static SO3Space::Point run(const typename SO3Space::Point &pos, const typename SO3Space::Vector &vel, double dt)
Definition: ConfigurationSpace.hpp:161
Definition: ConfigurationSpace.hpp:147
static SpaceT::Point run(const typename SpaceT::Point &pos, const typename SpaceT::Vector &vel, double dt)
Definition: ConfigurationSpace.hpp:148
static void run(const MatrixType &matrix, MatrixType &result)
Definition: ConfigurationSpace.hpp:62
Definition: ConfigurationSpace.hpp:51
static void run(const MatrixType &matrix, MatrixType &result)
Definition: ConfigurationSpace.hpp:52
static SE3Space::EuclideanPoint run(const typename SE3Space::Point &point)
Definition: ConfigurationSpace.hpp:94
static SO3Space::EuclideanPoint run(const typename SO3Space::Point &point)
Definition: ConfigurationSpace.hpp:83
Definition: ConfigurationSpace.hpp:71
static SpaceT::EuclideanPoint run(const typename SpaceT::Point &point)
Definition: ConfigurationSpace.hpp:72
static SE3Space::Point run(const typename SE3Space::EuclideanPoint &point)
Definition: ConfigurationSpace.hpp:132
static SO3Space::Point run(const typename SO3Space::EuclideanPoint &point)
Definition: ConfigurationSpace.hpp:121
Definition: ConfigurationSpace.hpp:109
static SpaceT::Point run(const typename SpaceT::EuclideanPoint &point)
Definition: ConfigurationSpace.hpp:110