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..75a60de2d 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,7 @@ 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; + [[ ( "$DOCKER_IMAGE" == *"trusty"* || "$DOCKER_IMAGE" == *"jessie"* ) && $test_l =~ test-eusbullet-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/BULLET/BulletCollision.cpp b/irteus/BULLET/BulletCollision.cpp new file mode 100644 index 000000000..123370ff6 --- /dev/null +++ b/irteus/BULLET/BulletCollision.cpp @@ -0,0 +1,265 @@ +/////////////////////////////////////////////////////////////////////////////// +/// +/// $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. +/// + + +// 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 makeSphereModel(double radius) +{ + return (long)(new btSphereShape(radius)); +}; + +long makeBoxModel(double xsize, double ysize, double zsize) +{ + return (long)(new btBoxShape(0.5*btVector3(xsize, ysize, zsize))); +}; + +long makeCylinderModel(double radius, double height) +{ + return (long)(new btCylinderShapeZ(btVector3(radius, radius, 0.5*height))); +}; + +long makeCapsuleModel(double radius, double height) +{ + return (long)(new btCapsuleShapeZ(radius, 0.5*height)); +}; + +long 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 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 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 callMakeSphereModel(eusfloat_t *r) + { + CALL_WITH_BULLET_CHECK(return makeSphereModel(r[0]);) + } + + eusinteger_t callMakeBoxModel(eusfloat_t *xyz) + { + CALL_WITH_BULLET_CHECK(return makeBoxModel(xyz[0], xyz[1], xyz[2]);) + } + + eusinteger_t callMakeCylinderModel(eusfloat_t *rh) + { + CALL_WITH_BULLET_CHECK(return makeCylinderModel(rh[0], rh[1]);) + } + + eusinteger_t callMakeCapsuleModel(eusfloat_t *rh) + { + CALL_WITH_BULLET_CHECK(return makeCapsuleModel(rh[0], rh[1]);) + } + + eusinteger_t callMakeMeshModel(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 makeMeshModel(_verticesPoints, numVertices);) + } + + eusinteger_t callCalcCollisionDistance(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 = 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 callSetMargin(eusinteger_t modelAddr, eusfloat_t *margin) + { + CALL_WITH_BULLET_CHECK(return setMargin(modelAddr, margin[0]);) + } +} diff --git a/irteus/BULLET/Makefile b/irteus/BULLET/Makefile new file mode 100644 index 000000000..5ded45f98 --- /dev/null +++ b/irteus/BULLET/Makefile @@ -0,0 +1,33 @@ +include Makefile.$(ARCHDIR) + +CFLAGS = -I. -I.. -D$(ARCHDIR) -D$(MACHINE) -DHAVE_BULLET=$(HAVE_BULLET) `pkg-config bullet --cflags` +LDFLAGS = -L. -L.. `pkg-config bullet --libs` + +.SUFFIXES: .cpp + +SRCS = BulletCollision.cpp + +OBJECTS = $(ARCHDIR)/BulletCollision.o + +CLEAN = $(ARCHDIR)/$(LPFX)BULLET.$(LSFX) $(OBJECTS) *.$(LSFX) + +default: directory library + +directory: + if [ ! -e $(ARCHDIR) ]; then mkdir -p $(ARCHDIR); fi + +install: $(EUSDIR)/$(ARCHDIR)/lib/$(LPFX)BULLET.$(LSFX) + +$(EUSDIR)/$(ARCHDIR)/lib/$(LPFX)BULLET.$(LSFX): $(ARCHDIR)/$(LPFX)BULLET.$(LSFX) + cp $(ARCHDIR)/$(LPFX)BULLET.$(LSFX) $(EUSDIR)/$(ARCHDIR)/lib + +library: $(ARCHDIR)/$(LPFX)BULLET.$(LSFX) + +$(ARCHDIR)/$(LPFX)BULLET.$(LSFX): $(OBJECTS) + $(LD) $(SOFLAGS) $(OUTOPT)$(ARCHDIR)/$(LPFX)BULLET.$(LSFX) $(OBJECTS) $(LDFLAGS) + +$(OBJECTS): $(SRCS) + $(CC) $(CFLAGS) -DCOMPILE_LIB -c $(*F).cpp $(OBJOPT)$@ + +clean: + rm -f $(CLEAN) diff --git a/irteus/BULLET/Makefile.Cygwin b/irteus/BULLET/Makefile.Cygwin new file mode 100755 index 000000000..95ca8a1cc --- /dev/null +++ b/irteus/BULLET/Makefile.Cygwin @@ -0,0 +1,10 @@ +CC = c++ -O2 -falign-functions=4 +OBJOPT = -o +OUTOPT = -o +LD = c++ -shared -falign-functions=4 +EXELD = c++ -falign-functions=4 +SOFLAGS = +EXESFX = .exe +LSFX = dll +LPFX = lib +LIBS = -L$(ARCHDIR) -lRAPID diff --git a/irteus/BULLET/Makefile.Darwin b/irteus/BULLET/Makefile.Darwin new file mode 100644 index 000000000..87cb77a3e --- /dev/null +++ b/irteus/BULLET/Makefile.Darwin @@ -0,0 +1,10 @@ +CC = c++ -O2 -falign-functions=8 -fPIC -DDarwin -DGCC -I$(EUSDIR)/include +OBJOPT = -o +OUTOPT = -o +LD = c++ +SOFLAGS = -dynamiclib -flat_namespace -undefined suppress +EXELD = c++ +EXESFX = +LSFX = so +LPFX = lib +LIBS = -L$(ARCHDIR) -lRAPID diff --git a/irteus/BULLET/Makefile.Linux b/irteus/BULLET/Makefile.Linux new file mode 100644 index 000000000..dfe82555a --- /dev/null +++ b/irteus/BULLET/Makefile.Linux @@ -0,0 +1,19 @@ +CC = c++ -O2 -DLinux -DGCC -I$(EUSDIR)/include +OBJOPT = -o +OUTOPT = -o +LD = c++ +SOFLAGS = -shared +EXELD = c++ +EXESFX = +LSFX = so +LPFX = lib +LIBS = -L$(ARCHDIR) -lRAPID + + +ifeq ($(shell /bin/uname -m), x86_64) +CC += -m32 +LD += -m32 +EXELD += -m32 +endif + + diff --git a/irteus/BULLET/Makefile.Linux64 b/irteus/BULLET/Makefile.Linux64 new file mode 100644 index 000000000..0842f7ece --- /dev/null +++ b/irteus/BULLET/Makefile.Linux64 @@ -0,0 +1,10 @@ +CC = c++ -O2 -falign-functions=8 -fPIC -DLinux -DGCC -I$(EUSDIR)/include +OBJOPT = -o +OUTOPT = -o +LD = c++ +SOFLAGS = -shared +EXELD = c++ +EXESFX = +LSFX = so +LPFX = lib +LIBS = -L$(ARCHDIR) -lRAPID diff --git a/irteus/BULLET/Makefile.LinuxARM b/irteus/BULLET/Makefile.LinuxARM new file mode 100644 index 000000000..6fcae34de --- /dev/null +++ b/irteus/BULLET/Makefile.LinuxARM @@ -0,0 +1,10 @@ +CC = c++ -O2 -fPIC -falign-functions=4 -DLinux -DGCC -I$(EUSDIR)/include +OBJOPT = -o +OUTOPT = -o +LD = c++ +SOFLAGS = -shared +EXELD = c++ +EXESFX = +LSFX = so +LPFX = lib +LIBS = -L$(ARCHDIR) -lRAPID diff --git a/irteus/Makefile b/irteus/Makefile index c63f58ef4..d60a36716 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 @@ -44,7 +57,7 @@ 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 irtscene irtmodel irtsensor irtdyna irtrobot irtbvh irtcollada irtpointcloud eusbullet IRTEUSX=irtx IRTEUSIMG=irtimage eusjpeg png IRTEUSGL=irtgl irtglrgb irtviewer @@ -138,7 +151,7 @@ $(IRTEUSOBJS) $(INSTALLOBJDIR)/compile_irt.log: $(IRTEUS_L) -rm -f $(IRTEUS_C) $(IRTEUS_H) $(IRTEUSGOBJS): $(INSTALLOBJDIR)/compile_irtg.log -$(IRTEUSGOBJS) $(INSTALLOBJDIR)/compile_irtg.log: $(IRTEUSG_L) +$(IRTEUSGOBJS) $(INSTALLOBJDIR)/compile_irtg.log: $(IRTEUSG_L) $(INSTALLLIBDIR)/$(LPFX)BULLET.$(LSFX) $(BINDIR)/eusgl$(ESFX) "(setq *objdir* \"$(INSTALLOBJDIR)/\")" < ./compile_irtg.l > $(INSTALLOBJDIR)/compile_irtg.log -rm -f $(IRTEUSG_C) $(IRTEUSG_H) @@ -160,6 +173,12 @@ $(IRTEUSGLOBJS) $(INSTALLOBJDIR)/compile_irtgl.log: $(IRTEUSGL_L) PQP/$(ARCHDIR)/libPQP-static.a: make -C PQP +$(INSTALLLIBDIR)/$(LPFX)BULLET.$(LSFX): BULLET/$(ARCHDIR)/$(LPFX)BULLET.$(LSFX) + cp BULLET/$(ARCHDIR)/$(LPFX)BULLET.$(LSFX) $(INSTALLLIBDIR)/$(LPFX)BULLET.$(LSFX) + +BULLET/$(ARCHDIR)/$(LPFX)BULLET.$(LSFX): BULLET/BulletCollision.cpp + make -C BULLET MACHINE=$(MACHINE) HAVE_BULLET=$(HAVE_BULLET) + $(LIBNR): $(NROBJECTS) $(LD) $(SOFLAGS) $(OUTOPT)$(LIBNR) $(NROBJECTS) @@ -189,6 +208,7 @@ clean: -rm -f $(INSTALLOBJDIR)/compile_*.log chmod a-x Makefile* *.l *.c (cd PQP;make clean) + (cd BULLET;make clean) (cd $(EUSDIR)/lisp/image/jpeg/; make clean) -rm -f $(EUSDIR)/lib/llib/pgsql.c $(EUSDIR)/lib/llib/pgsql.h -rm -f $(EUSDIR)/lib/llib/time.c $(EUSDIR)/lib/llib/time.h @@ -211,6 +231,7 @@ $(INSTALLOBJDIR)/irtglrgb.$(OSFX): irtglrgb.l $(INSTALLOBJDIR)/irtviewer.$(OSFX): irtviewer.l $(INSTALLOBJDIR)/irtimage.$(OSFX): irtimage.l $(INSTALLOBJDIR)/pqp.$(OSFX): pqp.l +$(INSTALLOBJDIR)/eusbullet.$(OSFX): eusbullet.l $(INSTALLOBJDIR)/png.$(OSFX): png.l $(INSTALLOBJDIR)/pgsql.$(OSFX): $(EUSDIR)/lib/llib/pgsql.l $(INSTALLOBJDIR)/time.$(OSFX): $(EUSDIR)/lib/llib/time.l diff --git a/irteus/compile_irtg.l b/irteus/compile_irtg.l index a3e36f549..68a9528fb 100644 --- a/irteus/compile_irtg.l +++ b/irteus/compile_irtg.l @@ -31,6 +31,7 @@ (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 "eusbullet.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/eusbullet-collision.l b/irteus/demo/eusbullet-collision.l new file mode 100644 index 000000000..44b48730b --- /dev/null +++ b/irteus/demo/eusbullet-collision.l @@ -0,0 +1,125 @@ +(load "sample-arm-model.l") + + +(defun sample-collision-distance-body + (&key + (obj1 (make-cube 200 300 500)) + (obj2 (make-cube 400 600 1000)) + (collision-distance-func #'bt-collision-distance) ;; you can try #'pqp-collision-distance + (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) + ;; get distance beween target-link and obj + (setq ret (funcall collision-distance-func 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)) + (collision-distance-func #'bt-collision-distance) + (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) + ;; get distance beween target-link and obj + (setq ret (funcall collision-distance-func 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 (funcall collision-distance-func 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.l b/irteus/eusbullet.l new file mode 100644 index 000000000..fd318222d --- /dev/null +++ b/irteus/eusbullet.l @@ -0,0 +1,189 @@ +;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; +;;; +;;; $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") + +(defvar *eusbullet-lib* + (load-foreign (format nil "~A~A/lib/libBULLET.so" *eusdir* (unix:getenv "ARCHDIR")))) + +(export '(bt-collision-distance bt-collision-check)) + + +;; eusinteger_t callMakeSphereModel(eusfloat_t *r) +(defforeign _bt-make-sphere-model + *eusbullet-lib* + "callMakeSphereModel" + (:string) + :integer + ) + +;; eusinteger_t callMakeBoxModel(eusfloat_t *xyz) +(defforeign _bt-make-box-model + *eusbullet-lib* + "callMakeBoxModel" + (:string) + :integer + ) + +;; eusinteger_t callMakeCylinderModel(eusfloat_t *rh) +(defforeign _bt-make-cylinder-model + *eusbullet-lib* + "callMakeCylinderModel" + (:string) + :integer + ) + +;; eusinteger_t callMakeCapsuleModel(eusfloat_t *rh) +(defforeign _bt-make-capsule-model + *eusbullet-lib* + "callMakeCapsuleModel" + (:string) + :integer + ) + +;; eusinteger_t callMakeMeshModel(eusfloat_t *verticesPoints, eusinteger_t numVertices) +(defforeign _bt-make-mesh-model + *eusbullet-lib* + "callMakeMeshModel" + (:string :integer) + :integer + ) + +;; eusinteger_t callCalcCollisionDistance(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) +(defforeign _bt-calc-collision-distance + *eusbullet-lib* + "callCalcCollisionDistance" + (:integer :integer + :string :string :string :string + :string :string :string :string) + :integer + ) + +;; eusinteger_t callSetMargin(eusinteger_t modelAddr, eusfloat_t *margin) +(defforeign _bt-set-margin + *eusbullet-lib* + "callSetMargin" + (:integer :string) + :integer + ) + +(defun bt-make-model-from-body + (b &key (csg (send b :csg)) (margin nil) m) + "Make bullet model from body." + (cond ((assoc :sphere csg) + (setq m + (_bt-make-sphere-model + (float-vector (* 1e-3 (user::radius-of-sphere b)))) + )) + ((assoc :cube csg) + (setq m + (_bt-make-box-model + (float-vector + (* 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 + (_bt-make-cylinder-model + (float-vector + (* 1e-3 (user::radius-of-cylinder b)) + (* 1e-3 (user::height-of-cylinder b))) + ))) + (t + (setq m + (_bt-make-mesh-model + (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 + (_bt-set-margin m (float-vector margin))) + m) + +(defmethod cascaded-coords + (:make-btmodel + (&key (fat 0) vs m) + "Make bullet model and save pointer of the bullet model." + (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 + (_bt-make-mesh-model + (scale 1e-3 ;; [m] + (apply #'concatenate float-vector + (mapcar #'(lambda (v) (send self :inverse-transform-vector v)) vs))) + (length vs) + )) + (_bt-set-margin m (float-vector 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]). + If collision occurs, [distance] is 0 and nearest points are insignificant values. + 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))) + (_bt-calc-collision-distance + 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 :eusbullet "$Id$") diff --git a/irteus/irtext.l b/irteus/irtext.l index bedd28407..8b4216ebd 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")) (in-package "USER") (import '(collada::convert-irtmodel-to-collada collada::eus2collada))) (defun load-irteusx () diff --git a/irteus/test/test-eusbullet-collision.l b/irteus/test/test-eusbullet-collision.l new file mode 100644 index 000000000..3566b5a9a --- /dev/null +++ b/irteus/test/test-eusbullet-collision.l @@ -0,0 +1,43 @@ +(require :unittest "lib/llib/unittest.l") +(init-unit-test) + +(deftest test-eusbullet-collision-sphere-analytical + (let* ((radius1 100) + (radius2 200) + (obj1 (make-sphere radius1)) + (obj2 (make-sphere radius2)) + (cnt 0) + (bt-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 (bt-collision-distance obj1 obj2)) + (setq bt-dist (elt ret 0)) + ;; get analytical distance and compare + (setq analy-dist (- (norm (send obj1 :worldpos)) (+ radius1 radius2))) + (assert (eps= bt-dist analy-dist 1e-3)) + ;; 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)) + ) + )) + +(eval-when (load eval) + (run-all-tests) + (exit 0))