From 65968c8079a39ad0e85aa2adcdf2d946d5e9a22b Mon Sep 17 00:00:00 2001 From: Sean Curtis Date: Thu, 4 Jun 2020 07:31:57 -0700 Subject: [PATCH] WIP Adding testing (and fixing bugs) InterpMotion has no unit tests at all. This floats on top of pr_476 and adds the skeleton of a unit test. While writing the unit test, I encountered issues: 1. The default constructor (documented to be identity) wasn't. 2. The constructors were incredibly erratic. Some did partial initialization. This commit adds addresses issues as encountered. # Conflicts: # include/fcl/math/motion/interp_motion-inl.h --- include/fcl/math/motion/interp_motion-inl.h | 57 +++++------- include/fcl/math/motion/interp_motion.h | 1 - test/CMakeLists.txt | 3 +- test/math/CMakeLists.txt | 1 + test/math/motion/CMakeLists.txt | 8 ++ test/math/motion/test_interp_motion.cpp | 99 +++++++++++++++++++++ 6 files changed, 130 insertions(+), 39 deletions(-) create mode 100644 test/math/CMakeLists.txt create mode 100644 test/math/motion/CMakeLists.txt create mode 100644 test/math/motion/test_interp_motion.cpp diff --git a/include/fcl/math/motion/interp_motion-inl.h b/include/fcl/math/motion/interp_motion-inl.h index c00684142..fcc334bde 100644 --- a/include/fcl/math/motion/interp_motion-inl.h +++ b/include/fcl/math/motion/interp_motion-inl.h @@ -40,8 +40,7 @@ #include "fcl/math/motion/interp_motion.h" -namespace fcl -{ +namespace fcl { //============================================================================== extern template @@ -50,46 +49,21 @@ class FCL_EXPORT InterpMotion; //============================================================================== template InterpMotion::InterpMotion() - : MotionBase(), angular_axis(Vector3::UnitX()) -{ - // Default angular velocity is zero - angular_vel = 0; - - // Default reference point is local zero point - - // Default linear velocity is zero -} + : InterpMotion(Transform3::Identity(), Transform3::Identity(), + Vector3::Zero()) {} //============================================================================== template InterpMotion::InterpMotion( const Matrix3& R1, const Vector3& T1, const Matrix3& R2, const Vector3& T2) - : MotionBase(), - tf1(Transform3::Identity()), - tf2(Transform3::Identity()) -{ - tf1.linear() = R1; - tf1.translation() = T1; - - tf2.linear() = R2; - tf2.translation() = T2; - - tf = tf1; - - // Compute the velocities for the motion - computeVelocity(); -} + : InterpMotion(R1, T1, R2, T2, Vector3::Zero()) {} //============================================================================== template InterpMotion::InterpMotion( const Transform3& tf1_, const Transform3& tf2_) - : MotionBase(), tf1(tf1_), tf2(tf2_), tf(tf1) -{ - // Compute the velocities for the motion - computeVelocity(); -} + : InterpMotion(tf1_, tf2_, Vector3::Zero()) {} //============================================================================== template @@ -117,13 +91,22 @@ InterpMotion::InterpMotion( } //============================================================================== + +// Note: In the name of using delegating constructors, we introduce the +// silliness below. We *unpack* the transforms tf1_ and tf2_ and just so that we +// can pack them *back* into the member fields. Eigen forces us to do this +// because we *can't* go the other way; there is no constructor of a Transform3 +// based on a Matrix3 and a Vector3. It might be worth creating a utility that +// does that just so this could become more compact. The constructor that this +// delegates to would, instead, do something like: +// InterpMotion(MakeTransform(R1, T1), MakeTransform(R2, T2), O) {} +// And then the core functionality for initializing the class would go into this +// constructor. template -InterpMotion::InterpMotion( - const Transform3& tf1_, const Transform3& tf2_, const Vector3& O) - : MotionBase(), tf1(tf1_), tf2(tf2_), tf(tf1), reference_p(O) -{ - // Do nothing -} +InterpMotion::InterpMotion(const Transform3& tf1_, + const Transform3& tf2_, const Vector3& O) + : InterpMotion(tf1_.linear(), tf1_.translation(), tf2_.linear(), + tf2_.translation(), O) {} //============================================================================== template diff --git a/include/fcl/math/motion/interp_motion.h b/include/fcl/math/motion/interp_motion.h index b3454b779..12586e539 100644 --- a/include/fcl/math/motion/interp_motion.h +++ b/include/fcl/math/motion/interp_motion.h @@ -90,7 +90,6 @@ class FCL_EXPORT InterpMotion : public MotionBase void getTaylorModel(TMatrix3& tm, TVector3& tv) const; protected: - void computeVelocity(); Quaternion deltaRotation(S dt) const; diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 50e1c1c7b..c7d4af92e 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -97,6 +97,7 @@ foreach(test ${tests}) add_fcl_test(${test}) endforeach(test) +add_subdirectory(broadphase) add_subdirectory(geometry) +add_subdirectory(math) add_subdirectory(narrowphase) -add_subdirectory(broadphase) diff --git a/test/math/CMakeLists.txt b/test/math/CMakeLists.txt new file mode 100644 index 000000000..95692c7f1 --- /dev/null +++ b/test/math/CMakeLists.txt @@ -0,0 +1 @@ +add_subdirectory(motion) diff --git a/test/math/motion/CMakeLists.txt b/test/math/motion/CMakeLists.txt new file mode 100644 index 000000000..8d2686e02 --- /dev/null +++ b/test/math/motion/CMakeLists.txt @@ -0,0 +1,8 @@ +set(tests + test_interp_motion.cpp + ) + +# Build all the tests +foreach(test ${tests}) + add_fcl_test(${test}) +endforeach(test) diff --git a/test/math/motion/test_interp_motion.cpp b/test/math/motion/test_interp_motion.cpp new file mode 100644 index 000000000..12ac2be91 --- /dev/null +++ b/test/math/motion/test_interp_motion.cpp @@ -0,0 +1,99 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020. Toyota Research Institute + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of CNRS-LAAS and AIST nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** @author Sean Curtis (sean@tri.global) (2020) */ + +#include "fcl/math/motion/interp_motion.h" + +#include + +#include "eigen_matrix_compare.h" + +namespace fcl { +namespace { + +// TODO(SeanCurtis-TRI): Convert this to a parameterized test harness and use +// TEST_P. + +template +class InterpMotionTest : public ::testing::Test {}; + +typedef testing::Types MyTypes; +TYPED_TEST_CASE(InterpMotionTest, MyTypes); + +TYPED_TEST(InterpMotionTest, Construction) { + using S = TypeParam; + const Transform3 I = Transform3::Identity(); + + { + // Default constructor; everything should be identity. + InterpMotion motion{}; + + Transform3 X_FA; + motion.getCurrentTransform(X_FA); + EXPECT_TRUE(CompareMatrices(X_FA.matrix(), I.matrix())); + EXPECT_TRUE( + CompareMatrices(motion.getLinearVelocity(), Vector3::Zero())); + EXPECT_EQ(motion.getAngularVelocity(), S(0)); + EXPECT_TRUE(CompareMatrices(motion.getAngularAxis(), Vector3::UnitX())); + EXPECT_TRUE( + CompareMatrices(motion.getReferencePoint(), Vector3::Zero())); + } + + { + // Construct from start (R_FS, p_FSo) and goal (R_FG, p_FGo). + } + + { + // Construct from start X_FS and goal X_FG. + } + + { + // Construct from start (R_FS, p_FSo), goal (R_FG, p_FGo), and an origin of + // rotation (p_FO). + } + + { + // Construct from start X_FS, goal X_FG, and rotation origin p_FO. + } +} + +} // namespace +} // namespace fcl + +//============================================================================== +int main(int argc, char *argv[]) { + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +}