Skip to content

Instantly share code, notes, and snippets.

@schmrlng
Last active November 12, 2020 23:05
Show Gist options
  • Save schmrlng/e2eb17071cd8213d477d7a2b9227bb23 to your computer and use it in GitHub Desktop.
Save schmrlng/e2eb17071cd8213d477d7a2b9227bb23 to your computer and use it in GitHub Desktop.
FCL motion and continuous collision checking
#include <iostream>
#include <fcl/shape/geometric_shapes.h>
#include <fcl/collision.h>
#include <fcl/continuous_collision.h>
#include <fcl/ccd/motion.h>
using namespace std;
using namespace fcl;
int main(int argc, char** argv)
{
Transform3f tf0(Vec3f(1,1,1)), tf1(Vec3f(2,2,2));
Transform3f tf;
TranslationMotion tm = TranslationMotion(tf0, tf1);
InterpMotion im = InterpMotion(tf0, tf1);
ScrewMotion sm = ScrewMotion(tf0, tf1);
std::cout << "-------------- TranslationMotion --------------" << std::endl;
std::cout << "getCurrentTransform at motion initialization" << std::endl;
tm.getCurrentTransform(tf);
std::cout << "T = " << tf.getTranslation() << std::endl;
std::cout << "R = " << tf.getRotation() << std::endl;
std::cout << "q = " << tf.getQuatRotation() << std::endl;
for (double t = 0.0; t <= 1.0; t = t + 0.2) {
std::cout << "TranslationMotion interpolated at t = " << t << std::endl;
tm.integrate(t);
tm.getCurrentTransform(tf);
std::cout << "T = " << tf.getTranslation() << std::endl;
std::cout << "R = " << tf.getRotation() << std::endl;
std::cout << "q = " << tf.getQuatRotation() << std::endl;
}
std::cout << "-------------- InterpMotion --------------" << std::endl;
std::cout << "getCurrentTransform at motion initialization" << std::endl;
im.getCurrentTransform(tf);
std::cout << "T = " << tf.getTranslation() << std::endl;
std::cout << "R = " << tf.getRotation() << std::endl;
std::cout << "q = " << tf.getQuatRotation() << std::endl;
for (double t = 0.0; t <= 1.0; t = t + 0.2) {
std::cout << "InterpMotion interpolated at t = " << t << std::endl;
im.integrate(t);
im.getCurrentTransform(tf);
std::cout << "T = " << tf.getTranslation() << std::endl;
std::cout << "R = " << tf.getRotation() << std::endl;
std::cout << "q = " << tf.getQuatRotation() << std::endl;
}
std::cout << "-------------- ScrewMotion --------------" << std::endl;
std::cout << "getCurrentTransform at motion initialization" << std::endl;
sm.getCurrentTransform(tf);
std::cout << "T = " << tf.getTranslation() << std::endl;
std::cout << "R = " << tf.getRotation() << std::endl;
std::cout << "q = " << tf.getQuatRotation() << std::endl;
for (double t = 0.0; t <= 1.0; t = t + 0.2) {
std::cout << "ScrewMotion interpolated at t = " << t << std::endl;
sm.integrate(t);
sm.getCurrentTransform(tf);
std::cout << "T = " << tf.getTranslation() << std::endl;
std::cout << "R = " << tf.getRotation() << std::endl;
std::cout << "q = " << tf.getQuatRotation() << std::endl;
}
std::cout << "-------------- Continuous Collision Failures --------------" << std::endl;
ContinuousCollisionRequest request;
request.ccd_solver_type = CCDC_CONSERVATIVE_ADVANCEMENT;
request.ccd_motion_type = CCDM_TRANS;
ContinuousCollisionResult result;
std::shared_ptr<Box> box0(new Box(0.02,0.02,0.02));
std::shared_ptr<Box> box1(new Box(0.03,0.38,0.98));
tf0 = Transform3f(Vec3f(0.1,0.1,0.1));
tf1 = Transform3f(Vec3f(0.275,0.2,0.5));
Transform3f tf2 = Transform3f(Vec3f(0.416767, 0.332619, 0.552623));
CollisionObject co0(box0, tf0);
CollisionObject co1(box1, tf1);
continuousCollide(&co0, tf2, &co1, tf1, request, result);
std::cout << "CCDC_CONSERVATIVE_ADVANCEMENT, CCDM_TRANS" << std::endl;
std::cout << "box0 with side " << ((Box*)(co0.collisionGeometry().get()))->side << " from " << co0.getTranslation() << " to " << tf2.getTranslation() << std::endl;
std::cout << "box1 with side " << box1->side << " from " << co1.getTranslation() << " to " << tf1.getTranslation() << std::endl;
std::cout << "continuous collision result: " << result.is_collide << std::endl;
result.is_collide = false;
result.time_of_contact = 1.0;
request.ccd_motion_type = CCDM_LINEAR;
continuousCollide(&co0, tf2, &co1, tf1, request, result);
std::cout << "CCDC_CONSERVATIVE_ADVANCEMENT, CCDM_LINEAR" << std::endl;
std::cout << "box0 with side " << box0->side << " from " << co0.getTranslation() << " to " << tf2.getTranslation() << std::endl;
std::cout << "box1 with side " << box1->side << " from " << co1.getTranslation() << " to " << tf1.getTranslation() << std::endl;
std::cout << "continuous collision result: " << result.is_collide << std::endl;
result.is_collide = false;
result.time_of_contact = 1.0;
request.ccd_motion_type = CCDM_SCREW;
continuousCollide(&co0, tf2, &co1, tf1, request, result);
std::cout << "CCDC_CONSERVATIVE_ADVANCEMENT, CCDM_SCREW" << std::endl;
std::cout << "box0 with side " << box0->side << " from " << co0.getTranslation() << " to " << tf2.getTranslation() << std::endl;
std::cout << "box1 with side " << box1->side << " from " << co1.getTranslation() << " to " << tf1.getTranslation() << std::endl;
std::cout << "continuous collision result: " << result.is_collide << std::endl;
std::cout << "Collision Verification" << std::endl;
InterpMotion im0 = InterpMotion(co0.getTransform(), tf2);
InterpMotion im1 = InterpMotion(co1.getTransform(), tf1);
double toc = (.25 - .1) / (0.416767 - .1);
Transform3f tf3, tf4;
CollisionRequest req;
CollisionResult res;
im0.integrate(toc+.001);
im1.integrate(toc+.001);
im0.getCurrentTransform(tf3);
im1.getCurrentTransform(tf4);
collide(co0.collisionGeometry().get(), tf3, co1.collisionGeometry().get(), tf4, req, res);
std::cout << "------" << std::endl << "collision result at time t = " << toc+.001 << ": " << res.isCollision() << std::endl;
std::cout << "box0 with side " << box0->side << " at " << tf3.getTranslation() << std::endl;
std::cout << "box1 with side " << box1->side << " at " << tf4.getTranslation() << std::endl;
res.clear();
im0.integrate(toc-.001);
im1.integrate(toc-.001);
im0.getCurrentTransform(tf3);
im1.getCurrentTransform(tf4);
collide(co0.collisionGeometry().get(), tf3, co1.collisionGeometry().get(), tf4, req, res);
std::cout << "------" << std::endl << "collision result at time t = " << toc-.001 << ": " << res.isCollision() << std::endl;
std::cout << "box0 with side " << box0->side << " at " << tf3.getTranslation() << std::endl;
std::cout << "box1 with side " << box1->side << " at " << tf4.getTranslation() << std::endl;
return 0;
}
// Output
/*
-------------- TranslationMotion --------------
getCurrentTransform at motion initialization
T = (0 0 0)
R = [(1 0 0)(0 1 0)(0 0 1)]
q = (1 0 0 0)
TranslationMotion interpolated at t = 0
T = (1 1 1)
R = [(1 0 0)(0 1 0)(0 0 1)]
q = (1 0 0 0)
TranslationMotion interpolated at t = 0.2
T = (1.2 1.2 1.2)
R = [(1 0 0)(0 1 0)(0 0 1)]
q = (1 0 0 0)
TranslationMotion interpolated at t = 0.4
T = (1.4 1.4 1.4)
R = [(1 0 0)(0 1 0)(0 0 1)]
q = (1 0 0 0)
TranslationMotion interpolated at t = 0.6
T = (1.6 1.6 1.6)
R = [(1 0 0)(0 1 0)(0 0 1)]
q = (1 0 0 0)
TranslationMotion interpolated at t = 0.8
T = (1.8 1.8 1.8)
R = [(1 0 0)(0 1 0)(0 0 1)]
q = (1 0 0 0)
TranslationMotion interpolated at t = 1
T = (2 2 2)
R = [(1 0 0)(0 1 0)(0 0 1)]
q = (1 0 0 0)
-------------- InterpMotion --------------
getCurrentTransform at motion initialization
T = (1 1 1)
R = [(1 0 0)(0 1 0)(0 0 1)]
q = (1 0 0 0)
InterpMotion interpolated at t = 0
T = (1 1 1)
R = [(1 0 0)(0 1 0)(0 0 1)]
q = (1 0 0 0)
InterpMotion interpolated at t = 0.2
T = (1.2 1.2 1.2)
R = [(1 0 0)(0 1 0)(0 0 1)]
q = (1 0 0 0)
InterpMotion interpolated at t = 0.4
T = (1.4 1.4 1.4)
R = [(1 0 0)(0 1 0)(0 0 1)]
q = (1 0 0 0)
InterpMotion interpolated at t = 0.6
T = (1.6 1.6 1.6)
R = [(1 0 0)(0 1 0)(0 0 1)]
q = (1 0 0 0)
InterpMotion interpolated at t = 0.8
T = (1.8 1.8 1.8)
R = [(1 0 0)(0 1 0)(0 0 1)]
q = (1 0 0 0)
InterpMotion interpolated at t = 1
T = (2 2 2)
R = [(1 0 0)(0 1 0)(0 0 1)]
q = (1 0 0 0)
-------------- ScrewMotion --------------
getCurrentTransform at motion initialization
T = (1 1 1)
R = [(1 0 0)(0 1 0)(0 0 1)]
q = (1 0 0 0)
ScrewMotion interpolated at t = 0
T = (1 1 1)
R = [(1 0 0)(0 1 0)(0 0 1)]
q = (1 0 0 0)
ScrewMotion interpolated at t = 0.2
T = (1.34641 1.34641 1.34641)
R = [(1 0 0)(0 1 0)(0 0 1)]
q = (1 0 0 0)
ScrewMotion interpolated at t = 0.4
T = (1.69282 1.69282 1.69282)
R = [(1 0 0)(0 1 0)(0 0 1)]
q = (1 0 0 0)
ScrewMotion interpolated at t = 0.6
T = (2.03923 2.03923 2.03923)
R = [(1 0 0)(0 1 0)(0 0 1)]
q = (1 0 0 0)
ScrewMotion interpolated at t = 0.8
T = (2.38564 2.38564 2.38564)
R = [(1 0 0)(0 1 0)(0 0 1)]
q = (1 0 0 0)
ScrewMotion interpolated at t = 1
T = (2.73205 2.73205 2.73205)
R = [(1 0 0)(0 1 0)(0 0 1)]
q = (1 0 0 0)
-------------- Continuous Collision Failures --------------
CCDC_CONSERVATIVE_ADVANCEMENT, CCDM_TRANS
box0 with side (0.02 0.02 0.02) from (0.1 0.1 0.1) to (0.416767 0.332619 0.552623)
box1 with side (0.03 0.38 0.98) from (0.275 0.2 0.5) to (0.275 0.2 0.5)
continuous collision result: 1
CCDC_CONSERVATIVE_ADVANCEMENT, CCDM_LINEAR
box0 with side (0.02 0.02 0.02) from (0.1 0.1 0.1) to (0.416767 0.332619 0.552623)
box1 with side (0.03 0.38 0.98) from (0.275 0.2 0.5) to (0.275 0.2 0.5)
continuous collision result: 0
CCDC_CONSERVATIVE_ADVANCEMENT, CCDM_SCREW
box0 with side (0.02 0.02 0.02) from (0.1 0.1 0.1) to (0.416767 0.332619 0.552623)
box1 with side (0.03 0.38 0.98) from (0.275 0.2 0.5) to (0.275 0.2 0.5)
continuous collision result: 0
Collision Verification
------
collision result at time t = 0.474534: 1
box0 with side (0.02 0.02 0.02) at (0.250317 0.210386 0.314785)
box1 with side (0.03 0.38 0.98) at (0.275 0.2 0.5)
------
collision result at time t = 0.472534: 0
box0 with side (0.02 0.02 0.02) at (0.249683 0.20992 0.31388)
box1 with side (0.03 0.38 0.98) at (0.275 0.2 0.5)
Process finished with exit code 0
*/
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment