Skip to content
Snippets Groups Projects
Commit 36e37c79 authored by panjia1983's avatar panjia1983
Browse files

remove global pd due to license issue

parent 182e5ce4
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
README.md
+ 17
− 17
View file @ 36e37c79
......@@ -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.
include/fcl/penetration_depth.h deleted 100644 → 0
+ 0
− 113
View file @ 182e5ce4
#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
src/penetration_depth.cpp deleted 100644 → 0
+ 0
− 402
View file @ 182e5ce4
#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);
}
}
test/CMakeLists.txt
+ 12
− 13
View file @ 36e37c79
......@@ -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()
test/fcl_resources/config.h
+ 1
− 1
View file @ 36e37c79
......@@ -37,5 +37,5 @@
#ifndef FCL_TEST_RESOURCES_CONFIG_
#define FCL_TEST_RESOURCES_CONFIG_
#define TEST_RESOURCES_DIR "/home/jia/Workspace/fcl2/test/fcl_resources"
#define TEST_RESOURCES_DIR "/home/jia/Workspace/fcl/test/fcl_resources"
#endif
test/test_fcl_global_penetration.cpp deleted 100644 → 0
+ 0
− 69
View file @ 182e5ce4
#define BOOST_TEST_MODULE "FCL_GLOBAL_PENETRATION"
#include <boost/test/unit_test.hpp>
#include "libsvm_classifier.h"
#include "fcl/penetration_depth.h"
#include <boost/filesystem.hpp>
#include "fcl_resources/config.h"
#include "test_fcl_utility.h"
#include "fcl/collision.h"
#include "fcl/BVH/BVH_model.h"
using namespace fcl;
BOOST_AUTO_TEST_CASE(global_penetration_test)
{
LibSVMClassifier<6> classifier;
std::vector<Vec3f> p1, p2;
std::vector<Triangle> t1, t2;
boost::filesystem::path path(TEST_RESOURCES_DIR);
loadOBJFile((path / "env.obj").string().c_str(), p1, t1);
loadOBJFile((path / "rob.obj").string().c_str(), p2, t2);
BVHModel<OBBRSS>* m1 = new BVHModel<OBBRSS>();
BVHModel<OBBRSS>* m2 = new BVHModel<OBBRSS>();
m1->beginModel();
m1->addSubModel(p1, t1);
m1->endModel();
m2->beginModel();
m2->addSubModel(p2, t2);
m2->endModel();
std::cout << m1->computeVolume() << std::endl;
std::cout << m2->computeVolume() << std::endl;
std::cout << m1->computeMomentofInertia() << std::endl;
std::cout << m2->computeMomentofInertia() << std::endl;
std::cout << m1->computeMomentofInertiaRelatedToCOM() << std::endl;
std::cout << m2->computeMomentofInertiaRelatedToCOM() << std::endl;
Transform3f id;
CollisionObject o1(boost::shared_ptr<CollisionGeometry>(m1), id);
CollisionObject o2(boost::shared_ptr<CollisionGeometry>(m2), id);
default_transform_distancer = DefaultTransformDistancer(o2.getCollisionGeometry());
std::size_t KNN_K = 10;
std::vector<Transform3f> contact_vectors = penetrationDepthModelLearning(&o1, &o2, PDT_GENERAL_EULER, &classifier, 10000, 0, KNN_GNAT, KNN_K);
classifier.save("model.txt");
PenetrationDepthRequest request(&classifier, default_transform_distance_func);
request.contact_vectors = contact_vectors;
PenetrationDepthResult result;
penetrationDepth(o1.getCollisionGeometry(), Transform3f(),
o2.getCollisionGeometry(), Transform3f(),
request, result);
std::cout << result.pd_value << std::endl;
}
test/test_fcl_xmldata.cpp deleted 100644 → 0
+ 0
− 455
View file @ 182e5ce4
#define BOOST_TEST_MODULE "FCL_XMLDATA"
#include <tinyxml.h>
#include <boost/test/unit_test.hpp>
#include "fcl/intersect.h"
#include "fcl/collision.h"
#include "fcl/math/sampling.h"
#include "fcl/BVH/BVH_model.h"
#include <boost/filesystem.hpp>
#include <sstream>
#include "libsvm_classifier.h"
#include "fcl/penetration_depth.h"
#include "fcl/collision_data.h"
#include "fcl_resources/config.h"
#include "test_fcl_utility.h"
using namespace fcl;
static void loadSceneFile(const std::string& filename,
std::vector<std::vector<Vec3f> >& points_array,
std::vector<std::vector<Triangle> >& triangles_array,
std::vector<std::pair<Transform3f, Transform3f> >& motions)
{
TiXmlDocument doc(filename.c_str());
if(doc.LoadFile())
{
TiXmlHandle hdoc(&doc);
TiXmlElement* model, *motion;
model = doc.FirstChildElement("MODEL");
if(model)
{
TiXmlElement* object = model->FirstChildElement("OBJECT");
int i = 0;
while(object)
{
int object_id = -1;
object->Attribute("id", &object_id);
// std::cout << object_id << std::endl;
std::vector<Vec3f> points;
std::vector<Triangle> triangles;
TiXmlElement* grid = object->FirstChildElement("GRID");
int n_vertices = 0;
while(grid)
{
int grid_id;
double grid_x, grid_y, grid_z;
grid->Attribute("id", &grid_id);
grid->Attribute("x", &grid_x);
grid->Attribute("y", &grid_y);
grid->Attribute("z", &grid_z);
Vec3f p(grid_x, grid_y, grid_z);
if(grid_id - 1 == (int)points.size())
points.push_back(p);
else if(grid_id - 1 < (int)points.size())
points[grid_id - 1] = p;
else // if(grid_id - 1 > points.size())
{
points.resize(grid_id);
points.back() = p;
}
n_vertices++;
grid = grid->NextSiblingElement("GRID");
}
// std::cout << "#vertices " << n_vertices << std::endl;
TiXmlElement* tri = object->FirstChildElement("TRIA");
int n_tris = 0;
while(tri)
{
int tri_id;
int v1, v2, v3;
tri->Attribute("id", &tri_id);
tri->Attribute("g1", &v1);
tri->Attribute("g2", &v2);
tri->Attribute("g3", &v3);
Triangle t(v1-1, v2-1, v3-1);
if(tri_id - 1 == (int)triangles.size())
triangles.push_back(t);
else if(tri_id - 1 < (int)triangles.size())
triangles[tri_id - 1] = t;
else
{
triangles.resize(tri_id);
triangles.back() = t;
}
n_tris++;
tri = tri->NextSiblingElement("TRIA");
}
// std::cout << "#triangles " << n_tris << std::endl;
if(object_id - 1 == (int)points_array.size())
{
points_array.push_back(points);
triangles_array.push_back(triangles);
}
else if(object_id - 1 < (int)points_array.size())
{
points_array[object_id - 1] = points;
triangles_array[object_id - 1] = triangles;
}
else
{
points_array.resize(object_id);
triangles_array.resize(object_id);
points_array.back() = points;
triangles_array.back() = triangles;
}
object = object->NextSiblingElement("OBJECT");
i++;
}
// std::cout << "#objects " << i << std::endl;
}
motion = doc.FirstChildElement("MOTION");
if(motion)
{
TiXmlElement* frame = motion->FirstChildElement("FRAME");
int n_frame = 0;
while(frame)
{
int frame_id;
double frame_time;
frame->Attribute("id", &frame_id);
frame->Attribute("time", &frame_time);
Vec3f T1, T2;
Matrix3f R1, R2;
const char* obj1_pos_string = frame->Attribute("obj1_pos");
const char* obj2_pos_string = frame->Attribute("obj2_pos");
const char* obj1_dc_string = frame->Attribute("obj1_dc");
const char* obj2_dc_string = frame->Attribute("obj2_dc");
std::stringstream s1_pos(obj1_pos_string);
s1_pos >> T1[0] >> T1[1] >> T1[2];
std::stringstream s2_pos(obj2_pos_string);
s2_pos >> T2[0] >> T2[1] >> T2[2];
std::stringstream s1_mat(obj1_dc_string);
for(int j = 0; j < 3; ++j)
for(int k = 0; k < 3; ++k)
s1_mat >> R1(j, k);
std::stringstream s2_mat(obj2_dc_string);
for(int j = 0; j < 3; ++j)
for(int k = 0; k < 3; ++k)
s2_mat >> R2(j, k);
std::pair<Transform3f, Transform3f> motion = std::make_pair(Transform3f(R1, T1), Transform3f(R2, T2));
if(frame_id - 1 == (int)motions.size())
motions.push_back(motion);
else if(frame_id - 1 < (int)motions.size())
motions[frame_id - 1] = motion;
else
{
motions.resize(frame_id);
motions.back() = motion;
}
frame = frame->NextSiblingElement("FRAME");
n_frame++;
}
// std::cout << "#frames " << n_frame << std::endl;
}
}
else
std::cerr << "Failed to load file " << filename << std::endl;
}
BOOST_AUTO_TEST_CASE(scene_test)
{
std::vector<std::vector<Vec3f> > points_array;
std::vector<std::vector<Triangle> > triangles_array;
std::vector<std::pair<Transform3f, Transform3f> > motions;
boost::filesystem::path path(TEST_RESOURCES_DIR);
std::string filename = (path / "scenario-1-2-3/Model_1_Scenario_3.txt").string();
loadSceneFile(filename, points_array, triangles_array, motions);
BVHModel<OBBRSS> m1;
BVHModel<OBBRSS> m2;
m1.beginModel();
m1.addSubModel(points_array[0], triangles_array[0]);
m1.endModel();
m2.beginModel();
m2.addSubModel(points_array[1], triangles_array[1]);
m2.endModel();
CollisionResult result0;
CollisionRequest request0;
collide(&m1, motions[0].first, &m2, motions[0].second, request0, result0);
// std::cout << result0.numContacts() << std::endl;
CollisionResult result1;
CollisionRequest request1;
collide(&m1, motions[1].first, &m2, motions[1].second, request1, result1);
// std::cout << result1.numContacts() << std::endl;
}
static std::string num2string(int i)
{
std::string result;
std::ostringstream convert;
convert << i;
result = convert.str();
return result;
}
static void xml2obj(const std::string& in_filename, const std::string& out_filename_base)
{
std::vector<std::vector<Vec3f> > points_array;
std::vector<std::vector<Triangle> > triangles_array;
std::vector<std::pair<Transform3f, Transform3f> > motions;
loadSceneFile(in_filename, points_array, triangles_array, motions);
std::size_t n_obj = points_array.size();
// save objs in local frame
for(std::size_t i = 0; i < n_obj; ++i)
{
std::string out_filenameL = out_filename_base + num2string(i+1) + "L.obj";
saveOBJFile(out_filenameL.c_str(), points_array[i], triangles_array[i]);
}
// save objs in frame 1
for(std::size_t i = 0; i < n_obj; ++i)
{
std::string out_filenameF1 = out_filename_base + num2string(i+1) + "Frame1.obj";
std::vector<Vec3f> points(points_array[i].size());
for(std::size_t j = 0; j < points.size(); ++j)
points[j] = motions[i].first.transform(points_array[i][j]);
saveOBJFile(out_filenameF1.c_str(), points, triangles_array[i]);
}
// save objs in frame 2
for(std::size_t i = 0; i < n_obj; ++i)
{
std::string out_filenameF2 = out_filename_base + num2string(i+1) + "Frame2.obj";
std::vector<Vec3f> points(points_array[i].size());
for(std::size_t j = 0; j < points.size(); ++j)
points[j] = motions[i].second.transform(points_array[i][j]);
saveOBJFile(out_filenameF2.c_str(), points, triangles_array[i]);
}
}
static void scenePenetrationTest(const std::string& filename, PenetrationDepthType pd_type = PDT_GENERAL_EULER)
{
std::vector<std::vector<Vec3f> > points_array;
std::vector<std::vector<Triangle> > triangles_array;
std::vector<std::pair<Transform3f, Transform3f> > motions;
loadSceneFile(filename, points_array, triangles_array, motions);
BVHModel<OBBRSS>* m1 = new BVHModel<OBBRSS>();
BVHModel<OBBRSS>* m2 = new BVHModel<OBBRSS>();
m1->beginModel();
m1->addSubModel(points_array[0], triangles_array[0]);
m1->endModel();
m2->beginModel();
m2->addSubModel(points_array[1], triangles_array[1]);
m2->endModel();
Transform3f id;
CollisionObject o1(boost::shared_ptr<CollisionGeometry>(m1), id);
CollisionObject o2(boost::shared_ptr<CollisionGeometry>(m2), id);
default_transform_distancer = DefaultTransformDistancer(o1.getCollisionGeometry());
std::cout << "rotation weights ";
default_transform_distancer.printRotWeight();
std::size_t KNN_K = 10;
LibSVMClassifier<6> classifier;
std::vector<Transform3f> contact_vectors = penetrationDepthModelLearning(&o1, &o2, pd_type, &classifier, 100000, 0, KNN_GNAT, KNN_K);
classifier.save(filename + "model.txt");
for(std::size_t frame_id = 0; frame_id < motions.size(); ++frame_id)
{
PenetrationDepthRequest request(&classifier, default_transform_distance_func);
request.contact_vectors = contact_vectors;
PenetrationDepthResult result;
penetrationDepth(m1, motions[frame_id].first, m2, motions[frame_id].second, request, result);
std::cout << "pd value " << frame_id << " " << result.pd_value << std::endl;
std::cout << "resolved tf translation " << result.resolved_tf.getTranslation() << std::endl;
std::cout << "resolved tf rotation " << result.resolved_tf.getRotation() << std::endl;
int num_pd_contacts = 1; // only one contact for pd, you can set a larger number if you want more
CollisionRequest pd_contact_request(num_pd_contacts, true);
CollisionResult pd_contact_result;
collide(m1, motions[frame_id].first, m2, result.resolved_tf, pd_contact_request, pd_contact_result);
if(pd_contact_result.numContacts() > 0)
{
Contact contact = pd_contact_result.getContact(0);
std::cout << "contact pos " << contact.pos << std::endl;
std::cout << "contact normal " << contact.normal << std::endl;
}
}
}
BOOST_AUTO_TEST_CASE(scene_test_penetration)
{
RNG::setSeed(1);
boost::filesystem::path path(TEST_RESOURCES_DIR);
std::cout << "manyframes/Model_5" << std::endl;
std::string filename0 = (path / "manyframes/Model_5.xml").string();
scenePenetrationTest(filename0);
return;
std::cout << "scenario-1-2-3/Model_1_Scenario_1" << std::endl;
std::string filename1 = (path / "scenario-1-2-3/Model_1_Scenario_1.txt").string();
scenePenetrationTest(filename1);
std::cout << std::endl;
std::cout << "scenario-1-2-3/Model_1_Scenario_2" << std::endl;
std::string filename2 = (path / "scenario-1-2-3/Model_1_Scenario_2.txt").string();
scenePenetrationTest(filename2);
std::cout << std::endl;
std::cout << "scenario-1-2-3/Model_1_Scenario_3" << std::endl;
std::string filename3 = (path / "scenario-1-2-3/Model_1_Scenario_3.txt").string();
scenePenetrationTest(filename3);
std::cout << std::endl;
std::cout << "scenario-1-2-3/Model_2_Scenario_1" << std::endl;
std::string filename4 = (path / "scenario-1-2-3/Model_2_Scenario_1.txt").string();
scenePenetrationTest(filename4);
std::cout << std::endl;
std::cout << "scenario-1-2-3/Model_2_Scenario_2" << std::endl;
std::string filename5 = (path / "scenario-1-2-3/Model_2_Scenario_2.txt").string();
scenePenetrationTest(filename5);
std::cout << std::endl;
std::cout << "scenario-1-2-3/Model_2_Scenario_3" << std::endl;
std::string filename6 = (path / "scenario-1-2-3/Model_2_Scenario_3.txt").string();
scenePenetrationTest(filename6);
std::cout << std::endl;
std::cout << "scenario-1-2-3/Model_3_Scenario_1" << std::endl;
std::string filename7 = (path / "scenario-1-2-3/Model_3_Scenario_1.txt").string();
scenePenetrationTest(filename7);
std::cout << std::endl;
std::cout << "scenario-1-2-3/Model_3_Scenario_2" << std::endl;
std::string filename8 = (path / "scenario-1-2-3/Model_3_Scenario_2.txt").string();
scenePenetrationTest(filename8);
std::cout << std::endl;
std::cout << "scenario-1-2-3/Model_3_Scenario_3" << std::endl;
std::string filename9 = (path / "scenario-1-2-3/Model_3_Scenario_3.txt").string();
scenePenetrationTest(filename9);
std::cout << std::endl;
std::cout << "newdata/Model_1_Scenario_3" << std::endl;
std::string filename10 = (path / "newdata/Model_1_Scenario_3.xml").string();
scenePenetrationTest(filename10);
std::cout << std::endl;
std::cout << "newdata/Model_2_Scenario_3" << std::endl;
std::string filename11 = (path / "newdata/Model_2_Scenario_3.xml").string();
scenePenetrationTest(filename11);
std::cout << std::endl;
std::cout << "newdata/Model_3_Scenario_3" << std::endl;
std::string filename12 = (path / "newdata/Model_3_Scenario_3.xml").string();
scenePenetrationTest(filename12);
std::cout << std::endl;
}
BOOST_AUTO_TEST_CASE(xml2obj_test)
{
boost::filesystem::path path(TEST_RESOURCES_DIR);
std::string filename_manyframe0 = (path / "manyframes/Model_5.xml").string();
xml2obj(filename_manyframe0, "Model_5");
std::string filename_manyframe1 = (path / "manyframes/Model_1.xml").string();
xml2obj(filename_manyframe1, "Model_1");
std::string filename_manyframe2 = (path / "manyframes/Model_4.xml").string();
xml2obj(filename_manyframe2, "Model_4");
std::string filename1 = (path / "scenario-1-2-3/Model_1_Scenario_1.txt").string();
xml2obj(filename1, "Model_1_Scenario_1");
std::string filename2 = (path / "scenario-1-2-3/Model_1_Scenario_2.txt").string();
xml2obj(filename2, "Model_1_Scenario_2");
std::string filename3 = (path / "scenario-1-2-3/Model_1_Scenario_3.txt").string();
xml2obj(filename3, "Model_1_Scenario_3");
std::string filename4 = (path / "scenario-1-2-3/Model_2_Scenario_1.txt").string();
xml2obj(filename4, "Model_2_Scenario_1");
std::string filename5 = (path / "scenario-1-2-3/Model_2_Scenario_2.txt").string();
xml2obj(filename5, "Model_2_Scenario_2");
std::string filename6 = (path / "scenario-1-2-3/Model_2_Scenario_3.txt").string();
xml2obj(filename6, "Model_2_Scenario_3");
std::string filename7 = (path / "scenario-1-2-3/Model_3_Scenario_1.txt").string();
xml2obj(filename7, "Model_3_Scenario_1");
std::string filename8 = (path / "scenario-1-2-3/Model_3_Scenario_2.txt").string();
xml2obj(filename8, "Model_3_Scenario_2");
std::string filename9 = (path / "scenario-1-2-3/Model_3_Scenario_3.txt").string();
xml2obj(filename9, "Model_3_Scenario_3");
}
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment