#include <DynamicsUtilities.h>
Public Member Functions | |
DynamicsUtilities () | |
Constructor for DynamicsUtilities. | |
bool | isBaseFrame (const std::vector< std::string > baseFrames, const std::string frame) |
Check to see if the frame matches the index in the baseFrames vector. | |
double | mag (const KDL::Vector &vec) |
void | MultiplyJacobianTranspose (const KDL::Jacobian &jac, const KDL::Wrench &src, KDL::JntArray &dest) |
Multiply wrench by Jacobian transpose matrix. | |
int | MultiplyJacobianTransposeInverse (const KDL::Jacobian &jac, const KDL::JntArray &src, KDL::Wrench &dest) |
Multiply wrench by Jacobian transpose inverse matrix. | |
KDL::Wrench | SumWrenches (const std::vector< KDL::Wrench > &f_center) |
Sum Wrenches. | |
~DynamicsUtilities () |
Definition at line 11 of file DynamicsUtilities.h.
Constructor for DynamicsUtilities.
Definition at line 10 of file DynamicsUtilities.cpp.
Definition at line 14 of file DynamicsUtilities.cpp.
bool DynamicsUtilities::isBaseFrame | ( | const std::vector< std::string > | baseFrames, |
const std::string | frame | ||
) |
Check to see if the frame matches the index in the baseFrames vector.
baseFrames | |
frame |
Definition at line 25 of file DynamicsUtilities.cpp.
double DynamicsUtilities::mag | ( | const KDL::Vector & | vec | ) |
Definition at line 153 of file DynamicsUtilities.cpp.
void DynamicsUtilities::MultiplyJacobianTranspose | ( | const KDL::Jacobian & | jac, |
const KDL::Wrench & | src, | ||
KDL::JntArray & | dest | ||
) |
Multiply wrench by Jacobian transpose matrix.
jac | Jacobian matrix |
src | Wrench vector |
dest | Product of multiplication |
Definition at line 45 of file DynamicsUtilities.cpp.
int DynamicsUtilities::MultiplyJacobianTransposeInverse | ( | const KDL::Jacobian & | jac, |
const KDL::JntArray & | src, | ||
KDL::Wrench & | dest | ||
) |
Multiply wrench by Jacobian transpose inverse matrix.
jac | Jacobian matrix |
src | Wrench vector |
dest | Product of multiplication |
Definition at line 76 of file DynamicsUtilities.cpp.
KDL::Wrench DynamicsUtilities::SumWrenches | ( | const std::vector< KDL::Wrench > & | forceCenter | ) |
Sum Wrenches.
forceCenter | Input wrench values |
Definition at line 171 of file DynamicsUtilities.cpp.