diff --git a/examples/BasicDemo/CMakeLists.txt b/examples/BasicDemo/CMakeLists.txt index 7286d93f9..2ad8f8ccf 100644 --- a/examples/BasicDemo/CMakeLists.txt +++ b/examples/BasicDemo/CMakeLists.txt @@ -52,6 +52,7 @@ SET(AppBasicExampleGui_SRCS ../StandaloneMain/main_opengl_single_example.cpp ../ExampleBrowser/OpenGLGuiHelper.cpp ../ExampleBrowser/GL_ShapeDrawer.cpp + ../ExampleBrowser/CollisionShape2TriangleMesh.cpp ) #this define maps StandaloneExampleCreateFunc to the right 'CreateFunc' diff --git a/examples/BasicDemo/premake4.lua b/examples/BasicDemo/premake4.lua index 2b2a7d45f..463faaf35 100644 --- a/examples/BasicDemo/premake4.lua +++ b/examples/BasicDemo/premake4.lua @@ -48,6 +48,7 @@ files { "../StandaloneMain/main_opengl_single_example.cpp", "../ExampleBrowser/OpenGLGuiHelper.cpp", "../ExampleBrowser/GL_ShapeDrawer.cpp", + "../ExampleBrowser/CollisionShape2TriangleMesh.cpp", } if os.is("Linux") then initX11() end @@ -85,6 +86,7 @@ files { "../StandaloneMain/main_sw_tinyrenderer_single_example.cpp", "../ExampleBrowser/OpenGLGuiHelper.cpp", "../ExampleBrowser/GL_ShapeDrawer.cpp", + "../ExampleBrowser/CollisionShape2TriangleMesh.cpp", "../TinyRenderer/geometry.cpp", "../TinyRenderer/model.cpp", "../TinyRenderer/tgaimage.cpp", @@ -98,4 +100,4 @@ if os.is("Linux") then initX11() end if os.is("MacOSX") then links{"Cocoa.framework"} end - \ No newline at end of file + diff --git a/examples/CommonInterfaces/CommonExampleInterface.h b/examples/CommonInterfaces/CommonExampleInterface.h index 28859f99e..f3fb20047 100644 --- a/examples/CommonInterfaces/CommonExampleInterface.h +++ b/examples/CommonInterfaces/CommonExampleInterface.h @@ -48,6 +48,31 @@ public: }; +class ExampleEntries +{ + +public: + + virtual ~ExampleEntries() {} + + + virtual void initExampleEntries()=0; + + virtual void initOpenCLExampleEntries()=0; + + virtual int getNumRegisteredExamples()=0; + + virtual CommonExampleInterface::CreateFunc* getExampleCreateFunc(int index)=0; + + virtual const char* getExampleName(int index)=0; + + virtual const char* getExampleDescription(int index)=0; + + virtual int getExampleOption(int index)=0; + +}; + + CommonExampleInterface* StandaloneExampleCreateFunc(CommonExampleOptions& options); #ifdef B3_USE_STANDALONE_EXAMPLE diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt index 0a1c172f0..91536b947 100644 --- a/examples/ExampleBrowser/CMakeLists.txt +++ b/examples/ExampleBrowser/CMakeLists.txt @@ -7,7 +7,108 @@ INCLUDE_DIRECTORIES( FILE(GLOB GwenGUISupport_SRCS "GwenGUISupport/*" ) FILE(GLOB GwenGUISupport_HDRS "GwenGUISupport/*" ) -SET(ExtendedTutorialsSources + + +IF (WIN32) + INCLUDE_DIRECTORIES( + ${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs/Glew + ) + ADD_DEFINITIONS(-DGLEW_STATIC) +ELSE(WIN32) + IF(APPLE) + find_library(COCOA NAMES Cocoa) + ELSE(APPLE) + ADD_DEFINITIONS("-DGLEW_INIT_OPENGL11_FUNCTIONS=1") + ADD_DEFINITIONS("-DGLEW_STATIC") + ADD_DEFINITIONS("-DGLEW_DYNAMIC_LOAD_ALL_GLX_FUNCTIONS=1") + INCLUDE_DIRECTORIES( ${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs/Glew ) + ENDIF(APPLE) +ENDIF(WIN32) + + +ADD_LIBRARY(BulletExampleBrowserLib + OpenGLExampleBrowser.cpp + OpenGLGuiHelper.cpp + InProcessExampleBrowser.cpp + GL_ShapeDrawer.cpp + CollisionShape2TriangleMesh.cpp + CollisionShape2TriangleMesh.h + ../Utils/b3Clock.cpp + ../Utils/b3Clock.h + ../Utils/b3ResourcePath.cpp + ../Utils/b3ResourcePath.h + ${GwenGUISupport_SRCS} + ${GwenGUISupport_HDRS} + +) + +SET_TARGET_PROPERTIES(BulletExampleBrowserLib PROPERTIES VERSION ${BULLET_VERSION}) +SET_TARGET_PROPERTIES(BulletExampleBrowserLib PROPERTIES SOVERSION ${BULLET_VERSION}) +IF (BUILD_SHARED_LIBS) + IF (WIN32) + TARGET_LINK_LIBRARIES( + BulletExampleBrowserLib Bullet3Common BulletSoftBody BulletDynamics BulletCollision BulletInverseDynamicsUtils + BulletInverseDynamics LinearMath OpenGLWindow gwen + ${OPENGL_gl_LIBRARY} ${OPENGL_glu_LIBRARY} + ) + ELSE(WIN32) + IF(APPLE) + TARGET_LINK_LIBRARIES( + BulletExampleBrowserLib Bullet3Common BulletSoftBody BulletDynamics BulletCollision BulletInverseDynamicsUtils + BulletInverseDynamics LinearMath OpenGLWindow gwen + ${COCOA} ${OPENGL_gl_LIBRARY} ${OPENGL_glu_LIBRARY} + ) + ELSE(APPLE) + TARGET_LINK_LIBRARIES( + BulletExampleBrowserLib Bullet3Common BulletSoftBody BulletDynamics BulletCollision BulletInverseDynamicsUtils + BulletInverseDynamics LinearMath OpenGLWindow gwen + pthread dl + ) + ENDIF(APPLE) + ENDIF(WIN32) +ENDIF(BUILD_SHARED_LIBS) + +#################### +# +# Bullet Example Browser main app +# +#################### + +INCLUDE_DIRECTORIES( + . + ${BULLET_PHYSICS_SOURCE_DIR}/src + ${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs +) + + +LINK_LIBRARIES( + BulletExampleBrowserLib Bullet3Common BulletSoftBody BulletDynamics BulletCollision BulletInverseDynamicsUtils BulletInverseDynamics LinearMath OpenGLWindow gwen +) + +IF (WIN32) + INCLUDE_DIRECTORIES( + ${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs/Glew + ) + LINK_LIBRARIES( + ${OPENGL_gl_LIBRARY} ${OPENGL_glu_LIBRARY} + ) + ADD_DEFINITIONS(-DGLEW_STATIC) +ELSE(WIN32) + IF(APPLE) + find_library(COCOA NAMES Cocoa) + MESSAGE(${COCOA}) + link_libraries(${COCOA} ${OPENGL_gl_LIBRARY} ${OPENGL_glu_LIBRARY}) + ELSE(APPLE) + ADD_DEFINITIONS("-DGLEW_INIT_OPENGL11_FUNCTIONS=1") + ADD_DEFINITIONS("-DGLEW_STATIC") + ADD_DEFINITIONS("-DGLEW_DYNAMIC_LOAD_ALL_GLX_FUNCTIONS=1") + INCLUDE_DIRECTORIES( ${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs/Glew ) + LINK_LIBRARIES( pthread dl) + ENDIF(APPLE) +ENDIF(WIN32) + + +SET(ExtendedTutorialsSources ../ExtendedTutorials/SimpleBox.cpp ../ExtendedTutorials/MultipleBoxes.cpp ../ExtendedTutorials/SimpleJoint.cpp @@ -18,10 +119,14 @@ SET(ExtendedTutorialsSources ) SET(BulletExampleBrowser_SRCS - OpenGLExampleBrowser.cpp - OpenGLGuiHelper.cpp - InProcessExampleBrowser.cpp - GL_ShapeDrawer.cpp + + ../TinyRenderer/Bullet2TinyRenderer.cpp + ../TinyRenderer/geometry.cpp + ../TinyRenderer/model.cpp + ../TinyRenderer/tgaimage.cpp + ../TinyRenderer/our_gl.cpp + ../TinyRenderer/TinyRenderer.cpp + ../RenderingExamples/TinyRendererSetup.cpp ../SharedMemory/PhysicsServer.cpp ../SharedMemory/PhysicsClientSharedMemory.cpp ../SharedMemory/PhysicsClient.cpp @@ -181,106 +286,17 @@ SET(BulletExampleBrowser_SRCS ../ThirdPartyLibs/tinyxml/tinyxml.cpp ../ThirdPartyLibs/tinyxml/tinyxmlerror.cpp ../ThirdPartyLibs/tinyxml/tinyxmlparser.cpp - ../Utils/b3Clock.cpp - ../Utils/b3Clock.h - ../Utils/b3ResourcePath.cpp - ../Utils/b3ResourcePath.h - ${GwenGUISupport_SRCS} - ${GwenGUISupport_HDRS} - ${ExtendedTutorialsSources} ${BULLET_PHYSICS_SOURCE_DIR}/build3/bullet.rc ) -IF (WIN32) - INCLUDE_DIRECTORIES( - ${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs/Glew - ) - ADD_DEFINITIONS(-DGLEW_STATIC) -ELSE(WIN32) - IF(APPLE) - find_library(COCOA NAMES Cocoa) - ELSE(APPLE) - ADD_DEFINITIONS("-DGLEW_INIT_OPENGL11_FUNCTIONS=1") - ADD_DEFINITIONS("-DGLEW_STATIC") - ADD_DEFINITIONS("-DGLEW_DYNAMIC_LOAD_ALL_GLX_FUNCTIONS=1") - INCLUDE_DIRECTORIES( ${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs/Glew ) - ENDIF(APPLE) -ENDIF(WIN32) - - -ADD_LIBRARY(BulletExampleBrowserLib ${BulletExampleBrowser_SRCS} ) -SET_TARGET_PROPERTIES(BulletExampleBrowserLib PROPERTIES VERSION ${BULLET_VERSION}) -SET_TARGET_PROPERTIES(BulletExampleBrowserLib PROPERTIES SOVERSION ${BULLET_VERSION}) -IF (BUILD_SHARED_LIBS) - IF (WIN32) - TARGET_LINK_LIBRARIES( - BulletExampleBrowserLib Bullet3Common BulletSoftBody BulletDynamics BulletCollision BulletInverseDynamicsUtils - BulletInverseDynamics LinearMath OpenGLWindow gwen - ${OPENGL_gl_LIBRARY} ${OPENGL_glu_LIBRARY} - ) - ELSE(WIN32) - IF(APPLE) - TARGET_LINK_LIBRARIES( - BulletExampleBrowserLib Bullet3Common BulletSoftBody BulletDynamics BulletCollision BulletInverseDynamicsUtils - BulletInverseDynamics LinearMath OpenGLWindow gwen - ${COCOA} ${OPENGL_gl_LIBRARY} ${OPENGL_glu_LIBRARY} - ) - ELSE(APPLE) - TARGET_LINK_LIBRARIES( - BulletExampleBrowserLib Bullet3Common BulletSoftBody BulletDynamics BulletCollision BulletInverseDynamicsUtils - BulletInverseDynamics LinearMath OpenGLWindow gwen - pthread dl - ) - ENDIF(APPLE) - ENDIF(WIN32) -ENDIF(BUILD_SHARED_LIBS) - -#################### -# -# Bullet Example Browser main app -# -#################### - -INCLUDE_DIRECTORIES( - . - ${BULLET_PHYSICS_SOURCE_DIR}/src - ${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs -) - - -LINK_LIBRARIES( - BulletExampleBrowserLib Bullet3Common BulletSoftBody BulletDynamics BulletCollision BulletInverseDynamicsUtils BulletInverseDynamics LinearMath OpenGLWindow gwen -) - -IF (WIN32) - INCLUDE_DIRECTORIES( - ${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs/Glew - ) - LINK_LIBRARIES( - ${OPENGL_gl_LIBRARY} ${OPENGL_glu_LIBRARY} - ) - ADD_DEFINITIONS(-DGLEW_STATIC) -ELSE(WIN32) - IF(APPLE) - find_library(COCOA NAMES Cocoa) - MESSAGE(${COCOA}) - link_libraries(${COCOA} ${OPENGL_gl_LIBRARY} ${OPENGL_glu_LIBRARY}) - ELSE(APPLE) - ADD_DEFINITIONS("-DGLEW_INIT_OPENGL11_FUNCTIONS=1") - ADD_DEFINITIONS("-DGLEW_STATIC") - ADD_DEFINITIONS("-DGLEW_DYNAMIC_LOAD_ALL_GLX_FUNCTIONS=1") - INCLUDE_DIRECTORIES( ${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs/Glew ) - LINK_LIBRARIES( pthread dl) - ENDIF(APPLE) -ENDIF(WIN32) - - ADD_EXECUTABLE(App_ExampleBrowser main.cpp ExampleEntries.cpp ExampleEntries.h + ${ExtendedTutorialsSources} + ${BulletExampleBrowser_SRCS} ) FILE( MAKE_DIRECTORY "${PROJECT_BINARY_DIR}/data" ) diff --git a/examples/ExampleBrowser/CollisionShape2TriangleMesh.cpp b/examples/ExampleBrowser/CollisionShape2TriangleMesh.cpp new file mode 100644 index 000000000..1105955dc --- /dev/null +++ b/examples/ExampleBrowser/CollisionShape2TriangleMesh.cpp @@ -0,0 +1,192 @@ + +#include "CollisionShape2TriangleMesh.h" + +#include "btBulletCollisionCommon.h" +#include "BulletCollision/CollisionShapes/btShapeHull.h"//to create a tesselation of a generic btConvexShape + +void CollisionShape2TriangleMesh(btCollisionShape* collisionShape, const btTransform& parentTransform, btAlignedObjectArray& vertexPositions, btAlignedObjectArray& vertexNormals, btAlignedObjectArray& indicesOut) + +{ +//todo: support all collision shape types + switch (collisionShape->getShapeType()) + { + case SOFTBODY_SHAPE_PROXYTYPE: + { + //skip the soft body collision shape for now + break; + } + case STATIC_PLANE_PROXYTYPE: + { + //draw a box, oriented along the plane normal + const btStaticPlaneShape* staticPlaneShape = static_cast(collisionShape); + btScalar planeConst = staticPlaneShape->getPlaneConstant(); + const btVector3& planeNormal = staticPlaneShape->getPlaneNormal(); + btVector3 planeOrigin = planeNormal * planeConst; + btVector3 vec0,vec1; + btPlaneSpace1(planeNormal,vec0,vec1); + btScalar vecLen = 100.f; + btVector3 verts[4]; + + verts[0] = planeOrigin + vec0*vecLen + vec1*vecLen; + verts[1] = planeOrigin - vec0*vecLen + vec1*vecLen; + verts[2] = planeOrigin - vec0*vecLen - vec1*vecLen; + verts[3] = planeOrigin + vec0*vecLen - vec1*vecLen; + + int startIndex = vertexPositions.size(); + indicesOut.push_back(startIndex+0); + indicesOut.push_back(startIndex+1); + indicesOut.push_back(startIndex+2); + indicesOut.push_back(startIndex+0); + indicesOut.push_back(startIndex+2); + indicesOut.push_back(startIndex+3); + + btVector3 triNormal = parentTransform.getBasis()*planeNormal; + + + for (int i=0;i<4;i++) + { + btVector3 vtxPos; + btVector3 pos =parentTransform*verts[i]; + vertexPositions.push_back(pos); + vertexNormals.push_back(triNormal); + } + break; + } + case TRIANGLE_MESH_SHAPE_PROXYTYPE: + { + + + btBvhTriangleMeshShape* trimesh = (btBvhTriangleMeshShape*) collisionShape; + btVector3 trimeshScaling = trimesh->getLocalScaling(); + btStridingMeshInterface* meshInterface = trimesh->getMeshInterface(); + btAlignedObjectArray vertices; + btAlignedObjectArray indices; + + for (int partId=0;partIdgetNumSubParts();partId++) + { + + const unsigned char *vertexbase = 0; + int numverts = 0; + PHY_ScalarType type = PHY_INTEGER; + int stride = 0; + const unsigned char *indexbase = 0; + int indexstride = 0; + int numfaces = 0; + PHY_ScalarType indicestype = PHY_INTEGER; + //PHY_ScalarType indexType=0; + + btVector3 triangleVerts[3]; + meshInterface->getLockedReadOnlyVertexIndexBase(&vertexbase,numverts, type,stride,&indexbase,indexstride,numfaces,indicestype,partId); + btVector3 aabbMin,aabbMax; + + for (int triangleIndex = 0 ; triangleIndex < numfaces;triangleIndex++) + { + unsigned int* gfxbase = (unsigned int*)(indexbase+triangleIndex*indexstride); + + for (int j=2;j>=0;j--) + { + + int graphicsindex = indicestype==PHY_SHORT?((unsigned short*)gfxbase)[j]:gfxbase[j]; + if (type == PHY_FLOAT) + { + float* graphicsbase = (float*)(vertexbase+graphicsindex*stride); + triangleVerts[j] = btVector3( + graphicsbase[0]*trimeshScaling.getX(), + graphicsbase[1]*trimeshScaling.getY(), + graphicsbase[2]*trimeshScaling.getZ()); + } + else + { + double* graphicsbase = (double*)(vertexbase+graphicsindex*stride); + triangleVerts[j] = btVector3( btScalar(graphicsbase[0]*trimeshScaling.getX()), + btScalar(graphicsbase[1]*trimeshScaling.getY()), + btScalar(graphicsbase[2]*trimeshScaling.getZ())); + } + } + indices.push_back(vertices.size()); + vertices.push_back(triangleVerts[0]); + indices.push_back(vertices.size()); + vertices.push_back(triangleVerts[1]); + indices.push_back(vertices.size()); + vertices.push_back(triangleVerts[2]); + + btVector3 triNormal = (triangleVerts[1]-triangleVerts[0]).cross(triangleVerts[2]-triangleVerts[0]); + triNormal.normalize(); + + for (int v=0;v<3;v++) + { + + btVector3 pos =parentTransform*triangleVerts[v]; + indicesOut.push_back(vertexPositions.size()); + vertexPositions.push_back(pos); + vertexNormals.push_back(triNormal); + + } + + + } + } + + break; + } + default: + { + if (collisionShape->isConvex()) + { + btConvexShape* convex = (btConvexShape*)collisionShape; + { + btShapeHull* hull = new btShapeHull(convex); + hull->buildHull(0.0); + + { + //int strideInBytes = 9*sizeof(float); + //int numVertices = hull->numVertices(); + //int numIndices =hull->numIndices(); + + for (int t=0;tnumTriangles();t++) + { + + btVector3 triNormal; + + int index0 = hull->getIndexPointer()[t*3+0]; + int index1 = hull->getIndexPointer()[t*3+1]; + int index2 = hull->getIndexPointer()[t*3+2]; + btVector3 pos0 =parentTransform*hull->getVertexPointer()[index0]; + btVector3 pos1 =parentTransform*hull->getVertexPointer()[index1]; + btVector3 pos2 =parentTransform*hull->getVertexPointer()[index2]; + triNormal = (pos1-pos0).cross(pos2-pos0); + triNormal.normalize(); + + for (int v=0;v<3;v++) + { + int index = hull->getIndexPointer()[t*3+v]; + btVector3 pos =parentTransform*hull->getVertexPointer()[index]; + indicesOut.push_back(vertexPositions.size()); + vertexPositions.push_back(pos); + vertexNormals.push_back(triNormal); + } + } + } + } + } else + { + if (collisionShape->isCompound()) + { + btCompoundShape* compound = (btCompoundShape*) collisionShape; + for (int i=0;igetNumChildShapes();i++) + { + + btTransform childWorldTrans = parentTransform * compound->getChildTransform(i); + CollisionShape2TriangleMesh(compound->getChildShape(i),childWorldTrans,vertexPositions,vertexNormals,indicesOut); + } + } else + { + btAssert(0); + } + + } + } + }; +} + + diff --git a/examples/ExampleBrowser/CollisionShape2TriangleMesh.h b/examples/ExampleBrowser/CollisionShape2TriangleMesh.h new file mode 100644 index 000000000..6025d6413 --- /dev/null +++ b/examples/ExampleBrowser/CollisionShape2TriangleMesh.h @@ -0,0 +1,10 @@ +#ifndef COLLISION_SHAPE_2_GRAPHICS_H +#define COLLISION_SHAPE_2_GRAPHICS_H + +#include "LinearMath/btAlignedObjectArray.h" +#include "LinearMath/btTransform.h" +class btCollisionShape; + +void CollisionShape2TriangleMesh(btCollisionShape* collisionShape, const btTransform& parentTransform, btAlignedObjectArray& vertexPositions, btAlignedObjectArray& vertexNormals, btAlignedObjectArray& indicesOut); + +#endif //COLLISION_SHAPE_2_GRAPHICS_H diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index 6d8b86bea..3ac486cd6 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/examples/ExampleBrowser/ExampleEntries.cpp @@ -5,6 +5,8 @@ #include "../RenderingExamples/RenderInstancingDemo.h" #include "../RenderingExamples/CoordinateSystemDemo.h" #include "../RenderingExamples/RaytracerSetup.h" +#include "../RenderingExamples/TinyRendererSetup.h" + #include "../ForkLift/ForkLiftDemo.h" #include "../BasicDemo/BasicExample.h" #include "../Planar2D/Planar2D.h" @@ -84,6 +86,7 @@ struct ExampleEntry }; + static ExampleEntry gDefaultExamples[]= { @@ -223,6 +226,7 @@ static ExampleEntry gDefaultExamples[]= ExampleEntry(1,"Raytest", "Cast rays using the btCollisionWorld::rayTest method. The example shows how to receive the hit position and normal along the ray against the first object. Also it shows how to receive all the hits along a ray.", RaytestCreateFunc), ExampleEntry(1,"Raytracer","Implement an extremely simple ray tracer using the ray trace functionality in btCollisionWorld.", RayTracerCreateFunc), + ExampleEntry(0,"Experiments"), @@ -255,6 +259,7 @@ static ExampleEntry gDefaultExamples[]= ExampleEntry(1,"Instanced Rendering", "Simple example of fast instanced rendering, only active when using OpenGL3+.",RenderInstancingCreateFunc), ExampleEntry(1,"CoordinateSystemDemo","Show the axis and positive rotation direction around the axis.", CoordinateSystemCreateFunc), ExampleEntry(1,"Time Series", "Render some value(s) in a 2D graph window, shifting to the left", TimeSeriesCreateFunc), + ExampleEntry(1,"TinyRenderer", "Very small software renderer.", TinyRendererCreateFunc), //Extended Tutorials Added by Mobeen ExampleEntry(0,"Extended Tutorials"), diff --git a/examples/ExampleBrowser/ExampleEntries.h b/examples/ExampleBrowser/ExampleEntries.h index 286e73f57..0158863d0 100644 --- a/examples/ExampleBrowser/ExampleEntries.h +++ b/examples/ExampleBrowser/ExampleEntries.h @@ -6,31 +6,6 @@ -class ExampleEntries -{ - -public: - - virtual ~ExampleEntries() {} - - - virtual void initExampleEntries()=0; - - virtual void initOpenCLExampleEntries()=0; - - virtual int getNumRegisteredExamples()=0; - - virtual CommonExampleInterface::CreateFunc* getExampleCreateFunc(int index)=0; - - virtual const char* getExampleName(int index)=0; - - virtual const char* getExampleDescription(int index)=0; - - virtual int getExampleOption(int index)=0; - -}; - - class ExampleEntriesAll : public ExampleEntries { diff --git a/examples/ExampleBrowser/InProcessExampleBrowser.cpp b/examples/ExampleBrowser/InProcessExampleBrowser.cpp index 1f03b7e92..f7a1fd871 100644 --- a/examples/ExampleBrowser/InProcessExampleBrowser.cpp +++ b/examples/ExampleBrowser/InProcessExampleBrowser.cpp @@ -130,7 +130,6 @@ static ExampleEntryPhysicsServer gDefaultExamplesPhysicsServer[]= ExampleEntryPhysicsServer(1,"Physics Server (Replay Log)", "Create a physics server that replay a command log from disk.", PhysicsServerCreateFunc,PHYSICS_SERVER_REPLAY_FROM_COMMAND_LOG), - ExampleEntryPhysicsServer(1, "Physics Client", "Create a physics client that can communicate with a physics server over shared memory", PhysicsClientCreateFunc), }; diff --git a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp index 5804c2c65..13933af95 100644 --- a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp +++ b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp @@ -256,6 +256,21 @@ static void MyMouseButtonCallback(int button, int state, float x, float y) } #include +struct FileImporterByExtension +{ + std::string m_extension; + CommonExampleInterface::CreateFunc* m_createFunc; +}; + +static btAlignedObjectArray gFileImporterByExtension; + +void OpenGLExampleBrowser::registerFileImporter(const char* extension, CommonExampleInterface::CreateFunc* createFunc) +{ + FileImporterByExtension fi; + fi.m_extension = extension; + fi.m_createFunc = createFunc; + gFileImporterByExtension.push_back(fi); +} void openFileDemo(const char* filename) { @@ -279,20 +294,15 @@ void openFileDemo(const char* filename) char fullPath[1024]; sprintf(fullPath, "%s", filename); b3FileUtils::toLower(fullPath); - if (strstr(fullPath, ".urdf")) - { - sCurrentDemo = ImportURDFCreateFunc(options); - } else - { - if (strstr(fullPath, ".bullet")) - { - sCurrentDemo = SerializeBulletCreateFunc(options); - } - } + + for (int i=0;isetFileName(filename); - if (sCurrentDemo) { diff --git a/examples/ExampleBrowser/OpenGLExampleBrowser.h b/examples/ExampleBrowser/OpenGLExampleBrowser.h index 99d2ef73b..ac9acd2f6 100644 --- a/examples/ExampleBrowser/OpenGLExampleBrowser.h +++ b/examples/ExampleBrowser/OpenGLExampleBrowser.h @@ -20,6 +20,7 @@ public: virtual void setSharedMemoryInterface(class SharedMemoryInterface* sharedMem); + static void registerFileImporter(const char* extension, CommonExampleInterface::CreateFunc* createFunc); }; #endif //OPENGL_BROWSER_GUI_H diff --git a/examples/ExampleBrowser/OpenGLGuiHelper.cpp b/examples/ExampleBrowser/OpenGLGuiHelper.cpp index 5dd62fcdd..baf20915e 100644 --- a/examples/ExampleBrowser/OpenGLGuiHelper.cpp +++ b/examples/ExampleBrowser/OpenGLGuiHelper.cpp @@ -5,8 +5,9 @@ #include "../CommonInterfaces/CommonGraphicsAppInterface.h" #include "../CommonInterfaces/CommonRenderInterface.h" #include "Bullet3Common/b3Scalar.h" +#include "CollisionShape2TriangleMesh.h" + -#include "BulletCollision/CollisionShapes/btShapeHull.h"//to create a tesselation of a generic btConvexShape #include "../OpenGLWindow/GLInstanceGraphicsShape.h" //backwards compatibility @@ -206,222 +207,6 @@ int OpenGLGuiHelper::registerGraphicsInstance(int shapeIndex, const float* posit return m_data->m_glApp->m_renderer->registerGraphicsInstance(shapeIndex,position,quaternion,color,scaling); } -static void createCollisionShapeGraphicsObjectInternal(btCollisionShape* collisionShape, const btTransform& parentTransform, btAlignedObjectArray& verticesOut, btAlignedObjectArray& indicesOut) -{ -//todo: support all collision shape types - switch (collisionShape->getShapeType()) - { - case SOFTBODY_SHAPE_PROXYTYPE: - { - //skip the soft body collision shape for now - break; - } - case STATIC_PLANE_PROXYTYPE: - { - //draw a box, oriented along the plane normal - const btStaticPlaneShape* staticPlaneShape = static_cast(collisionShape); - btScalar planeConst = staticPlaneShape->getPlaneConstant(); - const btVector3& planeNormal = staticPlaneShape->getPlaneNormal(); - btVector3 planeOrigin = planeNormal * planeConst; - btVector3 vec0,vec1; - btPlaneSpace1(planeNormal,vec0,vec1); - btScalar vecLen = 100.f; - btVector3 verts[4]; - - verts[0] = planeOrigin + vec0*vecLen + vec1*vecLen; - verts[1] = planeOrigin - vec0*vecLen + vec1*vecLen; - verts[2] = planeOrigin - vec0*vecLen - vec1*vecLen; - verts[3] = planeOrigin + vec0*vecLen - vec1*vecLen; - - int startIndex = verticesOut.size(); - indicesOut.push_back(startIndex+0); - indicesOut.push_back(startIndex+1); - indicesOut.push_back(startIndex+2); - indicesOut.push_back(startIndex+0); - indicesOut.push_back(startIndex+2); - indicesOut.push_back(startIndex+3); - - btVector3 triNormal = parentTransform.getBasis()*planeNormal; - - - for (int i=0;i<4;i++) - { - GLInstanceVertex vtx; - btVector3 pos =parentTransform*verts[i]; - vtx.xyzw[0] = pos.x(); - vtx.xyzw[1] = pos.y(); - vtx.xyzw[2] = pos.z(); - vtx.xyzw[3] = 0.f; - - vtx.normal[0] =triNormal.x(); - vtx.normal[1] =triNormal.y(); - vtx.normal[2] =triNormal.z(); - - vtx.uv[0] = 0.5f; - vtx.uv[1] = 0.5f; - verticesOut.push_back(vtx); - } - break; - } - case TRIANGLE_MESH_SHAPE_PROXYTYPE: - { - - - btBvhTriangleMeshShape* trimesh = (btBvhTriangleMeshShape*) collisionShape; - btVector3 trimeshScaling = trimesh->getLocalScaling(); - btStridingMeshInterface* meshInterface = trimesh->getMeshInterface(); - btAlignedObjectArray vertices; - btAlignedObjectArray indices; - - for (int partId=0;partIdgetNumSubParts();partId++) - { - - const unsigned char *vertexbase = 0; - int numverts = 0; - PHY_ScalarType type = PHY_INTEGER; - int stride = 0; - const unsigned char *indexbase = 0; - int indexstride = 0; - int numfaces = 0; - PHY_ScalarType indicestype = PHY_INTEGER; - //PHY_ScalarType indexType=0; - - btVector3 triangleVerts[3]; - meshInterface->getLockedReadOnlyVertexIndexBase(&vertexbase,numverts, type,stride,&indexbase,indexstride,numfaces,indicestype,partId); - btVector3 aabbMin,aabbMax; - - for (int triangleIndex = 0 ; triangleIndex < numfaces;triangleIndex++) - { - unsigned int* gfxbase = (unsigned int*)(indexbase+triangleIndex*indexstride); - - for (int j=2;j>=0;j--) - { - - int graphicsindex = indicestype==PHY_SHORT?((unsigned short*)gfxbase)[j]:gfxbase[j]; - if (type == PHY_FLOAT) - { - float* graphicsbase = (float*)(vertexbase+graphicsindex*stride); - triangleVerts[j] = btVector3( - graphicsbase[0]*trimeshScaling.getX(), - graphicsbase[1]*trimeshScaling.getY(), - graphicsbase[2]*trimeshScaling.getZ()); - } - else - { - double* graphicsbase = (double*)(vertexbase+graphicsindex*stride); - triangleVerts[j] = btVector3( btScalar(graphicsbase[0]*trimeshScaling.getX()), - btScalar(graphicsbase[1]*trimeshScaling.getY()), - btScalar(graphicsbase[2]*trimeshScaling.getZ())); - } - } - indices.push_back(vertices.size()); - vertices.push_back(triangleVerts[0]); - indices.push_back(vertices.size()); - vertices.push_back(triangleVerts[1]); - indices.push_back(vertices.size()); - vertices.push_back(triangleVerts[2]); - - btVector3 triNormal = (triangleVerts[1]-triangleVerts[0]).cross(triangleVerts[2]-triangleVerts[0]); - triNormal.normalize(); - - for (int v=0;v<3;v++) - { - GLInstanceVertex vtx; - btVector3 pos =parentTransform*triangleVerts[v]; - vtx.xyzw[0] = pos.x(); - vtx.xyzw[1] = pos.y(); - vtx.xyzw[2] = pos.z(); - vtx.xyzw[3] = 0.f; - - - vtx.normal[0] =triNormal.x(); - vtx.normal[1] =triNormal.y(); - vtx.normal[2] =triNormal.z(); - - vtx.uv[0] = 0.5f; - vtx.uv[1] = 0.5f; - - indicesOut.push_back(verticesOut.size()); - verticesOut.push_back(vtx); - } - - - } - } - - break; - } - default: - { - if (collisionShape->isConvex()) - { - btConvexShape* convex = (btConvexShape*)collisionShape; - { - btShapeHull* hull = new btShapeHull(convex); - hull->buildHull(0.0); - - { - //int strideInBytes = 9*sizeof(float); - //int numVertices = hull->numVertices(); - //int numIndices =hull->numIndices(); - - for (int t=0;tnumTriangles();t++) - { - - btVector3 triNormal; - - int index0 = hull->getIndexPointer()[t*3+0]; - int index1 = hull->getIndexPointer()[t*3+1]; - int index2 = hull->getIndexPointer()[t*3+2]; - btVector3 pos0 =parentTransform*hull->getVertexPointer()[index0]; - btVector3 pos1 =parentTransform*hull->getVertexPointer()[index1]; - btVector3 pos2 =parentTransform*hull->getVertexPointer()[index2]; - triNormal = (pos1-pos0).cross(pos2-pos0); - triNormal.normalize(); - - for (int v=0;v<3;v++) - { - int index = hull->getIndexPointer()[t*3+v]; - GLInstanceVertex vtx; - btVector3 pos =parentTransform*hull->getVertexPointer()[index]; - vtx.xyzw[0] = pos.x(); - vtx.xyzw[1] = pos.y(); - vtx.xyzw[2] = pos.z(); - vtx.xyzw[3] = 0.f; - - vtx.normal[0] =triNormal.x(); - vtx.normal[1] =triNormal.y(); - vtx.normal[2] =triNormal.z(); - - vtx.uv[0] = 0.5f; - vtx.uv[1] = 0.5f; - - indicesOut.push_back(verticesOut.size()); - verticesOut.push_back(vtx); - } - } - } - } - } else - { - if (collisionShape->isCompound()) - { - btCompoundShape* compound = (btCompoundShape*) collisionShape; - for (int i=0;igetNumChildShapes();i++) - { - - btTransform childWorldTrans = parentTransform * compound->getChildTransform(i); - createCollisionShapeGraphicsObjectInternal(compound->getChildShape(i),childWorldTrans,verticesOut,indicesOut); - } - } else - { - btAssert(0); - } - - } - } - }; -} void OpenGLGuiHelper::createCollisionShapeGraphicsObject(btCollisionShape* collisionShape) { @@ -429,15 +214,39 @@ void OpenGLGuiHelper::createCollisionShapeGraphicsObject(btCollisionShape* colli if (collisionShape->getUserIndex()>=0) return; - btAlignedObjectArray vertices; + btAlignedObjectArray gfxVertices; + btAlignedObjectArray indices; btTransform startTrans;startTrans.setIdentity(); - createCollisionShapeGraphicsObjectInternal(collisionShape,startTrans,vertices,indices); + { + btAlignedObjectArray vertexPositions; + btAlignedObjectArray vertexNormals; + CollisionShape2TriangleMesh(collisionShape,startTrans,vertexPositions,vertexNormals,indices); + gfxVertices.resize(vertexPositions.size()); + for (int i=0;isetUserIndex(shapeId); } diff --git a/examples/ExampleBrowser/main.cpp b/examples/ExampleBrowser/main.cpp index 65b82021b..179f18036 100644 --- a/examples/ExampleBrowser/main.cpp +++ b/examples/ExampleBrowser/main.cpp @@ -1,12 +1,5 @@ -//#define EXAMPLE_CONSOLE_ONLY -#ifdef EXAMPLE_CONSOLE_ONLY - #include "EmptyBrowser.h" - typedef EmptyBrowser DefaultBrowser; -#else - #include "OpenGLExampleBrowser.h" - typedef OpenGLExampleBrowser DefaultBrowser; -#endif //EXAMPLE_CONSOLE_ONLY +#include "OpenGLExampleBrowser.h" #include "Bullet3Common/b3CommandLineArgs.h" #include "../Utils/b3Clock.h" @@ -14,6 +7,12 @@ #include "ExampleEntries.h" #include "Bullet3Common/b3Logging.h" +#include "../Importers/ImportObjDemo/ImportObjExample.h" +#include "../Importers/ImportBsp/ImportBspExample.h" +#include "../Importers/ImportColladaDemo/ImportColladaSetup.h" +#include "../Importers/ImportSTLDemo/ImportSTLSetup.h" +#include "../Importers/ImportURDFDemo/ImportURDFSetup.h" +#include "../Importers/ImportSDFDemo/ImportSDFSetup.h" int main(int argc, char* argv[]) @@ -25,8 +24,12 @@ int main(int argc, char* argv[]) ExampleEntriesAll examples; examples.initExampleEntries(); - ExampleBrowserInterface* exampleBrowser = new DefaultBrowser(&examples); + OpenGLExampleBrowser* exampleBrowser = new OpenGLExampleBrowser(&examples); bool init = exampleBrowser->init(argc,argv); + exampleBrowser->registerFileImporter(".urdf",ImportURDFCreateFunc); + exampleBrowser->registerFileImporter(".sdf",ImportSDFCreateFunc); + exampleBrowser->registerFileImporter(".obj",ImportObjCreateFunc); + clock.reset(); if (init) { diff --git a/examples/ExampleBrowser/premake4.lua b/examples/ExampleBrowser/premake4.lua index 5bbbf51f7..181e93b87 100644 --- a/examples/ExampleBrowser/premake4.lua +++ b/examples/ExampleBrowser/premake4.lua @@ -6,13 +6,9 @@ project "App_BulletExampleBrowser" hasCL = findOpenCL("clew") - if (hasCL) then - - -- project ("App_Bullet3_OpenCL_Demos_" .. vendor) - - initOpenCL("clew") - - end + if (hasCL) then + initOpenCL("clew") + end links{"BulletExampleBrowserLib","gwen", "OpenGL_Window","BulletSoftBody", "BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision","LinearMath","Bullet3Common"} initOpenGL() @@ -39,7 +35,7 @@ project "App_BulletExampleBrowser" } end - if _OPTIONS["lua"] then + if _OPTIONS["lua"] then includedirs{"../ThirdPartyLibs/lua-5.2.3/src"} links {"lua-5.2.3"} defines {"ENABLE_LUA"} @@ -51,8 +47,94 @@ project "App_BulletExampleBrowser" files { "main.cpp", "ExampleEntries.cpp", + + "../TinyRenderer/Bullet2TinyRenderer.cpp", + "../TinyRenderer/geometry.cpp", + "../TinyRenderer/model.cpp", + "../TinyRenderer/tgaimage.cpp", + "../TinyRenderer/our_gl.cpp", + "../TinyRenderer/TinyRenderer.cpp", + "../RenderingExamples/TinyRendererSetup.cpp", + "../SharedMemory/PhysicsClientC_API.cpp", + "../SharedMemory/PhysicsClientC_API.h", + "../SharedMemory/PhysicsServerExample.cpp", + "../SharedMemory/PhysicsClientExample.cpp", + "../SharedMemory/PhysicsServer.cpp", + "../SharedMemory/PhysicsServerSharedMemory.cpp", + "../SharedMemory/PhysicsClientSharedMemory.cpp", + "../SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp", + "../SharedMemory/PhysicsClient.cpp", + "../SharedMemory/PosixSharedMemory.cpp", + "../SharedMemory/Win32SharedMemory.cpp", + "../SharedMemory/InProcessMemory.cpp", + "../SharedMemory/PhysicsDirect.cpp", + "../SharedMemory/PhysicsDirect.h", + "../SharedMemory/PhysicsDirectC_API.cpp", + "../SharedMemory/PhysicsDirectC_API.h", + "../SharedMemory/PhysicsLoopBack.cpp", + "../SharedMemory/PhysicsLoopBack.h", + "../SharedMemory/PhysicsLoopBackC_API.cpp", + "../SharedMemory/PhysicsLoopBackC_API.h", + "../SharedMemory/PhysicsServerCommandProcessor.cpp", + "../SharedMemory/PhysicsServerCommandProcessor.h", + "../MultiThreading/MultiThreadingExample.cpp", + "../MultiThreading/b3PosixThreadSupport.cpp", + "../MultiThreading/b3Win32ThreadSupport.cpp", + "../MultiThreading/b3ThreadSupportInterface.cpp", + "../InverseDynamics/InverseDynamicsExample.cpp", + "../InverseDynamics/InverseDynamicsExample.h", + "../BasicDemo/BasicExample.*", + "../Tutorial/*", + "../ExtendedTutorials/SimpleBox.cpp", + "../ExtendedTutorials/MultipleBoxes.cpp", + "../ExtendedTutorials/SimpleJoint.cpp", + "../ExtendedTutorials/SimpleCloth.cpp", + "../ExtendedTutorials/Chain.cpp", + "../ExtendedTutorials/Bridge.cpp", + "../ExtendedTutorials/RigidBodyFromObj.cpp", + "../Collision/*", + "../Collision/Internal/*", + "../Benchmarks/*", + "../CommonInterfaces/*", + "../ForkLift/ForkLiftDemo.*", + "../Importers/**", + "../../Extras/Serialize/BulletWorldImporter/*", + "../../Extras/Serialize/BulletFileLoader/*", + "../Planar2D/Planar2D.*", + "../RenderingExamples/*", + "../VoronoiFracture/*", + "../SoftDemo/*", + "../RollingFrictionDemo/*", + "../FractureDemo/*", + "../DynamicControlDemo/*", + "../Constraints/*", + "../Vehicles/*", + "../Raycast/*", + "../MultiBody/MultiDofDemo.cpp", + "../MultiBody/TestJointTorqueSetup.cpp", + "../MultiBody/Pendulum.cpp", + "../MultiBody/MultiBodySoftContact.cpp", + "../MultiBody/MultiBodyConstraintFeedback.cpp", + "../MultiBody/InvertedPendulumPDControl.cpp", + "../RigidBody/RigidBodySoftContact.cpp", + "../ThirdPartyLibs/stb_image/*", + "../ThirdPartyLibs/Wavefront/tiny_obj_loader.*", + "../ThirdPartyLibs/tinyxml/*", + "../GyroscopicDemo/GyroscopicSetup.cpp", + "../GyroscopicDemo/GyroscopicSetup.h", + "../ThirdPartyLibs/tinyxml/tinystr.cpp", + "../ThirdPartyLibs/tinyxml/tinyxml.cpp", + "../ThirdPartyLibs/tinyxml/tinyxmlerror.cpp", + "../ThirdPartyLibs/tinyxml/tinyxmlparser.cpp", } - +if (hasCL and findOpenGL3()) then + files { + "../OpenCL/broadphase/*", + "../OpenCL/CommonOpenCL/*", + "../OpenCL/rigidbody/GpuConvexScene.cpp", + "../OpenCL/rigidbody/GpuRigidBodyDemo.cpp", + } + end if os.is("Linux") then initX11() end @@ -99,106 +181,22 @@ project "BulletExampleBrowserLib" files { "OpenGLExampleBrowser.cpp", "OpenGLGuiHelper.cpp", - "InProcessExampleBrowser.cpp", - "GL_ShapeDrawer.cpp", "OpenGLExampleBrowser.cpp", "../Utils/b3Clock.cpp", "*.h", "GwenGUISupport/*.cpp", "GwenGUISupport/*.h", - "../SharedMemory/PhysicsClientC_API.cpp", - "../SharedMemory/PhysicsClientC_API.h", - "../SharedMemory/PhysicsServerExample.cpp", - "../SharedMemory/PhysicsClientExample.cpp", - "../SharedMemory/PhysicsServer.cpp", - "../SharedMemory/PhysicsServerSharedMemory.cpp", - "../SharedMemory/PhysicsClientSharedMemory.cpp", - "../SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp", - "../SharedMemory/PhysicsClient.cpp", - "../SharedMemory/PosixSharedMemory.cpp", - "../SharedMemory/Win32SharedMemory.cpp", - "../SharedMemory/InProcessMemory.cpp", - "../SharedMemory/PhysicsDirect.cpp", - "../SharedMemory/PhysicsDirect.h", - "../SharedMemory/PhysicsDirectC_API.cpp", - "../SharedMemory/PhysicsDirectC_API.h", - "../SharedMemory/PhysicsLoopBack.cpp", - "../SharedMemory/PhysicsLoopBack.h", - "../SharedMemory/PhysicsLoopBackC_API.cpp", - "../SharedMemory/PhysicsLoopBackC_API.h", - "../SharedMemory/PhysicsServerCommandProcessor.cpp", - "../SharedMemory/PhysicsServerCommandProcessor.h", - "../MultiThreading/MultiThreadingExample.cpp", - "../MultiThreading/b3PosixThreadSupport.cpp", - "../MultiThreading/b3Win32ThreadSupport.cpp", - "../MultiThreading/b3ThreadSupportInterface.cpp", - "../InverseDynamics/InverseDynamicsExample.cpp", - "../InverseDynamics/InverseDynamicsExample.h", - "../BasicDemo/BasicExample.*", - "../Tutorial/*", - "../ExtendedTutorials/*", - "../Collision/*", - "../Collision/Internal/*", - "../Benchmarks/*", - "../CommonInterfaces/*", - "../ForkLift/ForkLiftDemo.*", - "../Importers/**", - "../../Extras/Serialize/BulletWorldImporter/*", - "../../Extras/Serialize/BulletFileLoader/*", - "../Planar2D/Planar2D.*", - "../RenderingExamples/*", - "../VoronoiFracture/*", - "../SoftDemo/*", - "../RollingFrictionDemo/*", - "../FractureDemo/*", - "../DynamicControlDemo/*", - "../Constraints/*", - "../Vehicles/*", - "../Raycast/*", - "../MultiBody/MultiDofDemo.cpp", - "../MultiBody/TestJointTorqueSetup.cpp", - "../MultiBody/Pendulum.cpp", - "../MultiBody/MultiBodySoftContact.cpp", - "../MultiBody/MultiBodyConstraintFeedback.cpp", - "../MultiBody/InvertedPendulumPDControl.cpp", - "../RigidBody/RigidBodySoftContact.cpp", - "../ThirdPartyLibs/stb_image/*", - "../ThirdPartyLibs/Wavefront/tiny_obj_loader.*", - "../ThirdPartyLibs/tinyxml/*", + "CollisionShape2TriangleMesh.cpp", + "CollisionShape2TriangleMesh.h", "../Utils/b3ResourcePath.*", - "../GyroscopicDemo/GyroscopicSetup.cpp", - "../GyroscopicDemo/GyroscopicSetup.h", - "../ThirdPartyLibs/urdf/urdfdom/urdf_parser/src/pose.cpp", - "../ThirdPartyLibs/urdf/urdfdom/urdf_parser/src/model.cpp", - "../ThirdPartyLibs/urdf/urdfdom/urdf_parser/src/link.cpp", - "../ThirdPartyLibs/urdf/urdfdom/urdf_parser/src/joint.cpp", - "../ThirdPartyLibs/urdf/urdfdom/urdf_parser/include/urdf_parser/urdf_parser.h", - "../ThirdPartyLibs/urdf/urdfdom_headers/urdf_exception/include/urdf_exception/exception.h", - "../ThirdPartyLibs/urdf/urdfdom_headers/urdf_model/include/urdf_model/pose.h", - "../ThirdPartyLibs/urdf/urdfdom_headers/urdf_model/include/urdf_model/model.h", - "../ThirdPartyLibs/urdf/urdfdom_headers/urdf_model/include/urdf_model/link.h", - "../ThirdPartyLibs/urdf/urdfdom_headers/urdf_model/include/urdf_model/joint.h", - "../ThirdPartyLibs/tinyxml/tinystr.cpp", - "../ThirdPartyLibs/tinyxml/tinyxml.cpp", - "../ThirdPartyLibs/tinyxml/tinyxmlerror.cpp", - "../ThirdPartyLibs/tinyxml/tinyxmlparser.cpp", - "../ThirdPartyLibs/urdf/boost_replacement/lexical_cast.h", - "../ThirdPartyLibs/urdf/boost_replacement/shared_ptr.h", - "../ThirdPartyLibs/urdf/boost_replacement/printf_console.cpp", - "../ThirdPartyLibs/urdf/boost_replacement/printf_console.h", - "../ThirdPartyLibs/urdf/boost_replacement/string_split.cpp", - "../ThirdPartyLibs/urdf/boost_replacement/string_split.h", + "GL_ShapeDrawer.cpp", + "InProcessExampleBrowser.cpp", + + } - if (hasCL and findOpenGL3()) then - files { - "../OpenCL/broadphase/*", - "../OpenCL/CommonOpenCL/*", - "../OpenCL/rigidbody/GpuConvexScene.cpp", - "../OpenCL/rigidbody/GpuRigidBodyDemo.cpp", - } - end + if os.is("Linux") then initX11() diff --git a/examples/Importers/ImportObjDemo/ImportObjExample.cpp b/examples/Importers/ImportObjDemo/ImportObjExample.cpp index 26b0e95e8..f4a8956b3 100644 --- a/examples/Importers/ImportObjDemo/ImportObjExample.cpp +++ b/examples/Importers/ImportObjDemo/ImportObjExample.cpp @@ -17,8 +17,11 @@ class ImportObjSetup : public CommonRigidBodyBase { + std::string m_fileName; + + public: - ImportObjSetup(struct GUIHelperInterface* helper); + ImportObjSetup(struct GUIHelperInterface* helper, const char* fileName); virtual ~ImportObjSetup(); virtual void initPhysics(); @@ -34,10 +37,16 @@ public: }; -ImportObjSetup::ImportObjSetup(struct GUIHelperInterface* helper) +ImportObjSetup::ImportObjSetup(struct GUIHelperInterface* helper, const char* fileName) :CommonRigidBodyBase(helper) { - + if (fileName) + { + m_fileName = fileName; + } else + { + m_fileName = "cube.obj";//sphere8.obj";//sponza_closed.obj";//sphere8.obj"; + } } ImportObjSetup::~ImportObjSetup() @@ -59,9 +68,9 @@ void ImportObjSetup::initPhysics() m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe); - const char* fileName = "cube.obj";//sphere8.obj";//sponza_closed.obj";//sphere8.obj"; + char relativeFileName[1024]; - if (b3ResourcePath::findResourcePath(fileName, relativeFileName, 1024)) + if (b3ResourcePath::findResourcePath(m_fileName.c_str(), relativeFileName, 1024)) { char pathPrefix[1024]; @@ -124,11 +133,6 @@ void ImportObjSetup::initPhysics() } - if (1) - { - - } - int shapeId = m_guiHelper->getRenderInterface()->registerShape(&gfxShape->m_vertices->at(0).xyzw[0], gfxShape->m_numvertices, &gfxShape->m_indices->at(0), gfxShape->m_numIndices,B3_GL_TRIANGLES,textureIndex); //int id = @@ -138,12 +142,12 @@ void ImportObjSetup::initPhysics() }} else { - b3Warning("Cannot find %s\n", fileName); + b3Warning("Cannot find %s\n", m_fileName.c_str()); } } CommonExampleInterface* ImportObjCreateFunc(struct CommonExampleOptions& options) { - return new ImportObjSetup(options.m_guiHelper); + return new ImportObjSetup(options.m_guiHelper, options.m_fileName); } diff --git a/examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp b/examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp index 848ee699c..deabf1dac 100644 --- a/examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp +++ b/examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp @@ -110,7 +110,7 @@ ImportSDFSetup::ImportSDFSetup(struct GUIHelperInterface* helper, int option, co //load additional urdf file names from file - FILE* f = fopen("urdf_files.txt","r"); + FILE* f = fopen("sdf_files.txt","r"); if (f) { int result; diff --git a/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp b/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp index 6aad57f3e..0c6a81b33 100644 --- a/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp +++ b/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp @@ -11,9 +11,6 @@ #include "../CommonInterfaces/CommonParameterInterface.h" #include "../../Utils/b3ResourcePath.h" -#ifdef ENABLE_ROS_URDF -#include "ROSURDFImporter.h" -#endif #include "BulletUrdfImporter.h" @@ -213,14 +210,6 @@ void ImportUrdfSetup::initPhysics() b3Printf("using new URDF\n"); bla = new BulletURDFImporter(m_guiHelper); } -#ifdef USE_ROS_URDF - else - { - b3Printf("using ROS URDF\n"); - bla = new ROSURDFImporter(m_guiHelper); - } - newURDF = !newURDF; -#endif//USE_ROS_URDF URDFImporterInterface& u2b = *bla; bool loadOk = u2b.loadURDF(m_fileName); diff --git a/examples/Importers/ImportURDFDemo/ROSURDFImporter.cpp b/examples/Importers/ImportURDFDemo/ROSURDFImporter.cpp deleted file mode 100644 index 5e6ce6fb9..000000000 --- a/examples/Importers/ImportURDFDemo/ROSURDFImporter.cpp +++ /dev/null @@ -1,905 +0,0 @@ -/* Copyright (C) 2015 Google - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - -#include "ROSURDFImporter.h" - - -#include "URDFImporterInterface.h" -#include "btBulletCollisionCommon.h" -#include "../ImportObjDemo/LoadMeshFromObj.h" -#include "../ImportSTLDemo/LoadMeshFromSTL.h" -#include "../ImportColladaDemo/LoadMeshFromCollada.h" -#include "BulletCollision/CollisionShapes/btShapeHull.h"//to create a tesselation of a generic btConvexShape -#include "../CommonInterfaces/CommonGUIHelperInterface.h" -#include "Bullet3Common/b3FileUtils.h" -#include - -#include "urdf/urdfdom/urdf_parser/include/urdf_parser/urdf_parser.h" -#include -#include -using namespace urdf; - -void ROSconvertURDFToVisualShape(const Visual* visual, const char* pathPrefix, const btTransform& visualTransform, btAlignedObjectArray& verticesOut, btAlignedObjectArray& indicesOut); -btCollisionShape* ROSconvertURDFToCollisionShape(const Collision* visual, const char* pathPrefix); - - - - -static void ROSprintTreeInternal(my_shared_ptr link,int level = 0) -{ - level+=2; - int count = 0; - for (std::vector >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++) - { - if (*child) - { - for(int j=0;jname << std::endl; - // first grandchild - ROSprintTreeInternal(*child,level); - } - else - { - for(int j=0;jname << " has a null child!" << *child << std::endl; - } - } - -} - - - - -struct ROSURDFInternalData -{ - my_shared_ptr m_robot; - std::vector > m_links; - struct GUIHelperInterface* m_guiHelper; - char m_pathPrefix[1024]; - -}; - -void ROSURDFImporter::printTree() -{ - ROSprintTreeInternal(m_data->m_robot->getRoot(),0); -} - -enum MyFileType -{ - FILE_STL=1, - FILE_COLLADA=2, - FILE_OBJ=3, -}; - - - -ROSURDFImporter::ROSURDFImporter(struct GUIHelperInterface* helper) -{ - m_data = new ROSURDFInternalData; - m_data->m_robot = 0; - m_data->m_guiHelper = helper; - m_data->m_pathPrefix[0]=0; - - - -} - -bool ROSURDFImporter::loadURDF(const char* fileName, bool forceFixedBase) -{ - - -//int argc=0; - char relativeFileName[1024]; - - b3FileUtils fu; - - bool fileFound = fu.findFile(fileName, relativeFileName, 1024); - - std::string xml_string; - m_data->m_pathPrefix[0] = 0; - - if (!fileFound){ - std::cerr << "URDF file not found" << std::endl; - return false; - } else - { - - int maxPathLen = 1024; - fu.extractPath(relativeFileName,m_data->m_pathPrefix,maxPathLen); - - - std::fstream xml_file(relativeFileName, std::fstream::in); - while ( xml_file.good() ) - { - std::string line; - std::getline( xml_file, line); - xml_string += (line + "\n"); - } - xml_file.close(); - } - - my_shared_ptr robot = parseURDF(xml_string); - if (!robot){ - std::cerr << "ERROR: Model Parsing the xml failed" << std::endl; - return false; - } - std::cout << "robot name is: " << robot->getName() << std::endl; - - // get info from parser - std::cout << "Successfully Parsed URDF" << std::endl; - // get root link - my_shared_ptr root_link=robot->getRoot(); - if (!root_link) - { - std::cout << "Failed to find root link in URDF" << std::endl; - return false; - } - - - m_data->m_robot = robot; - - m_data->m_robot->getLinks(m_data->m_links); - - //initialize the 'index' of each link - for (int i=0;i<(int)m_data->m_links.size();i++) - { - m_data->m_links[i]->m_link_index = i; - } - - return true; -} - -const char* ROSURDFImporter::getPathPrefix() -{ - return m_data->m_pathPrefix; -} - - -ROSURDFImporter::~ROSURDFImporter() -{ - delete m_data; -} - - -int ROSURDFImporter::getRootLinkIndex() const -{ - if (m_data->m_links.size()) - { - int rootLinkIndex = m_data->m_robot->getRoot()->m_link_index; - // btAssert(m_links[0]->m_link_index == rootLinkIndex); - return rootLinkIndex; - } - return -1; -}; - -void ROSURDFImporter::getLinkChildIndices(int linkIndex, btAlignedObjectArray& childLinkIndices) const -{ - childLinkIndices.resize(0); - int numChildren = m_data->m_links[linkIndex]->child_links.size(); - - for (int i=0;im_links[linkIndex]->child_links[i]->m_link_index; - childLinkIndices.push_back(childIndex); - } -} - -std::string ROSURDFImporter::getLinkName(int linkIndex) const -{ - std::string n = m_data->m_links[linkIndex]->name; - return n; -} - -std::string ROSURDFImporter::getJointName(int linkIndex) const -{ - return m_data->m_links[linkIndex]->parent_joint->name; -} - - -void ROSURDFImporter::getMassAndInertia(int linkIndex, btScalar& mass,btVector3& localInertiaDiagonal, btTransform& inertialFrame) const -{ - if ((*m_data->m_links[linkIndex]).inertial) - { - mass = (*m_data->m_links[linkIndex]).inertial->mass; - localInertiaDiagonal.setValue((*m_data->m_links[linkIndex]).inertial->ixx,(*m_data->m_links[linkIndex]).inertial->iyy,(*m_data->m_links[linkIndex]).inertial->izz); - inertialFrame.setOrigin(btVector3((*m_data->m_links[linkIndex]).inertial->origin.position.x,(*m_data->m_links[linkIndex]).inertial->origin.position.y,(*m_data->m_links[linkIndex]).inertial->origin.position.z)); - inertialFrame.setRotation(btQuaternion((*m_data->m_links[linkIndex]).inertial->origin.rotation.x,(*m_data->m_links[linkIndex]).inertial->origin.rotation.y,(*m_data->m_links[linkIndex]).inertial->origin.rotation.z,(*m_data->m_links[linkIndex]).inertial->origin.rotation.w)); - } else - { - mass = 1.f; - localInertiaDiagonal.setValue(1,1,1); - inertialFrame.setIdentity(); - } -} - -bool ROSURDFImporter::getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const -{ - jointLowerLimit = 0.f; - jointUpperLimit = 0.f; - jointDamping = 0.f; - jointFriction = 0.f; - - if ((*m_data->m_links[urdfLinkIndex]).parent_joint) - { - my_shared_ptr pj =(*m_data->m_links[urdfLinkIndex]).parent_joint; - - const urdf::Vector3 pos = pj->parent_to_joint_origin_transform.position; - const urdf::Rotation orn = pj->parent_to_joint_origin_transform.rotation; - jointDamping = pj->dynamics->damping; - jointFriction = pj->dynamics->friction; - - jointAxisInJointSpace.setValue(pj->axis.x,pj->axis.y,pj->axis.z); - parent2joint.setOrigin(btVector3(pos.x,pos.y,pos.z)); - parent2joint.setRotation(btQuaternion(orn.x,orn.y,orn.z,orn.w)); - - switch (pj->type) - { - case Joint::REVOLUTE: - jointType = URDFRevoluteJoint; - break; - case Joint::FIXED: - jointType = URDFFixedJoint; - break; - case Joint::PRISMATIC: - jointType = URDFPrismaticJoint; - break; - case Joint::PLANAR: - jointType = URDFPlanarJoint; - break; - case Joint::CONTINUOUS: - jointType = URDFContinuousJoint; - break; - default: - { - printf("Error: unknown joint type %d\n", pj->type); - btAssert(0); - } - - }; - - if (pj->limits) - { - jointLowerLimit = pj->limits.get()->lower; - jointUpperLimit = pj->limits.get()->upper; - } - return true; - } else - { - parent2joint.setIdentity(); - return false; - } -} - -bool ROSURDFImporter::getRootTransformInWorld(btTransform& rootTransformInWorld) const -{ -} - -void ROSconvertURDFToVisualShape(const Visual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray& verticesOut, btAlignedObjectArray& indicesOut) -{ - - - GLInstanceGraphicsShape* glmesh = 0; - - btConvexShape* convexColShape = 0; - - switch (visual->geometry->type) - { - case Geometry::CYLINDER: - { - printf("processing a cylinder\n"); - urdf::Cylinder* cyl = (urdf::Cylinder*)visual->geometry.get(); - btAlignedObjectArray vertices; - - //int numVerts = sizeof(barrel_vertices)/(9*sizeof(float)); - int numSteps = 32; - for (int i = 0; iradius*btSin(SIMD_2_PI*(float(i) / numSteps)), cyl->radius*btCos(SIMD_2_PI*(float(i) / numSteps)), cyl->length / 2.); - vertices.push_back(vert); - vert[2] = -cyl->length / 2.; - vertices.push_back(vert); - } - - btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3)); - cylZShape->setMargin(0.001); - convexColShape = cylZShape; - break; - } - case Geometry::BOX: - { - printf("processing a box\n"); - urdf::Box* box = (urdf::Box*)visual->geometry.get(); - btVector3 extents(box->dim.x, box->dim.y, box->dim.z); - btBoxShape* boxShape = new btBoxShape(extents*0.5f); - //btConvexShape* boxShape = new btConeShapeX(extents[2]*0.5,extents[0]*0.5); - convexColShape = boxShape; - convexColShape->setMargin(0.001); - break; - } - case Geometry::SPHERE: - { - printf("processing a sphere\n"); - urdf::Sphere* sphere = (urdf::Sphere*)visual->geometry.get(); - btScalar radius = sphere->radius; - btSphereShape* sphereShape = new btSphereShape(radius); - convexColShape = sphereShape; - convexColShape->setMargin(0.001); - break; - - break; - } - case Geometry::MESH: - { - if (visual->name.length()) - { - printf("visual->name=%s\n", visual->name.c_str()); - } - if (visual->geometry) - { - const urdf::Mesh* mesh = (const urdf::Mesh*) visual->geometry.get(); - if (mesh->filename.length()) - { - const char* filename = mesh->filename.c_str(); - printf("mesh->filename=%s\n", filename); - char fullPath[1024]; - int fileType = 0; - - char tmpPathPrefix[1024]; - std::string xml_string; - int maxPathLen = 1024; - b3FileUtils::extractPath(filename,tmpPathPrefix,maxPathLen); - - char visualPathPrefix[1024]; - sprintf(visualPathPrefix,"%s%s",urdfPathPrefix,tmpPathPrefix); - - - sprintf(fullPath, "%s%s", urdfPathPrefix, filename); - b3FileUtils::toLower(fullPath); - if (strstr(fullPath, ".dae")) - { - fileType = FILE_COLLADA; - } - if (strstr(fullPath, ".stl")) - { - fileType = FILE_STL; - } - if (strstr(fullPath,".obj")) - { - fileType = FILE_OBJ; - } - - - sprintf(fullPath, "%s%s", urdfPathPrefix, filename); - FILE* f = fopen(fullPath, "rb"); - if (f) - { - fclose(f); - - - - switch (fileType) - { - case FILE_OBJ: - { - glmesh = LoadMeshFromObj(fullPath,visualPathPrefix); - break; - } - - case FILE_STL: - { - glmesh = LoadMeshFromSTL(fullPath); - break; - } - case FILE_COLLADA: - { - - btAlignedObjectArray visualShapes; - btAlignedObjectArray visualShapeInstances; - btTransform upAxisTrans; upAxisTrans.setIdentity(); - float unitMeterScaling = 1; - int upAxis = 2; - - LoadMeshFromCollada(fullPath, - visualShapes, - visualShapeInstances, - upAxisTrans, - unitMeterScaling, - upAxis); - - glmesh = new GLInstanceGraphicsShape; - //int index = 0; - glmesh->m_indices = new b3AlignedObjectArray(); - glmesh->m_vertices = new b3AlignedObjectArray(); - - for (int i = 0; im_shapeIndex]; - - b3AlignedObjectArray verts; - verts.resize(gfxShape->m_vertices->size()); - - int baseIndex = glmesh->m_vertices->size(); - - for (int i = 0; im_vertices->size(); i++) - { - verts[i].normal[0] = gfxShape->m_vertices->at(i).normal[0]; - verts[i].normal[1] = gfxShape->m_vertices->at(i).normal[1]; - verts[i].normal[2] = gfxShape->m_vertices->at(i).normal[2]; - verts[i].uv[0] = gfxShape->m_vertices->at(i).uv[0]; - verts[i].uv[1] = gfxShape->m_vertices->at(i).uv[1]; - verts[i].xyzw[0] = gfxShape->m_vertices->at(i).xyzw[0]; - verts[i].xyzw[1] = gfxShape->m_vertices->at(i).xyzw[1]; - verts[i].xyzw[2] = gfxShape->m_vertices->at(i).xyzw[2]; - verts[i].xyzw[3] = gfxShape->m_vertices->at(i).xyzw[3]; - - } - - int curNumIndices = glmesh->m_indices->size(); - int additionalIndices = gfxShape->m_indices->size(); - glmesh->m_indices->resize(curNumIndices + additionalIndices); - for (int k = 0; km_indices->at(curNumIndices + k) = gfxShape->m_indices->at(k) + baseIndex; - } - - //compensate upAxisTrans and unitMeterScaling here - btMatrix4x4 upAxisMat; - upAxisMat.setIdentity(); -// upAxisMat.setPureRotation(upAxisTrans.getRotation()); - btMatrix4x4 unitMeterScalingMat; - unitMeterScalingMat.setPureScaling(btVector3(unitMeterScaling, unitMeterScaling, unitMeterScaling)); - btMatrix4x4 worldMat = unitMeterScalingMat*upAxisMat*instance->m_worldTransform; - //btMatrix4x4 worldMat = instance->m_worldTransform; - int curNumVertices = glmesh->m_vertices->size(); - int additionalVertices = verts.size(); - glmesh->m_vertices->reserve(curNumVertices + additionalVertices); - - for (int v = 0; vm_vertices->push_back(verts[v]); - } - } - glmesh->m_numIndices = glmesh->m_indices->size(); - glmesh->m_numvertices = glmesh->m_vertices->size(); - //glmesh = LoadMeshFromCollada(fullPath); - - break; - } - default: - { - printf("Error: unsupported file type for Visual mesh: %s\n", fullPath); - btAssert(0); - } - } - - - if (glmesh && (glmesh->m_numvertices>0)) - { - } - else - { - printf("issue extracting mesh from COLLADA/STL file %s\n", fullPath); - } - - } - else - { - printf("mesh geometry not found %s\n", fullPath); - } - - - } - } - - - break; - } - default: - { - printf("Error: unknown visual geometry type\n"); - } - } - - //if we have a convex, tesselate into localVertices/localIndices - if (convexColShape) - { - btShapeHull* hull = new btShapeHull(convexColShape); - hull->buildHull(0.0); - { - // int strideInBytes = 9*sizeof(float); - int numVertices = hull->numVertices(); - int numIndices = hull->numIndices(); - - - glmesh = new GLInstanceGraphicsShape; - // int index = 0; - glmesh->m_indices = new b3AlignedObjectArray(); - glmesh->m_vertices = new b3AlignedObjectArray(); - - - for (int i = 0; i < numVertices; i++) - { - GLInstanceVertex vtx; - btVector3 pos = hull->getVertexPointer()[i]; - vtx.xyzw[0] = pos.x(); - vtx.xyzw[1] = pos.y(); - vtx.xyzw[2] = pos.z(); - vtx.xyzw[3] = 1.f; - pos.normalize(); - vtx.normal[0] = pos.x(); - vtx.normal[1] = pos.y(); - vtx.normal[2] = pos.z(); - vtx.uv[0] = 0.5f; - vtx.uv[1] = 0.5f; - glmesh->m_vertices->push_back(vtx); - } - - btAlignedObjectArray indices; - for (int i = 0; i < numIndices; i++) - { - glmesh->m_indices->push_back(hull->getIndexPointer()[i]); - } - - glmesh->m_numvertices = glmesh->m_vertices->size(); - glmesh->m_numIndices = glmesh->m_indices->size(); - } - delete convexColShape; - convexColShape = 0; - - } - - if (glmesh && glmesh->m_numIndices>0 && glmesh->m_numvertices >0) - { - - int baseIndex = verticesOut.size(); - - - - for (int i = 0; i < glmesh->m_indices->size(); i++) - { - indicesOut.push_back(glmesh->m_indices->at(i) + baseIndex); - } - - for (int i = 0; i < glmesh->m_vertices->size(); i++) - { - GLInstanceVertex& v = glmesh->m_vertices->at(i); - btVector3 vert(v.xyzw[0],v.xyzw[1],v.xyzw[2]); - btVector3 vt = visualTransform*vert; - v.xyzw[0] = vt[0]; - v.xyzw[1] = vt[1]; - v.xyzw[2] = vt[2]; - btVector3 triNormal(v.normal[0],v.normal[1],v.normal[2]); - triNormal = visualTransform.getBasis()*triNormal; - v.normal[0] = triNormal[0]; - v.normal[1] = triNormal[1]; - v.normal[2] = triNormal[2]; - verticesOut.push_back(v); - } - } -} - - - - -btCollisionShape* convertURDFToCollisionShape(const Collision* visual, const char* urdfPathPrefix) -{ - btCollisionShape* shape = 0; - - switch (visual->geometry->type) - { - case Geometry::CYLINDER: - { - printf("processing a cylinder\n"); - urdf::Cylinder* cyl = (urdf::Cylinder*)visual->geometry.get(); - - btAlignedObjectArray vertices; - //int numVerts = sizeof(barrel_vertices)/(9*sizeof(float)); - int numSteps = 32; - for (int i=0;iradius*btSin(SIMD_2_PI*(float(i)/numSteps)),cyl->radius*btCos(SIMD_2_PI*(float(i)/numSteps)),cyl->length/2.); - vertices.push_back(vert); - vert[2] = -cyl->length/2.; - vertices.push_back(vert); - - } - btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3)); - cylZShape->setMargin(0.001); - cylZShape->initializePolyhedralFeatures(); - //btConvexShape* cylZShape = new btConeShapeZ(cyl->radius,cyl->length);//(vexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3)); - - //btVector3 halfExtents(cyl->radius,cyl->radius,cyl->length/2.); - //btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents); - - - shape = cylZShape; - break; - } - case Geometry::BOX: - { - printf("processing a box\n"); - urdf::Box* box = (urdf::Box*)visual->geometry.get(); - btVector3 extents(box->dim.x,box->dim.y,box->dim.z); - btBoxShape* boxShape = new btBoxShape(extents*0.5f); - //btConvexShape* boxShape = new btConeShapeX(extents[2]*0.5,extents[0]*0.5); - shape = boxShape; - shape ->setMargin(0.001); - break; - } - case Geometry::SPHERE: - { - printf("processing a sphere\n"); - urdf::Sphere* sphere = (urdf::Sphere*)visual->geometry.get(); - btScalar radius = sphere->radius; - btSphereShape* sphereShape = new btSphereShape(radius); - shape = sphereShape; - shape ->setMargin(0.001); - break; - - break; - } - case Geometry::MESH: - { - if (visual->name.length()) - { - printf("visual->name=%s\n",visual->name.c_str()); - } - if (visual->geometry) - { - const urdf::Mesh* mesh = (const urdf::Mesh*) visual->geometry.get(); - if (mesh->filename.length()) - { - const char* filename = mesh->filename.c_str(); - printf("mesh->filename=%s\n",filename); - char fullPath[1024]; - int fileType = 0; - sprintf(fullPath,"%s%s",urdfPathPrefix,filename); - b3FileUtils::toLower(fullPath); - char tmpPathPrefix[1024]; - int maxPathLen = 1024; - b3FileUtils::extractPath(filename,tmpPathPrefix,maxPathLen); - - char collisionPathPrefix[1024]; - sprintf(collisionPathPrefix,"%s%s",urdfPathPrefix,tmpPathPrefix); - - - - if (strstr(fullPath,".dae")) - { - fileType = FILE_COLLADA; - } - if (strstr(fullPath,".stl")) - { - fileType = FILE_STL; - } - if (strstr(fullPath,".obj")) - { - fileType = FILE_OBJ; - } - - sprintf(fullPath,"%s%s",urdfPathPrefix,filename); - FILE* f = fopen(fullPath,"rb"); - if (f) - { - fclose(f); - GLInstanceGraphicsShape* glmesh = 0; - - - switch (fileType) - { - case FILE_OBJ: - { - glmesh = LoadMeshFromObj(fullPath,collisionPathPrefix); - break; - } - case FILE_STL: - { - glmesh = LoadMeshFromSTL(fullPath); - break; - } - case FILE_COLLADA: - { - - btAlignedObjectArray visualShapes; - btAlignedObjectArray visualShapeInstances; - btTransform upAxisTrans;upAxisTrans.setIdentity(); - float unitMeterScaling=1; - int upAxis = 2; - LoadMeshFromCollada(fullPath, - visualShapes, - visualShapeInstances, - upAxisTrans, - unitMeterScaling, - upAxis ); - - glmesh = new GLInstanceGraphicsShape; - // int index = 0; - glmesh->m_indices = new b3AlignedObjectArray(); - glmesh->m_vertices = new b3AlignedObjectArray(); - - for (int i=0;im_shapeIndex]; - - b3AlignedObjectArray verts; - verts.resize(gfxShape->m_vertices->size()); - - int baseIndex = glmesh->m_vertices->size(); - - for (int i=0;im_vertices->size();i++) - { - verts[i].normal[0] = gfxShape->m_vertices->at(i).normal[0]; - verts[i].normal[1] = gfxShape->m_vertices->at(i).normal[1]; - verts[i].normal[2] = gfxShape->m_vertices->at(i).normal[2]; - verts[i].uv[0] = gfxShape->m_vertices->at(i).uv[0]; - verts[i].uv[1] = gfxShape->m_vertices->at(i).uv[1]; - verts[i].xyzw[0] = gfxShape->m_vertices->at(i).xyzw[0]; - verts[i].xyzw[1] = gfxShape->m_vertices->at(i).xyzw[1]; - verts[i].xyzw[2] = gfxShape->m_vertices->at(i).xyzw[2]; - verts[i].xyzw[3] = gfxShape->m_vertices->at(i).xyzw[3]; - - } - - int curNumIndices = glmesh->m_indices->size(); - int additionalIndices = gfxShape->m_indices->size(); - glmesh->m_indices->resize(curNumIndices+additionalIndices); - for (int k=0;km_indices->at(curNumIndices+k)=gfxShape->m_indices->at(k)+baseIndex; - } - - //compensate upAxisTrans and unitMeterScaling here - btMatrix4x4 upAxisMat; - upAxisMat.setPureRotation(upAxisTrans.getRotation()); - btMatrix4x4 unitMeterScalingMat; - unitMeterScalingMat.setPureScaling(btVector3(unitMeterScaling,unitMeterScaling,unitMeterScaling)); - btMatrix4x4 worldMat = unitMeterScalingMat*instance->m_worldTransform*upAxisMat; - //btMatrix4x4 worldMat = instance->m_worldTransform; - int curNumVertices = glmesh->m_vertices->size(); - int additionalVertices = verts.size(); - glmesh->m_vertices->reserve(curNumVertices+additionalVertices); - - for(int v=0;vm_vertices->push_back(verts[v]); - } - } - glmesh->m_numIndices = glmesh->m_indices->size(); - glmesh->m_numvertices = glmesh->m_vertices->size(); - //glmesh = LoadMeshFromCollada(fullPath); - - break; - } - default: - { - printf("Unsupported file type in Collision: %s\n",fullPath); - btAssert(0); - } - } - - - if (glmesh && (glmesh->m_numvertices>0)) - { - printf("extracted %d verticed from STL file %s\n", glmesh->m_numvertices,fullPath); - //int shapeId = m_glApp->m_instancingRenderer->registerShape(&gvertices[0].pos[0],gvertices.size(),&indices[0],indices.size()); - //convex->setUserIndex(shapeId); - btAlignedObjectArray convertedVerts; - convertedVerts.reserve(glmesh->m_numvertices); - for (int i=0;im_numvertices;i++) - { - convertedVerts.push_back(btVector3(glmesh->m_vertices->at(i).xyzw[0],glmesh->m_vertices->at(i).xyzw[1],glmesh->m_vertices->at(i).xyzw[2])); - } - //btConvexHullShape* cylZShape = new btConvexHullShape(&glmesh->m_vertices->at(0).xyzw[0], glmesh->m_numvertices, sizeof(GLInstanceVertex)); - btConvexHullShape* cylZShape = new btConvexHullShape(&convertedVerts[0].getX(), convertedVerts.size(), sizeof(btVector3)); - //cylZShape->initializePolyhedralFeatures(); - //btVector3 halfExtents(cyl->radius,cyl->radius,cyl->length/2.); - //btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents); - cylZShape->setMargin(0.001); - shape = cylZShape; - } else - { - printf("issue extracting mesh from STL file %s\n", fullPath); - } - - } else - { - printf("mesh geometry not found %s\n",fullPath); - } - - - } - } - - - break; - } - default: - { - printf("Error: unknown visual geometry type\n"); - } - } - return shape; -} - - - - -int ROSURDFImporter::convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const -{ - btAlignedObjectArray vertices; - btAlignedObjectArray indices; - btTransform startTrans; startTrans.setIdentity(); - int graphicsIndex = -1; - - for (int v = 0; v < (int)m_data->m_links[linkIndex]->visual_array.size(); v++) - { - const Visual* vis = m_data->m_links[linkIndex]->visual_array[v].get(); - btVector3 childPos(vis->origin.position.x, vis->origin.position.y, vis->origin.position.z); - btQuaternion childOrn(vis->origin.rotation.x, vis->origin.rotation.y, vis->origin.rotation.z, vis->origin.rotation.w); - btTransform childTrans; - childTrans.setOrigin(childPos); - childTrans.setRotation(childOrn); - - ROSconvertURDFToVisualShape(vis, pathPrefix, inertialFrame.inverse()*childTrans, vertices, indices); - - } - - if (vertices.size() && indices.size()) - { - graphicsIndex = m_data->m_guiHelper->registerGraphicsShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size()); - } - - return graphicsIndex; - -} - - class btCompoundShape* ROSURDFImporter::convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const -{ - - btCompoundShape* compoundShape = new btCompoundShape(); - compoundShape->setMargin(0.001); - - for (int v=0;v<(int)m_data->m_links[linkIndex]->collision_array.size();v++) - { - const Collision* col = m_data->m_links[linkIndex]->collision_array[v].get(); - btCollisionShape* childShape = convertURDFToCollisionShape(col ,pathPrefix); - - if (childShape) - { - btVector3 childPos(col->origin.position.x, col->origin.position.y, col->origin.position.z); - btQuaternion childOrn(col->origin.rotation.x, col->origin.rotation.y, col->origin.rotation.z, col->origin.rotation.w); - btTransform childTrans; - childTrans.setOrigin(childPos); - childTrans.setRotation(childOrn); - compoundShape->addChildShape(localInertiaFrame.inverse()*childTrans,childShape); - - } - } - - return compoundShape; -} diff --git a/examples/Importers/ImportURDFDemo/ROSURDFImporter.h b/examples/Importers/ImportURDFDemo/ROSURDFImporter.h deleted file mode 100644 index 289fcbb8b..000000000 --- a/examples/Importers/ImportURDFDemo/ROSURDFImporter.h +++ /dev/null @@ -1,47 +0,0 @@ -#ifndef ROS_URDF_IMPORTER_H -#define ROS_URDF_IMPORTER_H - -#include "URDFImporterInterface.h" - - -class ROSURDFImporter : public URDFImporterInterface -{ - - struct ROSURDFInternalData* m_data; - - -public: - - ROSURDFImporter(struct GUIHelperInterface* guiHelper); - - virtual ~ROSURDFImporter(); - - // Note that forceFixedBase is ignored in this implementation. - virtual bool loadURDF(const char* fileName, bool forceFixedBase = false); - - virtual const char* getPathPrefix(); - - void printTree(); //for debugging - - virtual int getRootLinkIndex() const; - - virtual void getLinkChildIndices(int linkIndex, btAlignedObjectArray& childLinkIndices) const; - - virtual std::string getLinkName(int linkIndex) const; - - virtual std::string getJointName(int linkIndex) const; - - virtual void getMassAndInertia(int linkIndex, btScalar& mass,btVector3& localInertiaDiagonal, btTransform& inertialFrame) const; - - virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit,btScalar& jointDamping, btScalar& jointFriction) const; - - virtual bool getRootTransformInWorld(btTransform& rootTransformInWorld) const; - - virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const; - - virtual class btCompoundShape* convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const; - -}; - - -#endif //ROS_URDF_IMPORTER_H diff --git a/examples/InverseDynamics/InverseDynamicsExample.cpp b/examples/InverseDynamics/InverseDynamicsExample.cpp index a2ac8f1d6..7e53fd464 100644 --- a/examples/InverseDynamics/InverseDynamicsExample.cpp +++ b/examples/InverseDynamics/InverseDynamicsExample.cpp @@ -208,7 +208,7 @@ void InverseDynamicsExample::initPhysics() } // add joint target controls qd.resize(m_multiBody->getNumDofs()); - qd[3]=B3_HALF_PI; + qd_name.resize(m_multiBody->getNumDofs()); q_name.resize(m_multiBody->getNumDofs()); diff --git a/examples/InverseDynamics/premake4.lua b/examples/InverseDynamics/premake4.lua index 1114283ef..99c89235d 100644 --- a/examples/InverseDynamics/premake4.lua +++ b/examples/InverseDynamics/premake4.lua @@ -69,6 +69,7 @@ files { "../StandaloneMain/main_opengl_single_example.cpp", "../ExampleBrowser/OpenGLGuiHelper.cpp", "../ExampleBrowser/GL_ShapeDrawer.cpp", + "../ExampleBrowser/CollisionShape2TriangleMesh.cpp", "../Utils/b3ResourcePath.cpp", "../Utils/b3ResourcePath.h", "../RenderingExamples/TimeSeriesCanvas.cpp", @@ -126,6 +127,7 @@ files { "../StandaloneMain/main_sw_tinyrenderer_single_example.cpp", "../ExampleBrowser/OpenGLGuiHelper.cpp", "../ExampleBrowser/GL_ShapeDrawer.cpp", + "../ExampleBrowser/CollisionShape2TriangleMesh.cpp", "../TinyRenderer/geometry.cpp", "../TinyRenderer/model.cpp", "../TinyRenderer/tgaimage.cpp", @@ -159,4 +161,4 @@ if os.is("Linux") then initX11() end if os.is("MacOSX") then links{"Cocoa.framework"} end - \ No newline at end of file + diff --git a/examples/OpenGLWindow/X11OpenGLWindow.cpp b/examples/OpenGLWindow/X11OpenGLWindow.cpp index f2a077145..68aa22720 100644 --- a/examples/OpenGLWindow/X11OpenGLWindow.cpp +++ b/examples/OpenGLWindow/X11OpenGLWindow.cpp @@ -1095,7 +1095,7 @@ int X11OpenGLWindow::getHeight() const int X11OpenGLWindow::fileOpenDialog(char* filename, int maxNameLength) { int len = 0; - FILE * output = popen("zenity --file-selection --file-filter=\"*.urdf\" --file-filter=\"*.*\"","r"); + FILE * output = popen("zenity --file-selection --file-filter=\"*.urdf\" --file-filter=\"*.sdf\" --file-filter=\"*.obj\" --file-filter=\"*.*\"","r"); if (output) { while( fgets(filename, maxNameLength-1, output) != NULL ) diff --git a/examples/RenderingExamples/TinyRendererSetup.cpp b/examples/RenderingExamples/TinyRendererSetup.cpp new file mode 100644 index 000000000..d10f6f0de --- /dev/null +++ b/examples/RenderingExamples/TinyRendererSetup.cpp @@ -0,0 +1,277 @@ + +#include "RaytracerSetup.h" + +#include "../CommonInterfaces/CommonGraphicsAppInterface.h" +#include "Bullet3Common/b3Quaternion.h" +#include "Bullet3Common/b3AlignedObjectArray.h" +#include "../CommonInterfaces/CommonRenderInterface.h" +#include "../TinyRenderer/TinyRenderer.h" + +#include "../CommonInterfaces/Common2dCanvasInterface.h" +//#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h" +#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h" +//#include "BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h" +//#include "BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h" +#include "../CommonInterfaces/CommonExampleInterface.h" +#include "LinearMath/btAlignedObjectArray.h" +#include "btBulletCollisionCommon.h" +#include "../CommonInterfaces/CommonGUIHelperInterface.h" +#include "../ExampleBrowser/CollisionShape2TriangleMesh.h" + +struct TinyRendererSetup : public CommonExampleInterface +{ + + struct CommonGraphicsApp* m_app; + struct TinyRendererSetupInternalData* m_internalData; + + TinyRendererSetup(struct CommonGraphicsApp* app); + + virtual ~TinyRendererSetup(); + + virtual void initPhysics(); + + virtual void exitPhysics(); + + virtual void stepSimulation(float deltaTime); + + + virtual void physicsDebugDraw(int debugFlags); + + virtual void syncPhysicsToGraphics(struct GraphicsPhysicsBridge& gfxBridge); + + virtual bool mouseMoveCallback(float x,float y); + + virtual bool mouseButtonCallback(int button, int state, float x, float y); + + virtual bool keyboardCallback(int key, int state); + + virtual void renderScene() + { + } +}; + +struct TinyRendererSetupInternalData +{ + int m_canvasIndex; + struct Common2dCanvasInterface* m_canvas; + TGAImage m_rgbColorBuffer; + b3AlignedObjectArray m_depthBuffer; + + + int m_width; + int m_height; + + btAlignedObjectArray m_shapePtr; + btAlignedObjectArray m_transforms; + btAlignedObjectArray m_renderObjects; + + btVoronoiSimplexSolver m_simplexSolver; + btScalar m_pitch; + btScalar m_roll; + btScalar m_yaw; + + TinyRendererSetupInternalData(int width, int height) + :m_canvasIndex(-1), + m_canvas(0), + m_roll(0), + m_pitch(0), + m_yaw(0), + + m_width(width), + m_height(height), + m_rgbColorBuffer(width,height,TGAImage::RGB) + { + btConeShape* cone = new btConeShape(1,1); + btSphereShape* sphere = new btSphereShape(1); + btBoxShape* box = new btBoxShape (btVector3(1,1,1)); + m_shapePtr.push_back(cone); + m_shapePtr.push_back(sphere); + m_shapePtr.push_back(box); + m_depthBuffer.resize(m_width*m_height); + + for (int i=0;i vertexPositions; + btAlignedObjectArray vertexNormals; + btAlignedObjectArray indicesOut; + btTransform ident; + ident.setIdentity(); + CollisionShape2TriangleMesh(m_shapePtr[i],ident,vertexPositions,vertexNormals,indicesOut); + + m_renderObjects.push_back(ob); + ob->registerMesh2(vertexPositions,vertexNormals,indicesOut); + } + //ob->registerMeshShape( + + + updateTransforms(); + } + void updateTransforms() + { + int numObjects = m_shapePtr.size(); + m_transforms.resize(numObjects); + for (int i=0;im_canvas = m_app->m_2dCanvasInterface; + + + if (m_internalData->m_canvas) + { + + m_internalData->m_canvasIndex = m_internalData->m_canvas->createCanvas("tinyrenderer",m_internalData->m_width,m_internalData->m_height); + for (int i=0;im_width;i++) + { + for (int j=0;jm_height;j++) + { + unsigned char red=255; + unsigned char green=255; + unsigned char blue=255; + unsigned char alpha=255; + m_internalData->m_canvas->setPixel(m_internalData->m_canvasIndex,i,j,red,green,blue,alpha); + } + } + m_internalData->m_canvas->refreshImageData(m_internalData->m_canvasIndex); + + //int bitmapId = gfxBridge.createRenderBitmap(width,height); + } + + + + +} + + +void TinyRendererSetup::exitPhysics() +{ + + if (m_internalData->m_canvas && m_internalData->m_canvasIndex>=0) + { + m_internalData->m_canvas->destroyCanvas(m_internalData->m_canvasIndex); + } +} + + +void TinyRendererSetup::stepSimulation(float deltaTime) +{ + + m_internalData->updateTransforms(); + + TGAColor clearColor; + clearColor.bgra[0] = 255; + clearColor.bgra[1] = 255; + clearColor.bgra[2] = 255; + clearColor.bgra[3] = 255; + for(int y=0;ym_height;++y) + { + for(int x=0;xm_width;++x) + { + m_internalData->m_rgbColorBuffer.set(x,y,clearColor); + m_internalData->m_depthBuffer[x+y*m_internalData->m_width] = -1e30f; + } + } + + + ATTRIBUTE_ALIGNED16(float modelMat[16]); + ATTRIBUTE_ALIGNED16(float viewMat[16]); + CommonRenderInterface* render = this->m_app->m_renderer; + render->getActiveCamera()->getCameraViewMatrix(viewMat); + + + + for (int o=0;om_internalData->m_renderObjects.size();o++) + { + + const btTransform& tr = m_internalData->m_transforms[o]; + tr.getOpenGLMatrix(modelMat); + + for (int i=0;i<4;i++) + { + for (int j=0;j<4;j++) + { + m_internalData->m_renderObjects[o]->m_modelMatrix[i][j] = modelMat[i+4*j]; + m_internalData->m_renderObjects[o]->m_viewMatrix[i][j] = viewMat[i+4*j]; + } + } + TinyRenderer::renderObject(*m_internalData->m_renderObjects[o]); + } + + for(int y=0;ym_height;++y) + { + for(int x=0;xm_width;++x) + { + + const TGAColor& color = m_internalData->m_rgbColorBuffer.get(x,y); + m_internalData->m_canvas->setPixel(m_internalData->m_canvasIndex,x,(m_internalData->m_height-1-y), + color.bgra[2],color.bgra[1],color.bgra[0],255); + } + } + + //m_internalData->m_canvas->setPixel(m_internalData->m_canvasIndex,x,y,255,0,0,255); + + m_internalData->m_canvas->refreshImageData(m_internalData->m_canvasIndex); +} + + +void TinyRendererSetup::physicsDebugDraw(int debugDrawFlags) +{ +} + +bool TinyRendererSetup::mouseMoveCallback(float x,float y) +{ + return false; +} + +bool TinyRendererSetup::mouseButtonCallback(int button, int state, float x, float y) +{ + return false; +} + +bool TinyRendererSetup::keyboardCallback(int key, int state) +{ + return false; +} + + +void TinyRendererSetup::syncPhysicsToGraphics(GraphicsPhysicsBridge& gfxBridge) +{ +} + + CommonExampleInterface* TinyRendererCreateFunc(struct CommonExampleOptions& options) + { + return new TinyRendererSetup(options.m_guiHelper->getAppInterface()); + } diff --git a/examples/RenderingExamples/TinyRendererSetup.h b/examples/RenderingExamples/TinyRendererSetup.h new file mode 100644 index 000000000..76d07bae0 --- /dev/null +++ b/examples/RenderingExamples/TinyRendererSetup.h @@ -0,0 +1,6 @@ +#ifndef TINYRENDERER_SETUP_H +#define TINYRENDERER_SETUP_H + +class CommonExampleInterface* TinyRendererCreateFunc(struct CommonExampleOptions& options); + +#endif //TINYRENDERER_SETUP_H diff --git a/examples/SharedMemory/premake4.lua b/examples/SharedMemory/premake4.lua index 169849f53..c92eefe43 100644 --- a/examples/SharedMemory/premake4.lua +++ b/examples/SharedMemory/premake4.lua @@ -67,26 +67,10 @@ files { "../Importers/ImportColladaDemo/LoadMeshFromCollada.cpp", "../Importers/ImportColladaDemo/ColladaGraphicsInstance.h", "../ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp", - "../ThirdPartyLibs/urdf/urdfdom/urdf_parser/src/pose.cpp", - "../ThirdPartyLibs/urdf/urdfdom/urdf_parser/src/model.cpp", - "../ThirdPartyLibs/urdf/urdfdom/urdf_parser/src/link.cpp", - "../ThirdPartyLibs/urdf/urdfdom/urdf_parser/src/joint.cpp", - "../ThirdPartyLibs/urdf/urdfdom/urdf_parser/include/urdf_parser/urdf_parser.h", - "../ThirdPartyLibs/urdf/urdfdom_headers/urdf_exception/include/urdf_exception/exception.h", - "../ThirdPartyLibs/urdf/urdfdom_headers/urdf_model/include/urdf_model/pose.h", - "../ThirdPartyLibs/urdf/urdfdom_headers/urdf_model/include/urdf_model/model.h", - "../ThirdPartyLibs/urdf/urdfdom_headers/urdf_model/include/urdf_model/link.h", - "../ThirdPartyLibs/urdf/urdfdom_headers/urdf_model/include/urdf_model/joint.h", "../ThirdPartyLibs/tinyxml/tinystr.cpp", "../ThirdPartyLibs/tinyxml/tinyxml.cpp", "../ThirdPartyLibs/tinyxml/tinyxmlerror.cpp", "../ThirdPartyLibs/tinyxml/tinyxmlparser.cpp", - "../ThirdPartyLibs/urdf/boost_replacement/lexical_cast.h", - "../ThirdPartyLibs/urdf/boost_replacement/shared_ptr.h", - "../ThirdPartyLibs/urdf/boost_replacement/printf_console.cpp", - "../ThirdPartyLibs/urdf/boost_replacement/printf_console.h", - "../ThirdPartyLibs/urdf/boost_replacement/string_split.cpp", - "../ThirdPartyLibs/urdf/boost_replacement/string_split.h", } diff --git a/examples/ThirdPartyLibs/urdf/boost_replacement/lexical_cast.h b/examples/ThirdPartyLibs/urdf/boost_replacement/lexical_cast.h deleted file mode 100644 index 40864694a..000000000 --- a/examples/ThirdPartyLibs/urdf/boost_replacement/lexical_cast.h +++ /dev/null @@ -1,27 +0,0 @@ -#ifndef BOOST_REPLACEMENT_LEXICAL_CAST_H -#define BOOST_REPLACEMENT_LEXICAL_CAST_H - -#include - -namespace boost -{ - - template T lexical_cast(const char* txt) - { - double result = atof(txt); - return result; - }; - - struct bad_lexical_cast - { - const char* what() - { - return ("bad lexical cast\n"); - } - - }; - -} //namespace boost - -#endif - diff --git a/examples/ThirdPartyLibs/urdf/boost_replacement/printf_console.cpp b/examples/ThirdPartyLibs/urdf/boost_replacement/printf_console.cpp deleted file mode 100644 index 9c9014ac6..000000000 --- a/examples/ThirdPartyLibs/urdf/boost_replacement/printf_console.cpp +++ /dev/null @@ -1,28 +0,0 @@ -#include "printf_console.h" -#include - - -void logError(const char* msg, const char* arg0, const char* arg1, const char* arg2) -{ - printf("%s %s %s %s\n", msg,arg0,arg1,arg2); - -} - -void logDebug(const char* msg, float v0, float v1) -{ - printf("%s %f %f\n", msg, v0, v1); -}; -void logDebug(const char* msg, const char* msg1, const char* arg1) -{ - printf("%s %s %s\n", msg, msg1, arg1); - -} - -void logInform(const char* msg, const char* arg0) -{ - printf("%s %s\n", msg, arg0); -} -void logWarn(const char* msg,int id, const char* arg0) -{ - printf("%s %d %s\n", msg,id,arg0); -} diff --git a/examples/ThirdPartyLibs/urdf/boost_replacement/printf_console.h b/examples/ThirdPartyLibs/urdf/boost_replacement/printf_console.h deleted file mode 100644 index 247aab2c5..000000000 --- a/examples/ThirdPartyLibs/urdf/boost_replacement/printf_console.h +++ /dev/null @@ -1,13 +0,0 @@ -#ifndef PRINTF_CONSOLE_H -#define PRINTF_CONSOLE_H - - -void logError(const char* msg="", const char* arg0="", const char* arg1="", const char* arg2=""); -void logDebug(const char* msg, float v0, float v1); -void logDebug(const char* msg, const char* msg1="", const char* arg1=""); -void logInform(const char* msg, const char* arg0=""); -void logWarn(const char* msg,int id, const char* arg0=""); - -#endif - - diff --git a/examples/ThirdPartyLibs/urdf/boost_replacement/shared_ptr.h b/examples/ThirdPartyLibs/urdf/boost_replacement/shared_ptr.h deleted file mode 100644 index 5d29732b6..000000000 --- a/examples/ThirdPartyLibs/urdf/boost_replacement/shared_ptr.h +++ /dev/null @@ -1,210 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library Maya Plugin -Copyright (c) 2008 Walt Disney Studios - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising -from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must -not claim that you wrote the original software. If you use this -software in a product, an acknowledgment in the product documentation -would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must -not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. - -Written by: Nicola Candussi - -Modified by Francisco Gochez -Dec 2011 - Added deferencing operator -*/ - -//my_shared_ptr - -#ifndef DYN_SHARED_PTR_H -#define DYN_SHARED_PTR_H - -#define DYN_SHARED_PTR_THREAD_SAFE - - -#include - -#ifdef _WIN32 -#include - -class my_shared_count { -public: - my_shared_count(): m_count(1) { } - ~my_shared_count() { } - - long increment() - { -#ifdef DYN_SHARED_PTR_THREAD_SAFE - return InterlockedIncrement(&m_count); -#else - return ++m_count; -#endif - } - - long decrement() { -#ifdef DYN_SHARED_PTR_THREAD_SAFE - return InterlockedDecrement(&m_count); -#else - return ++m_count; -#endif - } - - long use_count() { return m_count; } - -private: - long m_count; -}; -#else //ifdef WIN32 - - -#include - -class my_shared_count { -public: - my_shared_count(): m_count(1) { -#ifdef DYN_SHARED_PTR_THREAD_SAFE - pthread_mutex_init(&m_mutex, 0); -#endif - } - ~my_shared_count() { -#ifdef DYN_SHARED_PTR_THREAD_SAFE - pthread_mutex_destroy(&m_mutex); -#endif - } - - long increment() - { -#ifdef DYN_SHARED_PTR_THREAD_SAFE - pthread_mutex_lock(&m_mutex); -#endif - long c = ++m_count; -#ifdef DYN_SHARED_PTR_THREAD_SAFE - pthread_mutex_unlock(&m_mutex); -#endif - return c; - } - - long decrement() { -#ifdef DYN_SHARED_PTR_THREAD_SAFE - pthread_mutex_lock(&m_mutex); -#endif - long c = --m_count; -#ifdef DYN_SHARED_PTR_THREAD_SAFE - pthread_mutex_unlock(&m_mutex); -#endif - return c; - } - - long use_count() { return m_count; } - -private: - long m_count; - mutable pthread_mutex_t m_mutex; -}; - -#endif - - -template -class my_shared_ptr -{ -public: - my_shared_ptr(): m_ptr(NULL), m_count(NULL) { } - my_shared_ptr(my_shared_ptr const& other): - m_ptr(other.m_ptr), - m_count(other.m_count) - { - if(other.m_count != NULL) other.m_count->increment(); - } - - template - my_shared_ptr(my_shared_ptr const& other): - m_ptr(other.m_ptr), - m_count(other.m_count) - { - if(other.m_count != NULL) other.m_count->increment(); - } - - my_shared_ptr(T const* other): m_ptr(const_cast(other)), m_count(NULL) - { - if(other != NULL) m_count = new my_shared_count; - } - - ~my_shared_ptr() - { - giveup_ownership(); - } - - void reset(T const* other) - { - if(m_ptr == other) return; - giveup_ownership(); - m_ptr = const_cast(other); - if(other != NULL) m_count = new my_shared_count; - else m_count = NULL; - } - - T* get() { return m_ptr; } - T const* get() const { return m_ptr; } - T* operator->() { return m_ptr; } - T const* operator->() const { return m_ptr; } - operator bool() const { return m_ptr != NULL; } - T& operator*() const - { - assert(m_ptr != 0); - return *m_ptr; - } - - bool operator<(my_shared_ptr const& rhs) const { return m_ptr < rhs.m_ptr; } - - my_shared_ptr& operator=(my_shared_ptr const& other) { - if(m_ptr == other.m_ptr) return *this; - giveup_ownership(); - m_ptr = other.m_ptr; - m_count = other.m_count; - if(other.m_count != NULL) m_count->increment(); - return *this; - } - - template - my_shared_ptr& operator=(my_shared_ptr& other) { - if(m_ptr == other.m_ptr) return *this; - giveup_ownership(); - m_ptr = other.m_ptr; - m_count = other.m_count; - if(other.m_count != NULL) m_count->increment(); - return *this; - } - -protected: - - template friend class my_shared_ptr; - void giveup_ownership() - { - if(m_count != NULL) { - if( m_count->decrement() == 0) { - delete m_ptr; - m_ptr = NULL; - delete m_count; - m_count = NULL; - } - } - } - -protected: - T *m_ptr; - my_shared_count *m_count; - -}; - - -#endif diff --git a/examples/ThirdPartyLibs/urdf/boost_replacement/string_split.cpp b/examples/ThirdPartyLibs/urdf/boost_replacement/string_split.cpp deleted file mode 100644 index 6c8a294a4..000000000 --- a/examples/ThirdPartyLibs/urdf/boost_replacement/string_split.cpp +++ /dev/null @@ -1,253 +0,0 @@ - - -#include -//#include -#include -#include -#include - -#include "string_split.h" - -namespace boost -{ - void split( std::vector&pieces, const std::string& vector_str, std::vector separators) - { - assert(separators.size()==1); - if (separators.size()==1) - { - char** strArray = str_split(vector_str.c_str(),separators[0].c_str()); - int numSubStr = str_array_len(strArray); - for (int i=0;i is_any_of(const char* seps) - { - std::vector strArray; - - int numSeps = strlen(seps); - for (int i=0;i -#include -#include - -namespace boost -{ - void split( std::vector&pieces, const std::string& vector_str, std::vector separators); - std::vector is_any_of(const char* seps); -}; - -///The string split C code is by Lars Wirzenius -///See http://stackoverflow.com/questions/2531605/how-to-split-a-string-with-a-delimiter-larger-than-one-single-char - - -/* Split a string into substrings. Return dynamic array of dynamically - allocated substrings, or NULL if there was an error. Caller is - expected to free the memory, for example with str_array_free. */ -char** str_split(const char* input, const char* sep); - -/* Free a dynamic array of dynamic strings. */ -void str_array_free(char** array); - -/* Return length of a NULL-delimited array of strings. */ -size_t str_array_len(char** array); - -#endif //STRING_SPLIT_H - diff --git a/examples/ThirdPartyLibs/urdf/premake4.lua b/examples/ThirdPartyLibs/urdf/premake4.lua deleted file mode 100644 index 526c9114a..000000000 --- a/examples/ThirdPartyLibs/urdf/premake4.lua +++ /dev/null @@ -1,46 +0,0 @@ - - - project "urdf_test" - - flags {"FloatStrict"} - - language "C++" - - kind "ConsoleApp" - targetdir "../../bin" - --- links { --- } - - includedirs { - ".", - "..", - - } - - - files { - "urdfdom/urdf_parser/src/check_urdf.cpp", - "urdfdom/urdf_parser/src/pose.cpp", - "urdfdom/urdf_parser/src/model.cpp", - "urdfdom/urdf_parser/src/link.cpp", - "urdfdom/urdf_parser/src/joint.cpp", - "urdfdom/urdf_parser/include/urdf_parser/urdf_parser.h", - "urdfdom_headers/urdf_exception/include/urdf_exception/exception.h", - "urdfdom_headers/urdf_model/include/urdf_model/pose.h", - "urdfdom_headers/urdf_model/include/urdf_model/model.h", - "urdfdom_headers/urdf_model/include/urdf_model/link.h", - "urdfdom_headers/urdf_model/include/urdf_model/joint.h", - "../tinyxml/tinystr.cpp", - "../tinyxml/tinyxml.cpp", - "../tinyxml/tinyxmlerror.cpp", - "../tinyxml/tinyxmlparser.cpp", - "boost_replacement/lexical_cast.h", - "boost_replacement/shared_ptr.h", - "boost_replacement/printf_console.cpp", - "boost_replacement/printf_console.h", - "boost_replacement/string_split.cpp", - "boost_replacement/string_split.h", - - - } diff --git a/examples/ThirdPartyLibs/urdf/urdfdom/LICENSE b/examples/ThirdPartyLibs/urdf/urdfdom/LICENSE deleted file mode 100644 index e80920e25..000000000 --- a/examples/ThirdPartyLibs/urdf/urdfdom/LICENSE +++ /dev/null @@ -1,15 +0,0 @@ -Software License Agreement (Apache License) - -Copyright 2011 John Hsu - -Licensed under the Apache License, Version 2.0 (the "License"); -you may not use this file except in compliance with the License. -You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - -Unless required by applicable law or agreed to in writing, software -distributed under the License is distributed on an "AS IS" BASIS, -WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -See the License for the specific language governing permissions and -limitations under the License. diff --git a/examples/ThirdPartyLibs/urdf/urdfdom/README.txt b/examples/ThirdPartyLibs/urdf/urdfdom/README.txt deleted file mode 100644 index 4e3bff6d8..000000000 --- a/examples/ThirdPartyLibs/urdf/urdfdom/README.txt +++ /dev/null @@ -1,7 +0,0 @@ -The URDF (U-Robot Description Format) library - provides core data structures and a simple XML parsers - for populating the class data structures from an URDF file. - -For now, the details of the URDF specifications reside on - http://ros.org/wiki/urdf - diff --git a/examples/ThirdPartyLibs/urdf/urdfdom/urdf_parser/include/urdf_parser/urdf_parser.h b/examples/ThirdPartyLibs/urdf/urdfdom/urdf_parser/include/urdf_parser/urdf_parser.h deleted file mode 100644 index 336af0fa3..000000000 --- a/examples/ThirdPartyLibs/urdf/urdfdom/urdf_parser/include/urdf_parser/urdf_parser.h +++ /dev/null @@ -1,63 +0,0 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ - -/* Author: Wim Meeussen */ - -#ifndef URDF_PARSER_URDF_PARSER_H -#define URDF_PARSER_URDF_PARSER_H - -#include - -#include -#include "tinyxml/tinyxml.h" - -//#include - -#ifndef M_PI -#define M_PI 3.1415925438 -#endif //M_PI - - -#include - - - - -namespace urdf{ - - my_shared_ptr parseURDF(const std::string &xml_string); - -} - -#endif diff --git a/examples/ThirdPartyLibs/urdf/urdfdom/urdf_parser/src/check_urdf.cpp b/examples/ThirdPartyLibs/urdf/urdfdom/urdf_parser/src/check_urdf.cpp deleted file mode 100644 index 67e9eb165..000000000 --- a/examples/ThirdPartyLibs/urdf/urdfdom/urdf_parser/src/check_urdf.cpp +++ /dev/null @@ -1,137 +0,0 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ - -/* Author: Wim Meeussen */ - -#include "urdf/urdfdom/urdf_parser/include/urdf_parser/urdf_parser.h" -#include -#include - -using namespace urdf; - -void printTree(my_shared_ptr link,int level = 0) -{ - level+=2; - int count = 0; - for (std::vector >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++) - { - if (*child) - { - for(int j=0;jname << std::endl; - // first grandchild - printTree(*child,level); - } - else - { - for(int j=0;jname << " has a null child!" << *child << std::endl; - } - } - -} - - -#define MSTRINGIFY(A) #A - - -const char* urdf_char = MSTRINGIFY( - - - - - - - - - - - - - - - - - - - - -); - - -int main(int argc, char** argv) -{ - - std::string xml_string; - - if (argc < 2){ - std::cerr << "No URDF file name provided, using a dummy test URDF" << std::endl; - - xml_string = std::string(urdf_char); - - } else - { - - - std::fstream xml_file(argv[1], std::fstream::in); - while ( xml_file.good() ) - { - std::string line; - std::getline( xml_file, line); - xml_string += (line + "\n"); - } - xml_file.close(); - } - - my_shared_ptr robot = parseURDF(xml_string); - if (!robot){ - std::cerr << "ERROR: Model Parsing the xml failed" << std::endl; - return -1; - } - std::cout << "robot name is: " << robot->getName() << std::endl; - - // get info from parser - std::cout << "---------- Successfully Parsed XML ---------------" << std::endl; - // get root link - my_shared_ptr root_link=robot->getRoot(); - if (!root_link) return -1; - - std::cout << "root Link: " << root_link->name << " has " << root_link->child_links.size() << " child(ren)" << std::endl; - - - // print entire tree - printTree(root_link); - return 0; -} - diff --git a/examples/ThirdPartyLibs/urdf/urdfdom/urdf_parser/src/joint.cpp b/examples/ThirdPartyLibs/urdf/urdfdom/urdf_parser/src/joint.cpp deleted file mode 100644 index bde0c5f73..000000000 --- a/examples/ThirdPartyLibs/urdf/urdfdom/urdf_parser/src/joint.cpp +++ /dev/null @@ -1,579 +0,0 @@ -/********************************************************************* -* Software Ligcense Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ - -/* Author: John Hsu */ - -#include -#include -#ifdef URDF_USE_BOOST -#include -#else -#include -#endif -#include - -#ifdef URDF_USE_CONSOLE_BRIDGE - #include -#else - #include "urdf/boost_replacement/printf_console.h" -#endif - -#include -#include - -namespace urdf{ - -bool parsePose(Pose &pose, TiXmlElement* xml); - -bool parseJointDynamics(JointDynamics &jd, TiXmlElement* config) -{ - jd.clear(); - - // Get joint damping - const char* damping_str = config->Attribute("damping"); - if (damping_str == NULL){ - logDebug("urdfdom.joint_dynamics: no damping, defaults to 0"); - jd.damping = 0; - } - else - { - try - { - jd.damping = boost::lexical_cast(damping_str); - } - catch (boost::bad_lexical_cast &e) - { - logError("damping value (%s) is not a float: %s",damping_str, e.what()); - return false; - } - } - - // Get joint friction - const char* friction_str = config->Attribute("friction"); - if (friction_str == NULL){ - logDebug("urdfdom.joint_dynamics: no friction, defaults to 0"); - jd.friction = 0; - } - else - { - try - { - jd.friction = boost::lexical_cast(friction_str); - } - catch (boost::bad_lexical_cast &e) - { - logError("friction value (%s) is not a float: %s",friction_str, e.what()); - return false; - } - } - - if (damping_str == NULL && friction_str == NULL) - { - logError("joint dynamics element specified with no damping and no friction"); - return false; - } - else{ - logDebug("urdfdom.joint_dynamics: damping %f and friction %f", jd.damping, jd.friction); - return true; - } -} - -bool parseJointLimits(JointLimits &jl, TiXmlElement* config) -{ - jl.clear(); - - // Get lower joint limit - const char* lower_str = config->Attribute("lower"); - if (lower_str == NULL){ - logDebug("urdfdom.joint_limit: no lower, defaults to 0"); - jl.lower = 0; - } - else - { - try - { - jl.lower = boost::lexical_cast(lower_str); - } - catch (boost::bad_lexical_cast &e) - { - logError("lower value (%s) is not a float: %s", lower_str, e.what()); - return false; - } - } - - // Get upper joint limit - const char* upper_str = config->Attribute("upper"); - if (upper_str == NULL){ - logDebug("urdfdom.joint_limit: no upper, , defaults to 0"); - jl.upper = 0; - } - else - { - try - { - jl.upper = boost::lexical_cast(upper_str); - } - catch (boost::bad_lexical_cast &e) - { - logError("upper value (%s) is not a float: %s",upper_str, e.what()); - return false; - } - } - - // Get joint effort limit - const char* effort_str = config->Attribute("effort"); - if (effort_str == NULL){ - logError("joint limit: no effort"); - return false; - } - else - { - try - { - jl.effort = boost::lexical_cast(effort_str); - } - catch (boost::bad_lexical_cast &e) - { - logError("effort value (%s) is not a float: %s",effort_str, e.what()); - return false; - } - } - - // Get joint velocity limit - const char* velocity_str = config->Attribute("velocity"); - if (velocity_str == NULL){ - logError("joint limit: no velocity"); - return false; - } - else - { - try - { - jl.velocity = boost::lexical_cast(velocity_str); - } - catch (boost::bad_lexical_cast &e) - { - logError("velocity value (%s) is not a float: %s",velocity_str, e.what()); - return false; - } - } - - return true; -} - -bool parseJointSafety(JointSafety &js, TiXmlElement* config) -{ - js.clear(); - - // Get soft_lower_limit joint limit - const char* soft_lower_limit_str = config->Attribute("soft_lower_limit"); - if (soft_lower_limit_str == NULL) - { - logDebug("urdfdom.joint_safety: no soft_lower_limit, using default value"); - js.soft_lower_limit = 0; - } - else - { - try - { - js.soft_lower_limit = boost::lexical_cast(soft_lower_limit_str); - } - catch (boost::bad_lexical_cast &e) - { - logError("soft_lower_limit value (%s) is not a float: %s",soft_lower_limit_str, e.what()); - return false; - } - } - - // Get soft_upper_limit joint limit - const char* soft_upper_limit_str = config->Attribute("soft_upper_limit"); - if (soft_upper_limit_str == NULL) - { - logDebug("urdfdom.joint_safety: no soft_upper_limit, using default value"); - js.soft_upper_limit = 0; - } - else - { - try - { - js.soft_upper_limit = boost::lexical_cast(soft_upper_limit_str); - } - catch (boost::bad_lexical_cast &e) - { - logError("soft_upper_limit value (%s) is not a float: %s",soft_upper_limit_str, e.what()); - return false; - } - } - - // Get k_position_ safety "position" gain - not exactly position gain - const char* k_position_str = config->Attribute("k_position"); - if (k_position_str == NULL) - { - logDebug("urdfdom.joint_safety: no k_position, using default value"); - js.k_position = 0; - } - else - { - try - { - js.k_position = boost::lexical_cast(k_position_str); - } - catch (boost::bad_lexical_cast &e) - { - logError("k_position value (%s) is not a float: %s",k_position_str, e.what()); - return false; - } - } - // Get k_velocity_ safety velocity gain - const char* k_velocity_str = config->Attribute("k_velocity"); - if (k_velocity_str == NULL) - { - logError("joint safety: no k_velocity"); - return false; - } - else - { - try - { - js.k_velocity = boost::lexical_cast(k_velocity_str); - } - catch (boost::bad_lexical_cast &e) - { - logError("k_velocity value (%s) is not a float: %s",k_velocity_str, e.what()); - return false; - } - } - - return true; -} - -bool parseJointCalibration(JointCalibration &jc, TiXmlElement* config) -{ - jc.clear(); - - // Get rising edge position - const char* rising_position_str = config->Attribute("rising"); - if (rising_position_str == NULL) - { - logDebug("urdfdom.joint_calibration: no rising, using default value"); - jc.rising.reset(0); - } - else - { - try - { - jc.rising.reset(new double(boost::lexical_cast(rising_position_str))); - } - catch (boost::bad_lexical_cast &e) - { - logError("risingvalue (%s) is not a float: %s",rising_position_str, e.what()); - return false; - } - } - - // Get falling edge position - const char* falling_position_str = config->Attribute("falling"); - if (falling_position_str == NULL) - { - logDebug("urdfdom.joint_calibration: no falling, using default value"); - jc.falling.reset(0); - } - else - { - try - { - jc.falling.reset(new double(boost::lexical_cast(falling_position_str))); - } - catch (boost::bad_lexical_cast &e) - { - logError("fallingvalue (%s) is not a float: %s",falling_position_str, e.what()); - return false; - } - } - - return true; -} - -bool parseJointMimic(JointMimic &jm, TiXmlElement* config) -{ - jm.clear(); - - // Get name of joint to mimic - const char* joint_name_str = config->Attribute("joint"); - - if (joint_name_str == NULL) - { - logError("joint mimic: no mimic joint specified"); - return false; - } - else - jm.joint_name = joint_name_str; - - // Get mimic multiplier - const char* multiplier_str = config->Attribute("multiplier"); - - if (multiplier_str == NULL) - { - logDebug("urdfdom.joint_mimic: no multiplier, using default value of 1"); - jm.multiplier = 1; - } - else - { - try - { - jm.multiplier = boost::lexical_cast(multiplier_str); - } - catch (boost::bad_lexical_cast &e) - { - logError("multiplier value (%s) is not a float: %s",multiplier_str, e.what()); - return false; - } - } - - - // Get mimic offset - const char* offset_str = config->Attribute("offset"); - if (offset_str == NULL) - { - logDebug("urdfdom.joint_mimic: no offset, using default value of 0"); - jm.offset = 0; - } - else - { - try - { - jm.offset = boost::lexical_cast(offset_str); - } - catch (boost::bad_lexical_cast &e) - { - logError("offset value (%s) is not a float: %s",offset_str, e.what()); - return false; - } - } - - return true; -} - -bool parseJoint(Joint &joint, TiXmlElement* config) -{ - joint.clear(); - - // Get Joint Name - const char *name = config->Attribute("name"); - if (!name) - { - logError("unnamed joint found"); - return false; - } - joint.name = name; - - // Get transform from Parent Link to Joint Frame - TiXmlElement *origin_xml = config->FirstChildElement("origin"); - if (!origin_xml) - { - logDebug("urdfdom: Joint [%s] missing origin tag under parent describing transform from Parent Link to Joint Frame, (using Identity transform).", joint.name.c_str()); - joint.parent_to_joint_origin_transform.clear(); - } - else - { - if (!parsePose(joint.parent_to_joint_origin_transform, origin_xml)) - { - joint.parent_to_joint_origin_transform.clear(); - logError("Malformed parent origin element for joint [%s]", joint.name.c_str()); - return false; - } - } - - // Get Parent Link - TiXmlElement *parent_xml = config->FirstChildElement("parent"); - if (parent_xml) - { - const char *pname = parent_xml->Attribute("link"); - if (!pname) - { - logInform("no parent link name specified for Joint link [%s]. this might be the root?", joint.name.c_str()); - } - else - { - joint.parent_link_name = std::string(pname); - } - } - - // Get Child Link - TiXmlElement *child_xml = config->FirstChildElement("child"); - if (child_xml) - { - const char *pname = child_xml->Attribute("link"); - if (!pname) - { - logInform("no child link name specified for Joint link [%s].", joint.name.c_str()); - } - else - { - joint.child_link_name = std::string(pname); - } - } - - // Get Joint type - const char* type_char = config->Attribute("type"); - if (!type_char) - { - logError("joint [%s] has no type, check to see if it's a reference.", joint.name.c_str()); - return false; - } - - std::string type_str = type_char; - if (type_str == "planar") - joint.type = Joint::PLANAR; - else if (type_str == "floating") - joint.type = Joint::FLOATING; - else if (type_str == "revolute") - joint.type = Joint::REVOLUTE; - else if (type_str == "continuous") - joint.type = Joint::CONTINUOUS; - else if (type_str == "prismatic") - joint.type = Joint::PRISMATIC; - else if (type_str == "fixed") - joint.type = Joint::FIXED; - else - { - logError("Joint [%s] has no known type [%s]", joint.name.c_str(), type_str.c_str()); - return false; - } - - // Get Joint Axis - if (joint.type != Joint::FLOATING && joint.type != Joint::FIXED) - { - // axis - TiXmlElement *axis_xml = config->FirstChildElement("axis"); - if (!axis_xml){ - logDebug("urdfdom: no axis elemement for Joint link [%s], defaulting to (1,0,0) axis", joint.name.c_str()); - joint.axis = Vector3(1.0, 0.0, 0.0); - } - else{ - if (axis_xml->Attribute("xyz")){ - try { - joint.axis.init(axis_xml->Attribute("xyz")); - } - catch (ParseError &e) { - joint.axis.clear(); - logError("Malformed axis element for joint [%s]: %s", joint.name.c_str(), e.what()); - return false; - } - } - } - } - - // Get limit - TiXmlElement *limit_xml = config->FirstChildElement("limit"); - if (limit_xml) - { - joint.limits.reset(new JointLimits()); - if (!parseJointLimits(*joint.limits, limit_xml)) - { - logError("Could not parse limit element for joint [%s]", joint.name.c_str()); - joint.limits.reset(0); - return false; - } - } - else if (joint.type == Joint::REVOLUTE) - { - logError("Joint [%s] is of type REVOLUTE but it does not specify limits", joint.name.c_str()); - return false; - } - else if (joint.type == Joint::PRISMATIC) - { - logError("Joint [%s] is of type PRISMATIC without limits", joint.name.c_str()); - return false; - } - - // Get safety - TiXmlElement *safety_xml = config->FirstChildElement("safety_controller"); - if (safety_xml) - { - joint.safety.reset(new JointSafety()); - if (!parseJointSafety(*joint.safety, safety_xml)) - { - logError("Could not parse safety element for joint [%s]", joint.name.c_str()); - joint.safety.reset(0); - return false; - } - } - - // Get calibration - TiXmlElement *calibration_xml = config->FirstChildElement("calibration"); - if (calibration_xml) - { - joint.calibration.reset(new JointCalibration()); - if (!parseJointCalibration(*joint.calibration, calibration_xml)) - { - logError("Could not parse calibration element for joint [%s]", joint.name.c_str()); - joint.calibration.reset(0); - return false; - } - } - - // Get Joint Mimic - TiXmlElement *mimic_xml = config->FirstChildElement("mimic"); - if (mimic_xml) - { - joint.mimic.reset(new JointMimic()); - if (!parseJointMimic(*joint.mimic, mimic_xml)) - { - logError("Could not parse mimic element for joint [%s]", joint.name.c_str()); - joint.mimic.reset(0); - return false; - } - } - - // Get Dynamics - TiXmlElement *prop_xml = config->FirstChildElement("dynamics"); - if (prop_xml) - { - joint.dynamics.reset(new JointDynamics()); - if (!parseJointDynamics(*joint.dynamics, prop_xml)) - { - logError("Could not parse joint_dynamics element for joint [%s]", joint.name.c_str()); - joint.dynamics.reset(0); - return false; - } - } - - return true; -} - - - - -} diff --git a/examples/ThirdPartyLibs/urdf/urdfdom/urdf_parser/src/link.cpp b/examples/ThirdPartyLibs/urdf/urdfdom/urdf_parser/src/link.cpp deleted file mode 100644 index c25aaf696..000000000 --- a/examples/ThirdPartyLibs/urdf/urdfdom/urdf_parser/src/link.cpp +++ /dev/null @@ -1,506 +0,0 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ - -/* Author: Wim Meeussen */ - - -#include -#include -//#include -//#include -#ifdef URDF_USE_BOOST -#include -#else -#include -#endif - -#include -#include -#ifdef URDF_USE_CONSOLE_BRIDGE -#include -#else -#include "urdf/boost_replacement/printf_console.h" -#endif - - - -namespace urdf{ - -bool parsePose(Pose &pose, TiXmlElement* xml); - -bool parseMaterial(Material &material, TiXmlElement *config, bool only_name_is_ok) -{ - bool has_rgb = false; - bool has_filename = false; - - material.clear(); - - if (!config->Attribute("name")) - { - logError("Material must contain a name attribute"); - return false; - } - - material.name = config->Attribute("name"); - - // texture - TiXmlElement *t = config->FirstChildElement("texture"); - - if (t) - { - if (t->Attribute("filename")) - { - material.texture_filename = t->Attribute("filename"); - has_filename = true; - } - } - - // color - TiXmlElement *c = config->FirstChildElement("color"); - if (c) - { - if (c->Attribute("rgba")) { - - try { - material.color.init(c->Attribute("rgba")); - has_rgb = true; - } - catch (ParseError &e) { - material.color.clear(); - logError(std::string("Material [" + material.name + "] has malformed color rgba values: " + e.what()).c_str()); - } - } - } - - if (!has_rgb && !has_filename) { - if (!only_name_is_ok) // no need for an error if only name is ok - { - if (!has_rgb) logError(std::string("Material ["+material.name+"] color has no rgba").c_str()); - if (!has_filename) logError(std::string("Material ["+material.name+"] not defined in file").c_str()); - } - return false; - } - return true; -} - - -bool parseSphere(Sphere &s, TiXmlElement *c) -{ - s.clear(); - - s.type = Geometry::SPHERE; - if (!c->Attribute("radius")) - { - logError("Sphere shape must have a radius attribute"); - return false; - } - - try - { - s.radius = boost::lexical_cast(c->Attribute("radius")); - } - catch (boost::bad_lexical_cast &e) - { - // std::stringstream stm; - // stm << "radius [" << c->Attribute("radius") << "] is not a valid float: " << e.what(); - // logError(stm.str().c_str()); - logError("radius issue"); - return false; - } - - return true; -} - -bool parseBox(Box &b, TiXmlElement *c) -{ - b.clear(); - - b.type = Geometry::BOX; - if (!c->Attribute("size")) - { - logError("Box shape has no size attribute"); - return false; - } - try - { - b.dim.init(c->Attribute("size")); - } - catch (ParseError &e) - { - b.dim.clear(); - logError(e.what()); - return false; - } - return true; -} - -bool parseCylinder(Cylinder &y, TiXmlElement *c) -{ - y.clear(); - - y.type = Geometry::CYLINDER; - if (!c->Attribute("length") || - !c->Attribute("radius")) - { - logError("Cylinder shape must have both length and radius attributes"); - return false; - } - - try - { - y.length = boost::lexical_cast(c->Attribute("length")); - } - catch (boost::bad_lexical_cast &e) - { - // std::stringstream stm; - // stm << "length [" << c->Attribute("length") << "] is not a valid float"; - //logError(stm.str().c_str()); - logError("length"); - return false; - } - - try - { - y.radius = boost::lexical_cast(c->Attribute("radius")); - } - catch (boost::bad_lexical_cast &e) - { - // std::stringstream stm; - // stm << "radius [" << c->Attribute("radius") << "] is not a valid float"; - //logError(stm.str().c_str()); - logError("radius"); - return false; - } - return true; -} - - -bool parseMesh(Mesh &m, TiXmlElement *c) -{ - m.clear(); - - m.type = Geometry::MESH; - if (!c->Attribute("filename")) { - logError("Mesh must contain a filename attribute"); - return false; - } - - m.filename = c->Attribute("filename"); - - if (c->Attribute("scale")) { - try { - m.scale.init(c->Attribute("scale")); - } - catch (ParseError &e) { - m.scale.clear(); - logError("Mesh scale was specified, but could not be parsed: %s", e.what()); - return false; - } - } - else - { - m.scale.x = m.scale.y = m.scale.z = 1; - } - return true; -} - -my_shared_ptr parseGeometry(TiXmlElement *g) -{ - my_shared_ptr geom; - if (!g) return geom; - - TiXmlElement *shape = g->FirstChildElement(); - if (!shape) - { - logError("Geometry tag contains no child element."); - return geom; - } - - const std::string type_name = shape->ValueTStr().c_str(); - if (type_name == "sphere") - { - Sphere *s = new Sphere(); - geom.reset(s); - if (parseSphere(*s, shape)) - return geom; - } - else if (type_name == "box") - { - Box *b = new Box(); - geom.reset(b); - if (parseBox(*b, shape)) - return geom; - } - else if (type_name == "cylinder") - { - Cylinder *c = new Cylinder(); - geom.reset(c); - if (parseCylinder(*c, shape)) - return geom; - } - else if (type_name == "mesh") - { - Mesh *m = new Mesh(); - geom.reset(m); - if (parseMesh(*m, shape)) - return geom; - } - else - { - logError("Unknown geometry type '%s'", type_name.c_str()); - return geom; - } - - return my_shared_ptr(); -} - -bool parseInertial(Inertial &i, TiXmlElement *config) -{ - i.clear(); - - // Origin - TiXmlElement *o = config->FirstChildElement("origin"); - if (o) - { - if (!parsePose(i.origin, o)) - return false; - } - - TiXmlElement *mass_xml = config->FirstChildElement("mass"); - if (!mass_xml) - { - logError("Inertial element must have a mass element"); - return false; - } - if (!mass_xml->Attribute("value")) - { - logError("Inertial: mass element must have value attribute"); - return false; - } - - try - { - i.mass = boost::lexical_cast(mass_xml->Attribute("value")); - } - catch (boost::bad_lexical_cast &e) - { - // std::stringstream stm; - // stm << "Inertial: mass [" << mass_xml->Attribute("value") - // << "] is not a float"; - //logError(stm.str().c_str()); - logError("Inertial mass issue"); - return false; - } - - TiXmlElement *inertia_xml = config->FirstChildElement("inertia"); - if (!inertia_xml) - { - logError("Inertial element must have inertia element"); - return false; - } - if (!(inertia_xml->Attribute("ixx") && inertia_xml->Attribute("ixy") && inertia_xml->Attribute("ixz") && - inertia_xml->Attribute("iyy") && inertia_xml->Attribute("iyz") && - inertia_xml->Attribute("izz"))) - { - logError("Inertial: inertia element must have ixx,ixy,ixz,iyy,iyz,izz attributes"); - return false; - } - try - { - i.ixx = boost::lexical_cast(inertia_xml->Attribute("ixx")); - i.ixy = boost::lexical_cast(inertia_xml->Attribute("ixy")); - i.ixz = boost::lexical_cast(inertia_xml->Attribute("ixz")); - i.iyy = boost::lexical_cast(inertia_xml->Attribute("iyy")); - i.iyz = boost::lexical_cast(inertia_xml->Attribute("iyz")); - i.izz = boost::lexical_cast(inertia_xml->Attribute("izz")); - } - catch (boost::bad_lexical_cast &e) - { - /* std::stringstream stm; - stm << "Inertial: one of the inertia elements is not a valid double:" - << " ixx [" << inertia_xml->Attribute("ixx") << "]" - << " ixy [" << inertia_xml->Attribute("ixy") << "]" - << " ixz [" << inertia_xml->Attribute("ixz") << "]" - << " iyy [" << inertia_xml->Attribute("iyy") << "]" - << " iyz [" << inertia_xml->Attribute("iyz") << "]" - << " izz [" << inertia_xml->Attribute("izz") << "]"; - logError(stm.str().c_str()); - */ - logError("Inertia error"); - - return false; - } - return true; -} - -bool parseVisual(Visual &vis, TiXmlElement *config) -{ - vis.clear(); - - // Origin - TiXmlElement *o = config->FirstChildElement("origin"); - if (o) { - if (!parsePose(vis.origin, o)) - return false; - } - - // Geometry - TiXmlElement *geom = config->FirstChildElement("geometry"); - vis.geometry = parseGeometry(geom); - if (!vis.geometry) - return false; - - const char *name_char = config->Attribute("name"); - if (name_char) - vis.name = name_char; - - // Material - TiXmlElement *mat = config->FirstChildElement("material"); - if (mat) { - // get material name - if (!mat->Attribute("name")) { - logError("Visual material must contain a name attribute"); - return false; - } - vis.material_name = mat->Attribute("name"); - - // try to parse material element in place - vis.material.reset(new Material()); - if (!parseMaterial(*vis.material, mat, true)) - { - logDebug("urdfdom: material has only name, actual material definition may be in the model"); - } - } - - return true; -} - -bool parseCollision(Collision &col, TiXmlElement* config) -{ - col.clear(); - - // Origin - TiXmlElement *o = config->FirstChildElement("origin"); - if (o) { - if (!parsePose(col.origin, o)) - return false; - } - - // Geometry - TiXmlElement *geom = config->FirstChildElement("geometry"); - col.geometry = parseGeometry(geom); - if (!col.geometry) - return false; - - const char *name_char = config->Attribute("name"); - if (name_char) - col.name = name_char; - - return true; -} - -bool parseLink(Link &link, TiXmlElement* config) -{ - - link.clear(); - - const char *name_char = config->Attribute("name"); - if (!name_char) - { - logError("No name given for the link."); - return false; - } - link.name = std::string(name_char); - - // Inertial (optional) - TiXmlElement *i = config->FirstChildElement("inertial"); - if (i) - { - link.inertial.reset(new Inertial()); - if (!parseInertial(*link.inertial, i)) - { - logError("Could not parse inertial element for Link [%s]", link.name.c_str()); - return false; - } - } - - // Multiple Visuals (optional) - for (TiXmlElement* vis_xml = config->FirstChildElement("visual"); vis_xml; vis_xml = vis_xml->NextSiblingElement("visual")) - { - - my_shared_ptr vis; - vis.reset(new Visual()); - if (parseVisual(*vis, vis_xml)) - { - link.visual_array.push_back(vis); - } - else - { - vis.reset(0); - logError("Could not parse visual element for Link [%s]", link.name.c_str()); - return false; - } - } - - // Visual (optional) - // Assign the first visual to the .visual ptr, if it exists - if (!link.visual_array.empty()) - link.visual = link.visual_array[0]; - - // Multiple Collisions (optional) - for (TiXmlElement* col_xml = config->FirstChildElement("collision"); col_xml; col_xml = col_xml->NextSiblingElement("collision")) - { - my_shared_ptr col; - col.reset(new Collision()); - if (parseCollision(*col, col_xml)) - { - link.collision_array.push_back(col); - } - else - { - col.reset(0); - logError("Could not parse collision element for Link [%s]", link.name.c_str()); - return false; - } - } - - // Collision (optional) - // Assign the first collision to the .collision ptr, if it exists - if (!link.collision_array.empty()) - link.collision = link.collision_array[0]; - return true; - -} - -} diff --git a/examples/ThirdPartyLibs/urdf/urdfdom/urdf_parser/src/model.cpp b/examples/ThirdPartyLibs/urdf/urdfdom/urdf_parser/src/model.cpp deleted file mode 100644 index e8562d09b..000000000 --- a/examples/ThirdPartyLibs/urdf/urdfdom/urdf_parser/src/model.cpp +++ /dev/null @@ -1,240 +0,0 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ - -/* Author: Wim Meeussen */ -//#include -#include -#include "urdf/urdfdom/urdf_parser/include/urdf_parser/urdf_parser.h" -#ifdef URDF_USE_CONSOLE_BRIDGE - #include -#else - #include "urdf/boost_replacement/printf_console.h" -#endif - -namespace urdf{ - -bool parseMaterial(Material &material, TiXmlElement *config, bool only_name_is_ok); -bool parseLink(Link &link, TiXmlElement *config); -bool parseJoint(Joint &joint, TiXmlElement *config); - - -my_shared_ptr parseURDF(const std::string &xml_string) -{ - my_shared_ptr model(new ModelInterface); - model->clear(); - - TiXmlDocument xml_doc; - xml_doc.Parse(xml_string.c_str()); - if (xml_doc.Error()) - { - logError(xml_doc.ErrorDesc()); - xml_doc.ClearError(); - model.reset(0); - return model; - } - - TiXmlElement *robot_xml = xml_doc.FirstChildElement("robot"); - if (!robot_xml) - { - logError("Could not find the 'robot' element in the xml file"); - model.reset(0); - return model; - } - - // Get robot name - const char *name = robot_xml->Attribute("name"); - if (!name) - { - logError("No name given for the robot."); - model.reset(0); - return model; - } - model->name_ = std::string(name); - - // Get all Material elements - for (TiXmlElement* material_xml = robot_xml->FirstChildElement("material"); material_xml; material_xml = material_xml->NextSiblingElement("material")) - { - my_shared_ptr material; - material.reset(new Material); - - try { - parseMaterial(*material, material_xml, false); // material needs to be fully defined here - if (model->getMaterial(material->name)) - { - logError("material '%s' is not unique.", material->name.c_str()); - material.reset(0); - model.reset(0); - return model; - } - else - { - model->materials_.insert(make_pair(material->name,material)); - logDebug("urdfdom: successfully added a new material '%s'", material->name.c_str()); - } - } - catch (ParseError &e) { - logError("material xml is not initialized correctly"); - material.reset(0); - model.reset(0); - return model; - } - } - - // Get all Link elements - for (TiXmlElement* link_xml = robot_xml->FirstChildElement("link"); link_xml; link_xml = link_xml->NextSiblingElement("link")) - { - my_shared_ptr link; - link.reset(new Link); - model->m_numLinks++; - - try { - parseLink(*link, link_xml); - if (model->getLink(link->name)) - { - logError("link '%s' is not unique.", link->name.c_str()); - model.reset(0); - return model; - } - else - { - // set link visual material - logDebug("urdfdom: setting link '%s' material", link->name.c_str()); - if (link->visual) - { - if (!link->visual->material_name.empty()) - { - if (model->getMaterial(link->visual->material_name)) - { - logDebug("urdfdom: setting link '%s' material to '%s'", link->name.c_str(),link->visual->material_name.c_str()); - link->visual->material = model->getMaterial( link->visual->material_name.c_str() ); - } - else - { - if (link->visual->material) - { - logDebug("urdfdom: link '%s' material '%s' defined in Visual.", link->name.c_str(),link->visual->material_name.c_str()); - model->materials_.insert(make_pair(link->visual->material->name,link->visual->material)); - } - else - { - logError("link '%s' material '%s' undefined.", link->name.c_str(),link->visual->material_name.c_str()); - model.reset(0); - return model; - } - } - } - } - - model->links_.insert(make_pair(link->name,link)); - logDebug("urdfdom: successfully added a new link '%s'", link->name.c_str()); - } - } - catch (ParseError &e) { - logError("link xml is not initialized correctly"); - model.reset(0); - return model; - } - } - if (model->links_.empty()){ - logError("No link elements found in urdf file"); - model.reset(0); - return model; - } - - // Get all Joint elements - for (TiXmlElement* joint_xml = robot_xml->FirstChildElement("joint"); joint_xml; joint_xml = joint_xml->NextSiblingElement("joint")) - { - my_shared_ptr joint; - joint.reset(new Joint); - model->m_numJoints++; - - if (parseJoint(*joint, joint_xml)) - { - if (model->getJoint(joint->name)) - { - logError("joint '%s' is not unique.", joint->name.c_str()); - model.reset(0); - return model; - } - else - { - model->joints_.insert(make_pair(joint->name,joint)); - logDebug("urdfdom: successfully added a new joint '%s'", joint->name.c_str()); - } - } - else - { - logError("joint xml is not initialized correctly"); - model.reset(0); - return model; - } - } - - - // every link has children links and joints, but no parents, so we create a - // local convenience data structure for keeping child->parent relations - std::map parent_link_tree; - parent_link_tree.clear(); - - // building tree: name mapping - try - { - model->initTree(parent_link_tree); - } - catch(ParseError &e) - { - logError("Failed to build tree: %s", e.what()); - model.reset(0); - return model; - } - - // find the root link - try - { - model->initRoot(parent_link_tree); - } - catch(ParseError &e) - { - logError("Failed to find root link: %s", e.what()); - model.reset(0); - return model; - } - - return model; -} - - - -} - diff --git a/examples/ThirdPartyLibs/urdf/urdfdom/urdf_parser/src/pose.cpp b/examples/ThirdPartyLibs/urdf/urdfdom/urdf_parser/src/pose.cpp deleted file mode 100644 index e90247c0e..000000000 --- a/examples/ThirdPartyLibs/urdf/urdfdom/urdf_parser/src/pose.cpp +++ /dev/null @@ -1,91 +0,0 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ - -/* Author: Wim Meeussen, John Hsu */ - - -#include -#include -#include -//#include -#include - -#ifdef URDF_USE_CONSOLE_BRIDGE -#include -#else -#include "urdf/boost_replacement/printf_console.h" -#endif - -#include -#include - - -namespace urdf{ - -bool parsePose(Pose &pose, TiXmlElement* xml) -{ - pose.clear(); - if (xml) - { - const char* xyz_str = xml->Attribute("xyz"); - if (xyz_str != NULL) - { - try { - pose.position.init(xyz_str); - } - catch (ParseError &e) { - logError(e.what()); - return false; - } - } - - const char* rpy_str = xml->Attribute("rpy"); - if (rpy_str != NULL) - { - try { - pose.rotation.init(rpy_str); - } - catch (ParseError &e) { - logError(e.what()); - return false; - } - } - } - return true; -} - - -} - - diff --git a/examples/ThirdPartyLibs/urdf/urdfdom/urdf_parser/src/twist.cpp b/examples/ThirdPartyLibs/urdf/urdfdom/urdf_parser/src/twist.cpp deleted file mode 100644 index 4980e17de..000000000 --- a/examples/ThirdPartyLibs/urdf/urdfdom/urdf_parser/src/twist.cpp +++ /dev/null @@ -1,85 +0,0 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ - -/* Author: John Hsu */ - - -#include -#include -#include -#include -#include -#include -#include - -namespace urdf{ - -bool parseTwist(Twist &twist, TiXmlElement* xml) -{ - twist.clear(); - if (xml) - { - const char* linear_char = xml->Attribute("linear"); - if (linear_char != NULL) - { - try { - twist.linear.init(linear_char); - } - catch (ParseError &e) { - twist.linear.clear(); - logError("Malformed linear string [%s]: %s", linear_char, e.what()); - return false; - } - } - - const char* angular_char = xml->Attribute("angular"); - if (angular_char != NULL) - { - try { - twist.angular.init(angular_char); - } - catch (ParseError &e) { - twist.angular.clear(); - logError("Malformed angular [%s]: %s", angular_char, e.what()); - return false; - } - } - } - return true; -} - -} - - - diff --git a/examples/ThirdPartyLibs/urdf/urdfdom/urdf_parser/src/urdf_model_state.cpp b/examples/ThirdPartyLibs/urdf/urdfdom/urdf_parser/src/urdf_model_state.cpp deleted file mode 100644 index f30b8456e..000000000 --- a/examples/ThirdPartyLibs/urdf/urdfdom/urdf_parser/src/urdf_model_state.cpp +++ /dev/null @@ -1,154 +0,0 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ - -/* Author: John Hsu */ - - -#include -#include -#include -#include -#include -#include -#include - -namespace urdf{ - -bool parseModelState(ModelState &ms, TiXmlElement* config) -{ - ms.clear(); - - const char *name_char = config->Attribute("name"); - if (!name_char) - { - logError("No name given for the model_state."); - return false; - } - ms.name = std::string(name_char); - - const char *time_stamp_char = config->Attribute("time_stamp"); - if (time_stamp_char) - { - try { - double sec = boost::lexical_cast(time_stamp_char); - ms.time_stamp.set(sec); - } - catch (boost::bad_lexical_cast &e) { - logError("Parsing time stamp [%s] failed: %s", time_stamp_char, e.what()); - return false; - } - } - - TiXmlElement *joint_state_elem = config->FirstChildElement("joint_state"); - if (joint_state_elem) - { - boost::shared_ptr joint_state; - joint_state.reset(new JointState()); - - const char *joint_char = joint_state_elem->Attribute("joint"); - if (joint_char) - joint_state->joint = std::string(joint_char); - else - { - logError("No joint name given for the model_state."); - return false; - } - - // parse position - const char *position_char = joint_state_elem->Attribute("position"); - if (position_char) - { - - std::vector pieces; - boost::split( pieces, position_char, boost::is_any_of(" ")); - for (unsigned int i = 0; i < pieces.size(); ++i){ - if (pieces[i] != ""){ - try { - joint_state->position.push_back(boost::lexical_cast(pieces[i].c_str())); - } - catch (boost::bad_lexical_cast &e) { - throw ParseError("position element ("+ pieces[i] +") is not a valid float"); - } - } - } - } - - // parse velocity - const char *velocity_char = joint_state_elem->Attribute("velocity"); - if (velocity_char) - { - - std::vector pieces; - boost::split( pieces, velocity_char, boost::is_any_of(" ")); - for (unsigned int i = 0; i < pieces.size(); ++i){ - if (pieces[i] != ""){ - try { - joint_state->velocity.push_back(boost::lexical_cast(pieces[i].c_str())); - } - catch (boost::bad_lexical_cast &e) { - throw ParseError("velocity element ("+ pieces[i] +") is not a valid float"); - } - } - } - } - - // parse effort - const char *effort_char = joint_state_elem->Attribute("effort"); - if (effort_char) - { - - std::vector pieces; - boost::split( pieces, effort_char, boost::is_any_of(" ")); - for (unsigned int i = 0; i < pieces.size(); ++i){ - if (pieces[i] != ""){ - try { - joint_state->effort.push_back(boost::lexical_cast(pieces[i].c_str())); - } - catch (boost::bad_lexical_cast &e) { - throw ParseError("effort element ("+ pieces[i] +") is not a valid float"); - } - } - } - } - - // add to vector - ms.joint_states.push_back(joint_state); - } -}; - - - -} - - diff --git a/examples/ThirdPartyLibs/urdf/urdfdom/urdf_parser/src/urdf_sensor.cpp b/examples/ThirdPartyLibs/urdf/urdfdom/urdf_parser/src/urdf_sensor.cpp deleted file mode 100644 index 85a886d3f..000000000 --- a/examples/ThirdPartyLibs/urdf/urdfdom/urdf_parser/src/urdf_sensor.cpp +++ /dev/null @@ -1,364 +0,0 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ - -/* Author: John Hsu */ - - -#include -#include -#include -#include -#include -#include -#include - -namespace urdf{ - -bool parsePose(Pose &pose, TiXmlElement* xml); - -bool parseCamera(Camera &camera, TiXmlElement* config) -{ - camera.clear(); - camera.type = VisualSensor::CAMERA; - - TiXmlElement *image = config->FirstChildElement("image"); - if (image) - { - const char* width_char = image->Attribute("width"); - if (width_char) - { - try - { - camera.width = boost::lexical_cast(width_char); - } - catch (boost::bad_lexical_cast &e) - { - logError("Camera image width [%s] is not a valid int: %s", width_char, e.what()); - return false; - } - } - else - { - logError("Camera sensor needs an image width attribute"); - return false; - } - - const char* height_char = image->Attribute("height"); - if (height_char) - { - try - { - camera.height = boost::lexical_cast(height_char); - } - catch (boost::bad_lexical_cast &e) - { - logError("Camera image height [%s] is not a valid int: %s", height_char, e.what()); - return false; - } - } - else - { - logError("Camera sensor needs an image height attribute"); - return false; - } - - const char* format_char = image->Attribute("format"); - if (format_char) - camera.format = std::string(format_char); - else - { - logError("Camera sensor needs an image format attribute"); - return false; - } - - const char* hfov_char = image->Attribute("hfov"); - if (hfov_char) - { - try - { - camera.hfov = boost::lexical_cast(hfov_char); - } - catch (boost::bad_lexical_cast &e) - { - logError("Camera image hfov [%s] is not a valid float: %s", hfov_char, e.what()); - return false; - } - } - else - { - logError("Camera sensor needs an image hfov attribute"); - return false; - } - - const char* near_char = image->Attribute("near"); - if (near_char) - { - try - { - camera.near = boost::lexical_cast(near_char); - } - catch (boost::bad_lexical_cast &e) - { - logError("Camera image near [%s] is not a valid float: %s", near_char, e.what()); - return false; - } - } - else - { - logError("Camera sensor needs an image near attribute"); - return false; - } - - const char* far_char = image->Attribute("far"); - if (far_char) - { - try - { - camera.far = boost::lexical_cast(far_char); - } - catch (boost::bad_lexical_cast &e) - { - logError("Camera image far [%s] is not a valid float: %s", far_char, e.what()); - return false; - } - } - else - { - logError("Camera sensor needs an image far attribute"); - return false; - } - - } - else - { - logError("Camera sensor has no element"); - return false; - } - return true; -} - -bool parseRay(Ray &ray, TiXmlElement* config) -{ - ray.clear(); - ray.type = VisualSensor::RAY; - - TiXmlElement *horizontal = config->FirstChildElement("horizontal"); - if (horizontal) - { - const char* samples_char = horizontal->Attribute("samples"); - if (samples_char) - { - try - { - ray.horizontal_samples = boost::lexical_cast(samples_char); - } - catch (boost::bad_lexical_cast &e) - { - logError("Ray horizontal samples [%s] is not a valid float: %s", samples_char, e.what()); - return false; - } - } - - const char* resolution_char = horizontal->Attribute("resolution"); - if (resolution_char) - { - try - { - ray.horizontal_resolution = boost::lexical_cast(resolution_char); - } - catch (boost::bad_lexical_cast &e) - { - logError("Ray horizontal resolution [%s] is not a valid float: %s", resolution_char, e.what()); - return false; - } - } - - const char* min_angle_char = horizontal->Attribute("min_angle"); - if (min_angle_char) - { - try - { - ray.horizontal_min_angle = boost::lexical_cast(min_angle_char); - } - catch (boost::bad_lexical_cast &e) - { - logError("Ray horizontal min_angle [%s] is not a valid float: %s", min_angle_char, e.what()); - return false; - } - } - - const char* max_angle_char = horizontal->Attribute("max_angle"); - if (max_angle_char) - { - try - { - ray.horizontal_max_angle = boost::lexical_cast(max_angle_char); - } - catch (boost::bad_lexical_cast &e) - { - logError("Ray horizontal max_angle [%s] is not a valid float: %s", max_angle_char, e.what()); - return false; - } - } - } - - TiXmlElement *vertical = config->FirstChildElement("vertical"); - if (vertical) - { - const char* samples_char = vertical->Attribute("samples"); - if (samples_char) - { - try - { - ray.vertical_samples = boost::lexical_cast(samples_char); - } - catch (boost::bad_lexical_cast &e) - { - logError("Ray vertical samples [%s] is not a valid float: %s", samples_char, e.what()); - return false; - } - } - - const char* resolution_char = vertical->Attribute("resolution"); - if (resolution_char) - { - try - { - ray.vertical_resolution = boost::lexical_cast(resolution_char); - } - catch (boost::bad_lexical_cast &e) - { - logError("Ray vertical resolution [%s] is not a valid float: %s", resolution_char, e.what()); - return false; - } - } - - const char* min_angle_char = vertical->Attribute("min_angle"); - if (min_angle_char) - { - try - { - ray.vertical_min_angle = boost::lexical_cast(min_angle_char); - } - catch (boost::bad_lexical_cast &e) - { - logError("Ray vertical min_angle [%s] is not a valid float: %s", min_angle_char, e.what()); - return false; - } - } - - const char* max_angle_char = vertical->Attribute("max_angle"); - if (max_angle_char) - { - try - { - ray.vertical_max_angle = boost::lexical_cast(max_angle_char); - } - catch (boost::bad_lexical_cast &e) - { - logError("Ray vertical max_angle [%s] is not a valid float: %s", max_angle_char, e.what()); - return false; - } - } - } -} - -boost::shared_ptr parseVisualSensor(TiXmlElement *g) -{ - boost::shared_ptr visual_sensor; - - // get sensor type - TiXmlElement *sensor_xml; - if (g->FirstChildElement("camera")) - { - Camera *camera = new Camera(); - visual_sensor.reset(camera); - sensor_xml = g->FirstChildElement("camera"); - if (!parseCamera(*camera, sensor_xml)) - visual_sensor.reset(); - } - else if (g->FirstChildElement("ray")) - { - Ray *ray = new Ray(); - visual_sensor.reset(ray); - sensor_xml = g->FirstChildElement("ray"); - if (!parseRay(*ray, sensor_xml)) - visual_sensor.reset(); - } - else - { - logError("No know sensor types [camera|ray] defined in block"); - } - return visual_sensor; -} - - -bool parseSensor(Sensor &sensor, TiXmlElement* config) -{ - sensor.clear(); - - const char *name_char = config->Attribute("name"); - if (!name_char) - { - logError("No name given for the sensor."); - return false; - } - sensor.name = std::string(name_char); - - // parse parent_link_name - const char *parent_link_name_char = config->Attribute("parent_link_name"); - if (!parent_link_name_char) - { - logError("No parent_link_name given for the sensor."); - return false; - } - sensor.parent_link_name = std::string(parent_link_name_char); - - // parse origin - TiXmlElement *o = config->FirstChildElement("origin"); - if (o) - { - if (!parsePose(sensor.origin, o)) - return false; - } - - // parse sensor - sensor.sensor = parseVisualSensor(config); - return true; -} - - -} - - diff --git a/examples/ThirdPartyLibs/urdf/urdfdom/urdf_parser/src/world.cpp b/examples/ThirdPartyLibs/urdf/urdfdom/urdf_parser/src/world.cpp deleted file mode 100644 index ddc27c5f3..000000000 --- a/examples/ThirdPartyLibs/urdf/urdfdom/urdf_parser/src/world.cpp +++ /dev/null @@ -1,71 +0,0 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ - -/* Author: Wim Meeussen */ - - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace urdf{ - -bool parseWorld(World &world, TiXmlElement* config) -{ - - // to be implemented - - return true; -} - -bool exportWorld(World &world, TiXmlElement* xml) -{ - TiXmlElement * world_xml = new TiXmlElement("world"); - world_xml->SetAttribute("name", world.name); - - // to be implemented - // exportModels(*world.models, world_xml); - - xml->LinkEndChild(world_xml); - - return true; -} - -} diff --git a/examples/ThirdPartyLibs/urdf/urdfdom/urdf_parser/test/memtest.cpp b/examples/ThirdPartyLibs/urdf/urdfdom/urdf_parser/test/memtest.cpp deleted file mode 100644 index d835eb3db..000000000 --- a/examples/ThirdPartyLibs/urdf/urdfdom/urdf_parser/test/memtest.cpp +++ /dev/null @@ -1,20 +0,0 @@ -#include "urdf_parser/urdf_parser.h" -#include -#include - -int main(int argc, char** argv){ - while (true){ - std::string xml_string; - std::fstream xml_file(argv[1], std::fstream::in); - while ( xml_file.good() ) - { - std::string line; - std::getline( xml_file, line); - xml_string += (line + "\n"); - } - xml_file.close(); - - - urdf::parseURDF(xml_string); - } -} diff --git a/examples/ThirdPartyLibs/urdf/urdfdom_headers/LICENSE b/examples/ThirdPartyLibs/urdf/urdfdom_headers/LICENSE deleted file mode 100644 index e80920e25..000000000 --- a/examples/ThirdPartyLibs/urdf/urdfdom_headers/LICENSE +++ /dev/null @@ -1,15 +0,0 @@ -Software License Agreement (Apache License) - -Copyright 2011 John Hsu - -Licensed under the Apache License, Version 2.0 (the "License"); -you may not use this file except in compliance with the License. -You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - -Unless required by applicable law or agreed to in writing, software -distributed under the License is distributed on an "AS IS" BASIS, -WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -See the License for the specific language governing permissions and -limitations under the License. diff --git a/examples/ThirdPartyLibs/urdf/urdfdom_headers/README.txt b/examples/ThirdPartyLibs/urdf/urdfdom_headers/README.txt deleted file mode 100644 index 6a841d52e..000000000 --- a/examples/ThirdPartyLibs/urdf/urdfdom_headers/README.txt +++ /dev/null @@ -1,6 +0,0 @@ -The URDF (U-Robot Description Format) headers - provides core data structure headers for URDF. - -For now, the details of the URDF specifications reside on - http://ros.org/wiki/urdf - diff --git a/examples/ThirdPartyLibs/urdf/urdfdom_headers/urdf_exception/include/urdf_exception/exception.h b/examples/ThirdPartyLibs/urdf/urdfdom_headers/urdf_exception/include/urdf_exception/exception.h deleted file mode 100644 index 24222f1ff..000000000 --- a/examples/ThirdPartyLibs/urdf/urdfdom_headers/urdf_exception/include/urdf_exception/exception.h +++ /dev/null @@ -1,53 +0,0 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ - -// URDF exceptions -#ifndef URDF_INTERFACE_EXCEPTION_H_ -#define URDF_INTERFACE_EXCEPTION_H_ - -#include -#include - -namespace urdf -{ - -class ParseError: public std::runtime_error -{ -public: - ParseError(const std::string &error_msg) : std::runtime_error(error_msg) {}; -}; - -} - -#endif diff --git a/examples/ThirdPartyLibs/urdf/urdfdom_headers/urdf_model/include/urdf_model/color.h b/examples/ThirdPartyLibs/urdf/urdfdom_headers/urdf_model/include/urdf_model/color.h deleted file mode 100644 index 9c15dd77b..000000000 --- a/examples/ThirdPartyLibs/urdf/urdfdom_headers/urdf_model/include/urdf_model/color.h +++ /dev/null @@ -1,101 +0,0 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ - -/* Author: Josh Faust */ - -#ifndef URDF_INTERFACE_COLOR_H -#define URDF_INTERFACE_COLOR_H - -#include -#include -#include -//#include -//#include - -namespace urdf -{ - -class Color -{ -public: - Color() {this->clear();}; - float r; - float g; - float b; - float a; - - void clear() - { - r = g = b = 0.0f; - a = 1.0f; - } - bool init(const std::string &vector_str) - { - this->clear(); - std::vector pieces; - std::vector rgba; - - boost::split( pieces, vector_str, boost::is_any_of(" ")); - for (unsigned int i = 0; i < pieces.size(); ++i) - { - if (!pieces[i].empty()) - { - try - { - rgba.push_back(boost::lexical_cast(pieces[i].c_str())); - } - catch (boost::bad_lexical_cast &e) - { - return false; - } - } - } - - if (rgba.size() != 4) - { - return false; - } - this->r = rgba[0]; - this->g = rgba[1]; - this->b = rgba[2]; - this->a = rgba[3]; - - return true; - }; -}; - -} - -#endif - diff --git a/examples/ThirdPartyLibs/urdf/urdfdom_headers/urdf_model/include/urdf_model/joint.h b/examples/ThirdPartyLibs/urdf/urdfdom_headers/urdf_model/include/urdf_model/joint.h deleted file mode 100644 index cd889dcec..000000000 --- a/examples/ThirdPartyLibs/urdf/urdfdom_headers/urdf_model/include/urdf_model/joint.h +++ /dev/null @@ -1,234 +0,0 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ - -/* Author: Wim Meeussen */ - -#ifndef URDF_INTERFACE_JOINT_H -#define URDF_INTERFACE_JOINT_H - -#include -#include -#ifdef URDF_USE_BOOST -#include -#define my_shared_ptr my_shared_ptr -#else -#include -#endif - -#include - - -namespace urdf{ - -class Link; - -class JointDynamics -{ -public: - JointDynamics() { this->clear(); }; - double damping; - double friction; - - void clear() - { - damping = 0; - friction = 0; - }; -}; - -class JointLimits -{ -public: - JointLimits() { this->clear(); }; - double lower; - double upper; - double effort; - double velocity; - - void clear() - { - lower = 0; - upper = 0; - effort = 0; - velocity = 0; - }; -}; - -/// \brief Parameters for Joint Safety Controllers -class JointSafety -{ -public: - /// clear variables on construction - JointSafety() { this->clear(); }; - - /// - /// IMPORTANT: The safety controller support is very much PR2 specific, not intended for generic usage. - /// - /// Basic safety controller operation is as follows - /// - /// current safety controllers will take effect on joints outside the position range below: - /// - /// position range: [JointSafety::soft_lower_limit + JointLimits::velocity / JointSafety::k_position, - /// JointSafety::soft_uppper_limit - JointLimits::velocity / JointSafety::k_position] - /// - /// if (joint_position is outside of the position range above) - /// velocity_limit_min = -JointLimits::velocity + JointSafety::k_position * (joint_position - JointSafety::soft_lower_limit) - /// velocity_limit_max = JointLimits::velocity + JointSafety::k_position * (joint_position - JointSafety::soft_upper_limit) - /// else - /// velocity_limit_min = -JointLimits::velocity - /// velocity_limit_max = JointLimits::velocity - /// - /// velocity range: [velocity_limit_min + JointLimits::effort / JointSafety::k_velocity, - /// velocity_limit_max - JointLimits::effort / JointSafety::k_velocity] - /// - /// if (joint_velocity is outside of the velocity range above) - /// effort_limit_min = -JointLimits::effort + JointSafety::k_velocity * (joint_velocity - velocity_limit_min) - /// effort_limit_max = JointLimits::effort + JointSafety::k_velocity * (joint_velocity - velocity_limit_max) - /// else - /// effort_limit_min = -JointLimits::effort - /// effort_limit_max = JointLimits::effort - /// - /// Final effort command sent to the joint is saturated by [effort_limit_min,effort_limit_max] - /// - /// Please see wiki for more details: http://www.ros.org/wiki/pr2_controller_manager/safety_limits - /// - double soft_upper_limit; - double soft_lower_limit; - double k_position; - double k_velocity; - - void clear() - { - soft_upper_limit = 0; - soft_lower_limit = 0; - k_position = 0; - k_velocity = 0; - }; -}; - - -class JointCalibration -{ -public: - JointCalibration() { this->clear(); }; - double reference_position; - my_shared_ptr rising, falling; - - void clear() - { - reference_position = 0; - }; -}; - -class JointMimic -{ -public: - JointMimic() { this->clear(); }; - double offset; - double multiplier; - std::string joint_name; - - void clear() - { - offset = 0.0; - multiplier = 0.0; - joint_name.clear(); - }; -}; - - -class Joint -{ -public: - - Joint() { this->clear(); }; - - std::string name; - enum - { - UNKNOWN, REVOLUTE, CONTINUOUS, PRISMATIC, FLOATING, PLANAR, FIXED - } type; - - /// \brief type_ meaning of axis_ - /// ------------------------------------------------------ - /// UNKNOWN unknown type - /// REVOLUTE rotation axis - /// PRISMATIC translation axis - /// FLOATING N/A - /// PLANAR plane normal axis - /// FIXED N/A - Vector3 axis; - - /// child Link element - /// child link frame is the same as the Joint frame - std::string child_link_name; - - /// parent Link element - /// origin specifies the transform from Parent Link to Joint Frame - std::string parent_link_name; - /// transform from Parent Link frame to Joint frame - Pose parent_to_joint_origin_transform; - - /// Joint Dynamics - my_shared_ptr dynamics; - - /// Joint Limits - my_shared_ptr limits; - - /// Unsupported Hidden Feature - my_shared_ptr safety; - - /// Unsupported Hidden Feature - my_shared_ptr calibration; - - /// Option to Mimic another Joint - my_shared_ptr mimic; - - void clear() - { - this->axis.clear(); - this->child_link_name.clear(); - this->parent_link_name.clear(); - this->parent_to_joint_origin_transform.clear(); - this->dynamics.reset(0); - this->limits.reset(0); - this->safety.reset(0); - this->calibration.reset(0); - this->type = UNKNOWN; - }; -}; - -} - -#endif diff --git a/examples/ThirdPartyLibs/urdf/urdfdom_headers/urdf_model/include/urdf_model/link.h b/examples/ThirdPartyLibs/urdf/urdfdom_headers/urdf_model/include/urdf_model/link.h deleted file mode 100644 index 22e64cf5a..000000000 --- a/examples/ThirdPartyLibs/urdf/urdfdom_headers/urdf_model/include/urdf_model/link.h +++ /dev/null @@ -1,262 +0,0 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ - -/* Author: Wim Meeussen */ - -#ifndef URDF_INTERFACE_LINK_H -#define URDF_INTERFACE_LINK_H - -#include -#include -#include - - -#ifdef URDF_USE_BOOST - #include - #include -#else - #include -#endif - -#include "joint.h" -#include "color.h" -#include "pose.h" - -namespace urdf{ - -class Geometry -{ -public: - enum {SPHERE, BOX, CYLINDER, MESH} type; - - virtual ~Geometry(void) - { - } -}; - -class Sphere : public Geometry -{ -public: - Sphere() { this->clear(); type = SPHERE; }; - double radius; - - void clear() - { - radius = 0; - }; -}; - -class Box : public Geometry -{ -public: - Box() { this->clear(); type = BOX; } - Vector3 dim; - - void clear() - { - this->dim.clear(); - }; -}; - -class Cylinder : public Geometry -{ -public: - Cylinder() { this->clear(); type = CYLINDER; }; - double length; - double radius; - - void clear() - { - length = 0; - radius = 0; - }; -}; - -class Mesh : public Geometry -{ -public: - Mesh() { this->clear(); type = MESH; }; - std::string filename; - Vector3 scale; - - void clear() - { - filename.clear(); - // default scale - scale.x = 1; - scale.y = 1; - scale.z = 1; - }; -}; - -class Material -{ -public: - Material() { this->clear(); }; - std::string name; - std::string texture_filename; - Color color; - - void clear() - { - color.clear(); - texture_filename.clear(); - name.clear(); - }; -}; - -class Inertial -{ -public: - Inertial() { this->clear(); }; - Pose origin; - double mass; - double ixx,ixy,ixz,iyy,iyz,izz; - - void clear() - { - origin.clear(); - mass = 0; - ixx = ixy = ixz = iyy = iyz = izz = 0; - }; -}; - -class Visual -{ -public: - Visual() { this->clear(); }; - Pose origin; - my_shared_ptr geometry; - - std::string material_name; - my_shared_ptr material; - - void clear() - { - origin.clear(); - material_name.clear(); - material.reset(0); - geometry.reset(0); - name.clear(); - }; - - std::string name; -}; - -class Collision -{ -public: - Collision() { this->clear(); }; - Pose origin; - my_shared_ptr geometry; - - void clear() - { - origin.clear(); - geometry.reset(0); - name.clear(); - }; - - std::string name; - -}; - - -class Link -{ -public: - Link() { this->clear(); }; - - std::string name; - - /// inertial element - my_shared_ptr inertial; - - /// visual element - my_shared_ptr visual; - - /// collision element - my_shared_ptr collision; - - /// if more than one collision element is specified, all collision elements are placed in this array (the collision member points to the first element of the array) - std::vector > collision_array; - - /// if more than one visual element is specified, all visual elements are placed in this array (the visual member points to the first element of the array) - std::vector > visual_array; - - /// Parent Joint element - /// explicitly stating "parent" because we want directional-ness for tree structure - /// every link can have one parent - my_shared_ptr parent_joint; - - std::vector > child_joints; - std::vector > child_links; - - mutable int m_link_index; - - const Link* getParent() const - {return parent_link_;} - - void setParent(const my_shared_ptr &parent) - { - parent_link_ = parent.get(); - } - - void clear() - { - this->name.clear(); - this->inertial.reset(0); - this->visual.reset(0); - this->collision.reset(0); - this->parent_joint.reset(0); - this->child_joints.clear(); - this->child_links.clear(); - this->collision_array.clear(); - this->visual_array.clear(); - this->m_link_index=-1; - this->parent_link_ = NULL; - }; - -private: -// boost::weak_ptr parent_link_; - const Link* parent_link_; - -}; - - - - -} - -#endif diff --git a/examples/ThirdPartyLibs/urdf/urdfdom_headers/urdf_model/include/urdf_model/model.h b/examples/ThirdPartyLibs/urdf/urdfdom_headers/urdf_model/include/urdf_model/model.h deleted file mode 100644 index 8e93d94e4..000000000 --- a/examples/ThirdPartyLibs/urdf/urdfdom_headers/urdf_model/include/urdf_model/model.h +++ /dev/null @@ -1,220 +0,0 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ - -/* Author: Wim Meeussen */ - -#ifndef URDF_INTERFACE_MODEL_H -#define URDF_INTERFACE_MODEL_H - -#include -#include -//#include -#include -#include //printf -#include - -namespace urdf { - -class ModelInterface -{ -public: - my_shared_ptr getRoot(void) const{return this->root_link_;}; - my_shared_ptr getLink(const std::string& name) const - { - my_shared_ptr ptr; - if (this->links_.find(name) == this->links_.end()) - ptr.reset(0); - else - ptr = this->links_.find(name)->second; - return ptr; - }; - - my_shared_ptr getJoint(const std::string& name) const - { - my_shared_ptr ptr; - if (this->joints_.find(name) == this->joints_.end()) - ptr.reset(0); - else - ptr = this->joints_.find(name)->second; - return ptr; - }; - - - const std::string& getName() const {return name_;}; - void getLinks(std::vector >& links) const - { - for (std::map >::const_iterator link = this->links_.begin();link != this->links_.end(); link++) - { - links.push_back(link->second); - } - }; - - void clear() - { - m_numLinks=0; - m_numJoints = 0; - name_.clear(); - this->links_.clear(); - this->joints_.clear(); - this->materials_.clear(); - this->root_link_.reset(0); - }; - - /// non-const getLink() - void getLink(const std::string& name,my_shared_ptr &link) const - { - my_shared_ptr ptr; - if (this->links_.find(name) == this->links_.end()) - ptr.reset(0); - else - ptr = this->links_.find(name)->second; - link = ptr; - }; - - /// non-const getMaterial() - my_shared_ptr getMaterial(const std::string& name) const - { - my_shared_ptr ptr; - if (this->materials_.find(name) == this->materials_.end()) - ptr.reset(0); - else - ptr = this->materials_.find(name)->second; - return ptr; - }; - - void initTree(std::map &parent_link_tree) - { - // loop through all joints, for every link, assign children links and children joints - for (std::map >::iterator joint = this->joints_.begin();joint != this->joints_.end(); joint++) - { - std::string parent_link_name = joint->second->parent_link_name; - std::string child_link_name = joint->second->child_link_name; - - if (parent_link_name.empty() || child_link_name.empty()) - { - assert(0); - - // throw ParseError("Joint [" + joint->second->name + "] is missing a parent and/or child link specification."); - } - else - { - // find child and parent links - my_shared_ptr child_link, parent_link; - this->getLink(child_link_name, child_link); - if (!child_link) - { - printf("Error: child link [%s] of joint [%s] not found\n", child_link_name.c_str(),joint->first.c_str() ); - assert(0); -// throw ParseError("child link [" + child_link_name + "] of joint [" + joint->first + "] not found"); - } - this->getLink(parent_link_name, parent_link); - if (!parent_link) - { - assert(0); - -/* throw ParseError("parent link [" + parent_link_name + "] of joint [" + joint->first + "] not found. This is not valid according to the URDF spec. Every link you refer to from a joint needs to be explicitly defined in the robot description. To fix this problem you can either remove this joint [" + joint->first + "] from your urdf file, or add \"\" to your urdf file."); - - */} - - //set parent link for child link - child_link->setParent(parent_link); - - //set parent joint for child link - child_link->parent_joint = joint->second; - - //set child joint for parent link - parent_link->child_joints.push_back(joint->second); - - //set child link for parent link - parent_link->child_links.push_back(child_link); - - // fill in child/parent string map - parent_link_tree[child_link->name] = parent_link_name; - } - } - } - - void initRoot(const std::map &parent_link_tree) - { - this->root_link_.reset(0); - - // find the links that have no parent in the tree - for (std::map >::const_iterator l=this->links_.begin(); l!=this->links_.end(); l++) - { - std::map::const_iterator parent = parent_link_tree.find(l->first); - if (parent == parent_link_tree.end()) - { - // store root link - if (!this->root_link_) - { - getLink(l->first, this->root_link_); - } - // we already found a root link - else - { - assert(0); - // throw ParseError("Two root links found: [" + this->root_link_->name + "] and [" + l->first + "]"); - } - } - } - if (!this->root_link_) - { - assert(0); - //throw ParseError("No root link found. The robot xml is not a valid tree."); - } - } - - - /// \brief complete list of Links - std::map > links_; - /// \brief complete list of Joints - std::map > joints_; - /// \brief complete list of Materials - std::map > materials_; - - /// \brief The name of the robot model - std::string name_; - - /// \brief The root is always a link (the parent of the tree describing the robot) - my_shared_ptr root_link_; - - int m_numLinks;//includes parent - int m_numJoints; - - -}; - -} - -#endif diff --git a/examples/ThirdPartyLibs/urdf/urdfdom_headers/urdf_model/include/urdf_model/pose.h b/examples/ThirdPartyLibs/urdf/urdfdom_headers/urdf_model/include/urdf_model/pose.h deleted file mode 100644 index 93183c8f8..000000000 --- a/examples/ThirdPartyLibs/urdf/urdfdom_headers/urdf_model/include/urdf_model/pose.h +++ /dev/null @@ -1,265 +0,0 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ - -/* Author: Wim Meeussen */ - -#ifndef URDF_INTERFACE_POSE_H -#define URDF_INTERFACE_POSE_H - -#include -//#include -#include -#include -#ifndef M_PI -#define M_PI 3.141592538 -#endif //M_PI - -#ifdef URDF_USE_BOOST - #include - #include -#else - #include - #include -#endif //URDF_USE_BOOST - -#include -#include - -namespace urdf{ - -class Vector3 -{ -public: - Vector3(double _x,double _y, double _z) {this->x=_x;this->y=_y;this->z=_z;}; - Vector3() {this->clear();}; - double x; - double y; - double z; - - void clear() {this->x=this->y=this->z=0.0;}; - void init(const std::string &vector_str) - { - this->clear(); - std::vector pieces; - std::vector xyz; - boost::split( pieces, vector_str, boost::is_any_of(" ")); - for (unsigned int i = 0; i < pieces.size(); ++i){ - if (pieces[i] != ""){ - try { - xyz.push_back(boost::lexical_cast(pieces[i].c_str())); - } - catch (boost::bad_lexical_cast &e) - { - assert(0); - // throw ParseError("Unable to parse component [" + pieces[i] + "] to a double (while parsing a vector value)"); - } - } - } - - - - if (xyz.size() != 3) - { - assert(0); - // throw ParseError("Parser found " + boost::lexical_cast(xyz.size()) + " elements but 3 expected while parsing vector [" + vector_str + "]"); - } - this->x = xyz[0]; - this->y = xyz[1]; - this->z = xyz[2]; - } - - Vector3 operator+(Vector3 vec) - { - return Vector3(this->x+vec.x,this->y+vec.y,this->z+vec.z); - }; -}; - -class Rotation -{ -public: - Rotation(double _x,double _y, double _z, double _w) {this->x=_x;this->y=_y;this->z=_z;this->w=_w;}; - Rotation() {this->clear();}; - void getQuaternion(double &quat_x,double &quat_y,double &quat_z, double &quat_w) const - { - quat_x = this->x; - quat_y = this->y; - quat_z = this->z; - quat_w = this->w; - }; - void getRPY(double &roll,double &pitch,double &yaw) const - { - double sqw; - double sqx; - double sqy; - double sqz; - - sqx = this->x * this->x; - sqy = this->y * this->y; - sqz = this->z * this->z; - sqw = this->w * this->w; - - roll = atan2(2 * (this->y*this->z + this->w*this->x), sqw - sqx - sqy + sqz); - double sarg = -2 * (this->x*this->z - this->w*this->y); - pitch = sarg <= -1.0 ? -0.5*M_PI : (sarg >= 1.0 ? 0.5*M_PI : asin(sarg)); - yaw = atan2(2 * (this->x*this->y + this->w*this->z), sqw + sqx - sqy - sqz); - - }; - void setFromQuaternion(double quat_x,double quat_y,double quat_z,double quat_w) - { - this->x = quat_x; - this->y = quat_y; - this->z = quat_z; - this->w = quat_w; - this->normalize(); - }; - void setFromRPY(double roll, double pitch, double yaw) - { - double phi, the, psi; - - phi = roll / 2.0; - the = pitch / 2.0; - psi = yaw / 2.0; - - this->x = sin(phi) * cos(the) * cos(psi) - cos(phi) * sin(the) * sin(psi); - this->y = cos(phi) * sin(the) * cos(psi) + sin(phi) * cos(the) * sin(psi); - this->z = cos(phi) * cos(the) * sin(psi) - sin(phi) * sin(the) * cos(psi); - this->w = cos(phi) * cos(the) * cos(psi) + sin(phi) * sin(the) * sin(psi); - - this->normalize(); - }; - - double x,y,z,w; - - void init(const std::string &rotation_str) - { - this->clear(); - Vector3 rpy; - rpy.init(rotation_str); - setFromRPY(rpy.x, rpy.y, rpy.z); - } - - void clear() { this->x=this->y=this->z=0.0;this->w=1.0; } - - void normalize() - { - double s = sqrt(this->x * this->x + - this->y * this->y + - this->z * this->z + - this->w * this->w); - if (s == 0.0) - { - this->x = 0.0; - this->y = 0.0; - this->z = 0.0; - this->w = 1.0; - } - else - { - this->x /= s; - this->y /= s; - this->z /= s; - this->w /= s; - } - }; - - // Multiplication operator (copied from gazebo) - Rotation operator*( const Rotation &qt ) const - { - Rotation c; - - c.x = this->w * qt.x + this->x * qt.w + this->y * qt.z - this->z * qt.y; - c.y = this->w * qt.y - this->x * qt.z + this->y * qt.w + this->z * qt.x; - c.z = this->w * qt.z + this->x * qt.y - this->y * qt.x + this->z * qt.w; - c.w = this->w * qt.w - this->x * qt.x - this->y * qt.y - this->z * qt.z; - - return c; - }; - /// Rotate a vector using the quaternion - Vector3 operator*(Vector3 vec) const - { - Rotation tmp; - Vector3 result; - - tmp.w = 0.0; - tmp.x = vec.x; - tmp.y = vec.y; - tmp.z = vec.z; - - tmp = (*this) * (tmp * this->GetInverse()); - - result.x = tmp.x; - result.y = tmp.y; - result.z = tmp.z; - - return result; - }; - // Get the inverse of this quaternion - Rotation GetInverse() const - { - Rotation q; - - double norm = this->w*this->w+this->x*this->x+this->y*this->y+this->z*this->z; - - if (norm > 0.0) - { - q.w = this->w / norm; - q.x = -this->x / norm; - q.y = -this->y / norm; - q.z = -this->z / norm; - } - - return q; - }; - - -}; - -class Pose -{ -public: - Pose() { this->clear(); }; - - Vector3 position; - Rotation rotation; - - void clear() - { - this->position.clear(); - this->rotation.clear(); - }; -}; - -} - -#endif diff --git a/examples/ThirdPartyLibs/urdf/urdfdom_headers/urdf_model/include/urdf_model/twist.h b/examples/ThirdPartyLibs/urdf/urdfdom_headers/urdf_model/include/urdf_model/twist.h deleted file mode 100644 index 5560de34a..000000000 --- a/examples/ThirdPartyLibs/urdf/urdfdom_headers/urdf_model/include/urdf_model/twist.h +++ /dev/null @@ -1,68 +0,0 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ - -/* Author: John Hsu */ - -#ifndef URDF_TWIST_H -#define URDF_TWIST_H - -#include -#include -#include -#include -#include - -namespace urdf{ - - -class Twist -{ -public: - Twist() { this->clear(); }; - - Vector3 linear; - // Angular velocity represented by Euler angles - Vector3 angular; - - void clear() - { - this->linear.clear(); - this->angular.clear(); - }; -}; - -} - -#endif - diff --git a/examples/ThirdPartyLibs/urdf/urdfdom_headers/urdf_model_state/include/urdf_model_state/model_state.h b/examples/ThirdPartyLibs/urdf/urdfdom_headers/urdf_model_state/include/urdf_model_state/model_state.h deleted file mode 100644 index b1327191a..000000000 --- a/examples/ThirdPartyLibs/urdf/urdfdom_headers/urdf_model_state/include/urdf_model_state/model_state.h +++ /dev/null @@ -1,141 +0,0 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ - -/* Author: John Hsu */ - -#ifndef URDF_MODEL_STATE_H -#define URDF_MODEL_STATE_H - -#include -#include -#include -#include -#include - -#include "urdf_model/pose.h" -#include - - -namespace urdf{ - -class Time -{ -public: - Time() { this->clear(); }; - - void set(double _seconds) - { - this->sec = (int32_t)(floor(_seconds)); - this->nsec = (int32_t)(round((_seconds - this->sec) * 1e9)); - this->Correct(); - }; - - operator double () - { - return (static_cast(this->sec) + - static_cast(this->nsec)*1e-9); - }; - - int32_t sec; - int32_t nsec; - - void clear() - { - this->sec = 0; - this->nsec = 0; - }; -private: - void Correct() - { - // Make any corrections - if (this->nsec >= 1e9) - { - this->sec++; - this->nsec = (int32_t)(this->nsec - 1e9); - } - else if (this->nsec < 0) - { - this->sec--; - this->nsec = (int32_t)(this->nsec + 1e9); - } - }; -}; - - -class JointState -{ -public: - JointState() { this->clear(); }; - - /// joint name - std::string joint; - - std::vector position; - std::vector velocity; - std::vector effort; - - void clear() - { - this->joint.clear(); - this->position.clear(); - this->velocity.clear(); - this->effort.clear(); - } -}; - -class ModelState -{ -public: - ModelState() { this->clear(); }; - - /// state name must be unique - std::string name; - - Time time_stamp; - - void clear() - { - this->name.clear(); - this->time_stamp.set(0); - this->joint_states.clear(); - }; - - std::vector > joint_states; - -}; - -} - -#endif - diff --git a/examples/ThirdPartyLibs/urdf/urdfdom_headers/urdf_model_state/include/urdf_model_state/twist.h b/examples/ThirdPartyLibs/urdf/urdfdom_headers/urdf_model_state/include/urdf_model_state/twist.h deleted file mode 100644 index 05f1917b9..000000000 --- a/examples/ThirdPartyLibs/urdf/urdfdom_headers/urdf_model_state/include/urdf_model_state/twist.h +++ /dev/null @@ -1,42 +0,0 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ - -#ifndef URDF_MODEL_STATE_TWIST_ -#define URDF_MODEL_STATE_TWIST_ - -#warning "Please Use #include " - -#include - -#endif diff --git a/examples/ThirdPartyLibs/urdf/urdfdom_headers/urdf_sensor/include/urdf_sensor/sensor.h b/examples/ThirdPartyLibs/urdf/urdfdom_headers/urdf_sensor/include/urdf_sensor/sensor.h deleted file mode 100644 index 3b996957b..000000000 --- a/examples/ThirdPartyLibs/urdf/urdfdom_headers/urdf_sensor/include/urdf_sensor/sensor.h +++ /dev/null @@ -1,176 +0,0 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ - -/* Author: John Hsu */ - -/* example - - - - - 1.5708 - - - - - - - - - - - - - - -*/ - - - -#ifndef URDF_SENSOR_H -#define URDF_SENSOR_H - -#include -#include -#include -#include -#include -#include "urdf_model/pose.h" -#include "urdf_model/joint.h" -#include "urdf_model/link.h" - -namespace urdf{ - -class VisualSensor -{ -public: - enum {CAMERA, RAY} type; - virtual ~VisualSensor(void) - { - } -}; - -class Camera : public VisualSensor -{ -public: - Camera() { this->clear(); }; - unsigned int width, height; - /// format is optional: defaults to R8G8B8), but can be - /// (L8|R8G8B8|B8G8R8|BAYER_RGGB8|BAYER_BGGR8|BAYER_GBRG8|BAYER_GRBG8) - std::string format; - double hfov; - double near; - double far; - - void clear() - { - hfov = 0; - width = 0; - height = 0; - format.clear(); - near = 0; - far = 0; - }; -}; - -class Ray : public VisualSensor -{ -public: - Ray() { this->clear(); }; - unsigned int horizontal_samples; - double horizontal_resolution; - double horizontal_min_angle; - double horizontal_max_angle; - unsigned int vertical_samples; - double vertical_resolution; - double vertical_min_angle; - double vertical_max_angle; - - void clear() - { - // set defaults - horizontal_samples = 1; - horizontal_resolution = 1; - horizontal_min_angle = 0; - horizontal_max_angle = 0; - vertical_samples = 1; - vertical_resolution = 1; - vertical_min_angle = 0; - vertical_max_angle = 0; - }; -}; - - -class Sensor -{ -public: - Sensor() { this->clear(); }; - - /// sensor name must be unique - std::string name; - - /// update rate in Hz - double update_rate; - - /// transform from parent frame to optical center - /// with z-forward and x-right, y-down - Pose origin; - - /// sensor - boost::shared_ptr sensor; - - - /// Parent link element name. A pointer is stored in parent_link_. - std::string parent_link_name; - - boost::shared_ptr getParent() const - {return parent_link_.lock();}; - - void setParent(boost::shared_ptr parent) - { this->parent_link_ = parent; } - - void clear() - { - this->name.clear(); - this->sensor.reset(); - this->parent_link_name.clear(); - this->parent_link_.reset(); - }; - -private: - boost::weak_ptr parent_link_; - -}; -} -#endif diff --git a/examples/ThirdPartyLibs/urdf/urdfdom_headers/urdf_world/include/urdf_world/world.h b/examples/ThirdPartyLibs/urdf/urdfdom_headers/urdf_world/include/urdf_world/world.h deleted file mode 100644 index eb13fc4b1..000000000 --- a/examples/ThirdPartyLibs/urdf/urdfdom_headers/urdf_world/include/urdf_world/world.h +++ /dev/null @@ -1,114 +0,0 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ - -/* Author: John Hsu */ - -/* encapsulates components in a world - see http://ros.org/wiki/usdf/XML/urdf_world and - for details -*/ -/* example world XML - - - - - ... - - - - - - - - - - - - - - - - - - - -*/ - -#ifndef USDF_STATE_H -#define USDF_STATE_H - -#include -#include -#include -#include -#include -#include - -#include "urdf_model/model.h" -#include "urdf_model/pose.h" -#include "urdf_model/twist.h" - -namespace urdf{ - -class Entity -{ -public: - boost::shared_ptr model; - Pose origin; - Twist twist; -}; - -class World -{ -public: - World() { this->clear(); }; - - /// world name must be unique - std::string name; - - std::vector models; - - void initXml(TiXmlElement* config); - - void clear() - { - this->name.clear(); - }; -}; -} - -#endif - diff --git a/examples/TinyRenderer/Bullet2TinyRenderer.cpp b/examples/TinyRenderer/Bullet2TinyRenderer.cpp new file mode 100644 index 000000000..de4e0a758 --- /dev/null +++ b/examples/TinyRenderer/Bullet2TinyRenderer.cpp @@ -0,0 +1,164 @@ + + +#include "TinyRenderer.h" +#include +#include "LinearMath/btHashMap.h" +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" +#include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h" +#include "BulletCollision/CollisionShapes/btCollisionShape.h" + +class Bullet2TinyRenderer +{ + + btAlignedObjectArray m_swRenderObjects; + + btHashMap m_colObject2gfxIndex; + btHashMap m_colShape2gfxIndex; + + + int m_swWidth; + int m_swHeight; + TGAImage m_rgbColorBuffer; + + b3AlignedObjectArray m_depthBuffer; + int m_textureHandle; + unsigned char* m_image; + + +public: + Bullet2TinyRenderer (int swWidth, int swHeight): + m_swWidth(swWidth), + m_swHeight(swHeight), + m_rgbColorBuffer(swWidth,swHeight,TGAImage::RGB) + { + + m_depthBuffer.resize(swWidth*swHeight); + m_image=new unsigned char[m_swWidth*m_swHeight*4]; + + } + + virtual ~Bullet2TinyRenderer() + { + for (int i=0;igetCollisionShape()]; + + + + //if (colIndex>=0 && shapeIndex>=0) + //{ + // m_col2gfxIndex.insert(colIndex,shapeIndex); + //} + + } + + /* virtual int registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices) + { + + + if (shapeIndex>=0) + { + TinyRenderObjectData* swObj = new TinyRenderObjectData(m_swWidth,m_swHeight,m_rgbColorBuffer,m_depthBuffer); + swObj->registerMeshShape(vertices,numvertices,indices,numIndices); + //swObj->createCube(1,1,1); + m_swRenderObjects.insert(shapeIndex,swObj); + } + return shapeIndex; + } +*/ + virtual void render(const btDiscreteDynamicsWorld* rbWorld, float viewMat[16]) + { + //clear the color buffer + TGAColor clearColor; + clearColor.bgra[0] = 255; + clearColor.bgra[1] = 255; + clearColor.bgra[2] = 255; + clearColor.bgra[3] = 255; + + clearBuffers(clearColor); + + ATTRIBUTE_ALIGNED16(float modelMat[16]); + + + for (int i=0;igetNumCollisionObjects();i++) + { + btCollisionObject* colObj = rbWorld->getCollisionObjectArray()[i]; + + int* colObjIndexPtr = m_colObject2gfxIndex[colObj]; + + if (colObjIndexPtr) + { + int colObjIndex = *colObjIndexPtr; + TinyRenderObjectData* renderObj = m_swRenderObjects[colObjIndex]; + + //sync the object transform + const btTransform& tr = colObj->getWorldTransform(); + tr.getOpenGLMatrix(modelMat); + + for (int i=0;i<4;i++) + { + for (int j=0;j<4;j++) + { + renderObj->m_modelMatrix[i][j] = modelMat[i+4*j]; + renderObj->m_viewMatrix[i][j] = viewMat[i+4*j]; + } + } + TinyRenderer::renderObject(*renderObj); + } + } + + for(int y=0;y10) + { + counter=0; + getFrameBuffer().write_tga_file("/Users/erwincoumans/develop/bullet3/framebuf.tga",true); + } + + } + +}; + diff --git a/examples/TinyRenderer/Bullet2TinyRenderer.h b/examples/TinyRenderer/Bullet2TinyRenderer.h new file mode 100644 index 000000000..e69de29bb diff --git a/examples/TinyRenderer/TinyRenderer.cpp b/examples/TinyRenderer/TinyRenderer.cpp index 7130f84db..344316996 100644 --- a/examples/TinyRenderer/TinyRenderer.cpp +++ b/examples/TinyRenderer/TinyRenderer.cpp @@ -10,7 +10,8 @@ #include "../Utils/b3ResourcePath.h" #include "Bullet3Common/b3MinMax.h" #include "../OpenGLWindow/ShapeData.h" - +#include "LinearMath/btAlignedObjectArray.h" +#include "LinearMath/btVector3.h" Vec3f light_dir_world(1,1,1); @@ -142,6 +143,40 @@ void TinyRenderObjectData::registerMeshShape(const float* vertices, int numVerti } } } + +void TinyRenderObjectData::registerMesh2(btAlignedObjectArray& vertices, btAlignedObjectArray& normals,btAlignedObjectArray& indices) +{ + if (0==m_model) + { + int numVertices = vertices.size(); + int numIndices = indices.size(); + + m_model = new Model(); + char relativeFileName[1024]; + if (b3ResourcePath::findResourcePath("floor_diffuse.tga", relativeFileName, 1024)) + { + m_model->loadDiffuseTexture(relativeFileName); + } + + for (int i=0;iaddVertex(vertices[i].x(), + vertices[i].y(), + vertices[i].z(), + normals[i].x(), + normals[i].y(), + normals[i].z(), + 0.5,0.5); + } + for (int i=0;iaddTriangle(indices[i],indices[i],indices[i], + indices[i+1],indices[i+1],indices[i+1], + indices[i+2],indices[i+2],indices[i+2]); + } + } +} + void TinyRenderObjectData::createCube(float halfExtentsX,float halfExtentsY,float halfExtentsZ) { m_model = new Model(); diff --git a/examples/TinyRenderer/TinyRenderer.h b/examples/TinyRenderer/TinyRenderer.h index 86514b545..b076119fc 100644 --- a/examples/TinyRenderer/TinyRenderer.h +++ b/examples/TinyRenderer/TinyRenderer.h @@ -3,6 +3,11 @@ #include "geometry.h" #include "Bullet3Common/b3AlignedObjectArray.h" +#include "Bullet3Common/b3Vector3.h" +#include "LinearMath/btAlignedObjectArray.h" +#include "LinearMath/btVector3.h" + + #include "tgaimage.h" struct TinyRenderObjectData @@ -29,6 +34,8 @@ struct TinyRenderObjectData void loadModel(const char* fileName); void createCube(float HalfExtentsX,float HalfExtentsY,float HalfExtentsZ); void registerMeshShape(const float* vertices, int numVertices,const int* indices, int numIndices); + + void registerMesh2(btAlignedObjectArray& vertices, btAlignedObjectArray& normals,btAlignedObjectArray& indices); void* m_userData; int m_userIndex; diff --git a/examples/TinyRenderer/main.cpp b/examples/TinyRenderer/main.cpp index 4f50e7227..9995b8f66 100644 --- a/examples/TinyRenderer/main.cpp +++ b/examples/TinyRenderer/main.cpp @@ -89,9 +89,9 @@ int main(int argc, char* argv[]) TinyRenderObjectData renderData(textureWidth, textureHeight,rgbColorBuffer,depthBuffer);//, "african_head/african_head.obj");//floor.obj"); //renderData.loadModel("african_head/african_head.obj"); - //renderData.loadModel("floor.obj"); + renderData.loadModel("floor.obj"); - renderData.createCube(1,1,1); + //renderData.createCube(1,1,1); myArgs.GetCmdLineArgument("mp4_file",gVideoFileName); diff --git a/examples/TinyRenderer/model.cpp b/examples/TinyRenderer/model.cpp index f52b01126..36a955acc 100644 --- a/examples/TinyRenderer/model.cpp +++ b/examples/TinyRenderer/model.cpp @@ -1,3 +1,4 @@ + #include #include #include