diff --git a/src/collision_detection/fcl/collision_common.cpp b/src/collision_detection/fcl/collision_common.cpp index e7c048ef..1f9d7db4 100644 --- a/src/collision_detection/fcl/collision_common.cpp +++ b/src/collision_detection/fcl/collision_common.cpp @@ -14,9 +14,6 @@ namespace mplib::collision_detection::fcl { const fcl::DistanceRequest &request, \ fcl::DistanceResult &result) -DEFINE_TEMPLATE_FCL_COMMON(float); -DEFINE_TEMPLATE_FCL_COMMON(double); - template FCLObject::FCLObject(const std::string &name_, const Pose &pose_, const std::vector> &shapes_, @@ -72,4 +69,7 @@ S distance(const FCLObjectPtr &obj1, const FCLObjectPtr &obj2, return result.min_distance; } + +DEFINE_TEMPLATE_FCL_COMMON(float); +DEFINE_TEMPLATE_FCL_COMMON(double); } // namespace mplib::collision_detection::fcl diff --git a/src/collision_detection/fcl/fcl_model.cpp b/src/collision_detection/fcl/fcl_model.cpp index 3af34c38..27578250 100644 --- a/src/collision_detection/fcl/fcl_model.cpp +++ b/src/collision_detection/fcl/fcl_model.cpp @@ -21,9 +21,6 @@ namespace mplib::collision_detection::fcl { // Explicit Template Instantiation Definition ========================================== #define DEFINE_TEMPLATE_FCL_MODEL(S) template class FCLModelTpl -DEFINE_TEMPLATE_FCL_MODEL(float); -DEFINE_TEMPLATE_FCL_MODEL(double); - template FCLModelTpl::FCLModelTpl(const std::string &urdf_filename, bool convex, bool verbose) : use_convex_(convex), verbose_(verbose) { @@ -392,4 +389,7 @@ WorldDistanceResultTpl FCLModelTpl::distanceWith( return ret; } + +DEFINE_TEMPLATE_FCL_MODEL(float); +DEFINE_TEMPLATE_FCL_MODEL(double); } // namespace mplib::collision_detection::fcl diff --git a/src/collision_detection/fcl/fcl_utils.cpp b/src/collision_detection/fcl/fcl_utils.cpp index 8adca817..ce9a9f9b 100644 --- a/src/collision_detection/fcl/fcl_utils.cpp +++ b/src/collision_detection/fcl/fcl_utils.cpp @@ -13,9 +13,6 @@ namespace mplib::collision_detection::fcl { template fcl::ConvexPtr loadMeshAsConvex(const std::string &mesh_path, \ const Vector3 &scale) -DEFINE_TEMPLATE_FCL_UTILS(float); -DEFINE_TEMPLATE_FCL_UTILS(double); - template fcl::BVHModel_OBBRSSPtr loadMeshAsBVH(const std::string &mesh_path, const Vector3 &scale) { @@ -55,4 +52,7 @@ fcl::ConvexPtr loadMeshAsConvex(const std::string &mesh_path, return std::make_shared>(vertices_ptr, triangles.size(), faces, true); } + +DEFINE_TEMPLATE_FCL_UTILS(float); +DEFINE_TEMPLATE_FCL_UTILS(double); } // namespace mplib::collision_detection::fcl diff --git a/src/core/articulated_model.cpp b/src/core/articulated_model.cpp index b40f3f2e..16cc431f 100644 --- a/src/core/articulated_model.cpp +++ b/src/core/articulated_model.cpp @@ -11,9 +11,6 @@ namespace mplib { // Explicit Template Instantiation Definition ========================================== #define DEFINE_TEMPLATE_ARTICULATED_MODEL(S) template class ArticulatedModelTpl -DEFINE_TEMPLATE_ARTICULATED_MODEL(float); -DEFINE_TEMPLATE_ARTICULATED_MODEL(double); - template ArticulatedModelTpl::ArticulatedModelTpl(const std::string &urdf_filename, const std::string &srdf_filename, @@ -136,4 +133,7 @@ void ArticulatedModelTpl::setQpos(const VectorX &qpos, bool full) { fcl_model_->updateCollisionObjects(link_pose); } + +DEFINE_TEMPLATE_ARTICULATED_MODEL(float); +DEFINE_TEMPLATE_ARTICULATED_MODEL(double); } // namespace mplib diff --git a/src/core/attached_body.cpp b/src/core/attached_body.cpp index 5311faae..6b385098 100644 --- a/src/core/attached_body.cpp +++ b/src/core/attached_body.cpp @@ -5,9 +5,6 @@ namespace mplib { // Explicit Template Instantiation Definition ========================================== #define DEFINE_TEMPLATE_ATTACHED_BODY(S) template class AttachedBodyTpl -DEFINE_TEMPLATE_ATTACHED_BODY(float); -DEFINE_TEMPLATE_ATTACHED_BODY(double); - template AttachedBodyTpl::AttachedBodyTpl(const std::string &name, const FCLObjectPtr &object, const ArticulatedModelPtr &attached_articulation, @@ -31,4 +28,7 @@ void AttachedBodyTpl::updatePose() const { object_->shapes[i]->setTransform(object_pose * object_->shape_poses[i]); } + +DEFINE_TEMPLATE_ATTACHED_BODY(float); +DEFINE_TEMPLATE_ATTACHED_BODY(double); } // namespace mplib diff --git a/src/kinematics/kdl/kdl_model.cpp b/src/kinematics/kdl/kdl_model.cpp index 324048d6..c586ffbd 100644 --- a/src/kinematics/kdl/kdl_model.cpp +++ b/src/kinematics/kdl/kdl_model.cpp @@ -22,9 +22,6 @@ namespace mplib::kinematics::kdl { // Explicit Template Instantiation Definition ========================================== #define DEFINE_TEMPLATE_KDL_MODEL(S) template class KDLModelTpl -DEFINE_TEMPLATE_KDL_MODEL(float); -DEFINE_TEMPLATE_KDL_MODEL(double); - template KDLModelTpl::KDLModelTpl(const std::string &urdf_filename, const std::vector &link_names, @@ -185,4 +182,7 @@ std::tuple, int> KDLModelTpl::TreeIKNRJL( return {q1, retval}; } + +DEFINE_TEMPLATE_KDL_MODEL(float); +DEFINE_TEMPLATE_KDL_MODEL(double); } // namespace mplib::kinematics::kdl diff --git a/src/kinematics/pinocchio/pinocchio_model.cpp b/src/kinematics/pinocchio/pinocchio_model.cpp index 84876345..8618a37f 100644 --- a/src/kinematics/pinocchio/pinocchio_model.cpp +++ b/src/kinematics/pinocchio/pinocchio_model.cpp @@ -15,9 +15,6 @@ namespace mplib::kinematics::pinocchio { // Explicit Template Instantiation Definition ========================================== #define DEFINE_TEMPLATE_PINOCCHIO_MODEL(S) template class PinocchioModelTpl -DEFINE_TEMPLATE_PINOCCHIO_MODEL(float); -DEFINE_TEMPLATE_PINOCCHIO_MODEL(double); - template PinocchioModelTpl::PinocchioModelTpl(const std::string &urdf_filename, const Vector3 &gravity, bool verbose) @@ -697,4 +694,7 @@ std::tuple, bool, Vector6> PinocchioModelTpl::computeIKCLIKJL( return {qposPinocchio2User(q), success, err}; } + +DEFINE_TEMPLATE_PINOCCHIO_MODEL(float); +DEFINE_TEMPLATE_PINOCCHIO_MODEL(double); } // namespace mplib::kinematics::pinocchio diff --git a/src/planning/ompl/fixed_joint.cpp b/src/planning/ompl/fixed_joint.cpp index be4f7512..e9f8260c 100644 --- a/src/planning/ompl/fixed_joint.cpp +++ b/src/planning/ompl/fixed_joint.cpp @@ -11,9 +11,6 @@ namespace mplib::planning::ompl { template VectorX removeFixedJoints(const FixedJointsTpl &fixed_joints, \ const VectorX &state) -DEFINE_TEMPLATE_FIXED_JOINT(float); -DEFINE_TEMPLATE_FIXED_JOINT(double); - template bool isFixedJoint(const FixedJointsTpl &fixed_joints, size_t articulation_idx, size_t joint_idx) { @@ -54,4 +51,7 @@ VectorX removeFixedJoints(const FixedJointsTpl &fixed_joints, return ret; } + +DEFINE_TEMPLATE_FIXED_JOINT(float); +DEFINE_TEMPLATE_FIXED_JOINT(double); } // namespace mplib::planning::ompl diff --git a/src/planning/ompl/ompl_planner.cpp b/src/planning/ompl/ompl_planner.cpp index f7adfb2c..ce3e04da 100644 --- a/src/planning/ompl/ompl_planner.cpp +++ b/src/planning/ompl/ompl_planner.cpp @@ -16,9 +16,6 @@ namespace mplib::planning::ompl { // Explicit Template Instantiation Definition ========================================== #define DEFINE_TEMPLATE_OMPL_PLANNER(S) template class OMPLPlannerTpl -DEFINE_TEMPLATE_OMPL_PLANNER(float); -DEFINE_TEMPLATE_OMPL_PLANNER(double); - #define PI 3.14159265359 template @@ -340,4 +337,7 @@ MatrixX OMPLPlannerTpl::simplifyPath(const MatrixX &path) const { return ret; } + +DEFINE_TEMPLATE_OMPL_PLANNER(float); +DEFINE_TEMPLATE_OMPL_PLANNER(double); } // namespace mplib::planning::ompl diff --git a/src/planning/ompl/ompl_utils.cpp b/src/planning/ompl/ompl_utils.cpp index c359190d..19f93d4f 100644 --- a/src/planning/ompl/ompl_utils.cpp +++ b/src/planning/ompl/ompl_utils.cpp @@ -14,9 +14,6 @@ namespace mplib::planning::ompl { template VectorX state2Eigen(const ob::State *state_raw, \ const ob::SpaceInformation *si, bool is_rvss) -DEFINE_TEMPLATE_OMPL_UTILS(float); -DEFINE_TEMPLATE_OMPL_UTILS(double); - template std::vector compoundState2Vector(const ob::State *state_raw, const ob::SpaceInformation *si) { @@ -63,4 +60,7 @@ VectorX state2Eigen(const ob::State *state_raw, const ob::SpaceInformation *s return vector2Eigen(state_vec); } + +DEFINE_TEMPLATE_OMPL_UTILS(float); +DEFINE_TEMPLATE_OMPL_UTILS(double); } // namespace mplib::planning::ompl diff --git a/src/planning_world.cpp b/src/planning_world.cpp index 3b7283d9..a8fd0d52 100644 --- a/src/planning_world.cpp +++ b/src/planning_world.cpp @@ -11,9 +11,6 @@ namespace mplib { // Explicit Template Instantiation Definition ========================================== #define DEFINE_TEMPLATE_PLANNING_WORLD(S) template class PlanningWorldTpl -DEFINE_TEMPLATE_PLANNING_WORLD(float); -DEFINE_TEMPLATE_PLANNING_WORLD(double); - template PlanningWorldTpl::PlanningWorldTpl( const std::vector &articulations, @@ -553,4 +550,7 @@ WorldDistanceResultTpl PlanningWorldTpl::distance( return ret1.min_distance < ret2.min_distance ? ret1 : ret2; } + +DEFINE_TEMPLATE_PLANNING_WORLD(float); +DEFINE_TEMPLATE_PLANNING_WORLD(double); } // namespace mplib diff --git a/src/utils/assimp_loader.cpp b/src/utils/assimp_loader.cpp index 420ff1b2..d3879efa 100644 --- a/src/utils/assimp_loader.cpp +++ b/src/utils/assimp_loader.cpp @@ -15,9 +15,6 @@ namespace mplib { const aiNode *node, const Vector3 &scale, int vertices_offset, \ std::vector> &vertices, std::vector &triangles) const -DEFINE_TEMPLATE_ASSIMP_LOADER(float); -DEFINE_TEMPLATE_ASSIMP_LOADER(double); - AssimpLoader::AssimpLoader() : importer_(new Assimp::Importer()) { // set list of ignored parameters (parameters used for rendering) importer_->SetPropertyInteger( @@ -117,4 +114,7 @@ size_t AssimpLoader::_dfsBuildMesh(const aiNode *node, const Vector3 &scale, return nbVertices; } + +DEFINE_TEMPLATE_ASSIMP_LOADER(float); +DEFINE_TEMPLATE_ASSIMP_LOADER(double); } // namespace mplib diff --git a/src/utils/conversion.cpp b/src/utils/conversion.cpp index d1144e56..5ca1567a 100644 --- a/src/utils/conversion.cpp +++ b/src/utils/conversion.cpp @@ -11,9 +11,6 @@ namespace mplib { template pinocchio::InertiaTpl convertInertial(const urdf::Inertial &Y); \ template pinocchio::InertiaTpl convertInertial(const urdf::InertialSharedPtr &Y) -DEFINE_TEMPLATE_CONVERSION(float); -DEFINE_TEMPLATE_CONVERSION(double); - template Isometry3 toIsometry(const pinocchio::SE3Tpl &T) { Isometry3 ret; @@ -70,4 +67,7 @@ pinocchio::InertiaTpl convertInertial(const urdf::InertialSharedPtr &Y) { return pinocchio::InertiaTpl::Zero(); } + +DEFINE_TEMPLATE_CONVERSION(float); +DEFINE_TEMPLATE_CONVERSION(double); } // namespace mplib