35 lines
1.3 KiB
C++
35 lines
1.3 KiB
C++
/**
|
|
* @file testInertialUtils.cpp
|
|
*
|
|
* @brief Conversions for the utility functions from the matlab implementation
|
|
*
|
|
* @date Nov 28, 2011
|
|
* @author Alex Cunningham
|
|
*/
|
|
|
|
#include <CppUnitLite/TestHarness.h>
|
|
|
|
#include <gtsam_unstable/dynamics/inertialUtils.h>
|
|
|
|
using namespace gtsam;
|
|
|
|
static const double tol = 1e-5;
|
|
|
|
/* ************************************************************************* */
|
|
TEST(testInertialUtils, RRTMbn) {
|
|
EXPECT(assert_equal(Matrix::Identity(3,3), dynamics::RRTMbn(zero(3)), tol));
|
|
EXPECT(assert_equal(Matrix::Identity(3,3), dynamics::RRTMbn(Rot3()), tol));
|
|
EXPECT(assert_equal(dynamics::RRTMbn(Vector_(3, 0.3, 0.2, 0.1)), dynamics::RRTMbn(Rot3::ypr(0.1, 0.2, 0.3)), tol));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST(testInertialUtils, RRTMnb) {
|
|
EXPECT(assert_equal(Matrix::Identity(3,3), dynamics::RRTMnb(zero(3)), tol));
|
|
EXPECT(assert_equal(Matrix::Identity(3,3), dynamics::RRTMnb(Rot3()), tol));
|
|
EXPECT(assert_equal(dynamics::RRTMnb(Vector_(3, 0.3, 0.2, 0.1)), dynamics::RRTMnb(Rot3::ypr(0.1, 0.2, 0.3)), tol));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
|
|
/* ************************************************************************* */
|