forked from pz4kybsvg/Conception
You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
22 lines
530 B
22 lines
530 B
#pragma once
|
|
|
|
#include <Eigen/Dense>
|
|
|
|
#include "drake/common/constants.h"
|
|
#include "drake/common/eigen_types.h"
|
|
|
|
namespace drake {
|
|
namespace math {
|
|
template <typename Derived>
|
|
drake::Matrix3<typename Derived::Scalar> VectorToSkewSymmetric(
|
|
const Eigen::MatrixBase<Derived>& p) {
|
|
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Eigen::MatrixBase<Derived>, 3);
|
|
|
|
drake::Matrix3<typename Derived::Scalar> ret;
|
|
ret << 0.0, -p(2), p(1), p(2), 0.0, -p(0), -p(1), p(0), 0.0;
|
|
return ret;
|
|
}
|
|
|
|
} // namespace math
|
|
} // namespace drake
|