Commit 36e37c79 authored by panjia1983's avatar panjia1983
Browse files

remove global pd due to license issue

parent 182e5ce4
......@@ -6,7 +6,7 @@ FCL is a library for performing three types of proximity queries on a pair of ge
- Distance computation: computing the minimum distance between a pair of models, i.e., the distance between the closest pair of points.
- Tolerance verification: determining whether two models are closer or farther than a tolerance distance.
- Continuous collision detection: detecting whether the two moving models overlap during the movement, and optionally, the time of contact.
- Global penetration depth: detecting the minimum motion required to separate two in-collision objects, and return the configuration where the collision has been resolved.
# - Global penetration depth: detecting the minimum motion required to separate two in-collision objects, and return the configuration where the collision has been resolved.
- Contact information: for collision detection, continuous collision detection and global penetration depth, the contact information (including contact normals and contact points) can be returned optionally.
FCL has the following features
......@@ -123,20 +123,20 @@ ContinuousCollisionResult result;
// perform continuous collision test
continuousCollide(o1, tf_goal_o1, o2, tf_goal_o2, request, result);
```
For global penetration depth computation, we need a pre-computation step. Beside that, the online penetration depth function uses exactly the same pipeline as shown above:
```cpp
// Given two objects o1 and o2
CollisionObject* o1 = ...
CollisionObject* o2 = ...
// the precomputation step for global penetration computation (see test_fcl_xmldata.cpp for an example)
...
// set the penetration depth request structure, here we just use the default setting
PenetrationDepthRequest request;
// result will be returned via the penetration depth result structure
PenetrationDepthResult result;
// perform global penetration depth test
penetrationDepth(o1, o2,request, result);
```
#For global penetration depth computation, we need a pre-computation step. Beside that, the online penetration depth function uses exactly the same pipeline as shown above:
#```cpp
#// Given two objects o1 and o2
#CollisionObject* o1 = ...
#CollisionObject* o2 = ...
#// the precomputation step for global penetration computation (see test_fcl_xmldata.cpp for an example)
#...
#// set the penetration depth request structure, here we just use the default setting
#PenetrationDepthRequest request;
#// result will be returned via the penetration depth result structure
#PenetrationDepthResult result;
#// perform global penetration depth test
#penetrationDepth(o1, o2,request, result);
#```
FCL supports broadphase collision/distance between two groups of objects and can avoid the n square complexity. For collision, broadphase algorithm can return all the collision pairs. For distance, it can return the pair with the minimum distance. FCL uses a CollisionManager structure to manage all the objects involving the collision or distance operations.
```cpp
......@@ -186,6 +186,6 @@ For more examples, please refer to the test folder:
- test_fcl_distance.cpp: provide examples for distance test
- test_fcl_broadphase.cpp: provide examples for broadphase collision/distance test
- test_fcl_frontlist.cpp: provide examples for frontlist collision acceleration
- test_fcl_global_penetration.cpp: provide examples for global penetration depth test
- test_fcl_xmldata.cpp: provide examples for more global penetration depth test based on xml data
#- test_fcl_global_penetration.cpp: provide examples for global penetration depth test
#- test_fcl_xmldata.cpp: provide examples for more global penetration depth test based on xml data
- test_fcl_octomap.cpp: provide examples for collision/distance computation between octomap data and other data types.
#ifndef FCL_PENETRATION_DEPTH_H
#define FCL_PENETRATION_DEPTH_H
#include "fcl/math/vec_3f.h"
#include "fcl/collision_object.h"
#include "fcl/collision_data.h"
#include "fcl/learning/classifier.h"
#include "fcl/knn/nearest_neighbors.h"
#include <boost/mem_fn.hpp>
#include <iostream>
namespace fcl
{
typedef double (*TransformDistance)(const Transform3f&, const Transform3f&);
class DefaultTransformDistancer
{
public:
const CollisionGeometry* o1;
const CollisionGeometry* o2;
FCL_REAL rot_x_weight, rot_y_weight, rot_z_weight;
DefaultTransformDistancer() : o1(NULL), o2(NULL)
{
rot_x_weight = rot_y_weight = rot_z_weight = 1;
}
DefaultTransformDistancer(const CollisionGeometry* o2_)
{
o2 = o2_;
init();
}
DefaultTransformDistancer(const CollisionGeometry* o1_, const CollisionGeometry* o2_)
{
o1 = o1_;
o2 = o2_;
init();
}
void init()
{
rot_x_weight = rot_y_weight = rot_z_weight = 1;
if(o2)
{
FCL_REAL V = o2->computeVolume();
Matrix3f C = o2->computeMomentofInertiaRelatedToCOM();
FCL_REAL eigen_values[3];
Vec3f eigen_vectors[3];
eigen(C, eigen_values, eigen_vectors);
rot_x_weight = eigen_values[0] * 4 / V;
rot_y_weight = eigen_values[1] * 4 / V;
rot_z_weight = eigen_values[2] * 4 / V;
// std::cout << rot_x_weight << " " << rot_y_weight << " " << rot_z_weight << std::endl;
}
}
void printRotWeight() const
{
std::cout << rot_x_weight << " " << rot_y_weight << " " << rot_z_weight << std::endl;
}
double distance(const Transform3f& tf1, const Transform3f& tf2)
{
Transform3f tf;
relativeTransform(tf1, tf2, tf);
const Quaternion3f& quat = tf.getQuatRotation();
const Vec3f& trans = tf.getTranslation();
FCL_REAL d = rot_x_weight * quat[1] * quat[1] + rot_y_weight * quat[2] * quat[2] + rot_z_weight * quat[3] * quat[3] + trans[0] * trans[0] + trans[1] * trans[1] + trans[2] * trans[2];
return d;
}
};
static DefaultTransformDistancer default_transform_distancer;
static NearestNeighbors<Transform3f>::DistanceFunction default_transform_distance_func = boost::bind(&DefaultTransformDistancer::distance, &default_transform_distancer, _1, _2);
/// @brief precompute a learning model for penetration computation
std::vector<Transform3f> penetrationDepthModelLearning(const CollisionObject* o1,
const CollisionObject* o2,
PenetrationDepthType pd_type,
void* classifier,
std::size_t n_samples,
FCL_REAL margin,
KNNSolverType knn_solver_type,
std::size_t knn_k,
NearestNeighbors<Transform3f>::DistanceFunction distance_func = default_transform_distance_func);
/// @brief penetration depth computation between two in-collision objects
FCL_REAL penetrationDepth(const CollisionGeometry* o1, const Transform3f& tf1,
const CollisionGeometry* o2, const Transform3f& tf2,
const PenetrationDepthRequest& request,
PenetrationDepthResult& result);
FCL_REAL penetrationDepth(const CollisionObject* o1,
const CollisionObject* o2,
const PenetrationDepthRequest& request,
PenetrationDepthResult& result);
}
#endif
#include "fcl/penetration_depth.h"
#include "fcl/learning/classifier.h"
#include "fcl/math/sampling.h"
#include "fcl/collision.h"
#include "fcl/exception.h"
#include "fcl/knn/nearest_neighbors_linear.h"
#include "fcl/knn/nearest_neighbors_GNAT.h"
#include "fcl/knn/nearest_neighbors_sqrtapprox.h"
#if FCL_HAVE_FLANN
#include "fcl/knn/nearest_neighbors_flann.h"
#endif
#include "fcl/ccd/motion.h"
#include "fcl/continuous_collision.h"
namespace fcl
{
std::vector<Transform3f> penetrationDepthModelLearning(const CollisionObject* o1,
const CollisionObject* o2,
PenetrationDepthType pd_type,
void* classifier_,
std::size_t n_samples,
FCL_REAL margin,
KNNSolverType knn_solver_type,
std::size_t knn_k,
NearestNeighbors<Transform3f>::DistanceFunction distance_func)
{
std::vector<Transform3f> support_transforms_positive;
std::vector<Transform3f> support_transforms_negative;
switch(pd_type)
{
case PDT_TRANSLATIONAL:
{
SVMClassifier<3>* classifier = static_cast<SVMClassifier<3>*>(classifier_);
SamplerR<3> sampler;
Vecnf<3> lower_bound, upper_bound;
AABB aabb1 = o1->getAABB();
AABB aabb2 = o2->getAABB();
lower_bound[0] = aabb1.min_[0] - aabb2.max_[0] - margin;
lower_bound[1] = aabb1.min_[1] - aabb2.max_[1] - margin;
lower_bound[2] = aabb1.min_[2] - aabb2.max_[2] - margin;
upper_bound[0] = aabb1.max_[0] - aabb2.min_[0] + margin;
upper_bound[1] = aabb1.max_[1] - aabb2.min_[1] + margin;
upper_bound[2] = aabb1.max_[2] - aabb2.min_[2] + margin;
sampler.setBound(lower_bound, upper_bound);
std::vector<Item<3> > data(n_samples);
for(std::size_t i = 0; i < n_samples; ++i)
{
Vecnf<3> q = sampler.sample();
Transform3f tf(o2->getQuatRotation(), Vec3f(q[0], q[1], q[2]));
CollisionRequest request;
CollisionResult result;
int n_collide = collide(o1->getCollisionGeometry(), o1->getTransform(),
o2->getCollisionGeometry(), tf, request, result);
data[i] = Item<3>(q, (n_collide > 0));
}
// classifier.setScaler(Scaler<3>(lower_bound, upper_bound));
classifier->setScaler(computeScaler(data));
classifier->learn(data);
std::vector<Item<3> > svs = classifier->getSupportVectors();
for(std::size_t i = 0; i < svs.size(); ++i)
{
const Vecnf<3>& q = svs[i].q;
if(svs[i].label)
support_transforms_positive.push_back(Transform3f(o2->getQuatRotation(), Vec3f(q[0], q[1], q[2])));
else
support_transforms_negative.push_back(Transform3f(o2->getQuatRotation(), Vec3f(q[0], q[1], q[2])));
}
break;
}
case PDT_GENERAL_EULER:
{
SVMClassifier<6>* classifier = static_cast<SVMClassifier<6>*>(classifier_);
SamplerSE3Euler sampler;
FCL_REAL r1 = o1->getCollisionGeometry()->aabb_radius;
FCL_REAL r2 = o2->getCollisionGeometry()->aabb_radius;
Vec3f c1 = o1->getCollisionGeometry()->aabb_center;
Vec3f c2 = o2->getCollisionGeometry()->aabb_center;
FCL_REAL r = r1 + r2 + margin;
sampler.setBound(Vec3f(-r, -r, -r), Vec3f(r, r, r));
std::vector<Item<6> > data(n_samples);
for(std::size_t i = 0; i < n_samples; ++i)
{
Vecnf<6> q = sampler.sample();
Quaternion3f quat;
quat.fromEuler(q[3], q[4], q[5]);
Transform3f tf(quat, -quat.transform(c2) + c1 + Vec3f(q[0], q[1], q[2]));
CollisionRequest request;
CollisionResult result;
int n_collide = collide(o1->getCollisionGeometry(), Transform3f(),
o2->getCollisionGeometry(), tf, request, result);
data[i] = Item<6>(q, (n_collide > 0));
}
classifier->setScaler(computeScaler(data));
classifier->learn(data);
std::vector<Item <6> > svs = classifier->getSupportVectors();
for(std::size_t i = 0; i < svs.size(); ++i)
{
const Vecnf<6>& q = svs[i].q;
Quaternion3f quat;
quat.fromEuler(q[3], q[4], q[5]);
if(svs[i].label)
support_transforms_positive.push_back(Transform3f(quat, -quat.transform(c2) + c1 + Vec3f(q[0], q[1], q[2])));
else
support_transforms_negative.push_back(Transform3f(quat, -quat.transform(c2) + c1 + Vec3f(q[0], q[1], q[2])));
}
break;
}
case PDT_GENERAL_QUAT:
{
SVMClassifier<7>* classifier = static_cast<SVMClassifier<7>*>(classifier_);
SamplerSE3Quat sampler;
FCL_REAL r1 = o1->getCollisionGeometry()->aabb_radius;
FCL_REAL r2 = o2->getCollisionGeometry()->aabb_radius;
Vec3f c1 = o1->getCollisionGeometry()->aabb_center;
Vec3f c2 = o2->getCollisionGeometry()->aabb_center;
FCL_REAL r = r1 + r2 + margin;
sampler.setBound(Vec3f(-r, -r, -r), Vec3f(r, r, r));
std::vector<Item<7> > data(n_samples);
for(std::size_t i = 0; i < n_samples; ++i)
{
Vecnf<7> q = sampler.sample();
Quaternion3f quat(q[3], q[4], q[5], q[6]);
Transform3f tf(quat, -quat.transform(c2) + c1 + Vec3f(q[0], q[1], q[2]));
CollisionRequest request;
CollisionResult result;
int n_collide = collide(o1->getCollisionGeometry(), Transform3f(),
o2->getCollisionGeometry(), tf, request, result);
data[i] = Item<7>(q, (n_collide > 0));
}
classifier->setScaler(computeScaler(data));
classifier->learn(data);
std::vector<Item<7> > svs = classifier->getSupportVectors();
for(std::size_t i = 0; i < svs.size(); ++i)
{
const Vecnf<7>& q = svs[i].q;
Quaternion3f quat(q[3], q[4], q[5], q[6]);
if(svs[i].label)
support_transforms_positive.push_back(Transform3f(quat, -quat.transform(c2) + c1 + Vec3f(q[0], q[1], q[2])));
else
support_transforms_negative.push_back(Transform3f(quat, -quat.transform(c2) + c1 + Vec3f(q[0], q[1], q[2])));
}
break;
}
case PDT_GENERAL_EULER_BALL:
{
SVMClassifier<6>* classifier = static_cast<SVMClassifier<6>*>(classifier_);
SamplerSE3Euler_ball sampler;
FCL_REAL r1 = o1->getCollisionGeometry()->aabb_radius;
FCL_REAL r2 = o2->getCollisionGeometry()->aabb_radius;
Vec3f c1 = o1->getCollisionGeometry()->aabb_center;
Vec3f c2 = o2->getCollisionGeometry()->aabb_center;
sampler.setBound(r1 + r2 + margin);
std::vector<Item<6> > data(n_samples);
for(std::size_t i = 0; i < n_samples; ++i)
{
Vecnf<6> q = sampler.sample();
Quaternion3f quat;
quat.fromEuler(q[3], q[4], q[5]);
Transform3f tf(quat, -quat.transform(c2) + c1 + Vec3f(q[0], q[1], q[2]));
CollisionRequest request;
CollisionResult result;
int n_collide = collide(o1->getCollisionGeometry(), Transform3f(),
o2->getCollisionGeometry(), tf, request, result);
data[i] = Item<6>(q, (n_collide > 0));
}
classifier->setScaler(computeScaler(data));
classifier->learn(data);
std::vector<Item<6> > svs = classifier->getSupportVectors();
for(std::size_t i = 0; i < svs.size(); ++i)
{
const Vecnf<6>& q = svs[i].q;
Quaternion3f quat;
quat.fromEuler(q[3], q[4], q[5]);
if(svs[i].label)
support_transforms_positive.push_back(Transform3f(quat, -quat.transform(c2) + c1 + Vec3f(q[0], q[1], q[2])));
else
support_transforms_negative.push_back(Transform3f(quat, -quat.transform(c2) + c1 + Vec3f(q[0], q[1], q[2])));
}
break;
}
case PDT_GENERAL_QUAT_BALL:
{
SVMClassifier<7>* classifier = static_cast<SVMClassifier<7>*>(classifier_);
SamplerSE3Quat_ball sampler;
FCL_REAL r1 = o1->getCollisionGeometry()->aabb_radius;
FCL_REAL r2 = o2->getCollisionGeometry()->aabb_radius;
Vec3f c1 = o1->getCollisionGeometry()->aabb_center;
Vec3f c2 = o2->getCollisionGeometry()->aabb_center;
sampler.setBound(r1 + r2 + margin);
std::vector<Item<7> > data(n_samples);
for(std::size_t i = 0; i < n_samples; ++i)
{
Vecnf<7> q = sampler.sample();
Quaternion3f quat(q[3], q[4], q[5], q[6]);
Transform3f tf(quat, -quat.transform(c2) + c1 + Vec3f(q[0], q[1], q[2]));
CollisionRequest request;
CollisionResult result;
int n_collide = collide(o1->getCollisionGeometry(), Transform3f(),
o2->getCollisionGeometry(), tf, request, result);
data[i] = Item<7>(q, (n_collide > 0));
}
classifier->setScaler(computeScaler(data));
classifier->learn(data);
std::vector<Item<7> > svs = classifier->getSupportVectors();
for(std::size_t i = 0; i < svs.size(); ++i)
{
const Vecnf<7>& q = svs[i].q;
Quaternion3f quat(q[3], q[4], q[5], q[6]);
if(svs[i].label)
support_transforms_positive.push_back(Transform3f(quat, -quat.transform(c2) + c1 + Vec3f(q[0], q[1], q[2])));
else
support_transforms_negative.push_back(Transform3f(quat, -quat.transform(c2) + c1 + Vec3f(q[0], q[1], q[2])));
}
break;
}
default:
;
}
// from support transforms compute contact transforms
NearestNeighbors<Transform3f>* knn_solver_positive = NULL;
NearestNeighbors<Transform3f>* knn_solver_negative = NULL;
switch(knn_solver_type)
{
case KNN_LINEAR:
knn_solver_positive = new NearestNeighborsLinear<Transform3f>();
knn_solver_negative = new NearestNeighborsLinear<Transform3f>();
break;
case KNN_GNAT:
knn_solver_positive = new NearestNeighborsGNAT<Transform3f>();
knn_solver_negative = new NearestNeighborsGNAT<Transform3f>();
break;
case KNN_SQRTAPPROX:
knn_solver_positive = new NearestNeighborsSqrtApprox<Transform3f>();
knn_solver_negative = new NearestNeighborsSqrtApprox<Transform3f>();
break;
}
knn_solver_positive->setDistanceFunction(distance_func);
knn_solver_negative->setDistanceFunction(distance_func);
knn_solver_positive->add(support_transforms_positive);
knn_solver_negative->add(support_transforms_negative);
std::vector<Transform3f> contact_vectors;
for(std::size_t i = 0; i < support_transforms_positive.size(); ++i)
{
std::vector<Transform3f> nbh;
knn_solver_negative->nearestK(support_transforms_positive[i], knn_k, nbh);
for(std::size_t j = 0; j < nbh.size(); ++j)
{
ContinuousCollisionRequest request;
request.ccd_motion_type = CCDM_LINEAR;
request.num_max_iterations = 100;
ContinuousCollisionResult result;
continuousCollide(o1->getCollisionGeometry(), Transform3f(), Transform3f(),
o2->getCollisionGeometry(), nbh[j], support_transforms_positive[i],
request, result);
//std::cout << result.time_of_contact << std::endl;
contact_vectors.push_back(result.contact_tf2);
}
}
for(std::size_t i = 0; i < support_transforms_negative.size(); ++i)
{
std::vector<Transform3f> nbh;
knn_solver_positive->nearestK(support_transforms_negative[i], knn_k, nbh);
for(std::size_t j = 0; j < nbh.size(); ++j)
{
ContinuousCollisionRequest request;
request.ccd_motion_type = CCDM_LINEAR;
request.num_max_iterations = 100;
ContinuousCollisionResult result;
continuousCollide(o1->getCollisionGeometry(), Transform3f(), Transform3f(),
o2->getCollisionGeometry(), support_transforms_negative[i], nbh[j],
request, result);
//std::cout << result.time_of_contact << std::endl;
contact_vectors.push_back(result.contact_tf2);
}
}
delete knn_solver_positive;
delete knn_solver_negative;
return contact_vectors;
}
FCL_REAL penetrationDepth(const CollisionGeometry* o1, const Transform3f& tf1,
const CollisionGeometry* o2, const Transform3f& tf2,
const PenetrationDepthRequest& request,
PenetrationDepthResult& result)
{
NearestNeighbors<Transform3f>* knn_solver = NULL;
switch(request.knn_solver_type)
{
case KNN_LINEAR:
knn_solver = new NearestNeighborsLinear<Transform3f>();
break;
case KNN_GNAT:
knn_solver = new NearestNeighborsGNAT<Transform3f>();
break;
case KNN_SQRTAPPROX:
knn_solver = new NearestNeighborsSqrtApprox<Transform3f>();
break;
}
knn_solver->setDistanceFunction(request.distance_func);
knn_solver->add(request.contact_vectors);
Transform3f tf;
relativeTransform2(tf1, tf2, tf);
result.resolved_tf = tf1 * knn_solver->nearest(tf);
result.pd_value = request.distance_func(result.resolved_tf, tf2);
result.pd_value = std::sqrt(result.pd_value);
delete knn_solver;
return result.pd_value;
}
FCL_REAL penetrationDepth(const CollisionObject* o1,
const CollisionObject* o2,
const PenetrationDepthRequest& request,
PenetrationDepthResult& result)
{
return penetrationDepth(o1->getCollisionGeometry(), o1->getTransform(),
o2->getCollisionGeometry(), o2->getTransform(),
request,
result);
}
}
......@@ -29,21 +29,20 @@ add_fcl_test(test_fcl_math test_fcl_math.cpp test_fcl_utility.cpp)
add_fcl_test(test_fcl_sphere_capsule test_fcl_sphere_capsule.cpp)
add_fcl_test(test_fcl_simple test_fcl_simple.cpp)
add_fcl_test(test_fcl_global_penetration test_fcl_global_penetration.cpp libsvm/svm.cpp test_fcl_utility.cpp)
#add_fcl_test(test_fcl_global_penetration test_fcl_global_penetration.cpp libsvm/svm.cpp test_fcl_utility.cpp)
if (FCL_HAVE_OCTOMAP)
add_fcl_test(test_fcl_octomap test_fcl_octomap.cpp test_fcl_utility.cpp)
endif()
if (FCL_HAVE_TINYXML)
add_executable(test_fcl_xmldata test_fcl_xmldata.cpp test_fcl_utility.cpp libsvm/svm.cpp)
target_link_libraries(test_fcl_xmldata
fcl
${TINYXML_LIBRARY_DIRS}
${Boost_SYSTEM_LIBRARY}
${Boost_THREAD_LIBRARY}
${Boost_DATE_TIME_LIBRARY}
${Boost_UNIT_TEST_FRAMEWORK_LIBRARY})
add_test(test_fcl_xmldata ${EXECUTABLE_OUTPUT_PATH}/test_fcl_xmldata)
endif()
#if (FCL_HAVE_TINYXML)
# add_executable(test_fcl_xmldata test_fcl_xmldata.cpp test_fcl_utility.cpp libsvm/svm.cpp)
# target_link_libraries(test_fcl_xmldata
# fcl
# ${TINYXML_LIBRARY_DIRS}
# ${Boost_SYSTEM_LIBRARY}
# ${Boost_THREAD_LIBRARY}
# ${Boost_DATE_TIME_LIBRARY}
# ${Boost_UNIT_TEST_FRAMEWORK_LIBRARY})
# add_test(test_fcl_xmldata ${EXECUTABLE_OUTPUT_PATH}/test_fcl_xmldata)
#endif()