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

@@ -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<double>;
 //==============================================================================
 template <typename S>
 InterpMotion<S>::InterpMotion()
-  : MotionBase<S>(), angular_axis(Vector3<S>::UnitX())
-{
-  // Default angular velocity is zero
-  angular_vel = 0;
-
-  // Default reference point is local zero point
-
-  // Default linear velocity is zero
-}
+    : InterpMotion(Transform3<S>::Identity(), Transform3<S>::Identity(),
+                   Vector3<S>::Zero()) {}
 
 //==============================================================================
 template <typename S>
 InterpMotion<S>::InterpMotion(
     const Matrix3<S>& R1, const Vector3<S>& T1,
     const Matrix3<S>& R2, const Vector3<S>& T2)
-  : MotionBase<S>(),
-    tf1(Transform3<S>::Identity()),
-    tf2(Transform3<S>::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<S>::Zero()) {}
 
 //==============================================================================
 template <typename S>
 InterpMotion<S>::InterpMotion(
     const Transform3<S>& tf1_, const Transform3<S>& tf2_)
-  : MotionBase<S>(), tf1(tf1_), tf2(tf2_), tf(tf1)
-{
-  // Compute the velocities for the motion
-  computeVelocity();
-}
+  : InterpMotion<S>(tf1_, tf2_, Vector3<S>::Zero()) {}
 
 //==============================================================================
 template <typename S>
@@ -117,13 +91,22 @@ InterpMotion<S>::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 <typename S>
-InterpMotion<S>::InterpMotion(
-    const Transform3<S>& tf1_, const Transform3<S>& tf2_, const Vector3<S>& O)
-  : MotionBase<S>(), tf1(tf1_), tf2(tf2_), tf(tf1), reference_p(O)
-{
-  // Do nothing
-}
+InterpMotion<S>::InterpMotion(const Transform3<S>& tf1_,
+                              const Transform3<S>& tf2_, const Vector3<S>& O)
+    : InterpMotion(tf1_.linear(), tf1_.translation(), tf2_.linear(),
+                   tf2_.translation(), O) {}
 
 //==============================================================================
 template <typename S>

include/fcl/math/motion/interp_motion.h

@@ -90,7 +90,6 @@ class FCL_EXPORT InterpMotion : public MotionBase<S>
   void getTaylorModel(TMatrix3<S>& tm, TVector3<S>& tv) const;
 
 protected:
-
   void computeVelocity();
 
   Quaternion<S> deltaRotation(S dt) const;

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)

test/math/CMakeLists.txt

@@ -0,0 +1 @@
+add_subdirectory(motion)

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)

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 ([email protected]) (2020) */
+
+#include "fcl/math/motion/interp_motion.h"
+
+#include <gtest/gtest.h>
+
+#include "eigen_matrix_compare.h"
+
+namespace fcl {
+namespace {
+
+// TODO(SeanCurtis-TRI): Convert this to a parameterized test harness and use
+// TEST_P.
+
+template <typename S>
+class InterpMotionTest : public ::testing::Test {};
+
+typedef testing::Types<double, float> MyTypes;
+TYPED_TEST_CASE(InterpMotionTest, MyTypes);
+
+TYPED_TEST(InterpMotionTest, Construction) {
+  using S = TypeParam;
+  const Transform3<S> I = Transform3<S>::Identity();
+
+  {
+    // Default constructor; everything should be identity.
+    InterpMotion<S> motion{};
+
+    Transform3<S> X_FA;
+    motion.getCurrentTransform(X_FA);
+    EXPECT_TRUE(CompareMatrices(X_FA.matrix(), I.matrix()));
+    EXPECT_TRUE(
+        CompareMatrices(motion.getLinearVelocity(), Vector3<S>::Zero()));
+    EXPECT_EQ(motion.getAngularVelocity(), S(0));
+    EXPECT_TRUE(CompareMatrices(motion.getAngularAxis(), Vector3<S>::UnitX()));
+    EXPECT_TRUE(
+        CompareMatrices(motion.getReferencePoint(), Vector3<S>::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();
+}