Drake
|
#include <stdexcept>
#include <vector>
#include <utility>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <unordered_map>
#include "drake/common/drake_assert.h"
#include "drake/drakeUtil_export.h"
Go to the source code of this file.
Typedefs | |
template<typename Key , typename T > | |
using | eigen_aligned_unordered_map = std::unordered_map< Key, T, std::hash< Key >, std::equal_to< Key >, Eigen::aligned_allocator< std::pair< Key const, T > > > |
Functions | |
template<typename Derived > | |
void | sizecheck (const Eigen::MatrixBase< Derived > &mat, size_t rows, size_t cols) |
template<size_t Rows, size_t Cols, typename Derived > | |
void | eigenToCArrayOfArrays (const Eigen::MatrixBase< Derived > &source, double(&destination)[Rows][Cols]) |
template<size_t Size, typename Derived > | |
void | eigenVectorToCArray (const Eigen::MatrixBase< Derived > &source, double(&destination)[Size]) |
template<typename Derived > | |
void | eigenVectorToStdVector (const Eigen::MatrixBase< Derived > &source, std::vector< typename Derived::Scalar > &destination) |
template<typename Derived > | |
void | eigenToStdVectorOfStdVectors (const Eigen::MatrixBase< Derived > &source, std::vector< std::vector< typename Derived::Scalar > > &destination) |
template<typename T > | |
void | addOffset (std::vector< T > &v, const T &offset) |
DRAKEUTIL_EXPORT void | baseZeroToBaseOne (std::vector< int > &vec) |
DRAKEUTIL_EXPORT double | angleAverage (double theta1, double theta2) |
template<typename DerivedTorque , typename DerivedForce , typename DerivedNormal , typename DerivedPoint > | |
DRAKEUTIL_EXPORT std::pair< Eigen::Vector3d, double > | resolveCenterOfPressure (const Eigen::MatrixBase< DerivedTorque > &torque, const Eigen::MatrixBase< DerivedForce > &force, const Eigen::MatrixBase< DerivedNormal > &normal, const Eigen::MatrixBase< DerivedPoint > &point_on_contact_plane) |
template<typename DerivedA , typename DerivedB , typename DerivedQ , typename DerivedR , typename DerivedX > | |
void | care (Eigen::MatrixBase< DerivedA > const &A, Eigen::MatrixBase< DerivedB > const &B, Eigen::MatrixBase< DerivedQ > const &Q, Eigen::MatrixBase< DerivedR > const &R, Eigen::MatrixBase< DerivedX > &X) |
template<typename DerivedA , typename DerivedB , typename DerivedQ , typename DerivedR , typename DerivedK , typename DerivedS > | |
void | lqr (Eigen::MatrixBase< DerivedA > const &A, Eigen::MatrixBase< DerivedB > const &B, Eigen::MatrixBase< DerivedQ > const &Q, Eigen::MatrixBase< DerivedR > const &R, Eigen::MatrixBase< DerivedK > &K, Eigen::MatrixBase< DerivedS > &S) |
using eigen_aligned_unordered_map = std::unordered_map<Key, T, std::hash<Key>, std::equal_to<Key>, Eigen::aligned_allocator<std::pair<Key const, T> > > |
void addOffset | ( | std::vector< T > & | v, |
const T & | offset | ||
) |
DRAKEUTIL_EXPORT void baseZeroToBaseOne | ( | std::vector< int > & | vec | ) |
void care | ( | Eigen::MatrixBase< DerivedA > const & | A, |
Eigen::MatrixBase< DerivedB > const & | B, | ||
Eigen::MatrixBase< DerivedQ > const & | Q, | ||
Eigen::MatrixBase< DerivedR > const & | R, | ||
Eigen::MatrixBase< DerivedX > & | X | ||
) |
void eigenToCArrayOfArrays | ( | const Eigen::MatrixBase< Derived > & | source, |
double(&) | destination[Rows][Cols] | ||
) |
void eigenToStdVectorOfStdVectors | ( | const Eigen::MatrixBase< Derived > & | source, |
std::vector< std::vector< typename Derived::Scalar > > & | destination | ||
) |
void eigenVectorToCArray | ( | const Eigen::MatrixBase< Derived > & | source, |
double(&) | destination[Size] | ||
) |
void eigenVectorToStdVector | ( | const Eigen::MatrixBase< Derived > & | source, |
std::vector< typename Derived::Scalar > & | destination | ||
) |
void lqr | ( | Eigen::MatrixBase< DerivedA > const & | A, |
Eigen::MatrixBase< DerivedB > const & | B, | ||
Eigen::MatrixBase< DerivedQ > const & | Q, | ||
Eigen::MatrixBase< DerivedR > const & | R, | ||
Eigen::MatrixBase< DerivedK > & | K, | ||
Eigen::MatrixBase< DerivedS > & | S | ||
) |
DRAKEUTIL_EXPORT std::pair<Eigen::Vector3d, double> resolveCenterOfPressure | ( | const Eigen::MatrixBase< DerivedTorque > & | torque, |
const Eigen::MatrixBase< DerivedForce > & | force, | ||
const Eigen::MatrixBase< DerivedNormal > & | normal, | ||
const Eigen::MatrixBase< DerivedPoint > & | point_on_contact_plane | ||
) |
|
inline |