From 1f4da2f7da05f3106eea44366a1cfc92cd07277f Mon Sep 17 00:00:00 2001 From: Masaki Murooka Date: Wed, 18 Sep 2019 22:22:16 +0900 Subject: [PATCH 1/2] irteus: add eusbullet. --- .travis-osx.sh | 1 + .travis.sh | 5 +- .travis.yml | 2 +- README.md | 4 +- irteus/CBULLET.cpp | 264 +++++++++++++++++++++++++++++++++ irteus/Makefile | 30 +++- irteus/bullet.l | 126 ++++++++++++++++ irteus/compile_irtg.l | 2 + irteus/demo/sample-collision.l | 123 +++++++++++++++ irteus/eusbullet.c | 124 ++++++++++++++++ irteus/irtcollision.l | 81 ++++++++++ irteus/irtext.l | 2 +- irteus/test/test-collision.l | 149 +++++++++++++++++++ 13 files changed, 905 insertions(+), 8 deletions(-) create mode 100644 irteus/CBULLET.cpp create mode 100644 irteus/bullet.l create mode 100644 irteus/demo/sample-collision.l create mode 100644 irteus/eusbullet.c create mode 100644 irteus/irtcollision.l create mode 100644 irteus/test/test-collision.l diff --git a/.travis-osx.sh b/.travis-osx.sh index 25ec7e749..521d810ae 100755 --- a/.travis-osx.sh +++ b/.travis-osx.sh @@ -41,6 +41,7 @@ setup_make() { brew list mesalib-glw &>/dev/null || brew install mesalib-glw brew list wget &>/dev/null || brew install wget brew list poppler &>/dev/null || brew install poppler + brew list bullet &>/dev/null || brew install bullet travis_time_end # travis_time_start install.x11 diff --git a/.travis.sh b/.travis.sh index 21fe1c5be..ca81e7cfb 100755 --- a/.travis.sh +++ b/.travis.sh @@ -25,7 +25,7 @@ if [ "$(which sudo)" == "" ]; then apt-get update && apt-get install -y sudo; el travis_time_end travis_time_start setup.apt-get_install -sudo apt-get install -qq -y git make gcc g++ libjpeg-dev libxext-dev libx11-dev libgl1-mesa-dev libglu1-mesa-dev libpq-dev libpng-dev xfonts-100dpi xfonts-75dpi # msttcorefonts could not install on 14.04 travis +sudo apt-get install -qq -y git make gcc g++ libjpeg-dev libxext-dev libx11-dev libgl1-mesa-dev libglu1-mesa-dev libpq-dev libpng-dev xfonts-100dpi xfonts-75dpi pkg-config libbullet-dev # msttcorefonts could not install on 14.04 travis # sudo apt-get install -qq -y texlive-latex-base ptex-bin latex2html nkf poppler-utils || echo "ok" # 16.04 does ont have ptex bin travis_time_end @@ -51,6 +51,9 @@ for test_l in irteus/test/*.l; do # osrf/ubuntu_arm64:trusty takes >50 min, skip irteus-demo.l [[ "$DOCKER_IMAGE" == *"arm64:trusty"* && $test_l =~ irteus-demo.l ]] && continue; + # skip collision test because bullet of 2.83 or later version is not released in trusty and jessie. + # https://github.com/euslisp/jskeus/blob/6cb08aa6c66fa8759591de25b7da68baf76d5f09/irteus/Makefile#L37 + [[ ( "$DOCKER_IMAGE" == *"trusty"* || "$DOCKER_IMAGE" == *"jessie"* ) && $test_l =~ test-collision.l ]] && continue; travis_time_start jskeus.source.${test_l##*/}.test diff --git a/.travis.yml b/.travis.yml index 7edb3e01c..00bd84b30 100644 --- a/.travis.yml +++ b/.travis.yml @@ -75,7 +75,7 @@ script: # Test installing head version jskeus via Homebrew formula - if [ "$TRAVIS_OS_NAME" = "osx" ]; then $CI_SOURCE_PATH/.travis-osx.sh; fi # Test doc generation - - if [ "$BUILD_DOC" == "true" ]; then sudo apt-get install -y -qq git make gcc g++ libjpeg-dev libxext-dev libx11-dev libgl1-mesa-dev libglu1-mesa-dev libpq-dev libpng12-dev xfonts-100dpi xfonts-75dpi; fi + - if [ "$BUILD_DOC" == "true" ]; then sudo apt-get install -y -qq git make gcc g++ libjpeg-dev libxext-dev libx11-dev libgl1-mesa-dev libglu1-mesa-dev libpq-dev libpng12-dev xfonts-100dpi xfonts-75dpi pkg-config libbullet-dev; fi - if [ "$BUILD_DOC" == "true" ]; then make; fi - if [ "$BUILD_DOC" == "true" ]; then sudo apt-get install -y -qq texlive-binaries texlive-lang-cjk poppler-utils nkf latex2html; fi - if [ "$BUILD_DOC" == "true" ]; then (source bashrc.eus; cd doc/; make pdf); fi diff --git a/README.md b/README.md index ff63740b1..95d68f3e2 100644 --- a/README.md +++ b/README.md @@ -127,12 +127,12 @@ PDF files are also available from [here](https://github.com/euslisp/jskeus/raw/m For Ubuntu users: ``` -$ sudo apt-get install git make gcc g++ libjpeg-dev libxext-dev libx11-dev libgl1-mesa-dev libglu1-mesa-dev libpq-dev libpng12-dev xfonts-100dpi xfonts-75dpi gsfonts-x11 texlive-fonts-extra xfonts-100dpi-transcoded xfonts-75dpi-transcoded msttcorefonts +$ sudo apt-get install git make gcc g++ libjpeg-dev libxext-dev libx11-dev libgl1-mesa-dev libglu1-mesa-dev libpq-dev libpng12-dev xfonts-100dpi xfonts-75dpi gsfonts-x11 texlive-fonts-extra xfonts-100dpi-transcoded xfonts-75dpi-transcoded msttcorefonts pkg-config libbullet-dev ``` For Mac OSX users using Homebrew: ``` -$ brew install jpeg libpng mesalib-glw wget +$ brew install jpeg libpng mesalib-glw wget bullet ``` '''NOTE:''' diff --git a/irteus/CBULLET.cpp b/irteus/CBULLET.cpp new file mode 100644 index 000000000..6b367b7aa --- /dev/null +++ b/irteus/CBULLET.cpp @@ -0,0 +1,264 @@ +/////////////////////////////////////////////////////////////////////////////// +/// +/// $Id$ +/// +/// Copyright (c) 1987- JSK, The University of Tokyo. All Rights Reserved. +/// +/// This software is a collection of EusLisp code for robot applications, +/// which has been developed by the JSK Laboratory for the IRT project. +/// For more information on EusLisp and its application to the robotics, +/// please refer to the following papers. +/// +/// Toshihiro Matsui +/// Multithread object-oriented language euslisp for parallel and +/// asynchronous programming in robotics +/// Workshop on Concurrent Object-based Systems, +/// IEEE 6th Symposium on Parallel and Distributed Processing, 1994 +/// +/// Permission to use this software for educational, research +/// and non-profit purposes, without fee, and without a written +/// agreement is hereby granted to all researchers working on +/// the IRT project at the University of Tokyo, provided that the +/// above copyright notice remains intact. +/// + +// for eus.h +#include +#include +#include +#include +#include +#include +#include + +#define class eus_class +#define throw eus_throw +#define export eus_export +#define vector eus_vector +#define string eus_string +#include // include eus.h just for eusfloat_t ... +#undef class +#undef throw +#undef export +#undef vector +#undef string + +#if HAVE_BULLET +#include +#include +#include +#include +#include +#endif + +#if HAVE_BULLET +#define CALL_WITH_BULLET_CHECK(X) X +#else +#define CALL_WITH_BULLET_CHECK(X) fprintf(stderr, "jskeus is compiled without bullet, so you can not use function %s. Please install bullet >= 2.83.\n", __PRETTY_FUNCTION__); return -1; +#endif + + +#if HAVE_BULLET +struct btDistanceInfo +{ // this class is copied from https://github.com/bulletphysics/bullet3/blob/master/test/collision/btDistanceInfo.h + btVector3 m_pointOnA; + btVector3 m_pointOnB; + btVector3 m_normalBtoA; + btScalar m_distance; +}; + + +struct ConvexWrap +{ // this class is copied from https://github.com/bulletphysics/bullet3/blob/master/test/collision/main.cpp + btConvexShape* m_convex; + btTransform m_worldTrans; + inline btScalar getMargin() const + { + return m_convex->getMargin(); + } + inline btVector3 getObjectCenterInWorld() const + { + return m_worldTrans.getOrigin(); + } + inline const btTransform& getWorldTransform() const + { + return m_worldTrans; + } + inline btVector3 getLocalSupportWithMargin(const btVector3& dir) const + { + return m_convex->localGetSupportingVertex(dir); + } + inline btVector3 getLocalSupportWithoutMargin(const btVector3& dir) const + { + return m_convex->localGetSupportingVertexWithoutMargin(dir); + } +}; + +long BT_MakeSphereModel(double radius) +{ + return (long)(new btSphereShape(radius)); +}; + +long BT_MakeBoxModel(double xsize, double ysize, double zsize) +{ + return (long)(new btBoxShape(0.5*btVector3(xsize, ysize, zsize))); +}; + +long BT_MakeCylinderModel(double radius, double height) +{ + return (long)(new btCylinderShapeZ(btVector3(radius, radius, 0.5*height))); +}; + +long BT_MakeCapsuleModel(double radius, double height) +{ + return (long)(new btCapsuleShapeZ(radius, 0.5*height)); +}; + +long BT_MakeMeshModel(double *verticesPoints, long numVertices) +{ + btConvexHullShape* pshape = new btConvexHullShape(); +#define SHRINK_FOR_MARGIN false + if (SHRINK_FOR_MARGIN) { + // Shrink vertices for default margin CONVEX_DISTANCE_MARGIN, + // which should be nonzero positive for fast computation of penetration distance. + // ref: https://pybullet.org/Bullet/phpBB3/viewtopic.php?t=2358#p9411 + // But sometimes, this doesn't work well (vertices become empty), so currently disabled. + btAlignedObjectArray vertices; + for (int i = 0; i < 3 * numVertices; i += 3) { + vertices.push_back(btVector3(verticesPoints[i], verticesPoints[i+1], verticesPoints[i+2])); + } + btAlignedObjectArray planes; + btGeometryUtil::getPlaneEquationsFromVertices(vertices, planes); + int sz = planes.size(); + for (int i = 0 ; i < sz ; i++) { + planes[i][3] += CONVEX_DISTANCE_MARGIN; + } + vertices.clear(); + btGeometryUtil::getVerticesFromPlaneEquations(planes, vertices); + sz = vertices.size(); + for (int i = 0 ; i < sz ; i++) { + pshape->addPoint(vertices[i]); + } + } else { + for (int i = 0; i < 3 * numVertices; i += 3) { + pshape->addPoint(btVector3(verticesPoints[i], verticesPoints[i+1], verticesPoints[i+2])); + } + } + return (long)pshape; +}; + +long BT_CalcCollisionDistance(long modelAddrA, long modelAddrB, + double *posA, double *quatA, double *posB, double *quatB, + double *dist, double *dir, double *pA, double *pB) +{ + ConvexWrap a, b; + a.m_convex = ((btConvexShape *)modelAddrA); + a.m_worldTrans.setOrigin(btVector3(posA[0], posA[1], posA[2])); + a.m_worldTrans.setRotation(btQuaternion(quatA[1], quatA[2], quatA[3], quatA[0])); // w is first element in euslisp + b.m_convex = ((btConvexShape *)modelAddrB); + b.m_worldTrans.setOrigin(btVector3(posB[0], posB[1], posB[2])); + b.m_worldTrans.setRotation(btQuaternion(quatB[1], quatB[2], quatB[3], quatB[0])); // w is first element in euslisp + // The origin of euslisp cylinder model is located on bottom, so local translation of half height is necessary + if(btCylinderShapeZ* cly = dynamic_cast(a.m_convex)) { + btVector3 heightOffset(btVector3(0, 0, cly->getHalfExtentsWithMargin().getZ())); + a.m_worldTrans.setOrigin(a.m_worldTrans.getOrigin() + a.m_worldTrans.getBasis() * heightOffset); + } + if(btCylinderShapeZ* cly = dynamic_cast(b.m_convex)) { + btVector3 heightOffset(btVector3(0, 0, cly->getHalfExtentsWithMargin().getZ())); + b.m_worldTrans.setOrigin(b.m_worldTrans.getOrigin() + b.m_worldTrans.getBasis() * heightOffset); + } + + btGjkCollisionDescription colDesc; + btVoronoiSimplexSolver simplexSolver; + btDistanceInfo distInfo; + int res = -1; + simplexSolver.reset(); + res = btComputeGjkEpaPenetration(a, b, colDesc, simplexSolver, &distInfo); + + // The result of btComputeGjkEpaPenetration is offseted by CONVEX_DISTANCE_MARGIN. + // Although the offset is considered internally in primitive shapes, not considered in convex hull shape. + // So, the result is modified manually. + if(dynamic_cast((btConvexShape *)modelAddrA)) { + distInfo.m_distance += CONVEX_DISTANCE_MARGIN; + distInfo.m_pointOnA += CONVEX_DISTANCE_MARGIN * distInfo.m_normalBtoA; + } + if(dynamic_cast((btConvexShape *)modelAddrB)) { + distInfo.m_distance += CONVEX_DISTANCE_MARGIN; + distInfo.m_pointOnB += - CONVEX_DISTANCE_MARGIN * distInfo.m_normalBtoA; + } + + *dist = distInfo.m_distance; + for (int i = 0; i < 3; i++) { + dir[i] = distInfo.m_normalBtoA[i]; + pA[i] = distInfo.m_pointOnA[i]; + pB[i] = distInfo.m_pointOnB[i]; + } + + return res; +}; + +long BT_SetMargin(long modelAddr, double margin) +{ + // shape are shrinked for CONVEX_DISTANCE_MARGIN, so CONVEX_DISTANCE_MARGIN is added to margin + ((btConvexShape *)modelAddr)->setMargin(CONVEX_DISTANCE_MARGIN+margin); + return 0; +}; +#endif + +extern "C" +{ + eusinteger_t C_BT_MakeSphereModel(eusfloat_t r) + { + CALL_WITH_BULLET_CHECK(return BT_MakeSphereModel(r);) + } + + eusinteger_t C_BT_MakeBoxModel(eusfloat_t xsize, eusfloat_t ysize, eusfloat_t zsize) + { + CALL_WITH_BULLET_CHECK(return BT_MakeBoxModel(xsize, ysize, zsize);) + } + + eusinteger_t C_BT_MakeCylinderModel(eusfloat_t radius, eusfloat_t height) + { + CALL_WITH_BULLET_CHECK(return BT_MakeCylinderModel(radius, height);) + } + + eusinteger_t C_BT_MakeCapsuleModel(eusfloat_t radius, eusfloat_t height) + { + CALL_WITH_BULLET_CHECK(return BT_MakeCapsuleModel(radius, height);) + } + + eusinteger_t C_BT_MakeMeshModel(eusfloat_t *verticesPoints, eusinteger_t numVertices) + { + #if HAVE_BULLET + double _verticesPoints[3*numVertices]; + for (int i = 0; i < 3 * numVertices; i++ ) { _verticesPoints[i] = verticesPoints[i]; } + #endif + CALL_WITH_BULLET_CHECK(return BT_MakeMeshModel(_verticesPoints, numVertices);) + } + + eusinteger_t C_BT_CalcCollisionDistance(eusinteger_t modelAddrA, eusinteger_t modelAddrB, + eusfloat_t *posA, eusfloat_t *quatA, eusfloat_t *posB, eusfloat_t *quatB, + eusfloat_t *dist, eusfloat_t *dir, eusfloat_t *pA, eusfloat_t *pB) + { + #if HAVE_BULLET + double _posA[3], _quatA[4], _posB[3], _quatB[4]; + double _dist[1], _dir[3], _pA[3], _pB[3]; + eusinteger_t ret; + for (int i = 0; i < 3; i++ ) {_posA[i] = posA[i]; _posB[i] = posB[i]; } + for (int i = 0; i < 4; i++ ) {_quatA[i] = quatA[i]; _quatB[i] = quatB[i]; } + _dist[0] = dist[0]; + for (int i = 0; i < 3; i++ ) {_dir[i] = dir[i]; _pA[i] = pA[i]; _pB[i] = pB[i];} + ret = BT_CalcCollisionDistance(modelAddrA, modelAddrB, + _posA, _quatA, _posB, _quatB, + _dist, _dir, _pA, _pB); + dist[0] = _dist[0]; + for (int i = 0; i < 3; i++ ) {dir[i] = _dir[i]; pA[i] = _pA[i]; pB[i] = _pB[i];} + #endif + CALL_WITH_BULLET_CHECK(return ret;) + } + + eusinteger_t C_BT_SetMargin(eusinteger_t modelAddr, eusfloat_t margin) + { + CALL_WITH_BULLET_CHECK(return BT_SetMargin(modelAddr, margin);) + } +} diff --git a/irteus/Makefile b/irteus/Makefile index c63f58ef4..9a93182a2 100644 --- a/irteus/Makefile +++ b/irteus/Makefile @@ -30,6 +30,19 @@ INSTALLBINDIR=$(IRTEUSDIR)/$(ARCHDIR)/bin INSTALLOBJDIR=$(IRTEUSDIR)/$(ARCHDIR)/obj INSTALLLIBDIR=$(IRTEUSDIR)/$(ARCHDIR)/lib +# check bullet version +BULLET_VER_MAJOR:=$(shell pkg-config bullet --modversion --silence-errors | cut -f1 -d.) +BULLET_VER_MINOR:=$(shell pkg-config bullet --modversion --silence-errors | cut -f2 -d.) +ifneq ($(and $(BULLET_VER_MAJOR),$(BULLET_VER_MINOR)),) + BULLET_GE_2_83=$(shell [ $(BULLET_VER_MAJOR) -gt 2 -o \( $(BULLET_VER_MAJOR) -eq 2 -a $(BULLET_VER_MINOR) -ge 83 \) ] && echo true) +endif +ifeq ($(BULLET_GE_2_83), true) + HAVE_BULLET=1 +else + HAVE_BULLET=0 +endif +$(info "-- HAVE_BULLET = ${HAVE_BULLET}") + # common WFLAGS= #-Wall @@ -40,11 +53,16 @@ LIBDIR=$(EUSDIR)/$(ARCHDIR)/lib PQPLIBDIR=PQP/$(ARCHDIR) PQPLIB=-L$(PQPLIBDIR) -lPQP-static +ifeq ($(HAVE_BULLET), 1) + BULLETCFLAGS=`pkg-config bullet --cflags` -DHAVE_BULLET=$(HAVE_BULLET) + BULLETLIB=`pkg-config bullet --libs` +endif + MODULES.L=irt_modules.l EUSLIB_MODULES.L=$(addprefix $(EUSDIR)/lib/,$(MODULES.L)) IRTEUS=irtmath irtutil irtgraph pgsql time -IRTEUSG=irtgeo pqp irtscene irtmodel irtsensor irtdyna irtrobot irtbvh irtcollada irtpointcloud +IRTEUSG=irtgeo pqp bullet irtcollision irtscene irtmodel irtsensor irtdyna irtrobot irtbvh irtcollada irtpointcloud IRTEUSX=irtx IRTEUSIMG=irtimage eusjpeg png IRTEUSGL=irtgl irtglrgb irtviewer @@ -100,7 +118,7 @@ IRTEUSGL_H=$(addprefix $(INSTALLOBJDIR)/,$(addsuffix .h,$(IRTEUSGL))) IRTEUSGL_L=$(addsuffix .l,$(IRTEUSGL)) IRTCOBJECTS=$(INSTALLOBJDIR)/irtc.$(OSFX) $(INSTALLOBJDIR)/irtgeoc.$(OSFX) -IRTGCOBJECTS=$(INSTALLOBJDIR)/CPQP.$(OSFX) $(INSTALLOBJDIR)/euspqp.$(OSFX) +IRTGCOBJECTS=$(INSTALLOBJDIR)/CPQP.$(OSFX) $(INSTALLOBJDIR)/euspqp.$(OSFX) $(INSTALLOBJDIR)/CBULLET.$(OSFX) $(INSTALLOBJDIR)/eusbullet.$(OSFX) IRTIMGCOBJECTS=$(INSTALLOBJDIR)/euspng.$(OSFX) IRTGLCOBJECTS=$(INSTALLOBJDIR)/irtglc.$(OSFX) NROBJECTS=$(INSTALLOBJDIR)/nr.$(OSFX) @@ -121,7 +139,7 @@ $(LIBIRTEUS): $(IRTEUSOBJS) $(IRTCOBJECTS) $(LIBNR) $(LIBIRTEUSG): $(IRTEUSGOBJS) $(IRTGCOBJECTS) $(LIBNR) PQP/$(ARCHDIR)/libPQP-static.a $(LD) $(SOFLAGS) $(OUTOPT)$(LIBIRTEUSG) $(IRTEUSGOBJS) \ - $(IRTGCOBJECTS) $(IMPLIB) $(PQPLIB) + $(IRTGCOBJECTS) $(IMPLIB) $(PQPLIB) $(BULLETLIB) $(LIBIRTEUSX): $(IRTEUSXOBJS) $(LIBNR) $(LD) $(SOFLAGS) $(OUTOPT)$(LIBIRTEUSX) $(IRTEUSXOBJS) $(IMPLIB) @@ -210,7 +228,9 @@ $(INSTALLOBJDIR)/irtgl.$(OSFX): irtgl.l $(INSTALLOBJDIR)/irtglrgb.$(OSFX): irtglrgb.l $(INSTALLOBJDIR)/irtviewer.$(OSFX): irtviewer.l $(INSTALLOBJDIR)/irtimage.$(OSFX): irtimage.l +$(INSTALLOBJDIR)/irtcollision.$(OSFX): irtcollision.l $(INSTALLOBJDIR)/pqp.$(OSFX): pqp.l +$(INSTALLOBJDIR)/bullet.$(OSFX): bullet.l $(INSTALLOBJDIR)/png.$(OSFX): png.l $(INSTALLOBJDIR)/pgsql.$(OSFX): $(EUSDIR)/lib/llib/pgsql.l $(INSTALLOBJDIR)/time.$(OSFX): $(EUSDIR)/lib/llib/time.l @@ -226,6 +246,10 @@ $(INSTALLOBJDIR)/CPQP.$(OSFX): CPQP.C defun.h $(CXX) $(CXXFLAGS) -c CPQP.C $(OBJOPT)$(INSTALLOBJDIR)/CPQP.$(OSFX) $(INSTALLOBJDIR)/euspqp.$(OSFX): euspqp.c defun.h $(CC) $(CFLAGS) $(WFLAGS) -c euspqp.c $(OBJOPT)$(INSTALLOBJDIR)/euspqp.$(OSFX) +$(INSTALLOBJDIR)/CBULLET.$(OSFX): CBULLET.cpp defun.h + $(CXX) $(CXXFLAGS) $(BULLETCFLAGS) -c CBULLET.cpp $(OBJOPT)$(INSTALLOBJDIR)/CBULLET.$(OSFX) +$(INSTALLOBJDIR)/eusbullet.$(OSFX): eusbullet.c defun.h + $(CC) $(CFLAGS) $(WFLAGS) -c eusbullet.c $(OBJOPT)$(INSTALLOBJDIR)/eusbullet.$(OSFX) $(INSTALLOBJDIR)/euspng.$(OSFX): euspng.c defun.h $(CC) $(CFLAGS) $(WFLAGS) -c euspng.c $(OBJOPT)$(INSTALLOBJDIR)/euspng.$(OSFX) $(INSTALLOBJDIR)/nr.$(OSFX): nr.c defun.h diff --git a/irteus/bullet.l b/irteus/bullet.l new file mode 100644 index 000000000..1098db891 --- /dev/null +++ b/irteus/bullet.l @@ -0,0 +1,126 @@ +;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; +;;; +;;; $Id$ +;;; +;;; Copyright (c) 1987- JSK, The University of Tokyo. All Rights Reserved. +;;; +;;; This software is a collection of EusLisp code for robot applications, +;;; which has been developed by the JSK Laboratory for the IRT project. +;;; For more information on EusLisp and its application to the robotics, +;;; please refer to the following papers. +;;; +;;; Toshihiro Matsui +;;; Multithread object-oriented language euslisp for parallel and +;;; asynchronous programming in robotics +;;; Workshop on Concurrent Object-based Systems, +;;; IEEE 6th Symposium on Parallel and Distributed Processing, 1994 +;;; +;;; Permission to use this software for educational, research +;;; and non-profit purposes, without fee, and without a written +;;; agreement is hereby granted to all researchers working on +;;; the IRT project at the University of Tokyo, provided that the +;;; above copyright notice remains intact. +;;; + +(require :irtmath) +(require :irtgeo) + +(in-package "GEOMETRY") + +(export '(bt-collision-distance bt-collision-check)) + +(defun bt-make-model-from-body + (b &key (csg (send b :csg)) (margin nil)) + "Make bullet model from body." + (let (m) + (cond ((assoc :sphere csg) + (setq m + (btmakespheremodel + (* 1e-3 (user::radius-of-sphere b))) + )) + ((assoc :cube csg) + (setq m + (btmakeboxmodel + (* 1e-3 (user::x-of-cube b)) + (* 1e-3 (user::y-of-cube b)) + (* 1e-3 (user::z-of-cube b)) + ))) + ((assoc :cylinder csg) + (setq m + (btmakecylindermodel + (* 1e-3 (user::radius-of-cylinder b)) + (* 1e-3 (user::height-of-cylinder b)) + ))) + (t + (setq m + (btmakemeshmodel + (scale 1e-3 ;; [m] + (apply #'concatenate float-vector + (mapcar #'(lambda (v) (send b :inverse-transform-vector v)) (send b :vertices)))) + (length (send b :vertices)) + )) + )) + (when margin + (btsetmargin m margin)) + m) + ) + +(defmethod cascaded-coords + (:make-btmodel + (&key (fat 0)) + "Make bullet model and save pointer of the bullet model." + (let (vs m) + (cond ((derivedp self body) + (setq m + (bt-make-model-from-body self :margin fat)) + ) + (t + (setq vs (flatten (send-all (send self :bodies) :vertices))) + (setq m + (btmakemeshmodel + (scale 1e-3 ;; [m] + (apply #'concatenate float-vector + (mapcar #'(lambda (v) (send self :inverse-transform-vector v)) vs))) + (length vs) + )) + (btsetmargin m fat) + )) + (setf (get self :btmodel) m) + m) + ) + ) + +(defun bt-collision-distance + (model1 model2 &key (fat 0) (fat2 nil) (qsize)) + "Calculate collision distance between model1 and model2 using Bullet. + Return value is (list [distance] [nearest point on model1] [nearest point on model2]). + qsize argument is not used, just for compatibility with pqp-collision-distance." + (let ((m1 (get model1 :btmodel)) + (m2 (get model2 :btmodel)) + (r1 (user::matrix2quaternion (send model1 :worldrot))) + (t1 (scale 1e-3 (send model1 :worldpos))) ;; [m] + (r2 (user::matrix2quaternion (send model2 :worldrot))) + (t2 (scale 1e-3 (send model2 :worldpos))) ;; [m] + (dist (float-vector 0)) + (dir (float-vector 0 0 0)) + (p1 (float-vector 0 0 0)) + (p2 (float-vector 0 0 0)) + r) + (if (null fat2) (setq fat2 fat)) + (if (null m1) (setq m1 (send model1 :make-btmodel :fat fat))) + (if (null m2) (setq m2 (send model2 :make-btmodel :fat fat2))) + (btcalccollisiondistance + m1 m2 t1 r1 t2 r2 + dist dir p1 p2) + (list (* 1e3 (elt dist 0)) (scale 1e3 p1) (scale 1e3 p2)) + )) + +(defun bt-collision-check + (model1 model2 &key (fat 0) (fat2 nil)) + "Check collision between model1 and model2 using Bullet. + If return value is 0, no collision. + Otherwise (return value is 1), collision." + (if (> (elt (bt-collision-distance model1 model2 :fat fat :fat2 fat2) 0) 0) 0 1) + ) + +(provide :bullet "$Id$") diff --git a/irteus/compile_irtg.l b/irteus/compile_irtg.l index a3e36f549..7d081bb5a 100644 --- a/irteus/compile_irtg.l +++ b/irteus/compile_irtg.l @@ -31,6 +31,8 @@ (comp:compile-file-if-src-newer "irtgeo.l" user::*objdir*) (comp:compile-file-if-src-newer "pqp.l" user::*objdir*) +(comp:compile-file-if-src-newer "bullet.l" user::*objdir*) +(comp:compile-file-if-src-newer "irtcollision.l" user::*objdir*) (comp:compile-file-if-src-newer "irtscene.l" user::*objdir*) (comp:compile-file-if-src-newer "irtmodel.l" user::*objdir*) (comp:compile-file-if-src-newer "irtsensor.l" user::*objdir*) diff --git a/irteus/demo/sample-collision.l b/irteus/demo/sample-collision.l new file mode 100644 index 000000000..5c0083e19 --- /dev/null +++ b/irteus/demo/sample-collision.l @@ -0,0 +1,123 @@ +(load "sample-arm-model.l") + + +(defun sample-collision-distance-body + (&key + (obj1 (make-cube 200 300 500)) + (obj2 (make-cube 400 600 1000)) + (obj1-coords-func + #'(lambda (cnt) (make-coords :pos (float-vector 1000 0 0)))) + (obj2-coords-func + #'(lambda (cnt) + (make-coords + :pos (float-vector (* 1500 (sin (/ cnt 100.0))) (* 500 (sin (+ (/ cnt 200.0) (deg2rad 45)))) 0) + :rpy (list (* pi (sin (/ cnt 200.0))) (+ (* pi (sin (/ cnt 400.0))) pi/2) 0) + ))) + ) + (let* ((cnt 0) + (ret) + ) + (when obj1-coords-func + (send obj1 :newcoords (funcall obj1-coords-func (float cnt)))) + (when obj2-coords-func + (send obj2 :newcoords (funcall obj2-coords-func (float cnt)))) + (send obj1 :set-color (float-vector 1 0 0) 0.5) + (send obj2 :set-color (float-vector 0 1 0) 0.4) + (objects (list obj1 obj2)) + + (do-until-key + ;; move object + (incf cnt) + (when obj1-coords-func + (send obj1 :newcoords (funcall obj1-coords-func (float cnt)))) + (when obj2-coords-func + (send obj2 :newcoords (funcall obj2-coords-func (float cnt)))) + (send *irtviewer* :draw-objects :flush nil) + ;; get distance beween target-link and obj + (setq ret (collision-distance obj1 obj2)) + ;; draw + (send (elt ret 1) :draw-on :flush nil :width 16 :size 50 :color (float-vector 1 0.4 0.4)) + (send (elt ret 2) :draw-on :flush nil :width 16 :size 50 :color (float-vector 0.4 1 0.4)) + (send (make-line (elt ret 1) (elt ret 2)) :draw-on :flush nil + :width 8 :color (if (> (elt ret 0) 0) (float-vector 0 1 1) (float-vector 1 1 0))) + (send *irtviewer* :viewer :flush) + (unix::usleep (* 20 1000)) + (x::window-main-one) + ) + )) +(warn "(sample-collision-distance-body)~%") + +(defun sample-collision-distance-sphere + () + (sample-collision-distance-body + :obj2 (make-sphere 600)) + ) + +(defun sample-collision-distance-cube + () + (sample-collision-distance-body) + ) + +(defun sample-collision-distance-cylinder + () + (sample-collision-distance-body + :obj2 (make-cylinder 400 1200) + ) + ) + +(defun sample-collision-distance-conv + () + (sample-collision-distance-body + :obj2 (make-cone (float-vector 0 0 1500) (list (float-vector -800 -500 0) (float-vector 800 -500 0) (float-vector 0 500 0))) + ) + ) + + +(defun sample-collision-distance-link + (&key + (obj (make-cube 200 500 500)) + (obj-coords-func + #'(lambda (cnt) (make-coords :pos (float-vector 500 0 250)))) + ) + (let* ((cnt 0) + (ret) + (robot (instance sarmclass :init)) + (target-link (elt (send robot :links) 4)) + (base-link (elt (send robot :links) 0)) + ) + (when obj-coords-func + (send obj :newcoords (funcall obj-coords-func (float cnt)))) + (send obj :set-color (float-vector 1 0 0) 0.5) + (objects (list robot obj)) + + (do-until-key + ;; move object and robot + (incf cnt) + (dolist (j (send robot :joint-list)) + (send j :joint-angle + (+ (* 0.49 (- (send j :max-angle) (send j :min-angle)) (sin (/ cnt 100.0))) + (* 0.5 (+ (send j :max-angle) (send j :min-angle))))) + ) + (when obj-coords-func + (send obj :newcoords (funcall obj-coords-func (float cnt)))) + (send obj :newcoords (funcall obj-coords-func (float cnt))) + (send *irtviewer* :draw-objects :flush nil) + ;; get distance beween target-link and obj + (setq ret (collision-distance target-link obj)) + (send (elt ret 1) :draw-on :flush nil :width 16 :size 50 :color (float-vector 1 0.4 0.4)) + (send (elt ret 2) :draw-on :flush nil :width 16 :size 50 :color (float-vector 0.4 1 0.4)) + (send (make-line (elt ret 1) (elt ret 2)) :draw-on :flush nil + :width 8 :color (if (> (elt ret 0) 0) (float-vector 0 1 1) (float-vector 1 1 0))) + ;; get distance beween target-link and base-link + (setq ret (collision-distance target-link base-link)) + ;; draw + (send (elt ret 1) :draw-on :flush nil :width 16 :size 50 :color (float-vector 1 0.4 0.4)) + (send (elt ret 2) :draw-on :flush nil :width 16 :size 50 :color (float-vector 0.4 1 0.4)) + (send (make-line (elt ret 1) (elt ret 2)) :draw-on :flush nil + :width 8 :color (if (> (elt ret 0) 0) (float-vector 0 1 1) (float-vector 1 1 0))) + (send *irtviewer* :viewer :flush) + (unix::usleep (* 20 1000)) + (x::window-main-one) + ) + )) +(warn "(sample-collision-distance-link)~%") diff --git a/irteus/eusbullet.c b/irteus/eusbullet.c new file mode 100644 index 000000000..acd02ca9a --- /dev/null +++ b/irteus/eusbullet.c @@ -0,0 +1,124 @@ +/////////////////////////////////////////////////////////////////////////////// +/// +/// $Id$ +/// +/// Copyright (c) 1987- JSK, The University of Tokyo. All Rights Reserved. +/// +/// This software is a collection of EusLisp code for robot applications, +/// which has been developed by the JSK Laboratory for the IRT project. +/// For more information on EusLisp and it's application to the robotics, +/// please refer to the following papers. +/// +/// Toshihiro Matsui +/// Multithread object-oriented language euslisp for parallel and +/// asynchronous programming in robotics +/// Workshop on Concurrent Object-based Systems, +/// IEEE 6th Symposium on Parallel and Distributed Processing, 1994 +/// +/// Permission to use this software for educational, research +/// and non-profit purposes, without fee, and without a written +/// agreement is hereby granted to all researchers working on +/// the IRT project at the University of Tokyo, provided that the +/// above copyright notice remains intact. +/// + +#pragma init (register_eusbullet) + +#include "eus.h" + +extern pointer ___eusbullet(); +static void register_eusbullet() +{ add_module_initializer("___eusbullet", ___eusbullet);} + +extern eusinteger_t C_BT_MakeSphereModel(eusfloat_t r); +extern eusinteger_t C_BT_MakeBoxModel(eusfloat_t xsize, eusfloat_t ysize, eusfloat_t zsize); +extern eusinteger_t C_BT_MakeCylinderModel(eusfloat_t radius, eusfloat_t height); +extern eusinteger_t C_BT_MakeCapsuleModel(eusfloat_t radius, eusfloat_t height); +extern eusinteger_t C_BT_MakeMeshModel(eusfloat_t *verticesPoints, eusinteger_t numVertices); +extern eusinteger_t C_BT_CalcCollisionDistance(eusinteger_t modelAddrA, eusinteger_t modelAddrB, + eusfloat_t *posA, eusfloat_t *quatA, eusfloat_t *posB, eusfloat_t *quatB, + eusfloat_t *dist, eusfloat_t *dir, eusfloat_t *pA, eusfloat_t *pB); +extern eusinteger_t C_BT_SetMargin(eusinteger_t modelAddr, eusfloat_t margin); + +pointer BTMAKESPHEREMODEL(register context *ctx, int n, register pointer *argv) +{ + numunion nu; + eusfloat_t r = ckfltval(argv[0]); + eusinteger_t addr = C_BT_MakeSphereModel(r); + return makeint(addr); +} + +pointer BTMAKEBOXMODEL(register context *ctx, int n, register pointer *argv) +{ + numunion nu; + eusfloat_t xsize = ckfltval(argv[0]); + eusfloat_t ysize = ckfltval(argv[1]); + eusfloat_t zsize = ckfltval(argv[2]); + eusinteger_t addr = C_BT_MakeBoxModel(xsize, ysize, zsize); + return makeint(addr); +} + +pointer BTMAKECYLINDERMODEL(register context *ctx, int n, register pointer *argv) +{ + numunion nu; + eusfloat_t radius = ckfltval(argv[0]); + eusfloat_t height = ckfltval(argv[1]); + eusinteger_t addr = C_BT_MakeCylinderModel(radius, height); + return makeint(addr); +} + +pointer BTMAKECAPSULEMODEL(register context *ctx, int n, register pointer *argv) +{ + numunion nu; + eusfloat_t radius = ckfltval(argv[0]); + eusfloat_t height = ckfltval(argv[1]); + eusinteger_t addr = C_BT_MakeCapsuleModel(radius, height); + return makeint(addr); +} + +pointer BTMAKEMESHMODEL(register context *ctx, int n, register pointer *argv) +{ + numunion nu; + eusfloat_t *verticesPoints = argv[0]->c.fvec.fv; + eusinteger_t numVertices = ckintval(argv[1]); + eusinteger_t addr = C_BT_MakeMeshModel(verticesPoints, numVertices); + return makeint(addr); +} + +pointer BTCALCCOLLISIONDISTANCE(register context *ctx, int n, register pointer *argv) +{ + eusinteger_t modelAddrA = intval(argv[0]); + eusinteger_t modelAddrB = intval(argv[1]); + eusfloat_t *posA = argv[2]->c.fvec.fv; + eusfloat_t *quatA = argv[3]->c.fvec.fv; + eusfloat_t *posB = argv[4]->c.fvec.fv; + eusfloat_t *quatB = argv[5]->c.fvec.fv; + eusfloat_t *dist = argv[6]->c.fvec.fv; + eusfloat_t *dir = argv[7]->c.fvec.fv; + eusfloat_t *pA = argv[8]->c.fvec.fv; + eusfloat_t *pB = argv[9]->c.fvec.fv; + eusinteger_t ret = C_BT_CalcCollisionDistance(modelAddrA, modelAddrB, posA, quatA, posB, quatB, dist, dir, pA, pB); + return makeint(ret); +} + +pointer BTSETMARGIN(register context *ctx, int n, register pointer *argv) +{ + numunion nu; + eusinteger_t modelAddr = intval(argv[0]); + eusfloat_t margin = ckfltval(argv[1]); + eusinteger_t ret = C_BT_SetMargin(modelAddr, margin); + return makeint(ret); +} + +#include "defun.h" // redefine defun for update defun() API +pointer ___eusbullet(register context *ctx, int n, register pointer *argv) +{ + pointer mod=argv[0]; + defun(ctx, "BTMAKESPHEREMODEL", mod, BTMAKESPHEREMODEL, NULL); + defun(ctx, "BTMAKEBOXMODEL", mod, BTMAKEBOXMODEL, NULL); + defun(ctx, "BTMAKECYLINDERMODEL", mod, BTMAKECYLINDERMODEL, NULL); + defun(ctx, "BTMAKECAPSULEMODEL", mod, BTMAKECAPSULEMODEL, NULL); + defun(ctx, "BTMAKEMESHMODEL", mod, BTMAKEMESHMODEL, NULL); + defun(ctx, "BTCALCCOLLISIONDISTANCE", mod, BTCALCCOLLISIONDISTANCE, NULL); + defun(ctx, "BTSETMARGIN", mod, BTSETMARGIN, NULL); +} diff --git a/irteus/irtcollision.l b/irteus/irtcollision.l new file mode 100644 index 000000000..64313a0ee --- /dev/null +++ b/irteus/irtcollision.l @@ -0,0 +1,81 @@ +;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; +;;; +;;; $Id$ +;;; +;;; Copyright (c) 1987- JSK, The University of Tokyo. All Rights Reserved. +;;; +;;; This software is a collection of EusLisp code for robot applications, +;;; which has been developed by the JSK Laboratory for the IRT project. +;;; For more information on EusLisp and its application to the robotics, +;;; please refer to the following papers. +;;; +;;; Toshihiro Matsui +;;; Multithread object-oriented language euslisp for parallel and +;;; asynchronous programming in robotics +;;; Workshop on Concurrent Object-based Systems, +;;; IEEE 6th Symposium on Parallel and Distributed Processing, 1994 +;;; +;;; Permission to use this software for educational, research +;;; and non-profit purposes, without fee, and without a written +;;; agreement is hereby granted to all researchers working on +;;; the IRT project at the University of Tokyo, provided that the +;;; above copyright notice remains intact. +;;; + +(in-package "USER") + +(require :pqp) +(require :bullet) + +(defconstant *collision-algorithm-pqp* :pqp) +(defconstant *collision-algorithm-bullet* :bullet) +(defvar *collision-algorithm* *collision-algorithm-pqp*) + +(defmethod cascaded-coords + (:make-collisionmodel + (&rest args &key &allow-other-keys) + "Make collision model and save pointer." + (cond ((eq *collision-algorithm* *collision-algorithm-pqp*) + (send* self :make-pqpmodel args)) + ((eq *collision-algorithm* *collision-algorithm-bullet*) + (send* self :make-btmodel args)) + (t + (error "invalid collision algorithm: ~a~%" *collision-algorithm*))) + ) + ) + +(defun collision-distance + (model1 model2 &rest args &key &allow-other-keys) + "Calculate collision distance between model1 and model2. + Return value is (list [distance] [nearest point on model1] [nearest point on model2])." + (cond ((eq *collision-algorithm* *collision-algorithm-pqp*) + (apply #'pqp-collision-distance model1 model2 args)) + ((eq *collision-algorithm* *collision-algorithm-bullet*) + (apply #'bt-collision-distance model1 model2 args)) + (t + (error "invalid collision algorithm: ~a~%" *collision-algorithm*))) + ) + +(defun collision-check + (model1 model2 &rest args &key &allow-other-keys) + "Check collision between model1 and model2. + If return value is 0, no collision. + Otherwise (return value is 1), collision." + (cond ((eq *collision-algorithm* *collision-algorithm-pqp*) + (apply #'pqp-collision-check model1 model2 args)) + ((eq *collision-algorithm* *collision-algorithm-bullet*) + (apply #'bt-collision-check model1 model2 args)) + (t + (error "invalid collision algorithm: ~a~%" *collision-algorithm*))) + ) + +(defun select-collision-algorithm + (alg) + "Select collision algorithm. + :pqp and :bullet are supported." + (setq *collision-algorithm* alg) + ) + +(in-package "GEOMETRY") + +(provide :irtcollision "$Id$") diff --git a/irteus/irtext.l b/irteus/irtext.l index bedd28407..977e8e760 100644 --- a/irteus/irtext.l +++ b/irteus/irtext.l @@ -34,7 +34,7 @@ (load-library (format nil "~A~A/lib/libirteusg" *eusdir* (unix:getenv "ARCHDIR")) - '("irtgeo" "euspqp" "pqp" "irtscene" "irtmodel" "irtdyna" "irtrobot" "irtsensor" "irtbvh" "irtcollada" "irtpointcloud")) + '("irtgeo" "euspqp" "pqp" "irtscene" "irtmodel" "irtdyna" "irtrobot" "irtsensor" "irtbvh" "irtcollada" "irtpointcloud" "eusbullet" "bullet" "irtcollision")) (in-package "USER") (import '(collada::convert-irtmodel-to-collada collada::eus2collada))) (defun load-irteusx () diff --git a/irteus/test/test-collision.l b/irteus/test/test-collision.l new file mode 100644 index 000000000..83bd04edf --- /dev/null +++ b/irteus/test/test-collision.l @@ -0,0 +1,149 @@ +(require :unittest "lib/llib/unittest.l") +(init-unit-test) + +(defun test-collision-sphere-analytical + () + (let* ((radius1 100) + (radius2 200) + (obj1 (make-sphere radius1)) + (obj2 (make-sphere radius2)) + (cnt 0) + (dist) + (analy-dist) + (ret) + ) + (send obj1 :set-color (float-vector 1 0 0) 0.5) + (send obj2 :set-color (float-vector 0 1 0) 0.4) + (objects (list obj1 obj2)) + (do-until-key + ;; move object + (incf cnt) + (send obj1 :newcoords (make-coords :pos (float-vector (* 500.0 (sin (/ cnt 20.0))) 50 0))) + (send *irtviewer* :draw-objects) + ;; get bullet distance + (setq ret (collision-distance obj1 obj2)) + (setq dist (elt ret 0)) + ;; get analytical distance and compare + (setq analy-dist (- (norm (send obj1 :worldpos)) (+ radius1 radius2))) + (cond ((eq *collision-algorithm* *collision-algorithm-pqp*) + ;; very large tolerance for pqp algorithm because + ;; 1) pqp does not return correct collision distance when collision occurs. + ;; 2) pqp does not return correct collision detection (i.e., 0 or 1 detection) when if one object is completely contained in the other object. + ;; 3) euspqp.c and CPQP.c do not treat euslisp sphere model as sphere but treat as polyhedron mesh. + (assert (eps= dist analy-dist 1e3)) + ) + ((eq *collision-algorithm* *collision-algorithm-bullet*) + (assert (eps= dist analy-dist 1e-3)) + ) + (t + (error "invalid collision algorithm: ~a~%" *collision-algorithm*) + )) + ;; draw + (send (elt ret 1) :draw-on :flush nil :width 16 :size 50 :color (float-vector 1 0.4 0.4)) + (send (elt ret 2) :draw-on :flush nil :width 16 :size 50 :color (float-vector 0.4 1 0.4)) + (send (make-line (elt ret 1) (elt ret 2)) :draw-on :flush nil + :width 8 :color (if (> (elt ret 0) 0) (float-vector 0 1 1) (float-vector 1 1 0))) + (send *irtviewer* :viewer :flush) + (unix::usleep (* 20 1000)) + (when (> cnt 100) + (return-from nil nil)) + ) + )) + +(deftest test-collision-sphere-analytical-pqp + (select-collision-algorithm *collision-algorithm-pqp*) + (test-collision-sphere-analytical) + ) + +(deftest test-collision-sphere-analytical-bullet + (select-collision-algorithm *collision-algorithm-bullet*) + (test-collision-sphere-analytical) + ) + +(defun test-collision-object-approx + (obj1 obj2) + (let* ((bbox-radius1 + (norm (v- (send (send obj1 :box) :center) (send (send obj1 :box) :maxpoint)))) + (bbox-radius2 + (norm (v- (send (send obj2 :box) :center) (send (send obj2 :box) :maxpoint)))) + (cnt 0) + (dist) + (approx-dist-min) + (approx-dist-max) + (ret) + ) + (send obj1 :set-color (float-vector 1 0 0) 0.5) + (send obj2 :set-color (float-vector 0 1 0) 0.4) + (objects (list obj1 obj2)) + (do-until-key + ;; move object + (incf cnt) + (send obj1 :newcoords + (make-coords :pos (float-vector (* 750.0 (sin (/ cnt 20.0))) 50 0) + :rpy (list (* pi (sin (/ cnt 200.0))) (+ (* pi (sin (/ cnt 400.0))) pi/2) 0))) + (send *irtviewer* :draw-objects) + ;; get bullet distance + (setq ret (collision-distance obj1 obj2)) + (setq dist (elt ret 0)) + ;; get approximate distance and compare + (setq approx-dist-max + (norm (v- (send (send obj1 :box) :center) (send (send obj2 :box) :center)))) + (setq approx-dist-min + (- approx-dist-max (+ bbox-radius1 bbox-radius2))) + ;; (format t "[result: ~a] ~a < ~a < ~a~%" + ;; (< approx-dist-min dist approx-dist-max) + ;; approx-dist-min dist approx-dist-max) + (assert (< approx-dist-min dist approx-dist-max)) + ;; draw + (send (elt ret 1) :draw-on :flush nil :width 16 :size 50 :color (float-vector 1 0.4 0.4)) + (send (elt ret 2) :draw-on :flush nil :width 16 :size 50 :color (float-vector 0.4 1 0.4)) + (send (make-line (elt ret 1) (elt ret 2)) :draw-on :flush nil + :width 8 :color (if (> (elt ret 0) 0) (float-vector 0 1 1) (float-vector 1 1 0))) + (send *irtviewer* :viewer :flush) + (unix::usleep (* 20 1000)) + (when (> cnt 100) + (return-from nil nil)) + ) + )) + +(deftest test-collision-cube-approx-pqp + (select-collision-algorithm *collision-algorithm-pqp*) + (test-collision-object-approx + (make-cube 100 150 200) (make-cube 200 200 300)) + ) + +(deftest test-collision-cube-approx-bullet + (select-collision-algorithm *collision-algorithm-bullet*) + (test-collision-object-approx + (make-cube 100 150 200) (make-cube 200 200 300)) + ) + +(deftest test-collision-cylinder-approx-pqp + (select-collision-algorithm *collision-algorithm-pqp*) + (test-collision-object-approx + (make-cylinder 100 200) (make-cube 200 200 300)) + ) + +(deftest test-collision-cylinder-approx-bullet + (select-collision-algorithm *collision-algorithm-bullet*) + (test-collision-object-approx + (make-cylinder 100 200) (make-cube 200 200 300)) + ) + +(deftest test-collision-mesh-approx-pqp + (select-collision-algorithm *collision-algorithm-pqp*) + (test-collision-object-approx + (make-cone (float-vector 0 0 200) (list (float-vector -200 -200 0) (float-vector 200 -200 0) (float-vector 0 250 0))) ;; cone is treated as mesh + (make-cube 200 200 300)) + ) + +(deftest test-collision-mesh-approx-bullet + (select-collision-algorithm *collision-algorithm-bullet*) + (test-collision-object-approx + (make-cone (float-vector 0 0 200) (list (float-vector -200 -200 0) (float-vector 200 -200 0) (float-vector 0 250 0))) ;; cone is treated as mesh + (make-cube 200 200 300)) + ) + +(eval-when (load eval) + (run-all-tests) + (exit 0)) From 0b8c0e234b5c4bc58331399633f9a4b248893d21 Mon Sep 17 00:00:00 2001 From: Masaki Murooka Date: Thu, 19 Sep 2019 15:54:06 +0900 Subject: [PATCH 2/2] doc: update collision document. --- doc/Makefile | 2 +- doc/fig/collision.jpg | Bin 0 -> 8506 bytes doc/{pqp.tex => irtcollision.tex} | 203 ++++++++++++++++++------------ doc/jmanual.tex | 4 +- irteus/irtcollision.l | 12 ++ 5 files changed, 138 insertions(+), 83 deletions(-) create mode 100644 doc/fig/collision.jpg rename doc/{pqp.tex => irtcollision.tex} (55%) diff --git a/doc/Makefile b/doc/Makefile index 57dcbabb7..3f94d582d 100644 --- a/doc/Makefile +++ b/doc/Makefile @@ -22,7 +22,7 @@ copy_eus_tex: (cd ${TMPDIR};for x in fig/*.jpg; do extractbb $$x; done) # workaround for https://github.com/backtracking/bibtex2html/issues/9 cp *.tex ${TMPDIR} platex --version | grep utf8 || nkf --in-place -e ${TMPDIR}/*.tex - for x in irtrobot irtmodel irtsensor irtscene irtdyna irtgeo irtgl irtutil irtviewer irtx irtmath irtbvh irtcollada irtgraph irtimage irtpointcloud png pqp; do\ + for x in irtrobot irtmodel irtsensor irtscene irtdyna irtgeo irtgl irtutil irtviewer irtx irtmath irtbvh irtcollada irtgraph irtimage irtpointcloud png irtcollision pqp bullet; do\ irteusgl ../eus/lib/llib/documentation.l "(make-document \"../irteus/$$x.l\" \"${TMPDIR}/$$x-func.tex\")" "(exit)"; \ done diff --git a/doc/fig/collision.jpg b/doc/fig/collision.jpg new file mode 100644 index 0000000000000000000000000000000000000000..e8b16a6fb08dea8c95e9ae37867a7ca1531dcd7f GIT binary patch literal 8506 zcmeHLXIN9&);=LXFmxpd2nMMUM5QS`fPfU~y(>kED7`8W+lUZAnmE!z=v|6*1e6Za zJ5mIcj?z2%qBD#Bo%`#XlV{~Q`($S)@7im<>s=eyj~fH16y+4;01yZOK=>Dc z8v-r>RHS4SWKb#!a*D&$R7dC#40N=#biAzWOb8KvaWP?jq4SdRMk>QkklUxFV=gtcWi(HVA zmAfdfps1;(t)r`_Z(wd=dDY6=#@5-z)y>_*)9dcN`@cN!d*~k;79R09^2x7Hu?dMu z$tkI6>3R7D&kKu+OG;n9s;RB3Z)j}l?CS36?du;Hd^LiPpNAGihqGB60gd0=V)39M`*uqfz1 z_Ky$#&KNj3LzYKYf24ROLc{i@O{9I%hN0A={5XnB-M|Lhr22tf_IO6vK1{?q7qru8 z9GpJp8wi+n7?X4^06>k3=wq=ii_Yt&$1ZRnONV{M+KGu^DI5XJ^^tr<*>8iF(nIx( zF9x>rwr%*>zmtSbWBmcd|H?A9uRa;{i?q8fR_;pQ>bwt&-@D6bcq!JVj=!NALk8te zP?qTBkGQXDAH})VJ;NZ!%fLlZtnLrI*BG%#`u<1sW^f8b2>fEvPpQ zpf3BZ4X^|md4^<6B)^T;RuG8Nt!r-#UMwLTSLci^jrMBCNjbWbi?Nc_~+y1u~EKHtUS&*bsSFyAdx-7Lp zGk9=Jq;Tb*>mdy5hen)QWVDd+J&ue4q-0Hu`HV;TF~<`)l9l<+N_f)6-9Ug(`wuCJ zx(HKK*2hh0V7bUx?jJ{)q0SmApJ#jU{EZ_zZz4D?HSXmc%RAL@$1XR|J>M9Y0yRjK z@rCu9X_(=%X17Zd^wqVpI?q+NpvE|$&k6^$2Wn#o{1N}PQ9O9*yS#zJO9NmaBR>1) z*Q0M1CuBZb4Fq3M7tB=d&uQ=ORwLrbcihl}cgA}RODuLR)!_E%e)E)&(tcw z?tz@DqgYF3w(yU|S>0P}uGSlo^$tqWc${RoBf{vcp+?N7_eMfO8MA@+6|djcJ79;b zH--kJEYPn@6mUMZ6_gg*OGrn3W}JU_))w)pY!U<0L_9QVAlWb0YbkSj9;Rgx>HuIU z`LX^8;BPQ+{KvT63LH>{T(_OR7W>t}`oQHg6gG~|XxqL6Xw+p7G?=ay4tQidy65P- zN7$2|lqnx#bKY(WsA=8|xxF1CdEEOCL|>!(x9=er-Xgbd;sBKId{29y_*aAWublSB zvbtO`M&Io6HzAd`tAGQTx#Kn|_wR9Q`ISurcG@tdu#z~N)hhU&vtjrTg!PHQw3|}nynUYE_<6mpUh>_& zGmZnYU*P~L+mY;>b$qFLMfdKOOunJC1Kq^|@laqg^-uXej!;>w`LMaOS34Myr7s{H zguSiU3?jrtD;{fsf4bJR4;@aG#<5=@T+n&WmUae!rZY>6tpV)FPP6~&~E+0Dz3 zAp3W$!O?$~7Fi3YyXHS`1(Ag8P;bau&A7!`N3}SR+4KjOU-Lo%4o)IHNgg+}&5Hx! z(Sc1QZJRv?2!dmk6q0YB4h(Y4XF63NkyCm_rItO9FXg^NzKG7j0fsqVvzU^}QxAe( z?+e~QiEaDOXRii`Vq2E%oO{~G*4jFAZCJYuONb`$YlM^|Ql`OxE$D}*V zN0?T&DSF3C4a|9m+poo0E;}Wc3HdVb^sX>zXm1`}ki^8TSK=kJNP{<6lu%T~xy2UR zr>xK5FUo`Wizc+W+pmf2$UQG_zP_AF>%r zq;~(A_R}25C{i)^2cq~w9-bpaiw@7HbBdzS+zj9qI$V2te$XNXYZSU`I^TKi%KWiQa)w4Qyy3C^J>3^ z8*2OEATNK|-Be3q;>W1N&z^geDDBMAX!``xRYBgk!Kao+5;2q#mKJ82&}Tx-Xet}- z+#Ufx`-m7xCA7m{kSKg<&M5q&()ANT9s`VStT~(QU~^|i{|R~xlGkprRQZpWqO>a| zloJ{2tF)c?AG598ukS)ib4PX*l(+^eiVCR=51f9dK$4J#Y@U^MHR0=F>9R{`FX)&| zRkK{~zkH&t;pXI;#7K$YlsVfJyEOdBXydWY3=v1$(+$;2D#hEq@yiRe&uH}|nJ&5s zPeoIR`go>UWsc})*E+I*P z($ZLT3UfR7>xvXg`NAi{ETej(tuynFs{IWOsh9Mu?&uB(b`m!dp4Ph~GDTlMp?~x^ z58A@eApcgmKp+~9X}vfh$;9IsXglojK)r-W1`A^)l;Pc+%9+#GI?~ zw2p?3VoNa`AOiGY6RG9Rb-2;!kT;qn%wuQmkcJfT8X7H~Ny!8#q8w(y>L+Z7(C+EloZJyxc?zMOHpkLhB!E3CRJA!OWP zsf?8ZokFMv!KfS6rua>v(}!7_DhA|^<(R6zGwgL=@5^|?MwA=+;1~FViB$}#0A5xo ze9n8dAshfx{yqD3yjzP>3^@#D7$3>&wJtjj1;qsd*6K#D`vPS#$meok#!b*6nMVX7aWS{|O_MB!O@~Jg->~u|b}ooUh>N(S zul^)NKdbf41e+wi_FF9u2!h^UZPp(a6-yX6d5py=j;3Z|3_WxNz(Fb9+ua2R#^WD_ z0WbXa_}Uwpf35B>&nZbSY!|F~DL5gnA_ASJrWGJT5itQ6hz4H<dm;N~5bAQ~T zgS<-qjJ8cPy7xXT+NMqk6tmlO*c`qL^x4Y_!CBT$jLA8e!l~js z^&)!f2Szv`e;*#1eBd`%r zk!UKLZ&WKN!!pNs=!zxMI=G|l^%f2Yy5`lUpI?66|NVJRki}SB$_*S~FoD2O$`|c( zDMgjtlo@v9lRGQ_#z947%srb}Ty9;cym&ELYXT6atH0z$Vf!3#(=|cl`BC4#NvUx+FWF7~rbp+A2rk%YLwrMSm!lv zaH0^sJ7XN||6Db1$UBI2XQuD{mh(Mf$dWh`X*)iBV0ZSja(BRU@HP38hghoXhnT1K zKKN$Y=pqhjKto9;sRxT(=9ZiYov7I1n`?l{0bl*y{{NCYiijMF! zx3?Axkvb{?(4sx}auA>#yU;!A0ssVn0tUaI7t;DGB0$!ONqB9=jnwb% zx|C<*qsi~cz`Ikw3o*qvNr1&>zBVssKeHLrFP4_9i#Wi2X>N79k6m%Tb$+q-*B}K> zwx^2UsKWpDkZGXH70yqkGK!Qn-!-WpDVPh(iaD>o!b`ZYvZ{luo%fywt9*|;oS+6c z#EBwex1Dh-yBIrm$3yL+y#est%8nD(J}mAt`BW;tPyH(P5P#P_*yNQK zonE#=nX+A;NsRJDhEMKGM&4-yMJJJ9bG>`)%oT#!_h`cHNK}MXTmnER%)%= z#B|!ryM*N1EuXCCh<4{BcH5hbOBK08OB&QfC|5=WK>7nQ1H%FQjG-zD0^vV4&R_KK z7cTnG<}i5ONO(7XXlQae)AvFDj?QZ**OU}n6E$k1BhS2h=Ynm40qi5HDj4(K2G$B5 z0G5?BIIk*R^=8za6@vrjdbmmL3zSzVhjVz^W(*H1Ze|`)+UL$;N%XR6Uj3rM7w~3Z z95OK@Nw?3{Bhll$E)=|Y#Xjm?osk8+%4K&i1E0Qs*B1Y59fI{3pX+`>(w8pc0H*9a zF#gh09-`aSU2{f();6WJtxxHsbBo2rs#HdkNiY=aBSb~_NPd`IFF*)sBiOx z=5KIwbZlM53%#u2LMdCuv#Oo# zoy!_xPr9x3zs=oJBkcYXTe5)ei}GJy4x|)cPZa_61Yz11JW?Q0?NKez{(anBW-Q`fqRS&MKrNW$-% zs2;XWpH;59OriLs4>ELHUqB5aOBt{j(HJ{;)X0GPX9&trfRH}HRC7))sZ literal 0 HcmV?d00001 diff --git a/doc/pqp.tex b/doc/irtcollision.tex similarity index 55% rename from doc/pqp.tex rename to doc/irtcollision.tex index def268c2f..c7648cdd0 100644 --- a/doc/pqp.tex +++ b/doc/irtcollision.tex @@ -1,26 +1,121 @@ \section{干渉計算} -干渉計算には２組の幾何モデルが交差するかを判定する物である． -irteusではノースカロライナ大学のLin氏らのグループにより開発されたPQPを -他言語インターフェースを介して利用できるようにしてある． -(他の干渉計算ソフトウェアパッケージについてはhttp://gamma.cs.unc.edu/research/collision/に詳しい．) -PQPは -(1)２つのモデルが交差するかを判定する衝突検出， -(2)２つのモデル間の最初距離を算出する距離計算， -(3)２つのモデルがある距離以下であるかを判定する近接検証， -等の３つ機能を提供する． +\subsection{干渉計算の概要} + +干渉計算は２組の幾何モデルが交差するかを判定しその距離を求める処理である． +主に以下の２つ機能を提供する． +\begin{itemize} + \item ２つのモデルが交差するかを判定する衝突検出 (collision-check関数) + \item ２つのモデル間の最短距離を算出する距離計算 (collision-distance関数) +\end{itemize} + +irteusでは，他言語インターフェースを介して外部ライブラリを呼び出すことで干渉計算を実行する． +外部ライブラリとして，PQPとBulletの呼び出しが実装されており，デフォルトではPQPが利用される． +以下のように，select-collision-algorithm関数により使用するライブラリを切り替えることができる． +{\baselineskip=10pt +\begin{verbatim} +(select-collision-algorithm *collision-algorithm-pqp*) ;; use PQP +(select-collision-algorithm *collision-algorithm-bullet*) ;; use Bullet +\end{verbatim} +} -PQPソフトウェアパッケージの使い方はirteus/PQP/README.txtに -書いてあり，irteus/PQP/src/PQP.hを読むことで理解できるようになって -いる． +個々の外部ライブラリの特徴については以降で詳しく説明する． +他の干渉計算ソフトウェアパッケージについては http://gamma.cs.unc.edu/research/collision/に詳しい．（情報が古い可能性があるので注意．例えばBulletは載っていない．） + +\input{irtcollision-func} + +\subsubsection{物体形状モデル同士の干渉計算例} +以下は，collision-checkやcollision-distanceを利用して，２つの立方体の衝突検出と距離計算を行い，最近点どうしを結ぶ線分を描画する例である． +干渉が生じているときにcollision-distance関数で得られる最近点は，PQPとBulletで仕様が異なる．詳しくは以降のBulletに関する説明を参照． +{\baselineskip=10pt +\begin{verbatim} +;; Make models +(setq *b0* (make-cube 100 100 100)) +(setq *b1* (make-cube 100 100 100)) + +;; Case 1 : no collision +(send *b0* :newcoords (make-coords :pos #f(100 100 -100) + :rpy (list (deg2rad 10) (deg2rad -20) (deg2rad 30)))) +(objects (list *b0* *b1*)) +(print (collision-check *b0* *b1*)) ;; Check collision +(let ((ret (collision-distance *b0* *b1*))) ;; Check distance and nearest points + (print (car ret)) ;; distance + (send (cadr ret) :draw-on :flush nil :size 20 :color #f(1 0 0)) ;; nearest point on *b0* + (send (caddr ret) :draw-on :flush nil :size 20 :color #f(1 0 0)) ;; nearest point on *b1* + (send *irtviewer* :viewer :draw-line (cadr ret) (caddr ret)) + (send *irtviewer* :viewer :viewsurface :flush)) + +;; Case 2 : collision +(send *b0* :newcoords (make-coords :pos #f(50 50 -50) + :rpy (list (deg2rad 10) (deg2rad -20) (deg2rad 30)))) +(objects (list *b0* *b1*)) +(print (collision-check *b0* *b1*)) ;; Check collision +(let ((ret (collision-distance *b0* *b1*))) ;; Check distance and nearest points + (print (car ret)) ;; distance + ;; In case of collision, nearest points are insignificant values. + (send (cadr ret) :draw-on :flush nil :size 20 :color #f(1 0 0)) ;; nearest point on *b0* + (send (caddr ret) :draw-on :flush nil :size 20 :color #f(1 0 0)) ;; nearest point on *b1* + (send *irtviewer* :viewer :draw-line (cadr ret) (caddr ret)) + (send *irtviewer* :viewer :viewsurface :flush)) +\end{verbatim} +} + +\begin{figure}[htb] + \begin{center} + \includegraphics[width=0.50\columnwidth]{fig/collision.jpg} + \caption{Collision detection} + \end{center} +\end{figure} + +\subsubsection{ロボット動作と干渉計算} +ハンドで物体をつかむ，という動作の静的なシミュレーションを行う場合に手（指）のリンクと対象物体の干渉を調べ，これが起こるところで物体をつかむ動作を停止させるということが出来る． + +{\baselineskip=10pt +\begin{verbatim} +(load "irteus/demo/sample-arm-model.l") +(setq *sarm* (instance sarmclass :init)) +(send *sarm* :reset-pose) +(setq a 42) +(send *sarm* :move-fingers a) +(setq *target* (make-cube 30 30 30)) +(send *target* :translate #f(350 200 400)) +(objects (list *sarm* *target*)) + +(send *sarm* :inverse-kinematics *target* :move-target (send *sarm* :end-coords) :debug-view t) +(while (> a 0) + (if (collision-check-objects + (list (send *sarm* :joint-fr :child-link) + (send *sarm* :joint-fl :child-link)) + (list *target*)) + (return)) + (decf a 0.1) + (send *irtviewer* :draw-objects) + (send *sarm* :move-fingers a)) +(send *sarm* :end-coords :assoc *target*) + +(dotimes (i 100) + (send *sarm* :joint0 :joint-angle 1 :relative t) + (send *irtviewer* :draw-objects)) +(send *sarm* :end-coords :dissoc *target*) +(dotimes (i 100) + (send *sarm* :joint0 :joint-angle -1 :relative t) + (send *irtviewer* :draw-objects)) +\end{verbatim} +} -\subsection{irteusからPQPの呼び出し} +同様の機能が，"irteus/demo/sample-arm-model.l"ファイルの:open-hand, +:close-handというメソッドで提供されている． + +\subsection{PQPによる干渉計算} + +PQPはノースカロライナ大学のLin氏らのグループにより開発された干渉計算ライブラリである． +PQPソフトウェアパッケージの使い方はirteus/PQP/README.txtに +書いてあり，irteus/PQP/src/PQP.hを読むことで理解できるようになっている． irteusでPQPを使うためのファイルは CPQP.C, euspqp.c, pqp.l からなる． ２つの幾何モデルが衝突してしるか否かを判定するためには， - {\baselineskip=10pt \begin{verbatim} (defun pqp-collision-check (model1 model2 @@ -90,74 +185,20 @@ \subsection{irteusからPQPの呼び出し} る以降，(pqpbeginmodel m)でPQPの幾何モデルのインスタンスを作成し， (pqpaddtri m v1 v2 v3 id)として面情報を登録している． -\subsubsection{物体形状モデル同士の干渉計算例} -pqp-collision-checkやpqp-collision-distanceを利用した例を示す． -{\baselineskip=10pt -\begin{verbatim} -;; Make models -(setq *b0* (make-cube 100 100 100)) -(setq *b1* (make-cube 100 100 100)) - -;; Case 1 : no collision -(send *b0* :newcoords (make-coords :pos #f(100 100 -100) - :rpy (list (deg2rad 10) (deg2rad -20) (deg2rad 30)))) -(objects (list *b0* *b1*)) -(print (pqp-collision-check *b0* *b1*)) ;; Check collision -(let ((ret (pqp-collision-distance *b0* *b1*))) ;; Check distance and nearest points - (print (car ret)) ;; distance - (send (cadr ret) :draw-on :flush nil :size 20 :color #f(1 0 0)) ;; nearest point on *b0* - (send (caddr ret) :draw-on :flush nil :size 20 :color #f(1 0 0)) ;; nearest point on *b1* - (send *irtviewer* :viewer :draw-line (cadr ret) (caddr ret)) - (send *irtviewer* :viewer :viewsurface :flush)) - -;; Case 2 : collision -(send *b0* :newcoords (make-coords :pos #f(50 50 -50) - :rpy (list (deg2rad 10) (deg2rad -20) (deg2rad 30)))) -(objects (list *b0* *b1*)) -(print (pqp-collision-check *b0* *b1*)) ;; Check collision -(let ((ret (pqp-collision-distance *b0* *b1*))) ;; Check distance and nearest points - (print (car ret)) ;; distance - ;; In case of collision, nearest points are insignificant values. - (send (cadr ret) :draw-on :flush nil :size 20 :color #f(1 0 0)) ;; nearest point on *b0* - (send (caddr ret) :draw-on :flush nil :size 20 :color #f(1 0 0)) ;; nearest point on *b1* - (send *irtviewer* :viewer :draw-line (cadr ret) (caddr ret)) - (send *irtviewer* :viewer :viewsurface :flush)) -\end{verbatim} -} - -\subsection{ロボット動作と干渉計算} - -ハンドで物体をつかむ，という動作の静的なシミュレーションを行う場合に -手（指）のリンクと対象物体の干渉を調べ，これが起こるところで物体をつか -む動作を停止させるということが出来る． +\input{pqp-func} -{\baselineskip=10pt -\begin{verbatim} -(objects (list *sarm* *target*)) +\subsection{Bulletによる干渉計算} -(send *sarm* :solve-ik *target* :debug-view t) -(while (> a 0) - (if (pqp-collision-check-objects - (list (send *sarm* :joint-fr :child-link) - (send *sarm* :joint-fl :child-link)) - (list *target*)) - (return)) - (decf a 0.1) - (send *irtviewer* :draw-objects) - (send *sarm* :move-fingers a)) -(send *sarm* :end-coords :assoc *target*) - -(dotimes (i 100) - (send *sarm* :joint0 :joint-angle 1 :relative t) - (send *irtviewer* :draw-objects)) -(send *sarm* :end-coords :dissoc *target*) -(dotimes (i 100) - (send *sarm* :joint0 :joint-angle -1 :relative t) - (send *irtviewer* :draw-objects)) -\end{verbatim} -} +Bulletは物理演算エンジンであり，その一部として干渉計算機能が提供されている． +irteusでBulletを使うためのファイルは +CBULLET.cpp, eusbullet.c, bullet.l +からなる． +関数内部の呼び出し順序はPQPと同様である． -同様の機能が，"irteus/demo/sample-arm-model.l"ファイルの:open-hand, -:close-handというメソッドで提供されている． +PQPとBulletの違いとして以下が挙げられる． +\begin{itemize} + \item 干渉が生じているときにcollision-distanceを呼ぶと，PQPでは，距離として0が返り，最近点として意味のない点が返される．一方Bulletでは，距離として干渉をなくすために動かすべき最短距離(penetration depthと呼ばれる)が返る．また，最近点としては，干渉をなくすための最短距離の両端の点が返される． + \item PQPは非凸のメッシュモデルをそのまま扱うことができるが，Bulletでは非凸のモデルの凸包を内部で計算しそれを扱っている． +\end{itemize} - \input{pqp-func} +\input{bullet-func} diff --git a/doc/jmanual.tex b/doc/jmanual.tex index aa0c40678..616a8268d 100644 --- a/doc/jmanual.tex +++ b/doc/jmanual.tex @@ -231,8 +231,10 @@ \part{irteus 拡張} \input{irtviewer} %% irtviewer.l -\input{pqp} +\input{irtcollision} +%% irtcollision.l %% pqp.l +%% bullet.l \input{irtbvh} diff --git a/irteus/irtcollision.l b/irteus/irtcollision.l index 64313a0ee..dc4d1607b 100644 --- a/irteus/irtcollision.l +++ b/irteus/irtcollision.l @@ -69,6 +69,18 @@ (error "invalid collision algorithm: ~a~%" *collision-algorithm*))) ) +(defun collision-check-objects + (obj1 obj2 &rest args &key &allow-other-keys) + "Check collision between obj1 and obj2. + obj1 and obj2 should be list of models. + If return value is nil, no collision. + Otherwise (return value is t), collision." + (dolist (o1 obj1) + (dolist (o2 obj2) + (if (> (collision-check o1 o2) 0) + (return-from collision-check-objects t)))) + nil) + (defun select-collision-algorithm (alg) "Select collision algorithm.