AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-14 13:50:04 +00:00
Code Issues Releases Wiki Activity

Merge branch 'master' of https://github.com/erwincoumans/bullet3

Browse Source
This commit is contained in:
Erwin Coumans 2016-05-12 23:13:44 -07:00
commit a725d59480
64 changed files with 1027 additions and 6101 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
examples/BasicDemo/CMakeLists.txt
View File

@ -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'

2
examples/BasicDemo/premake4.lua
View File

@ -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",

25
examples/CommonInterfaces/CommonExampleInterface.h
View File

@ -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

206
examples/ExampleBrowser/CMakeLists.txt
View File

@ -7,6 +7,107 @@ INCLUDE_DIRECTORIES(
FILE(GLOB GwenGUISupport_SRCS "GwenGUISupport/*" )
FILE(GLOB GwenGUISupport_HDRS "GwenGUISupport/*" )
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
@ -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" )

192
examples/ExampleBrowser/CollisionShape2TriangleMesh.cpp Normal file
View File

@ -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<btVector3>& vertexPositions, btAlignedObjectArray<btVector3>& vertexNormals, btAlignedObjectArray<int>& 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<const btStaticPlaneShape*>(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<btVector3> vertices;
btAlignedObjectArray<int> indices;
for (int partId=0;partId<meshInterface->getNumSubParts();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;t<hull->numTriangles();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;i<compound->getNumChildShapes();i++)
{
btTransform childWorldTrans = parentTransform * compound->getChildTransform(i);
CollisionShape2TriangleMesh(compound->getChildShape(i),childWorldTrans,vertexPositions,vertexNormals,indicesOut);
}
} else
{
btAssert(0);
}
}
}
};
}

10
examples/ExampleBrowser/CollisionShape2TriangleMesh.h Normal file
View File

@ -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<btVector3>& vertexPositions, btAlignedObjectArray<btVector3>& vertexNormals, btAlignedObjectArray<int>& indicesOut);
#endif //COLLISION_SHAPE_2_GRAPHICS_H

5
examples/ExampleBrowser/ExampleEntries.cpp
View File

@ -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[]=
{
@ -225,6 +228,7 @@ static ExampleEntry gDefaultExamples[]=
RayTracerCreateFunc),
ExampleEntry(0,"Experiments"),
ExampleEntry(1,"Physics Server", "Create a physics server that communicates with a physics client over shared memory",
@ -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"),

25
examples/ExampleBrowser/ExampleEntries.h
View File

@ -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
{

1
examples/ExampleBrowser/InProcessExampleBrowser.cpp
View File

@ -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),
};

34
examples/ExampleBrowser/OpenGLExampleBrowser.cpp
View File

@ -256,6 +256,21 @@ static void MyMouseButtonCallback(int button, int state, float x, float y)
}
#include <string.h>
struct FileImporterByExtension
{
std::string m_extension;
CommonExampleInterface::CreateFunc* m_createFunc;
};
static btAlignedObjectArray<FileImporterByExtension> 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,19 +294,14 @@ 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);
}
}
//physicsSetup->setFileName(filename);
for (int i=0;i<gFileImporterByExtension.size();i++)
{
if (strstr(fullPath, gFileImporterByExtension[i].m_extension.c_str()))
{
sCurrentDemo = gFileImporterByExtension[i].m_createFunc(options);
}
}
if (sCurrentDemo)

1
examples/ExampleBrowser/OpenGLExampleBrowser.h
View File

@ -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

251
examples/ExampleBrowser/OpenGLGuiHelper.cpp
View File

@ -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<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& 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<const btStaticPlaneShape*>(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<btVector3> vertices;
btAlignedObjectArray<int> indices;
for (int partId=0;partId<meshInterface->getNumSubParts();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;t<hull->numTriangles();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;i<compound->getNumChildShapes();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<GLInstanceVertex> vertices;
btAlignedObjectArray<GLInstanceVertex> gfxVertices;
btAlignedObjectArray<int> indices;
btTransform startTrans;startTrans.setIdentity();
createCollisionShapeGraphicsObjectInternal(collisionShape,startTrans,vertices,indices);
{
btAlignedObjectArray<btVector3> vertexPositions;
btAlignedObjectArray<btVector3> vertexNormals;
CollisionShape2TriangleMesh(collisionShape,startTrans,vertexPositions,vertexNormals,indices);
gfxVertices.resize(vertexPositions.size());
for (int i=0;i<vertexPositions.size();i++)
{
for (int j=0;j<4;j++)
{
gfxVertices[i].xyzw[j] = vertexPositions[i][j];
}
for (int j=0;j<3;j++)
{
gfxVertices[i].normal[j] = vertexNormals[i][j];
}
for (int j=0;j<2;j++)
{
gfxVertices[i].uv[j] = 0.5;//we don't have UV info...
}
if (vertices.size() && indices.size())
}
}
if (gfxVertices.size() && indices.size())
{
int shapeId = registerGraphicsShape(&vertices[0].xyzw[0],vertices.size(),&indices[0],indices.size());
int shapeId = registerGraphicsShape(&gfxVertices[0].xyzw[0],gfxVertices.size(),&indices[0],indices.size());
collisionShape->setUserIndex(shapeId);
}

21
examples/ExampleBrowser/main.cpp
View File

@ -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)
{

198
examples/ExampleBrowser/premake4.lua
View File

@ -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()

28
examples/Importers/ImportObjDemo/ImportObjExample.cpp
View File

@ -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);
}

2
examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp
View File

@ -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;

11
examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp
View File

@ -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);

905
examples/Importers/ImportURDFDemo/ROSURDFImporter.cpp
View File

@ -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 <string>
#include "urdf/urdfdom/urdf_parser/include/urdf_parser/urdf_parser.h"
#include <iostream>
#include <fstream>
using namespace urdf;
void ROSconvertURDFToVisualShape(const Visual* visual, const char* pathPrefix, const btTransform& visualTransform, btAlignedObjectArray<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut);
btCollisionShape* ROSconvertURDFToCollisionShape(const Collision* visual, const char* pathPrefix);
static void ROSprintTreeInternal(my_shared_ptr<const Link> link,int level = 0)
{
level+=2;
int count = 0;
for (std::vector<my_shared_ptr<Link> >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++)
{
if (*child)
{
for(int j=0;j<level;j++) std::cout << " "; //indent
std::cout << "child(" << (count++)+1 << "): " << (*child)->name << std::endl;
// first grandchild
ROSprintTreeInternal(*child,level);
}
else
{
for(int j=0;j<level;j++) std::cout << " "; //indent
std::cout << "root link: " << link->name << " has a null child!" << *child << std::endl;
}
}
}
struct ROSURDFInternalData
{
my_shared_ptr<ModelInterface> m_robot;
std::vector<my_shared_ptr<Link> > 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<ModelInterface> 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<const Link> 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<int>& childLinkIndices) const
{
childLinkIndices.resize(0);
int numChildren = m_data->m_links[linkIndex]->child_links.size();
for (int i=0;i<numChildren;i++)
{
int childIndex =m_data->m_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<Joint> 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<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& 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<btVector3> vertices;
//int numVerts = sizeof(barrel_vertices)/(9*sizeof(float));
int numSteps = 32;
for (int i = 0; i<numSteps; i++)
{
btVector3 vert(cyl->radius*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<GLInstanceGraphicsShape> visualShapes;
btAlignedObjectArray<ColladaGraphicsInstance> 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<int>();
glmesh->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>();
for (int i = 0; i<visualShapeInstances.size(); i++)
{
ColladaGraphicsInstance* instance = &visualShapeInstances[i];
GLInstanceGraphicsShape* gfxShape = &visualShapes[instance->m_shapeIndex];
b3AlignedObjectArray<GLInstanceVertex> verts;
verts.resize(gfxShape->m_vertices->size());
int baseIndex = glmesh->m_vertices->size();
for (int i = 0; i<gfxShape->m_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; k<additionalIndices; k++)
{
glmesh->m_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; v<verts.size(); v++)
{
btVector3 pos(verts[v].xyzw[0], verts[v].xyzw[1], verts[v].xyzw[2]);
pos = worldMat*pos;
verts[v].xyzw[0] = float(pos[0]);
verts[v].xyzw[1] = float(pos[1]);
verts[v].xyzw[2] = float(pos[2]);
glmesh->m_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<int>();
glmesh->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>();
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<int> 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<btVector3> vertices;
//int numVerts = sizeof(barrel_vertices)/(9*sizeof(float));
int numSteps = 32;
for (int i=0;i<numSteps;i++)
{
btVector3 vert(cyl->radius*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<GLInstanceGraphicsShape> visualShapes;
btAlignedObjectArray<ColladaGraphicsInstance> 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<int>();
glmesh->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>();
for (int i=0;i<visualShapeInstances.size();i++)
{
ColladaGraphicsInstance* instance = &visualShapeInstances[i];
GLInstanceGraphicsShape* gfxShape = &visualShapes[instance->m_shapeIndex];
b3AlignedObjectArray<GLInstanceVertex> verts;
verts.resize(gfxShape->m_vertices->size());
int baseIndex = glmesh->m_vertices->size();
for (int i=0;i<gfxShape->m_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;k<additionalIndices;k++)
{
glmesh->m_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;v<verts.size();v++)
{
btVector3 pos(verts[v].xyzw[0],verts[v].xyzw[1],verts[v].xyzw[2]);
pos = worldMat*pos;
verts[v].xyzw[0] = float(pos[0]);
verts[v].xyzw[1] = float(pos[1]);
verts[v].xyzw[2] = float(pos[2]);
glmesh->m_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<btVector3> convertedVerts;
convertedVerts.reserve(glmesh->m_numvertices);
for (int i=0;i<glmesh->m_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<GLInstanceVertex> vertices;
btAlignedObjectArray<int> 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;
}

47
examples/Importers/ImportURDFDemo/ROSURDFImporter.h
View File

@ -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<int>& 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

2
examples/InverseDynamics/InverseDynamicsExample.cpp
View File

@ -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());

2
examples/InverseDynamics/premake4.lua
View File

@ -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",

2
examples/OpenGLWindow/X11OpenGLWindow.cpp
View File

@ -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 )

277
examples/RenderingExamples/TinyRendererSetup.cpp Normal file
View File

@ -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<float> m_depthBuffer;
int m_width;
int m_height;
btAlignedObjectArray<btConvexShape*> m_shapePtr;
btAlignedObjectArray<btTransform> m_transforms;
btAlignedObjectArray<TinyRenderObjectData*> 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<m_shapePtr.size();i++)
{
TinyRenderObjectData* ob = new TinyRenderObjectData(m_width,m_height,m_rgbColorBuffer,m_depthBuffer);
btAlignedObjectArray<btVector3> vertexPositions;
btAlignedObjectArray<btVector3> vertexNormals;
btAlignedObjectArray<int> 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;i<numObjects;i++)
{
m_transforms[i].setIdentity();
btVector3 pos(0.f,0.f,-(2.5* numObjects * 0.5)+i*2.5f);
m_transforms[i].setIdentity();
m_transforms[i].setOrigin( pos );
btQuaternion orn;
if (i < 2)
{
orn.setEuler(m_yaw,m_pitch,m_roll);
m_transforms[i].setRotation(orn);
}
}
m_pitch += 0.005f;
m_yaw += 0.01f;
}
};
TinyRendererSetup::TinyRendererSetup(struct CommonGraphicsApp* app)
{
m_app = app;
m_internalData = new TinyRendererSetupInternalData(128,128);
}
TinyRendererSetup::~TinyRendererSetup()
{
delete m_internalData;
}
void TinyRendererSetup::initPhysics()
{
//request a visual bitma/texture we can render to
m_internalData->m_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;i<m_internalData->m_width;i++)
{
for (int j=0;j<m_internalData->m_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;y<m_internalData->m_height;++y)
{
for(int x=0;x<m_internalData->m_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;o<this->m_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;y<m_internalData->m_height;++y)
{
for(int x=0;x<m_internalData->m_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());
}

6
examples/RenderingExamples/TinyRendererSetup.h Normal file
View File

@ -0,0 +1,6 @@
#ifndef TINYRENDERER_SETUP_H
#define TINYRENDERER_SETUP_H
class CommonExampleInterface* TinyRendererCreateFunc(struct CommonExampleOptions& options);
#endif //TINYRENDERER_SETUP_H

16
examples/SharedMemory/premake4.lua
View File

@ -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",
}

27
examples/ThirdPartyLibs/urdf/boost_replacement/lexical_cast.h
View File

@ -1,27 +0,0 @@
#ifndef BOOST_REPLACEMENT_LEXICAL_CAST_H
#define BOOST_REPLACEMENT_LEXICAL_CAST_H
#include <stdlib.h>
namespace boost
{
template <typename T> 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

28
examples/ThirdPartyLibs/urdf/boost_replacement/printf_console.cpp
View File

@ -1,28 +0,0 @@
#include "printf_console.h"
#include <stdio.h>
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);
}

13
examples/ThirdPartyLibs/urdf/boost_replacement/printf_console.h
View File

@ -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

210
examples/ThirdPartyLibs/urdf/boost_replacement/shared_ptr.h
View File

@ -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 <nicola@fluidinteractive.com>
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 <assert.h>
#ifdef _WIN32
#include <windows.h>
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 <pthread.h>
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<typename T>
class my_shared_ptr
{
public:
my_shared_ptr(): m_ptr(NULL), m_count(NULL) { }
my_shared_ptr(my_shared_ptr<T> const& other):
m_ptr(other.m_ptr),
m_count(other.m_count)
{
if(other.m_count != NULL) other.m_count->increment();
}
template<typename U>
my_shared_ptr(my_shared_ptr<U> 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<T*>(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<T*>(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<T> const& rhs) const { return m_ptr < rhs.m_ptr; }
my_shared_ptr<T>& operator=(my_shared_ptr<T> 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<typename U>
my_shared_ptr<T>& operator=(my_shared_ptr<U>& 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<typename U> 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

253
examples/ThirdPartyLibs/urdf/boost_replacement/string_split.cpp
View File

@ -1,253 +0,0 @@
#include <assert.h>
//#include <stdbool.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "string_split.h"
namespace boost
{
void split( std::vector<std::string>&pieces, const std::string& vector_str, std::vector<std::string> 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<numSubStr;i++)
pieces.push_back(std::string(strArray[i]));
str_array_free(strArray);
}
}
std::vector<std::string> is_any_of(const char* seps)
{
std::vector<std::string> strArray;
int numSeps = strlen(seps);
for (int i=0;i<numSeps;i++)
{
char sep2[2] = {0,0};
sep2[0] = seps[i];
strArray.push_back(sep2);
}
return strArray;
}
};
/* Append an item to a dynamically allocated array of strings. On failure,
return NULL, in which case the original array is intact. The item
string is dynamically copied. If the array is NULL, allocate a new
array. Otherwise, extend the array. Make sure the array is always
NULL-terminated. Input string might not be '\0'-terminated. */
char **str_array_append(char **array, size_t nitems, const char *item,
size_t itemlen)
{
/* Make a dynamic copy of the item. */
char *copy;
if (item == NULL)
copy = NULL;
else {
copy = (char*)malloc(itemlen + 1);
if (copy == NULL)
return NULL;
memcpy(copy, item, itemlen);
copy[itemlen] = '\0';
}
/* Extend array with one element. Except extend it by two elements,
in case it did not yet exist. This might mean it is a teeny bit
too big, but we don't care. */
array = (char**)realloc(array, (nitems + 2) * sizeof(array[0]));
if (array == NULL) {
free(copy);
return NULL;
}
/* Add copy of item to array, and return it. */
array[nitems] = copy;
array[nitems+1] = NULL;
return array;
}
/* Free a dynamic array of dynamic strings. */
void str_array_free(char **array)
{
if (array == NULL)
return;
for (size_t i = 0; array[i] != NULL; ++i)
free(array[i]);
free(array);
}
/* 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)
{
size_t nitems = 0;
char **array = NULL;
const char *start = input;
const char *next = strstr(start, sep);
size_t seplen = strlen(sep);
const char *item;
size_t itemlen;
for (;;) {
next = strstr(start, sep);
if (next == NULL) {
/* Add the remaining string (or empty string, if input ends with
separator. */
char **newstr = str_array_append(array, nitems, start, strlen(start));
if (newstr == NULL) {
str_array_free(array);
return NULL;
}
array = newstr;
++nitems;
break;
} else if (next == input) {
/* Input starts with separator. */
item = "";
itemlen = 0;
} else {
item = start;
itemlen = next - item;
}
char **newstr = str_array_append(array, nitems, item, itemlen);
if (newstr == NULL) {
str_array_free(array);
return NULL;
}
array = newstr;
++nitems;
start = next + seplen;
}
if (nitems == 0) {
/* Input does not contain separator at all. */
assert(array == NULL);
array = str_array_append(array, nitems, input, strlen(input));
}
return array;
}
/* Return length of a NULL-delimited array of strings. */
size_t str_array_len(char **array)
{
size_t len;
for (len = 0; array[len] != NULL; ++len)
continue;
return len;
}
#ifdef UNIT_TEST_STRING
#define MAX_OUTPUT 20
int main(void)
{
struct {
const char *input;
const char *sep;
char *output[MAX_OUTPUT];
} tab[] = {
/* Input is empty string. Output should be a list with an empty
string. */
{
"",
"and",
{
"",
NULL,
},
},
/* Input is exactly the separator. Output should be two empty
strings. */
{
"and",
"and",
{
"",
"",
NULL,
},
},
/* Input is non-empty, but does not have separator. Output should
be the same string. */
{
"foo",
"and",
{
"foo",
NULL,
},
},
/* Input is non-empty, and does have separator. */
{
"foo bar 1 and foo bar 2",
" and ",
{
"foo bar 1",
"foo bar 2",
NULL,
},
},
};
const int tab_len = sizeof(tab) / sizeof(tab[0]);
bool errors;
errors = false;
for (int i = 0; i < tab_len; ++i) {
printf("test %d\n", i);
char **output = str_split(tab[i].input, tab[i].sep);
if (output == NULL) {
fprintf(stderr, "output is NULL\n");
errors = true;
break;
}
size_t num_output = str_array_len(output);
printf("num_output %lu\n", (unsigned long) num_output);
size_t num_correct = str_array_len(tab[i].output);
if (num_output != num_correct) {
fprintf(stderr, "wrong number of outputs (%lu, not %lu)\n",
(unsigned long) num_output, (unsigned long) num_correct);
errors = true;
} else {
for (size_t j = 0; j < num_output; ++j) {
if (strcmp(tab[i].output[j], output[j]) != 0) {
fprintf(stderr, "output[%lu] is '%s' not '%s'\n",
(unsigned long) j, output[j], tab[i].output[j]);
errors = true;
break;
}
}
}
str_array_free(output);
printf("\n");
}
if (errors)
return EXIT_FAILURE;
return 0;
}
#endif//

31
examples/ThirdPartyLibs/urdf/boost_replacement/string_split.h
View File

@ -1,31 +0,0 @@
#ifndef STRING_SPLIT_H
#define STRING_SPLIT_H
#include <cstring>
#include <vector>
#include <string>
namespace boost
{
void split( std::vector<std::string>&pieces, const std::string& vector_str, std::vector<std::string> separators);
std::vector<std::string> 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

46
examples/ThirdPartyLibs/urdf/premake4.lua
View File

@ -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",
}

15
examples/ThirdPartyLibs/urdf/urdfdom/LICENSE
View File

@ -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.

7
examples/ThirdPartyLibs/urdf/urdfdom/README.txt
View File

@ -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

63
examples/ThirdPartyLibs/urdf/urdfdom/urdf_parser/include/urdf_parser/urdf_parser.h
View File

@ -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 <string>
#include <map>
#include "tinyxml/tinyxml.h"
//#include <boost/function.hpp>
#ifndef M_PI
#define M_PI 3.1415925438
#endif //M_PI
#include <urdf/urdfdom_headers/urdf_model/include/urdf_model/model.h>
namespace urdf{
my_shared_ptr<ModelInterface> parseURDF(const std::string &xml_string);
}
#endif

137
examples/ThirdPartyLibs/urdf/urdfdom/urdf_parser/src/check_urdf.cpp
View File

@ -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 <iostream>
#include <fstream>
using namespace urdf;
void printTree(my_shared_ptr<const Link> link,int level = 0)
{
level+=2;
int count = 0;
for (std::vector<my_shared_ptr<Link> >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++)
{
if (*child)
{
for(int j=0;j<level;j++) std::cout << " "; //indent
std::cout << "child(" << (count++)+1 << "): " << (*child)->name << std::endl;
// first grandchild
printTree(*child,level);
}
else
{
for(int j=0;j<level;j++) std::cout << " "; //indent
std::cout << "root link: " << link->name << " has a null child!" << *child << std::endl;
}
}
}
#define MSTRINGIFY(A) #A
const char* urdf_char = MSTRINGIFY(
<robot name="test_robot">
<link name="link1" />
<link name="link2" />
<link name="link3" />
<link name="link4" />
<joint name="joint1" type="continuous">
<parent link="link1"/>
<child link="link2"/>
</joint>
<joint name="joint2" type="continuous">
<parent link="link1"/>
<child link="link3"/>
</joint>
<joint name="joint3" type="continuous">
<parent link="link3"/>
<child link="link4"/>
</joint>
</robot>);
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<ModelInterface> 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<const Link> 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;
}

579
examples/ThirdPartyLibs/urdf/urdfdom/urdf_parser/src/joint.cpp
View File

@ -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 <sstream>
#include <urdf/urdfdom_headers/urdf_model/include/urdf_model/joint.h>
#ifdef URDF_USE_BOOST
#include <boost/lexical_cast.hpp>
#else
#include <urdf/boost_replacement/lexical_cast.h>
#endif
#include <urdf/urdfdom_headers/urdf_model/include/urdf_model/pose.h>
#ifdef URDF_USE_CONSOLE_BRIDGE
#include <console_bridge/console.h>
#else
#include "urdf/boost_replacement/printf_console.h"
#endif
#include <tinyxml/tinyxml.h>
#include <urdf/urdfdom/urdf_parser/include/urdf_parser/urdf_parser.h>
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<double>(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<double>(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<double>(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<double>(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<double>(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<double>(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<double>(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<double>(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<double>(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<double>(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<double>(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<double>(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<double>(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<double>(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;
}
}

506
examples/ThirdPartyLibs/urdf/urdfdom/urdf_parser/src/link.cpp
View File

@ -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 <urdf/urdfdom/urdf_parser/include/urdf_parser/urdf_parser.h>
#include <urdf/urdfdom_headers/urdf_model/include/urdf_model/link.h>
//#include <fstream>
//#include <sstream>
#ifdef URDF_USE_BOOST
#include <boost/lexical_cast.hpp>
#else
#include <urdf/boost_replacement/lexical_cast.h>
#endif
#include <algorithm>
#include <tinyxml/tinyxml.h>
#ifdef URDF_USE_CONSOLE_BRIDGE
#include <console_bridge/console.h>
#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<double>(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<double>(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<double>(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<Geometry> parseGeometry(TiXmlElement *g)
{
my_shared_ptr<Geometry> 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<Geometry>();
}
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<double>(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<double>(inertia_xml->Attribute("ixx"));
i.ixy = boost::lexical_cast<double>(inertia_xml->Attribute("ixy"));
i.ixz = boost::lexical_cast<double>(inertia_xml->Attribute("ixz"));
i.iyy = boost::lexical_cast<double>(inertia_xml->Attribute("iyy"));
i.iyz = boost::lexical_cast<double>(inertia_xml->Attribute("iyz"));
i.izz = boost::lexical_cast<double>(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<Visual> 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<Collision> 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;
}
}

240
examples/ThirdPartyLibs/urdf/urdfdom/urdf_parser/src/model.cpp
View File

@ -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 <boost/algorithm/string.hpp>
#include <vector>
#include "urdf/urdfdom/urdf_parser/include/urdf_parser/urdf_parser.h"
#ifdef URDF_USE_CONSOLE_BRIDGE
#include <console_bridge/console.h>
#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<ModelInterface> parseURDF(const std::string &xml_string)
{
my_shared_ptr<ModelInterface> 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;
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;
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;
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<std::string, std::string> 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;
}
}

91
examples/ThirdPartyLibs/urdf/urdfdom/urdf_parser/src/pose.cpp
View File

@ -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 <urdf/urdfdom_headers/urdf_model/include/urdf_model/pose.h>
#include <fstream>
#include <sstream>
//#include <boost/lexical_cast.hpp>
#include <algorithm>
#ifdef URDF_USE_CONSOLE_BRIDGE
#include <console_bridge/console.h>
#else
#include "urdf/boost_replacement/printf_console.h"
#endif
#include <tinyxml/tinyxml.h>
#include <urdf/urdfdom_headers/urdf_exception/include/urdf_exception/exception.h>
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;
}
}

85
examples/ThirdPartyLibs/urdf/urdfdom/urdf_parser/src/twist.cpp
View File

@ -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 <urdf/urdfdom_headers/urdf_model/include/urdf_model/twist.h>
#include <fstream>
#include <sstream>
#include <boost/lexical_cast.hpp>
#include <algorithm>
#include <tinyxml.h>
#include <console_bridge/console.h>
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;
}
}

154
examples/ThirdPartyLibs/urdf/urdfdom/urdf_parser/src/urdf_model_state.cpp
View File

@ -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 <urdf/urdfdom_headers/urdf_model_state/include/urdf_model_state/model_state.h>
#include <fstream>
#include <sstream>
#include <boost/lexical_cast.hpp>
#include <algorithm>
#include <tinyxml.h>
#include <console_bridge/console.h>
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<double>(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<JointState> 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<std::string> 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<double>(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<std::string> 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<double>(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<std::string> 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<double>(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);
}
};
}

364
examples/ThirdPartyLibs/urdf/urdfdom/urdf_parser/src/urdf_sensor.cpp
View File

@ -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 <urdf/urdfdom_headers/urdf_sensor/include/urdf_sensor/sensor.h>
#include <fstream>
#include <sstream>
#include <boost/lexical_cast.hpp>
#include <algorithm>
#include <tinyxml.h>
#include <console_bridge/console.h>
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<unsigned int>(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<unsigned int>(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<double>(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<double>(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<double>(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 <image> 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<unsigned int>(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<double>(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<double>(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<double>(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<unsigned int>(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<double>(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<double>(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<double>(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<VisualSensor> parseVisualSensor(TiXmlElement *g)
{
boost::shared_ptr<VisualSensor> 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 <sensor> 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;
}
}

71
examples/ThirdPartyLibs/urdf/urdfdom/urdf_parser/src/world.cpp
View File

@ -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 <urdf/urdfdom_headers/urdf_world/include/urdf_world/world.h>
#include <urdf/urdfdom_headers/urdf_model/include/urdf_model/model.h>
#include <urdf_parser/urdf_parser.h>
#include <fstream>
#include <sstream>
#include <boost/lexical_cast.hpp>
#include <algorithm>
#include <tinyxml.h>
#include <console_bridge/console.h>
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;
}
}

20
examples/ThirdPartyLibs/urdf/urdfdom/urdf_parser/test/memtest.cpp
View File

@ -1,20 +0,0 @@
#include "urdf_parser/urdf_parser.h"
#include <fstream>
#include <iostream>
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);
}
}

15
examples/ThirdPartyLibs/urdf/urdfdom_headers/LICENSE
View File

@ -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.

6
examples/ThirdPartyLibs/urdf/urdfdom_headers/README.txt
View File

@ -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

53
examples/ThirdPartyLibs/urdf/urdfdom_headers/urdf_exception/include/urdf_exception/exception.h
View File

@ -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 <string>
#include <stdexcept>
namespace urdf
{
class ParseError: public std::runtime_error
{
public:
ParseError(const std::string &error_msg) : std::runtime_error(error_msg) {};
};
}
#endif

101
examples/ThirdPartyLibs/urdf/urdfdom_headers/urdf_model/include/urdf_model/color.h
View File

@ -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 <string>
#include <vector>
#include <math.h>
//#include <boost/algorithm/string.hpp>
//#include <boost/lexical_cast.hpp>
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<std::string> pieces;
std::vector<float> 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<double>(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

234
examples/ThirdPartyLibs/urdf/urdfdom_headers/urdf_model/include/urdf_model/joint.h
View File

@ -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 <string>
#include <vector>
#ifdef URDF_USE_BOOST
#include <boost/shared_ptr.hpp>
#define my_shared_ptr my_shared_ptr
#else
#include <urdf/boost_replacement/shared_ptr.h>
#endif
#include <urdf/urdfdom_headers/urdf_model/include/urdf_model/pose.h>
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<double> 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<JointDynamics> dynamics;
/// Joint Limits
my_shared_ptr<JointLimits> limits;
/// Unsupported Hidden Feature
my_shared_ptr<JointSafety> safety;
/// Unsupported Hidden Feature
my_shared_ptr<JointCalibration> calibration;
/// Option to Mimic another Joint
my_shared_ptr<JointMimic> 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

262
examples/ThirdPartyLibs/urdf/urdfdom_headers/urdf_model/include/urdf_model/link.h
View File

@ -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 <string>
#include <vector>
#include <map>
#ifdef URDF_USE_BOOST
#include <boost/shared_ptr.hpp>
#include <boost/weak_ptr.hpp>
#else
#include <urdf/boost_replacement/shared_ptr.h>
#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> geometry;
std::string material_name;
my_shared_ptr<Material> 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> 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> inertial;
/// visual element
my_shared_ptr<Visual> visual;
/// collision element
my_shared_ptr<Collision> 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<my_shared_ptr<Collision> > 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<my_shared_ptr<Visual> > 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<Joint> parent_joint;
std::vector<my_shared_ptr<Joint> > child_joints;
std::vector<my_shared_ptr<Link> > child_links;
mutable int m_link_index;
const Link* getParent() const
{return parent_link_;}
void setParent(const my_shared_ptr<Link> &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<Link> parent_link_;
const Link* parent_link_;
};
}
#endif

220
examples/ThirdPartyLibs/urdf/urdfdom_headers/urdf_model/include/urdf_model/model.h
View File

@ -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 <string>
#include <map>
//#include <boost/function.hpp>
#include <urdf/urdfdom_headers/urdf_model/include/urdf_model/link.h>
#include <stdio.h> //printf
#include <urdf/urdfdom_headers/urdf_exception/include/urdf_exception/exception.h>
namespace urdf {
class ModelInterface
{
public:
my_shared_ptr<const Link> getRoot(void) const{return this->root_link_;};
my_shared_ptr<const Link> getLink(const std::string& name) const
{
my_shared_ptr<const Link> ptr;
if (this->links_.find(name) == this->links_.end())
ptr.reset(0);
else
ptr = this->links_.find(name)->second;
return ptr;
};
my_shared_ptr<const Joint> getJoint(const std::string& name) const
{
my_shared_ptr<const Joint> 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<my_shared_ptr<Link> >& links) const
{
for (std::map<std::string,my_shared_ptr<Link> >::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> &link) const
{
my_shared_ptr<Link> 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<Material> getMaterial(const std::string& name) const
{
my_shared_ptr<Material> 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<std::string, std::string> &parent_link_tree)
{
// loop through all joints, for every link, assign children links and children joints
for (std::map<std::string,my_shared_ptr<Joint> >::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<Link> 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 \"<link name=\"" + parent_link_name + "\" />\" 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<std::string, std::string> &parent_link_tree)
{
this->root_link_.reset(0);
// find the links that have no parent in the tree
for (std::map<std::string, my_shared_ptr<Link> >::const_iterator l=this->links_.begin(); l!=this->links_.end(); l++)
{
std::map<std::string, std::string >::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<std::string, my_shared_ptr<Link> > links_;
/// \brief complete list of Joints
std::map<std::string, my_shared_ptr<Joint> > joints_;
/// \brief complete list of Materials
std::map<std::string, my_shared_ptr<Material> > 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<Link> root_link_;
int m_numLinks;//includes parent
int m_numJoints;
};
}
#endif

265
examples/ThirdPartyLibs/urdf/urdfdom_headers/urdf_model/include/urdf_model/pose.h
View File

@ -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 <string>
//#include <sstream>
#include <vector>
#include <math.h>
#ifndef M_PI
#define M_PI 3.141592538
#endif //M_PI
#ifdef URDF_USE_BOOST
#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>
#else
#include <urdf/boost_replacement/string_split.h>
#include <urdf/boost_replacement/lexical_cast.h>
#endif //URDF_USE_BOOST
#include <urdf/urdfdom_headers/urdf_exception/include/urdf_exception/exception.h>
#include <assert.h>
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<std::string> pieces;
std::vector<double> 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<double>(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<std::string>(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

68
examples/ThirdPartyLibs/urdf/urdfdom_headers/urdf_model/include/urdf_model/twist.h
View File

@ -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 <string>
#include <sstream>
#include <vector>
#include <math.h>
#include <urdf/urdfdom_headers/urdf_model/include/urdf_model/pose.h>
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

141
examples/ThirdPartyLibs/urdf/urdfdom_headers/urdf_model_state/include/urdf_model_state/model_state.h
View File

@ -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 <string>
#include <vector>
#include <map>
#include <boost/shared_ptr.hpp>
#include <boost/weak_ptr.hpp>
#include "urdf_model/pose.h"
#include <urdf_model/twist.h>
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<double>(this->sec) +
static_cast<double>(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<double> position;
std::vector<double> velocity;
std::vector<double> 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<boost::shared_ptr<JointState> > joint_states;
};
}
#endif

42
examples/ThirdPartyLibs/urdf/urdfdom_headers/urdf_model_state/include/urdf_model_state/twist.h
View File

@ -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 <urdf_model/twist.h>"
#include <urdf_model/twist.h>
#endif

176
examples/ThirdPartyLibs/urdf/urdfdom_headers/urdf_sensor/include/urdf_sensor/sensor.h
View File

@ -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
<sensor name="my_camera_sensor" update_rate="20">
<origin xyz="0 0 0" rpy="0 0 0"/>
<camera>
<horizontal_hov>1.5708</horizontal_hov>
<image width="640" height="480" format="R8G8B8"/>
<clip near="0.01" far="50.0"/>
</camera>
</sensor>
<sensor name="my_ray_sensor" update_rate="20">
<origin xyz="0 0 0" rpy="0 0 0"/>
<ray>
<scan>
<horizontal samples="100" resolution="1" min_angle="-1.5708" max_angle="1.5708"/>
<vertical samples="1" resolution="1" min_angle="0" max_angle="0"/>
</scan>
</ray>
</sensor>
*/
#ifndef URDF_SENSOR_H
#define URDF_SENSOR_H
#include <string>
#include <vector>
#include <map>
#include <boost/shared_ptr.hpp>
#include <boost/weak_ptr.hpp>
#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<VisualSensor> sensor;
/// Parent link element name. A pointer is stored in parent_link_.
std::string parent_link_name;
boost::shared_ptr<Link> getParent() const
{return parent_link_.lock();};
void setParent(boost::shared_ptr<Link> 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<Link> parent_link_;
};
}
#endif

114
examples/ThirdPartyLibs/urdf/urdfdom_headers/urdf_world/include/urdf_world/world.h
View File

@ -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
<world name="pr2_with_table">
<!-- include the models by including
either the complete urdf or
referencing the file name. -->
<model name="pr2">
...
</model>
<include filename="table.urdf" model_name="table_model"/>
<!-- models in the world -->
<entity model="pr2" name="prj">
<origin xyz="0 1 0" rpy="0 0 0"/>
<twist linear="0 0 0" angular="0 0 0"/>
</entity>
<entity model="pr2" name="prk">
<origin xyz="0 2 0" rpy="0 0 0"/>
<twist linear="0 0 0" angular="0 0 0"/>
</entity>
<entity model="table_model">
<origin xyz="0 3 0" rpy="0 0 0"/>
<twist linear="0 0 0" angular="0 0 0"/>
</entity>
</world>
*/
#ifndef USDF_STATE_H
#define USDF_STATE_H
#include <string>
#include <vector>
#include <map>
#include <tinyxml.h>
#include <boost/shared_ptr.hpp>
#include <boost/weak_ptr.hpp>
#include "urdf_model/model.h"
#include "urdf_model/pose.h"
#include "urdf_model/twist.h"
namespace urdf{
class Entity
{
public:
boost::shared_ptr<ModelInterface> model;
Pose origin;
Twist twist;
};
class World
{
public:
World() { this->clear(); };
/// world name must be unique
std::string name;
std::vector<Entity> models;
void initXml(TiXmlElement* config);
void clear()
{
this->name.clear();
};
};
}
#endif

164
examples/TinyRenderer/Bullet2TinyRenderer.cpp Normal file
View File

@ -0,0 +1,164 @@
#include "TinyRenderer.h"
#include <stdio.h>
#include "LinearMath/btHashMap.h"
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
#include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h"
#include "BulletCollision/CollisionShapes/btCollisionShape.h"
class Bullet2TinyRenderer
{
btAlignedObjectArray<TinyRenderObjectData*> m_swRenderObjects;
btHashMap<btHashPtr,int> m_colObject2gfxIndex;
btHashMap<btHashPtr,int> m_colShape2gfxIndex;
int m_swWidth;
int m_swHeight;
TGAImage m_rgbColorBuffer;
b3AlignedObjectArray<float> 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;i<m_swRenderObjects.size();i++)
{
TinyRenderObjectData* d = m_swRenderObjects[i];
delete d;
}
}
void clearBuffers(TGAColor& clearColor)
{
for(int y=0;y<m_swHeight;++y)
{
for(int x=0;x<m_swWidth;++x)
{
m_rgbColorBuffer.set(x,y,clearColor);
m_depthBuffer[x+y*m_swWidth] = -1e30f;
}
}
}
const TGAImage& getFrameBuffer() const
{
return m_rgbColorBuffer;
}
virtual void createCollisionObjectGraphicsObject(btCollisionObject* obj,const btVector3& color)
{
int* colIndexPtr = m_colObject2gfxIndex[obj];
int* shapeIndexPtr = m_colShape2gfxIndex[obj->getCollisionShape()];
//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;i<rbWorld->getNumCollisionObjects();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;y<m_swHeight;++y)
{
unsigned char* pi=m_image+(y)*m_swWidth*3;
for(int x=0;x<m_swWidth;++x)
{
const TGAColor& color = getFrameBuffer().get(x,y);
pi[0] = color.bgra[2];
pi[1] = color.bgra[1];
pi[2] = color.bgra[0];
pi+=3;
}
}
static int counter=0;
counter++;
if (counter>10)
{
counter=0;
getFrameBuffer().write_tga_file("/Users/erwincoumans/develop/bullet3/framebuf.tga",true);
}
}
};

0
examples/TinyRenderer/Bullet2TinyRenderer.h Normal file
View File

37
examples/TinyRenderer/TinyRenderer.cpp
View File

@ -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<btVector3>& vertices, btAlignedObjectArray<btVector3>& normals,btAlignedObjectArray<int>& 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;i<numVertices;i++)
{
m_model->addVertex(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;i<numIndices;i+=3)
{
m_model->addTriangle(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();

7
examples/TinyRenderer/TinyRenderer.h
View File

@ -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
@ -30,6 +35,8 @@ struct TinyRenderObjectData
void createCube(float HalfExtentsX,float HalfExtentsY,float HalfExtentsZ);
void registerMeshShape(const float* vertices, int numVertices,const int* indices, int numIndices);
void registerMesh2(btAlignedObjectArray<btVector3>& vertices, btAlignedObjectArray<btVector3>& normals,btAlignedObjectArray<int>& indices);
void* m_userData;
int m_userIndex;
};

4
examples/TinyRenderer/main.cpp
View File

@ -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);

1
examples/TinyRenderer/model.cpp
View File

@ -1,3 +1,4 @@
#include <iostream>
#include <fstream>
#include <sstream>