Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 3 additions & 3 deletions src/collision_detection/fcl/collision_common.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -14,9 +14,6 @@ namespace mplib::collision_detection::fcl {
const fcl::DistanceRequest<S> &request, \
fcl::DistanceResult<S> &result)

DEFINE_TEMPLATE_FCL_COMMON(float);
DEFINE_TEMPLATE_FCL_COMMON(double);

template <typename S>
FCLObject<S>::FCLObject(const std::string &name_, const Pose<S> &pose_,
const std::vector<fcl::CollisionObjectPtr<S>> &shapes_,
Expand Down Expand Up @@ -72,4 +69,7 @@ S distance(const FCLObjectPtr<S> &obj1, const FCLObjectPtr<S> &obj2,
return result.min_distance;
}


DEFINE_TEMPLATE_FCL_COMMON(float);
DEFINE_TEMPLATE_FCL_COMMON(double);
} // namespace mplib::collision_detection::fcl
6 changes: 3 additions & 3 deletions src/collision_detection/fcl/fcl_model.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -21,9 +21,6 @@ namespace mplib::collision_detection::fcl {
// Explicit Template Instantiation Definition ==========================================
#define DEFINE_TEMPLATE_FCL_MODEL(S) template class FCLModelTpl<S>

DEFINE_TEMPLATE_FCL_MODEL(float);
DEFINE_TEMPLATE_FCL_MODEL(double);

template <typename S>
FCLModelTpl<S>::FCLModelTpl(const std::string &urdf_filename, bool convex, bool verbose)
: use_convex_(convex), verbose_(verbose) {
Expand Down Expand Up @@ -392,4 +389,7 @@ WorldDistanceResultTpl<S> FCLModelTpl<S>::distanceWith(
return ret;
}


DEFINE_TEMPLATE_FCL_MODEL(float);
DEFINE_TEMPLATE_FCL_MODEL(double);
} // namespace mplib::collision_detection::fcl
6 changes: 3 additions & 3 deletions src/collision_detection/fcl/fcl_utils.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -13,9 +13,6 @@ namespace mplib::collision_detection::fcl {
template fcl::ConvexPtr<S> loadMeshAsConvex<S>(const std::string &mesh_path, \
const Vector3<S> &scale)

DEFINE_TEMPLATE_FCL_UTILS(float);
DEFINE_TEMPLATE_FCL_UTILS(double);

template <typename S>
fcl::BVHModel_OBBRSSPtr<S> loadMeshAsBVH(const std::string &mesh_path,
const Vector3<S> &scale) {
Expand Down Expand Up @@ -55,4 +52,7 @@ fcl::ConvexPtr<S> loadMeshAsConvex(const std::string &mesh_path,
return std::make_shared<fcl::Convex<S>>(vertices_ptr, triangles.size(), faces, true);
}


DEFINE_TEMPLATE_FCL_UTILS(float);
DEFINE_TEMPLATE_FCL_UTILS(double);
} // namespace mplib::collision_detection::fcl
6 changes: 3 additions & 3 deletions src/core/articulated_model.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -11,9 +11,6 @@ namespace mplib {
// Explicit Template Instantiation Definition ==========================================
#define DEFINE_TEMPLATE_ARTICULATED_MODEL(S) template class ArticulatedModelTpl<S>

DEFINE_TEMPLATE_ARTICULATED_MODEL(float);
DEFINE_TEMPLATE_ARTICULATED_MODEL(double);

template <typename S>
ArticulatedModelTpl<S>::ArticulatedModelTpl(const std::string &urdf_filename,
const std::string &srdf_filename,
Expand Down Expand Up @@ -136,4 +133,7 @@ void ArticulatedModelTpl<S>::setQpos(const VectorX<S> &qpos, bool full) {
fcl_model_->updateCollisionObjects(link_pose);
}


DEFINE_TEMPLATE_ARTICULATED_MODEL(float);
DEFINE_TEMPLATE_ARTICULATED_MODEL(double);
} // namespace mplib
6 changes: 3 additions & 3 deletions src/core/attached_body.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -5,9 +5,6 @@ namespace mplib {
// Explicit Template Instantiation Definition ==========================================
#define DEFINE_TEMPLATE_ATTACHED_BODY(S) template class AttachedBodyTpl<S>

DEFINE_TEMPLATE_ATTACHED_BODY(float);
DEFINE_TEMPLATE_ATTACHED_BODY(double);

template <typename S>
AttachedBodyTpl<S>::AttachedBodyTpl(const std::string &name, const FCLObjectPtr &object,
const ArticulatedModelPtr &attached_articulation,
Expand All @@ -31,4 +28,7 @@ void AttachedBodyTpl<S>::updatePose() const {
object_->shapes[i]->setTransform(object_pose * object_->shape_poses[i]);
}


DEFINE_TEMPLATE_ATTACHED_BODY(float);
DEFINE_TEMPLATE_ATTACHED_BODY(double);
} // namespace mplib
6 changes: 3 additions & 3 deletions src/kinematics/kdl/kdl_model.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -22,9 +22,6 @@ namespace mplib::kinematics::kdl {
// Explicit Template Instantiation Definition ==========================================
#define DEFINE_TEMPLATE_KDL_MODEL(S) template class KDLModelTpl<S>

DEFINE_TEMPLATE_KDL_MODEL(float);
DEFINE_TEMPLATE_KDL_MODEL(double);

template <typename S>
KDLModelTpl<S>::KDLModelTpl(const std::string &urdf_filename,
const std::vector<std::string> &link_names,
Expand Down Expand Up @@ -185,4 +182,7 @@ std::tuple<VectorX<S>, int> KDLModelTpl<S>::TreeIKNRJL(
return {q1, retval};
}


DEFINE_TEMPLATE_KDL_MODEL(float);
DEFINE_TEMPLATE_KDL_MODEL(double);
} // namespace mplib::kinematics::kdl
6 changes: 3 additions & 3 deletions src/kinematics/pinocchio/pinocchio_model.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -15,9 +15,6 @@ namespace mplib::kinematics::pinocchio {
// Explicit Template Instantiation Definition ==========================================
#define DEFINE_TEMPLATE_PINOCCHIO_MODEL(S) template class PinocchioModelTpl<S>

DEFINE_TEMPLATE_PINOCCHIO_MODEL(float);
DEFINE_TEMPLATE_PINOCCHIO_MODEL(double);

template <typename S>
PinocchioModelTpl<S>::PinocchioModelTpl(const std::string &urdf_filename,
const Vector3<S> &gravity, bool verbose)
Expand Down Expand Up @@ -697,4 +694,7 @@ std::tuple<VectorX<S>, bool, Vector6<S>> PinocchioModelTpl<S>::computeIKCLIKJL(
return {qposPinocchio2User(q), success, err};
}


DEFINE_TEMPLATE_PINOCCHIO_MODEL(float);
DEFINE_TEMPLATE_PINOCCHIO_MODEL(double);
} // namespace mplib::kinematics::pinocchio
6 changes: 3 additions & 3 deletions src/planning/ompl/fixed_joint.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -11,9 +11,6 @@ namespace mplib::planning::ompl {
template VectorX<S> removeFixedJoints<S>(const FixedJointsTpl<S> &fixed_joints, \
const VectorX<S> &state)

DEFINE_TEMPLATE_FIXED_JOINT(float);
DEFINE_TEMPLATE_FIXED_JOINT(double);

template <typename S>
bool isFixedJoint(const FixedJointsTpl<S> &fixed_joints, size_t articulation_idx,
size_t joint_idx) {
Expand Down Expand Up @@ -54,4 +51,7 @@ VectorX<S> removeFixedJoints(const FixedJointsTpl<S> &fixed_joints,
return ret;
}


DEFINE_TEMPLATE_FIXED_JOINT(float);
DEFINE_TEMPLATE_FIXED_JOINT(double);
} // namespace mplib::planning::ompl
6 changes: 3 additions & 3 deletions src/planning/ompl/ompl_planner.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -16,9 +16,6 @@ namespace mplib::planning::ompl {
// Explicit Template Instantiation Definition ==========================================
#define DEFINE_TEMPLATE_OMPL_PLANNER(S) template class OMPLPlannerTpl<S>

DEFINE_TEMPLATE_OMPL_PLANNER(float);
DEFINE_TEMPLATE_OMPL_PLANNER(double);

#define PI 3.14159265359

template <typename S>
Expand Down Expand Up @@ -340,4 +337,7 @@ MatrixX<S> OMPLPlannerTpl<S>::simplifyPath(const MatrixX<S> &path) const {
return ret;
}


DEFINE_TEMPLATE_OMPL_PLANNER(float);
DEFINE_TEMPLATE_OMPL_PLANNER(double);
} // namespace mplib::planning::ompl
6 changes: 3 additions & 3 deletions src/planning/ompl/ompl_utils.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -14,9 +14,6 @@ namespace mplib::planning::ompl {
template VectorX<S> state2Eigen<S>(const ob::State *state_raw, \
const ob::SpaceInformation *si, bool is_rvss)

DEFINE_TEMPLATE_OMPL_UTILS(float);
DEFINE_TEMPLATE_OMPL_UTILS(double);

template <typename S>
std::vector<S> compoundState2Vector(const ob::State *state_raw,
const ob::SpaceInformation *si) {
Expand Down Expand Up @@ -63,4 +60,7 @@ VectorX<S> state2Eigen(const ob::State *state_raw, const ob::SpaceInformation *s
return vector2Eigen<S, S>(state_vec);
}


DEFINE_TEMPLATE_OMPL_UTILS(float);
DEFINE_TEMPLATE_OMPL_UTILS(double);
} // namespace mplib::planning::ompl
6 changes: 3 additions & 3 deletions src/planning_world.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -11,9 +11,6 @@ namespace mplib {
// Explicit Template Instantiation Definition ==========================================
#define DEFINE_TEMPLATE_PLANNING_WORLD(S) template class PlanningWorldTpl<S>

DEFINE_TEMPLATE_PLANNING_WORLD(float);
DEFINE_TEMPLATE_PLANNING_WORLD(double);

template <typename S>
PlanningWorldTpl<S>::PlanningWorldTpl(
const std::vector<ArticulatedModelPtr> &articulations,
Expand Down Expand Up @@ -553,4 +550,7 @@ WorldDistanceResultTpl<S> PlanningWorldTpl<S>::distance(
return ret1.min_distance < ret2.min_distance ? ret1 : ret2;
}


DEFINE_TEMPLATE_PLANNING_WORLD(float);
DEFINE_TEMPLATE_PLANNING_WORLD(double);
} // namespace mplib
6 changes: 3 additions & 3 deletions src/utils/assimp_loader.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -15,9 +15,6 @@ namespace mplib {
const aiNode *node, const Vector3<S> &scale, int vertices_offset, \
std::vector<Vector3<S>> &vertices, std::vector<fcl::Triangle> &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(
Expand Down Expand Up @@ -117,4 +114,7 @@ size_t AssimpLoader::_dfsBuildMesh(const aiNode *node, const Vector3<S> &scale,
return nbVertices;
}


DEFINE_TEMPLATE_ASSIMP_LOADER(float);
DEFINE_TEMPLATE_ASSIMP_LOADER(double);
} // namespace mplib
6 changes: 3 additions & 3 deletions src/utils/conversion.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -11,9 +11,6 @@ namespace mplib {
template pinocchio::InertiaTpl<S> convertInertial<S>(const urdf::Inertial &Y); \
template pinocchio::InertiaTpl<S> convertInertial<S>(const urdf::InertialSharedPtr &Y)

DEFINE_TEMPLATE_CONVERSION(float);
DEFINE_TEMPLATE_CONVERSION(double);

template <typename S>
Isometry3<S> toIsometry(const pinocchio::SE3Tpl<S> &T) {
Isometry3<S> ret;
Expand Down Expand Up @@ -70,4 +67,7 @@ pinocchio::InertiaTpl<S> convertInertial(const urdf::InertialSharedPtr &Y) {
return pinocchio::InertiaTpl<S>::Zero();
}


DEFINE_TEMPLATE_CONVERSION(float);
DEFINE_TEMPLATE_CONVERSION(double);
} // namespace mplib