From 91241376d922c62d9e9994b20d4a2f2222318ee3 Mon Sep 17 00:00:00 2001 From: Josip Sumecki Date: Wed, 3 Jun 2020 10:42:45 +0200 Subject: [PATCH 1/8] Fixed issue with creating instances of type InterpMotion --- include/fcl/math/motion/interp_motion-inl.h | 5 ++++- include/fcl/math/motion/interp_motion.h | 2 +- 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/include/fcl/math/motion/interp_motion-inl.h b/include/fcl/math/motion/interp_motion-inl.h index 384367a80..ffca75602 100644 --- a/include/fcl/math/motion/interp_motion-inl.h +++ b/include/fcl/math/motion/interp_motion-inl.h @@ -44,6 +44,9 @@ namespace fcl { //============================================================================== +extern template +class FCL_EXPORT InterpMotion; + extern template class FCL_EXPORT InterpMotion; @@ -127,7 +130,7 @@ InterpMotion::InterpMotion( //============================================================================== template -bool InterpMotion::integrate(double dt) const +bool InterpMotion::integrate(S dt) const { if(dt > 1) dt = 1; diff --git a/include/fcl/math/motion/interp_motion.h b/include/fcl/math/motion/interp_motion.h index a7f58eb74..b3454b779 100644 --- a/include/fcl/math/motion/interp_motion.h +++ b/include/fcl/math/motion/interp_motion.h @@ -76,7 +76,7 @@ class FCL_EXPORT InterpMotion : public MotionBase /// @brief Integrate the motion from 0 to dt /// We compute the current transformation from zero point instead of from last integrate time, for precision. - bool integrate(double dt) const; + bool integrate(S dt) const; /// @brief Compute the motion bound for a bounding volume along a given direction n, which is defined in the visitor S computeMotionBound(const BVMotionBoundVisitor& mb_visitor) const; From 3023add2609c5a6bc78043176c8bc18651be88bd Mon Sep 17 00:00:00 2001 From: Josip Sumecki Date: Wed, 3 Jun 2020 11:11:17 +0200 Subject: [PATCH 2/8] Fixed issue with creating instances of type ScrewMotion --- include/fcl/math/motion/screw_motion-inl.h | 5 ++++- include/fcl/math/motion/screw_motion.h | 2 +- 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/include/fcl/math/motion/screw_motion-inl.h b/include/fcl/math/motion/screw_motion-inl.h index 53c5c5bfd..44e46d3e2 100644 --- a/include/fcl/math/motion/screw_motion-inl.h +++ b/include/fcl/math/motion/screw_motion-inl.h @@ -44,6 +44,9 @@ namespace fcl { //============================================================================== +extern template +class FCL_EXPORT ScrewMotion; + extern template class FCL_EXPORT ScrewMotion; @@ -92,7 +95,7 @@ ScrewMotion::ScrewMotion( //============================================================================== template -bool ScrewMotion::integrate(double dt) const +bool ScrewMotion::integrate(S dt) const { if(dt > 1) dt = 1; diff --git a/include/fcl/math/motion/screw_motion.h b/include/fcl/math/motion/screw_motion.h index fce33874e..1099ec385 100644 --- a/include/fcl/math/motion/screw_motion.h +++ b/include/fcl/math/motion/screw_motion.h @@ -65,7 +65,7 @@ class FCL_EXPORT ScrewMotion : public MotionBase /// @brief Integrate the motion from 0 to dt /// We compute the current transformation from zero point instead of from last integrate time, for precision. - bool integrate(double dt) const; + bool integrate(S dt) const; /// @brief Compute the motion bound for a bounding volume along a given direction n, which is defined in the visitor S computeMotionBound(const BVMotionBoundVisitor& mb_visitor) const; From 857d82fd18e94f1c3d0bd9f3b05a714cad0d6a8a Mon Sep 17 00:00:00 2001 From: Josip Sumecki Date: Wed, 3 Jun 2020 12:35:51 +0200 Subject: [PATCH 3/8] Fixed issue with creating continuous collision objects of template type float --- .../narrowphase/continuous_collision-inl.h | 37 +++++++++++++++++++ 1 file changed, 37 insertions(+) diff --git a/include/fcl/narrowphase/continuous_collision-inl.h b/include/fcl/narrowphase/continuous_collision-inl.h index 1adfb6ec2..466ea3fcc 100644 --- a/include/fcl/narrowphase/continuous_collision-inl.h +++ b/include/fcl/narrowphase/continuous_collision-inl.h @@ -55,6 +55,15 @@ namespace fcl { //============================================================================== +extern template +float continuousCollide( + const CollisionGeometry* o1, + const MotionBase* motion1, + const CollisionGeometry* o2, + const MotionBase* motion2, + const ContinuousCollisionRequest& request, + ContinuousCollisionResult& result); + extern template double continuousCollide( const CollisionGeometry* o1, @@ -65,6 +74,17 @@ double continuousCollide( ContinuousCollisionResult& result); //============================================================================== +extern template +float continuousCollide( + const CollisionGeometry* o1, + const Transform3& tf1_beg, + const Transform3& tf1_end, + const CollisionGeometry* o2, + const Transform3& tf2_beg, + const Transform3& tf2_end, + const ContinuousCollisionRequest& request, + ContinuousCollisionResult& result); + extern template double continuousCollide( const CollisionGeometry* o1, @@ -77,6 +97,15 @@ double continuousCollide( ContinuousCollisionResult& result); //============================================================================== +extern template +float continuousCollide( + const CollisionObject* o1, + const Transform3& tf1_end, + const CollisionObject* o2, + const Transform3& tf2_end, + const ContinuousCollisionRequest& request, + ContinuousCollisionResult& result); + extern template double continuousCollide( const CollisionObject* o1, @@ -87,6 +116,13 @@ double continuousCollide( ContinuousCollisionResult& result); //============================================================================== +extern template +float collide( + const ContinuousCollisionObject* o1, + const ContinuousCollisionObject* o2, + const ContinuousCollisionRequest& request, + ContinuousCollisionResult& result); + extern template double collide( const ContinuousCollisionObject* o1, @@ -94,6 +130,7 @@ double collide( const ContinuousCollisionRequest& request, ContinuousCollisionResult& result); + //============================================================================== template detail::ConservativeAdvancementFunctionMatrix& From e90d404e18ef96aa881eb6c6dc5a7c7a9c71e838 Mon Sep 17 00:00:00 2001 From: Josip Sumecki Date: Wed, 3 Jun 2020 13:23:00 +0200 Subject: [PATCH 4/8] Fixed issue with creating collision objects of template type float --- include/fcl/narrowphase/collision-inl.h | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/include/fcl/narrowphase/collision-inl.h b/include/fcl/narrowphase/collision-inl.h index 81dc22d5f..7274b35dd 100644 --- a/include/fcl/narrowphase/collision-inl.h +++ b/include/fcl/narrowphase/collision-inl.h @@ -48,6 +48,14 @@ namespace fcl { //============================================================================== +extern template +FCL_EXPORT +std::size_t collide( + const CollisionObject* o1, + const CollisionObject* o2, + const CollisionRequest& request, + CollisionResult& result); + extern template FCL_EXPORT std::size_t collide( @@ -67,6 +75,16 @@ std::size_t collide( const CollisionRequest& request, CollisionResult& result); +extern template +FCL_EXPORT +std::size_t collide( + const CollisionGeometry* o1, + const Transform3& tf1, + const CollisionGeometry* o2, + const Transform3& tf2, + const CollisionRequest& request, + CollisionResult& result); + //============================================================================== template detail::CollisionFunctionMatrix& getCollisionFunctionLookTable() From 21d28a56d145b18f50a5cff48a887122c6f23996 Mon Sep 17 00:00:00 2001 From: Josip Sumecki Date: Wed, 3 Jun 2020 14:19:58 +0200 Subject: [PATCH 5/8] Added support for float CollisionObject --- include/fcl/narrowphase/collision-inl.h | 20 +++++++++---------- .../fcl/narrowphase/collision_object-inl.h | 3 +++ 2 files changed, 13 insertions(+), 10 deletions(-) diff --git a/include/fcl/narrowphase/collision-inl.h b/include/fcl/narrowphase/collision-inl.h index 7274b35dd..679a19f19 100644 --- a/include/fcl/narrowphase/collision-inl.h +++ b/include/fcl/narrowphase/collision-inl.h @@ -65,16 +65,6 @@ std::size_t collide( CollisionResult& result); //============================================================================== -extern template -FCL_EXPORT -std::size_t collide( - const CollisionGeometry* o1, - const Transform3& tf1, - const CollisionGeometry* o2, - const Transform3& tf2, - const CollisionRequest& request, - CollisionResult& result); - extern template FCL_EXPORT std::size_t collide( @@ -85,6 +75,16 @@ std::size_t collide( const CollisionRequest& request, CollisionResult& result); +extern template +FCL_EXPORT +std::size_t collide( + const CollisionGeometry* o1, + const Transform3& tf1, + const CollisionGeometry* o2, + const Transform3& tf2, + const CollisionRequest& request, + CollisionResult& result); + //============================================================================== template detail::CollisionFunctionMatrix& getCollisionFunctionLookTable() diff --git a/include/fcl/narrowphase/collision_object-inl.h b/include/fcl/narrowphase/collision_object-inl.h index 0c50e52c8..8162730fa 100644 --- a/include/fcl/narrowphase/collision_object-inl.h +++ b/include/fcl/narrowphase/collision_object-inl.h @@ -44,6 +44,9 @@ namespace fcl { //============================================================================== +extern template +class FCL_EXPORT CollisionObject; + extern template class FCL_EXPORT CollisionObject; From 4df76bf198874f05b01ff46ed5da4704a6ae2796 Mon Sep 17 00:00:00 2001 From: Josip Sumecki Date: Wed, 3 Jun 2020 15:58:13 +0200 Subject: [PATCH 6/8] Fixed linker errors --- src/math/motion/interp_motion.cpp | 3 ++ src/math/motion/screw_motion.cpp | 3 ++ src/narrowphase/collision.cpp | 16 +++++++++ src/narrowphase/collision_object.cpp | 3 ++ src/narrowphase/continuous_collision.cpp | 36 +++++++++++++++++++ .../continuous_collision_object.cpp | 3 ++ 6 files changed, 64 insertions(+) diff --git a/src/math/motion/interp_motion.cpp b/src/math/motion/interp_motion.cpp index aa5e592c9..88ee3a09d 100644 --- a/src/math/motion/interp_motion.cpp +++ b/src/math/motion/interp_motion.cpp @@ -40,6 +40,9 @@ namespace fcl { +template +class InterpMotion; + template class InterpMotion; diff --git a/src/math/motion/screw_motion.cpp b/src/math/motion/screw_motion.cpp index 65d1066ea..9b2fc9e3a 100644 --- a/src/math/motion/screw_motion.cpp +++ b/src/math/motion/screw_motion.cpp @@ -40,6 +40,9 @@ namespace fcl { +template +class ScrewMotion; + template class ScrewMotion; diff --git a/src/narrowphase/collision.cpp b/src/narrowphase/collision.cpp index ad5186158..74522e8c7 100644 --- a/src/narrowphase/collision.cpp +++ b/src/narrowphase/collision.cpp @@ -41,6 +41,13 @@ namespace fcl { //============================================================================== +template +std::size_t collide( + const CollisionObject* o1, + const CollisionObject* o2, + const CollisionRequest& request, + CollisionResult& result); + template std::size_t collide( const CollisionObject* o1, @@ -49,6 +56,15 @@ std::size_t collide( CollisionResult& result); //============================================================================== +template +std::size_t collide( + const CollisionGeometry* o1, + const Transform3& tf1, + const CollisionGeometry* o2, + const Transform3& tf2, + const CollisionRequest& request, + CollisionResult& result); + template std::size_t collide( const CollisionGeometry* o1, diff --git a/src/narrowphase/collision_object.cpp b/src/narrowphase/collision_object.cpp index 6362db1f6..e8f336315 100644 --- a/src/narrowphase/collision_object.cpp +++ b/src/narrowphase/collision_object.cpp @@ -40,6 +40,9 @@ namespace fcl { +template +class CollisionObject; + template class CollisionObject; diff --git a/src/narrowphase/continuous_collision.cpp b/src/narrowphase/continuous_collision.cpp index bbbce19a9..f08156712 100644 --- a/src/narrowphase/continuous_collision.cpp +++ b/src/narrowphase/continuous_collision.cpp @@ -41,6 +41,15 @@ namespace fcl { //============================================================================== +template +float continuousCollide( + const CollisionGeometry* o1, + const MotionBase* motion1, + const CollisionGeometry* o2, + const MotionBase* motion2, + const ContinuousCollisionRequest& request, + ContinuousCollisionResult& result); + template double continuousCollide( const CollisionGeometry* o1, @@ -51,6 +60,17 @@ double continuousCollide( ContinuousCollisionResult& result); //============================================================================== +template +float continuousCollide( + const CollisionGeometry* o1, + const Transform3& tf1_beg, + const Transform3& tf1_end, + const CollisionGeometry* o2, + const Transform3& tf2_beg, + const Transform3& tf2_end, + const ContinuousCollisionRequest& request, + ContinuousCollisionResult& result); + template double continuousCollide( const CollisionGeometry* o1, @@ -63,6 +83,15 @@ double continuousCollide( ContinuousCollisionResult& result); //============================================================================== +template +float continuousCollide( + const CollisionObject* o1, + const Transform3& tf1_end, + const CollisionObject* o2, + const Transform3& tf2_end, + const ContinuousCollisionRequest& request, + ContinuousCollisionResult& result); + template double continuousCollide( const CollisionObject* o1, @@ -73,6 +102,13 @@ double continuousCollide( ContinuousCollisionResult& result); //============================================================================== +template +float collide( + const ContinuousCollisionObject* o1, + const ContinuousCollisionObject* o2, + const ContinuousCollisionRequest& request, + ContinuousCollisionResult& result); + template double collide( const ContinuousCollisionObject* o1, diff --git a/src/narrowphase/continuous_collision_object.cpp b/src/narrowphase/continuous_collision_object.cpp index 93b767204..1f000385f 100644 --- a/src/narrowphase/continuous_collision_object.cpp +++ b/src/narrowphase/continuous_collision_object.cpp @@ -40,6 +40,9 @@ namespace fcl { +template +class ContinuousCollisionObject; + template class ContinuousCollisionObject; From fa0921a8094485ff44254093c8e0c3bcdeea0fd1 Mon Sep 17 00:00:00 2001 From: Josip Sumecki Date: Thu, 4 Jun 2020 13:41:18 +0200 Subject: [PATCH 7/8] Removed forward declarations for float templates --- include/fcl/math/motion/interp_motion-inl.h | 3 -- include/fcl/math/motion/screw_motion-inl.h | 3 -- include/fcl/narrowphase/collision-inl.h | 18 ---------- .../fcl/narrowphase/collision_object-inl.h | 3 -- .../narrowphase/continuous_collision-inl.h | 36 ------------------- 5 files changed, 63 deletions(-) diff --git a/include/fcl/math/motion/interp_motion-inl.h b/include/fcl/math/motion/interp_motion-inl.h index ffca75602..c00684142 100644 --- a/include/fcl/math/motion/interp_motion-inl.h +++ b/include/fcl/math/motion/interp_motion-inl.h @@ -44,9 +44,6 @@ namespace fcl { //============================================================================== -extern template -class FCL_EXPORT InterpMotion; - extern template class FCL_EXPORT InterpMotion; diff --git a/include/fcl/math/motion/screw_motion-inl.h b/include/fcl/math/motion/screw_motion-inl.h index 44e46d3e2..ae72d6c70 100644 --- a/include/fcl/math/motion/screw_motion-inl.h +++ b/include/fcl/math/motion/screw_motion-inl.h @@ -44,9 +44,6 @@ namespace fcl { //============================================================================== -extern template -class FCL_EXPORT ScrewMotion; - extern template class FCL_EXPORT ScrewMotion; diff --git a/include/fcl/narrowphase/collision-inl.h b/include/fcl/narrowphase/collision-inl.h index 679a19f19..81dc22d5f 100644 --- a/include/fcl/narrowphase/collision-inl.h +++ b/include/fcl/narrowphase/collision-inl.h @@ -48,14 +48,6 @@ namespace fcl { //============================================================================== -extern template -FCL_EXPORT -std::size_t collide( - const CollisionObject* o1, - const CollisionObject* o2, - const CollisionRequest& request, - CollisionResult& result); - extern template FCL_EXPORT std::size_t collide( @@ -65,16 +57,6 @@ std::size_t collide( CollisionResult& result); //============================================================================== -extern template -FCL_EXPORT -std::size_t collide( - const CollisionGeometry* o1, - const Transform3& tf1, - const CollisionGeometry* o2, - const Transform3& tf2, - const CollisionRequest& request, - CollisionResult& result); - extern template FCL_EXPORT std::size_t collide( diff --git a/include/fcl/narrowphase/collision_object-inl.h b/include/fcl/narrowphase/collision_object-inl.h index 8162730fa..0c50e52c8 100644 --- a/include/fcl/narrowphase/collision_object-inl.h +++ b/include/fcl/narrowphase/collision_object-inl.h @@ -44,9 +44,6 @@ namespace fcl { //============================================================================== -extern template -class FCL_EXPORT CollisionObject; - extern template class FCL_EXPORT CollisionObject; diff --git a/include/fcl/narrowphase/continuous_collision-inl.h b/include/fcl/narrowphase/continuous_collision-inl.h index 466ea3fcc..f8248e6bd 100644 --- a/include/fcl/narrowphase/continuous_collision-inl.h +++ b/include/fcl/narrowphase/continuous_collision-inl.h @@ -55,15 +55,6 @@ namespace fcl { //============================================================================== -extern template -float continuousCollide( - const CollisionGeometry* o1, - const MotionBase* motion1, - const CollisionGeometry* o2, - const MotionBase* motion2, - const ContinuousCollisionRequest& request, - ContinuousCollisionResult& result); - extern template double continuousCollide( const CollisionGeometry* o1, @@ -74,17 +65,6 @@ double continuousCollide( ContinuousCollisionResult& result); //============================================================================== -extern template -float continuousCollide( - const CollisionGeometry* o1, - const Transform3& tf1_beg, - const Transform3& tf1_end, - const CollisionGeometry* o2, - const Transform3& tf2_beg, - const Transform3& tf2_end, - const ContinuousCollisionRequest& request, - ContinuousCollisionResult& result); - extern template double continuousCollide( const CollisionGeometry* o1, @@ -97,15 +77,6 @@ double continuousCollide( ContinuousCollisionResult& result); //============================================================================== -extern template -float continuousCollide( - const CollisionObject* o1, - const Transform3& tf1_end, - const CollisionObject* o2, - const Transform3& tf2_end, - const ContinuousCollisionRequest& request, - ContinuousCollisionResult& result); - extern template double continuousCollide( const CollisionObject* o1, @@ -116,13 +87,6 @@ double continuousCollide( ContinuousCollisionResult& result); //============================================================================== -extern template -float collide( - const ContinuousCollisionObject* o1, - const ContinuousCollisionObject* o2, - const ContinuousCollisionRequest& request, - ContinuousCollisionResult& result); - extern template double collide( const ContinuousCollisionObject* o1, From 6599ed9dbf4bf70c9062dd892a53827ca166d378 Mon Sep 17 00:00:00 2001 From: Sean Curtis Date: Thu, 4 Jun 2020 07:31:57 -0700 Subject: [PATCH 8/8] 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..30630f452 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_ 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(); +}