Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Merge remote-tracking branch 'upstream/master'

Browse Source
This commit is contained in:
YunfeiBai
2018-05-03 10:48:55 -07:00
parent aa6b8f66e9 d70a9ae3a3
commit e5036fbc3a
259 changed files with 30418 additions and 14219 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
examples/CommonInterfaces/CommonGUIHelperInterface.h
View File

@@ -94,8 +94,8 @@ struct GUIHelperInterface
int* segmentationMaskBuffer, int segmentationMaskBufferSizeInPixels,
int startPixelIndex, int destinationWidth, int destinationHeight, int* numPixelsCopied){}
virtual void setProjectiveTextureMatrices(const float viewMatrix[16], const float projectionMatrix[16])=0;
virtual void setProjectiveTexture(bool useProjectiveTexture)=0;
virtual void setProjectiveTextureMatrices(const float viewMatrix[16], const float projectionMatrix[16]){}
virtual void setProjectiveTexture(bool useProjectiveTexture){}
virtual void autogenerateGraphicsObjects(btDiscreteDynamicsWorld* rbWorld) =0;

3
examples/CommonInterfaces/CommonRenderInterface.h
View File

@@ -16,6 +16,7 @@ enum
B3_CREATE_SHADOWMAP_RENDERMODE,
B3_USE_SHADOWMAP_RENDERMODE,
B3_USE_SHADOWMAP_RENDERMODE_REFLECTION,
B3_USE_SHADOWMAP_RENDERMODE_REFLECTION_PLANE,
B3_USE_PROJECTIVE_TEXTURE_RENDERMODE,
};
@@ -77,6 +78,8 @@ struct CommonRenderInterface
virtual void replaceTexture(int shapeIndex, int textureIndex){};
virtual void removeTexture(int textureIndex) = 0;
virtual void setPlaneReflectionShapeIndex(int index) {}
virtual int getShapeIndexFromInstance(int srcIndex) {return -1;}
virtual bool readSingleInstanceTransformToCPU(float* position, float* orientation, int srcIndex)=0;

6
examples/Constraints/ConstraintDemo.cpp
View File

@@ -356,7 +356,7 @@ void AllConstraintDemo::initPhysics()
spSlider6Dof->getTranslationalLimitMotor()->m_enableMotor[0] = true;
spSlider6Dof->getTranslationalLimitMotor()->m_targetVelocity[0] = -5.0f;
spSlider6Dof->getTranslationalLimitMotor()->m_maxMotorForce[0] = 0.1f;
spSlider6Dof->getTranslationalLimitMotor()->m_maxMotorForce[0] = 6.0f;
m_dynamicsWorld->addConstraint(spSlider6Dof);
@@ -431,7 +431,7 @@ void AllConstraintDemo::initPhysics()
// pGen6DOF->getTranslationalLimitMotor()->m_enableMotor[0] = true;
// pGen6DOF->getTranslationalLimitMotor()->m_targetVelocity[0] = 5.0f;
// pGen6DOF->getTranslationalLimitMotor()->m_maxMotorForce[0] = 0.1f;
// pGen6DOF->getTranslationalLimitMotor()->m_maxMotorForce[0] = 6.0f;
// pGen6DOF->setAngularLowerLimit(btVector3(0., SIMD_HALF_PI*0.9, 0.));
@@ -662,7 +662,7 @@ void AllConstraintDemo::initPhysics()
pGen6Dof->getTranslationalLimitMotor()->m_enableMotor[0] = true;
pGen6Dof->getTranslationalLimitMotor()->m_targetVelocity[0] = 5.0f;
pGen6Dof->getTranslationalLimitMotor()->m_maxMotorForce[0] = 0.1f;
pGen6Dof->getTranslationalLimitMotor()->m_maxMotorForce[0] = 6.0f;
}
#endif

8
examples/Constraints/Dof6Spring2Setup.cpp
View File

@@ -300,11 +300,11 @@ void Dof6Spring2Setup::initPhysics()
#ifdef USE_6DOF2
constraint->enableMotor(5,true);
constraint->setTargetVelocity(5,3.f);
constraint->setMaxMotorForce(5,10.f);
constraint->setMaxMotorForce(5,600.f);
#else
constraint->getRotationalLimitMotor(2)->m_enableMotor = true;
constraint->getRotationalLimitMotor(2)->m_targetVelocity = 3.f;
constraint->getRotationalLimitMotor(2)->m_maxMotorForce = 10;
constraint->getRotationalLimitMotor(2)->m_maxMotorForce = 600.f;
#endif
constraint->setDbgDrawSize(btScalar(2.f));
m_dynamicsWorld->addConstraint(constraint, true);
@@ -335,13 +335,13 @@ void Dof6Spring2Setup::initPhysics()
#ifdef USE_6DOF2
constraint->enableMotor(5,true);
constraint->setTargetVelocity(5,3.f);
constraint->setMaxMotorForce(5,10.f);
constraint->setMaxMotorForce(5,600.f);
constraint->setServo(5,true);
constraint->setServoTarget(5, M_PI_2);
#else
constraint->getRotationalLimitMotor(2)->m_enableMotor = true;
constraint->getRotationalLimitMotor(2)->m_targetVelocity = 3.f;
constraint->getRotationalLimitMotor(2)->m_maxMotorForce = 10;
constraint->getRotationalLimitMotor(2)->m_maxMotorForce = 600.f;
//servo motor is not implemented in 6dofspring constraint
#endif
constraint->setDbgDrawSize(btScalar(2.f));

12
examples/ExampleBrowser/CMakeLists.txt
View File

@@ -226,7 +226,6 @@ SET(BulletExampleBrowser_SRCS
../MultiThreading/b3PosixThreadSupport.cpp
../MultiThreading/b3Win32ThreadSupport.cpp
../MultiThreading/b3ThreadSupportInterface.cpp
../MultiThreading/btTaskScheduler.cpp
../RenderingExamples/TinyRendererSetup.cpp
../RenderingExamples/TimeSeriesCanvas.cpp
../RenderingExamples/TimeSeriesCanvas.h
@@ -339,15 +338,8 @@ SET(BulletExampleBrowser_SRCS
../ThirdPartyLibs/stb_image/stb_image.h
../ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp
../ThirdPartyLibs/tinyxml/tinystr.cpp
../ThirdPartyLibs/tinyxml/tinyxml.cpp
../ThirdPartyLibs/tinyxml/tinyxmlerror.cpp
../ThirdPartyLibs/tinyxml/tinyxmlparser.cpp
../ThirdPartyLibs/tinyxml/tinystr.cpp
../ThirdPartyLibs/tinyxml/tinyxml.cpp
../ThirdPartyLibs/tinyxml/tinyxmlerror.cpp
../ThirdPartyLibs/tinyxml/tinyxmlparser.cpp
../ThirdPartyLibs/tinyxml2/tinyxml2.cpp
InProcessExampleBrowser.cpp
${BULLET_PHYSICS_SOURCE_DIR}/build3/bullet.rc
)

8
examples/ExampleBrowser/CollisionShape2TriangleMesh.cpp
View File

@@ -185,7 +185,13 @@ void CollisionShape2TriangleMesh(btCollisionShape* collisionShape, const btTrans
}
} else
{
btAssert(0);
if (collisionShape->getShapeType()==SDF_SHAPE_PROXYTYPE)
{
//not yet
} else
{
btAssert(0);
}
}
}

11
examples/ExampleBrowser/InProcessExampleBrowser.cpp
View File

@@ -18,6 +18,8 @@
void ExampleBrowserThreadFunc(void* userPtr,void* lsMemory);
void* ExampleBrowserMemoryFunc();
void ExampleBrowserMemoryReleaseFunc(void* ptr);
#include <stdio.h>
//#include "BulletMultiThreaded/PlatformDefinitions.h"
@@ -42,6 +44,7 @@ static b3ThreadSupportInterface* createExampleBrowserThreadSupport(int numThread
b3PosixThreadSupport::ThreadConstructionInfo constructionInfo("testThreads",
ExampleBrowserThreadFunc,
ExampleBrowserMemoryFunc,
ExampleBrowserMemoryReleaseFunc,
numThreads);
b3ThreadSupportInterface* threadSupport = new b3PosixThreadSupport(constructionInfo);
@@ -56,7 +59,7 @@ static b3ThreadSupportInterface* createExampleBrowserThreadSupport(int numThread
b3ThreadSupportInterface* createExampleBrowserThreadSupport(int numThreads)
{
b3Win32ThreadSupport::Win32ThreadConstructionInfo threadConstructionInfo("testThreads",ExampleBrowserThreadFunc,ExampleBrowserMemoryFunc,numThreads);
b3Win32ThreadSupport::Win32ThreadConstructionInfo threadConstructionInfo("testThreads",ExampleBrowserThreadFunc,ExampleBrowserMemoryFunc,ExampleBrowserMemoryReleaseFunc,numThreads);
b3Win32ThreadSupport* threadSupport = new b3Win32ThreadSupport(threadConstructionInfo);
return threadSupport;
@@ -303,6 +306,12 @@ void* ExampleBrowserMemoryFunc()
return new ExampleBrowserThreadLocalStorage;
}
void ExampleBrowserMemoryReleaseFunc(void* ptr)
{
ExampleBrowserThreadLocalStorage* p = (ExampleBrowserThreadLocalStorage*) ptr;
delete p;
}

10
examples/ExampleBrowser/OpenGLExampleBrowser.cpp
View File

@@ -410,6 +410,14 @@ void OpenGLExampleBrowserVisualizerFlagCallback(int flag, bool enable)
if (flag == COV_ENABLE_WIREFRAME)
{
visualWireframe = enable;
if (visualWireframe)
{
gDebugDrawFlags |= btIDebugDraw::DBG_DrawWireframe;
}
else
{
gDebugDrawFlags &= ~btIDebugDraw::DBG_DrawWireframe;
}
}
}
@@ -1257,7 +1265,7 @@ void OpenGLExampleBrowser::update(float deltaTime)
{
skip=gPngSkipFrames;
//printf("gPngFileName=%s\n",gPngFileName);
static int s_frameCount = 100;
static int s_frameCount = 0;
sprintf(staticPngFileName,"%s%d.png",gPngFileName,s_frameCount++);
//b3Printf("Made screenshot %s",staticPngFileName);

23
examples/ExampleBrowser/OpenGLGuiHelper.cpp
View File

@@ -433,10 +433,14 @@ void OpenGLGuiHelper::createCollisionShapeGraphicsObject(btCollisionShape* colli
if (collisionShape->getShapeType() == SOFTBODY_SHAPE_PROXYTYPE)
{
computeSoftBodyVertices(collisionShape, gfxVertices, indices);
int shapeId = registerGraphicsShape(&gfxVertices[0].xyzw[0], gfxVertices.size(), &indices[0], indices.size(), B3_GL_TRIANGLES,
m_data->m_checkedTexture);
b3Assert(shapeId >= 0);
collisionShape->setUserIndex(shapeId);
if (gfxVertices.size() && indices.size())
{
int shapeId = registerGraphicsShape(&gfxVertices[0].xyzw[0], gfxVertices.size(), &indices[0], indices.size(), B3_GL_TRIANGLES,
m_data->m_checkedTexture);
b3Assert(shapeId >= 0);
collisionShape->setUserIndex(shapeId);
}
}
if (collisionShape->getShapeType()==MULTI_SPHERE_SHAPE_PROXYTYPE)
{
@@ -1026,6 +1030,10 @@ void OpenGLGuiHelper::setVisualizerFlagCallback(VisualizerFlagCallback callback)
void OpenGLGuiHelper::setVisualizerFlag(int flag, int enable)
{
if (getRenderInterface() && flag==16)//COV_ENABLE_PLANAR_REFLECTION
{
getRenderInterface()->setPlaneReflectionShapeIndex(enable);
}
if (m_data->m_visualizerFlagCallback)
(m_data->m_visualizerFlagCallback)(flag,enable);
}
@@ -1261,6 +1269,11 @@ void OpenGLGuiHelper::autogenerateGraphicsObjects(btDiscreteDynamicsWorld* rbWor
btCollisionObject* colObj = sortedObjects[i];
//btRigidBody* body = btRigidBody::upcast(colObj);
//does this also work for btMultiBody/btMultiBodyLinkCollider?
btSoftBody* sb = btSoftBody::upcast(colObj);
if (sb)
{
colObj->getCollisionShape()->setUserPointer(sb);
}
createCollisionShapeGraphicsObject(colObj->getCollisionShape());
int colorIndex = colObj->getBroadphaseHandle()->getUid() & 3;
@@ -1309,6 +1322,8 @@ void OpenGLGuiHelper::computeSoftBodyVertices(btCollisionShape* collisionShape,
btAlignedObjectArray<GLInstanceVertex>& gfxVertices,
btAlignedObjectArray<int>& indices)
{
if (collisionShape->getUserPointer()==0)
return;
b3Assert(collisionShape->getUserPointer());
btSoftBody* psb = (btSoftBody*)collisionShape->getUserPointer();
gfxVertices.resize(psb->m_faces.size() * 3);

7
examples/ExampleBrowser/premake4.lua
View File

@@ -164,14 +164,11 @@ project "App_BulletExampleBrowser"
"../RigidBody/RigidBodySoftContact.cpp",
"../ThirdPartyLibs/stb_image/*",
"../ThirdPartyLibs/Wavefront/tiny_obj_loader.*",
"../ThirdPartyLibs/tinyxml/*",
"../ThirdPartyLibs/BussIK/*",
"../GyroscopicDemo/GyroscopicSetup.cpp",
"../GyroscopicDemo/GyroscopicSetup.h",
"../ThirdPartyLibs/tinyxml/tinystr.cpp",
"../ThirdPartyLibs/tinyxml/tinyxml.cpp",
"../ThirdPartyLibs/tinyxml/tinyxmlerror.cpp",
"../ThirdPartyLibs/tinyxml/tinyxmlparser.cpp",
"../ThirdPartyLibs/tinyxml2/tinyxml2.cpp",
"../ThirdPartyLibs/tinyxml2/tinyxml2.h",
}
if (hasCL and findOpenGL3()) then
files {

1
examples/ExtendedTutorials/SimpleCloth.cpp
View File

@@ -121,6 +121,7 @@ void SimpleClothExample::createSoftBody(const btScalar s,
fixed,true);
cloth->getCollisionShape()->setMargin(0.001f);
cloth->getCollisionShape()->setUserPointer((void*)cloth);
cloth->generateBendingConstraints(2,cloth->appendMaterial());
cloth->setTotalMass(10);
//cloth->m_cfg.citerations = 10;

71
examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp
View File

@@ -20,7 +20,8 @@ subject to the following restrictions:
#include <stdio.h> //fopen
#include "Bullet3Common/b3AlignedObjectArray.h"
#include <string>
#include "../../ThirdPartyLibs/tinyxml/tinyxml.h"
#include "../../ThirdPartyLibs/tinyxml2/tinyxml2.h"
using namespace tinyxml2;
#include "Bullet3Common/b3FileUtils.h"
#include "LinearMath/btHashMap.h"
@@ -90,14 +91,14 @@ void tokenize(const std::string& str, AddToken& tokenAdder, const std::string& d
}
void readFloatArray(TiXmlElement* source, btAlignedObjectArray<float>& floatArray, int& componentStride)
void readFloatArray(XMLElement* source, btAlignedObjectArray<float>& floatArray, int& componentStride)
{
int numVals, stride;
TiXmlElement* array = source->FirstChildElement("float_array");
XMLElement* array = source->FirstChildElement("float_array");
if(array)
{
componentStride = 1;
if (source->FirstChildElement("technique_common")->FirstChildElement("accessor")->QueryIntAttribute("stride", &stride)!= TIXML_NO_ATTRIBUTE)
if (source->FirstChildElement("technique_common")->FirstChildElement("accessor")->QueryIntAttribute("stride", &stride)!= XML_NO_ATTRIBUTE)
{
componentStride = stride;
}
@@ -140,11 +141,11 @@ btVector4 getVector4FromXmlText(const char* text)
}
void readLibraryGeometries(TiXmlDocument& doc, btAlignedObjectArray<GLInstanceGraphicsShape>& visualShapes, btHashMap<btHashString,int>& name2Shape, float extraScaling)
void readLibraryGeometries(XMLDocument& doc, btAlignedObjectArray<GLInstanceGraphicsShape>& visualShapes, btHashMap<btHashString,int>& name2Shape, float extraScaling)
{
btHashMap<btHashString,TiXmlElement* > allSources;
btHashMap<btHashString,XMLElement* > allSources;
btHashMap<btHashString,VertexSource> vertexSources;
for(TiXmlElement* geometry = doc.RootElement()->FirstChildElement("library_geometries")->FirstChildElement("geometry");
for(XMLElement* geometry = doc.RootElement()->FirstChildElement("library_geometries")->FirstChildElement("geometry");
geometry != NULL; geometry = geometry->NextSiblingElement("geometry"))
{
btAlignedObjectArray<btVector3> vertexPositions;
@@ -152,11 +153,11 @@ void readLibraryGeometries(TiXmlDocument& doc, btAlignedObjectArray<GLInstanceGr
btAlignedObjectArray<int> indices;
const char* geometryName = geometry->Attribute("id");
for (TiXmlElement* mesh = geometry->FirstChildElement("mesh");(mesh != NULL); mesh = mesh->NextSiblingElement("mesh"))
for (XMLElement* mesh = geometry->FirstChildElement("mesh");(mesh != NULL); mesh = mesh->NextSiblingElement("mesh"))
{
TiXmlElement* vertices2 = mesh->FirstChildElement("vertices");
XMLElement* vertices2 = mesh->FirstChildElement("vertices");
for (TiXmlElement* source = mesh->FirstChildElement("source");source != NULL;source = source->NextSiblingElement("source"))
for (XMLElement* source = mesh->FirstChildElement("source");source != NULL;source = source->NextSiblingElement("source"))
{
const char* srcId= source->Attribute("id");
// printf("source id=%s\n",srcId);
@@ -165,7 +166,7 @@ void readLibraryGeometries(TiXmlDocument& doc, btAlignedObjectArray<GLInstanceGr
const char* vertexId = vertices2->Attribute("id");
//printf("vertices id=%s\n",vertexId);
VertexSource vs;
for(TiXmlElement* input = vertices2->FirstChildElement("input");input != NULL;input = input->NextSiblingElement("input"))
for(XMLElement* input = vertices2->FirstChildElement("input");input != NULL;input = input->NextSiblingElement("input"))
{
const char* sem = input->Attribute("semantic");
std::string semName(sem);
@@ -187,7 +188,7 @@ void readLibraryGeometries(TiXmlDocument& doc, btAlignedObjectArray<GLInstanceGr
}
vertexSources.insert(vertexId,vs);
for (TiXmlElement* primitive = mesh->FirstChildElement("triangles"); primitive; primitive = primitive->NextSiblingElement("triangles"))
for (XMLElement* primitive = mesh->FirstChildElement("triangles"); primitive; primitive = primitive->NextSiblingElement("triangles"))
{
std::string positionSourceName;
std::string normalSourceName;
@@ -199,7 +200,7 @@ void readLibraryGeometries(TiXmlDocument& doc, btAlignedObjectArray<GLInstanceGr
int numIndices = 0;
{
for (TiXmlElement* input = primitive->FirstChildElement("input");input != NULL;input = input->NextSiblingElement("input"))
for (XMLElement* input = primitive->FirstChildElement("input");input != NULL;input = input->NextSiblingElement("input"))
{
const char* sem = input->Attribute("semantic");
std::string semName(sem);
@@ -241,7 +242,7 @@ void readLibraryGeometries(TiXmlDocument& doc, btAlignedObjectArray<GLInstanceGr
}
btAlignedObjectArray<float> positionFloatArray;
int posStride=1;
TiXmlElement** sourcePtr = allSources[positionSourceName.c_str()];
XMLElement** sourcePtr = allSources[positionSourceName.c_str()];
if (sourcePtr)
{
readFloatArray(*sourcePtr,positionFloatArray, posStride);
@@ -334,7 +335,7 @@ void readLibraryGeometries(TiXmlDocument& doc, btAlignedObjectArray<GLInstanceGr
}//for each geometry
}
void readNodeHierarchy(TiXmlElement* node,btHashMap<btHashString,int>& name2Shape, btAlignedObjectArray<ColladaGraphicsInstance>& visualShapeInstances, const btMatrix4x4& parentTransMat)
void readNodeHierarchy(XMLElement* node,btHashMap<btHashString,int>& name2Shape, btAlignedObjectArray<ColladaGraphicsInstance>& visualShapeInstances, const btMatrix4x4& parentTransMat)
{
@@ -343,7 +344,7 @@ void readNodeHierarchy(TiXmlElement* node,btHashMap<btHashString,int>& name2Shap
///todo(erwincoumans) we probably have to read the elements 'translate', 'scale', 'rotate' and 'matrix' in-order and accumulate them...
{
for (TiXmlElement* transElem = node->FirstChildElement("matrix");transElem;transElem=node->NextSiblingElement("matrix"))
for (XMLElement* transElem = node->FirstChildElement("matrix");transElem;transElem=node->NextSiblingElement("matrix"))
{
if (transElem->GetText())
{
@@ -367,7 +368,7 @@ void readNodeHierarchy(TiXmlElement* node,btHashMap<btHashString,int>& name2Shap
}
{
for (TiXmlElement* transElem = node->FirstChildElement("translate");transElem;transElem=node->NextSiblingElement("translate"))
for (XMLElement* transElem = node->FirstChildElement("translate");transElem;transElem=node->NextSiblingElement("translate"))
{
if (transElem->GetText())
{
@@ -381,7 +382,7 @@ void readNodeHierarchy(TiXmlElement* node,btHashMap<btHashString,int>& name2Shap
}
}
{
for(TiXmlElement* scaleElem = node->FirstChildElement("scale");
for(XMLElement* scaleElem = node->FirstChildElement("scale");
scaleElem!= NULL; scaleElem= node->NextSiblingElement("scale"))
{
if (scaleElem->GetText())
@@ -394,7 +395,7 @@ void readNodeHierarchy(TiXmlElement* node,btHashMap<btHashString,int>& name2Shap
}
}
{
for(TiXmlElement* rotateElem = node->FirstChildElement("rotate");
for(XMLElement* rotateElem = node->FirstChildElement("rotate");
rotateElem!= NULL; rotateElem= node->NextSiblingElement("rotate"))
{
if (rotateElem->GetText())
@@ -411,7 +412,7 @@ void readNodeHierarchy(TiXmlElement* node,btHashMap<btHashString,int>& name2Shap
nodeTrans = parentTransMat*nodeTrans;
for (TiXmlElement* instanceGeom = node->FirstChildElement("instance_geometry");
for (XMLElement* instanceGeom = node->FirstChildElement("instance_geometry");
instanceGeom!=0;
instanceGeom=instanceGeom->NextSiblingElement("instance_geometry"))
{
@@ -432,22 +433,22 @@ void readNodeHierarchy(TiXmlElement* node,btHashMap<btHashString,int>& name2Shap
}
}
for(TiXmlElement* childNode = node->FirstChildElement("node");
for(XMLElement* childNode = node->FirstChildElement("node");
childNode!= NULL; childNode = childNode->NextSiblingElement("node"))
{
readNodeHierarchy(childNode,name2Shape,visualShapeInstances, nodeTrans);
}
}
void readVisualSceneInstanceGeometries(TiXmlDocument& doc, btHashMap<btHashString,int>& name2Shape, btAlignedObjectArray<ColladaGraphicsInstance>& visualShapeInstances)
void readVisualSceneInstanceGeometries(XMLDocument& doc, btHashMap<btHashString,int>& name2Shape, btAlignedObjectArray<ColladaGraphicsInstance>& visualShapeInstances)
{
btHashMap<btHashString,TiXmlElement* > allVisualScenes;
btHashMap<btHashString,XMLElement* > allVisualScenes;
TiXmlElement* libVisualScenes = doc.RootElement()->FirstChildElement("library_visual_scenes");
XMLElement* libVisualScenes = doc.RootElement()->FirstChildElement("library_visual_scenes");
if (libVisualScenes==0)
return;
{
for(TiXmlElement* scene = libVisualScenes->FirstChildElement("visual_scene");
for(XMLElement* scene = libVisualScenes->FirstChildElement("visual_scene");
scene != NULL; scene = scene->NextSiblingElement("visual_scene"))
{
const char* sceneName = scene->Attribute("id");
@@ -455,16 +456,16 @@ void readVisualSceneInstanceGeometries(TiXmlDocument& doc, btHashMap<btHashStrin
}
}
TiXmlElement* scene = 0;
XMLElement* scene = 0;
{
TiXmlElement* scenes = doc.RootElement()->FirstChildElement("scene");
XMLElement* scenes = doc.RootElement()->FirstChildElement("scene");
if (scenes)
{
TiXmlElement* instanceSceneReference = scenes->FirstChildElement("instance_visual_scene");
XMLElement* instanceSceneReference = scenes->FirstChildElement("instance_visual_scene");
if (instanceSceneReference)
{
const char* instanceSceneUrl = instanceSceneReference->Attribute("url");
TiXmlElement** sceneInstancePtr = allVisualScenes[instanceSceneUrl+1];//skip #
XMLElement** sceneInstancePtr = allVisualScenes[instanceSceneUrl+1];//skip #
if (sceneInstancePtr)
{
scene = *sceneInstancePtr;
@@ -475,7 +476,7 @@ void readVisualSceneInstanceGeometries(TiXmlDocument& doc, btHashMap<btHashStrin
if (scene)
{
for(TiXmlElement* node = scene->FirstChildElement("node");
for(XMLElement* node = scene->FirstChildElement("node");
node != NULL; node = node->NextSiblingElement("node"))
{
btMatrix4x4 identity;
@@ -488,11 +489,11 @@ void readVisualSceneInstanceGeometries(TiXmlDocument& doc, btHashMap<btHashStrin
}
}
void getUnitMeterScalingAndUpAxisTransform(TiXmlDocument& doc, btTransform& tr, float& unitMeterScaling, int clientUpAxis)
void getUnitMeterScalingAndUpAxisTransform(XMLDocument& doc, btTransform& tr, float& unitMeterScaling, int clientUpAxis)
{
///todo(erwincoumans) those up-axis transformations have been quickly coded without rigorous testing
TiXmlElement* unitMeter = doc.RootElement()->FirstChildElement("asset")->FirstChildElement("unit");
XMLElement* unitMeter = doc.RootElement()->FirstChildElement("asset")->FirstChildElement("unit");
if (unitMeter)
{
const char* meterText = unitMeter->Attribute("meter");
@@ -500,7 +501,7 @@ void getUnitMeterScalingAndUpAxisTransform(TiXmlDocument& doc, btTransform& tr,
unitMeterScaling = atof(meterText);
}
TiXmlElement* upAxisElem = doc.RootElement()->FirstChildElement("asset")->FirstChildElement("up_axis");
XMLElement* upAxisElem = doc.RootElement()->FirstChildElement("asset")->FirstChildElement("up_axis");
if (upAxisElem)
{
switch (clientUpAxis)
@@ -573,8 +574,8 @@ void LoadMeshFromCollada(const char* relativeFileName, btAlignedObjectArray<GLIn
return;
}
TiXmlDocument doc(filename);
if (!doc.LoadFile())
XMLDocument doc;
if (doc.LoadFile(filename) != XML_SUCCESS)
return;
//We need units to be in meter, so apply a scaling using the asset/units meter

58
examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp
View File

@@ -1,5 +1,5 @@
#include "BulletMJCFImporter.h"
#include "../../ThirdPartyLibs/tinyxml/tinyxml.h"
#include "../../ThirdPartyLibs/tinyxml2/tinyxml2.h"
#include "Bullet3Common/b3FileUtils.h"
#include "Bullet3Common/b3HashMap.h"
#include "LinearMath/btQuickprof.h"
@@ -32,7 +32,7 @@
#include "BulletCollision/CollisionShapes/btConvexHullShape.h"
#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h"
#include "BulletCollision/CollisionShapes/btTriangleMesh.h"
using namespace tinyxml2;
@@ -232,7 +232,7 @@ struct BulletMJCFImporterInternalData
}
}
std::string sourceFileLocation(TiXmlElement* e)
std::string sourceFileLocation(XMLElement* e)
{
#if 0
//no C++11 snprintf etc
@@ -241,7 +241,7 @@ struct BulletMJCFImporterInternalData
return buf;
#else
char row[1024];
sprintf(row,"%d",e->Row());
sprintf(row,"%d",e->GetLineNum());
std::string str = m_sourceFileName.c_str() + std::string(":") + std::string(row);
return str;
#endif
@@ -261,7 +261,7 @@ struct BulletMJCFImporterInternalData
return 0;
}
void parseCompiler(TiXmlElement* root_xml, MJCFErrorLogger* logger)
void parseCompiler(XMLElement* root_xml, MJCFErrorLogger* logger)
{
const char* meshDirStr = root_xml->Attribute("meshdir");
@@ -284,10 +284,10 @@ struct BulletMJCFImporterInternalData
}
void parseAssets(TiXmlElement* root_xml, MJCFErrorLogger* logger)
void parseAssets(XMLElement* root_xml, MJCFErrorLogger* logger)
{
// <mesh name="index0" file="index0.stl"/>
for (TiXmlElement* child_xml = root_xml->FirstChildElement() ; child_xml ; child_xml = child_xml->NextSiblingElement())
for (XMLElement* child_xml = root_xml->FirstChildElement() ; child_xml ; child_xml = child_xml->NextSiblingElement())
{
std::string n = child_xml->Value();
if (n=="mesh")
@@ -307,11 +307,11 @@ struct BulletMJCFImporterInternalData
}
bool parseDefaults(MyMJCFDefaults& defaults, TiXmlElement* root_xml, MJCFErrorLogger* logger)
bool parseDefaults(MyMJCFDefaults& defaults, XMLElement* root_xml, MJCFErrorLogger* logger)
{
bool handled= false;
//rudimentary 'default' support, would need more work for better feature coverage
for (TiXmlElement* child_xml = root_xml->FirstChildElement() ; child_xml ; child_xml = child_xml->NextSiblingElement())
for (XMLElement* child_xml = root_xml->FirstChildElement() ; child_xml ; child_xml = child_xml->NextSiblingElement())
{
std::string n = child_xml->Value();
@@ -413,9 +413,9 @@ struct BulletMJCFImporterInternalData
handled=true;
return handled;
}
bool parseRootLevel(MyMJCFDefaults& defaults, TiXmlElement* root_xml,MJCFErrorLogger* logger)
bool parseRootLevel(MyMJCFDefaults& defaults, XMLElement* root_xml,MJCFErrorLogger* logger)
{
for (TiXmlElement* rootxml = root_xml->FirstChildElement() ; rootxml ; rootxml = rootxml->NextSiblingElement())
for (XMLElement* rootxml = root_xml->FirstChildElement() ; rootxml ; rootxml = rootxml->NextSiblingElement())
{
bool handled = false;
std::string n = rootxml->Value();
@@ -473,7 +473,7 @@ struct BulletMJCFImporterInternalData
return true;
}
bool parseJoint(MyMJCFDefaults& defaults, TiXmlElement* link_xml, int modelIndex, int parentLinkIndex, int linkIndex, MJCFErrorLogger* logger, const btTransform& parentToLinkTrans, btTransform& jointTransOut)
bool parseJoint(MyMJCFDefaults& defaults, XMLElement* link_xml, int modelIndex, int parentLinkIndex, int linkIndex, MJCFErrorLogger* logger, const btTransform& parentToLinkTrans, btTransform& jointTransOut)
{
bool jointHandled = false;
const char* jType = link_xml->Attribute("type");
@@ -649,7 +649,7 @@ struct BulletMJCFImporterInternalData
*/
return false;
}
bool parseGeom(MyMJCFDefaults& defaults, TiXmlElement* link_xml, int modelIndex, int linkIndex, MJCFErrorLogger* logger, btVector3& inertialShift)
bool parseGeom(MyMJCFDefaults& defaults, XMLElement* link_xml, int modelIndex, int linkIndex, MJCFErrorLogger* logger, btVector3& inertialShift)
{
UrdfLink** linkPtrPtr = m_models[modelIndex]->m_links.getAtIndex(linkIndex);
if (linkPtrPtr==0)
@@ -963,7 +963,7 @@ struct BulletMJCFImporterInternalData
return handledGeomType;
}
btTransform parseTransform(TiXmlElement* link_xml, MJCFErrorLogger* logger)
btTransform parseTransform(XMLElement* link_xml, MJCFErrorLogger* logger)
{
btTransform tr;
tr.setIdentity();
@@ -1032,6 +1032,11 @@ struct BulletMJCFImporterInternalData
//todo
break;
}
case URDF_GEOM_CDF:
{
//todo
break;
}
case URDF_GEOM_CYLINDER:
case URDF_GEOM_CAPSULE:
{
@@ -1090,7 +1095,7 @@ struct BulletMJCFImporterInternalData
return orgChildLinkIndex;
}
bool parseBody(MyMJCFDefaults& defaults, TiXmlElement* link_xml, int modelIndex, int orgParentLinkIndex, MJCFErrorLogger* logger)
bool parseBody(MyMJCFDefaults& defaults, XMLElement* link_xml, int modelIndex, int orgParentLinkIndex, MJCFErrorLogger* logger)
{
MyMJCFDefaults curDefaults = defaults;
@@ -1142,7 +1147,7 @@ struct BulletMJCFImporterInternalData
jointTrans.setIdentity();
bool skipFixedJoint = false;
for (TiXmlElement* xml = link_xml->FirstChildElement() ; xml ; xml = xml->NextSiblingElement())
for (XMLElement* xml = link_xml->FirstChildElement() ; xml ; xml = xml->NextSiblingElement())
{
bool handled = false;
std::string n = xml->Value();
@@ -1482,16 +1487,16 @@ bool BulletMJCFImporter::loadMJCF(const char* fileName, MJCFErrorLogger* logger,
bool BulletMJCFImporter::parseMJCFString(const char* xmlText, MJCFErrorLogger* logger)
{
TiXmlDocument xml_doc;
XMLDocument xml_doc;
xml_doc.Parse(xmlText);
if (xml_doc.Error())
{
logger->reportError(xml_doc.ErrorDesc());
logger->reportError(xml_doc.ErrorStr());
xml_doc.ClearError();
return false;
}
TiXmlElement *mujoco_xml = xml_doc.FirstChildElement("mujoco");
XMLElement *mujoco_xml = xml_doc.FirstChildElement("mujoco");
if (!mujoco_xml)
{
logger->reportWarning("Cannot find <mujoco> root element");
@@ -1507,28 +1512,28 @@ bool BulletMJCFImporter::parseMJCFString(const char* xmlText, MJCFErrorLogger* l
//<compiler>,<option>,<size>,<default>,<body>,<keyframe>,<contactpair>,
//<light>, <camera>,<constraint>,<tendon>,<actuator>,<customfield>,<textfield>
for (TiXmlElement* link_xml = mujoco_xml->FirstChildElement("default"); link_xml; link_xml = link_xml->NextSiblingElement("default"))
for (XMLElement* link_xml = mujoco_xml->FirstChildElement("default"); link_xml; link_xml = link_xml->NextSiblingElement("default"))
{
m_data->parseDefaults(m_data->m_globalDefaults,link_xml,logger);
}
for (TiXmlElement* link_xml = mujoco_xml->FirstChildElement("compiler"); link_xml; link_xml = link_xml->NextSiblingElement("compiler"))
for (XMLElement* link_xml = mujoco_xml->FirstChildElement("compiler"); link_xml; link_xml = link_xml->NextSiblingElement("compiler"))
{
m_data->parseCompiler(link_xml,logger);
}
for (TiXmlElement* link_xml = mujoco_xml->FirstChildElement("asset"); link_xml; link_xml = link_xml->NextSiblingElement("asset"))
for (XMLElement* link_xml = mujoco_xml->FirstChildElement("asset"); link_xml; link_xml = link_xml->NextSiblingElement("asset"))
{
m_data->parseAssets(link_xml,logger);
}
for (TiXmlElement* link_xml = mujoco_xml->FirstChildElement("body"); link_xml; link_xml = link_xml->NextSiblingElement("body"))
for (XMLElement* link_xml = mujoco_xml->FirstChildElement("body"); link_xml; link_xml = link_xml->NextSiblingElement("body"))
{
m_data->parseRootLevel(m_data->m_globalDefaults, link_xml,logger);
}
for (TiXmlElement* link_xml = mujoco_xml->FirstChildElement("worldbody"); link_xml; link_xml = link_xml->NextSiblingElement("worldbody"))
for (XMLElement* link_xml = mujoco_xml->FirstChildElement("worldbody"); link_xml; link_xml = link_xml->NextSiblingElement("worldbody"))
{
m_data->parseRootLevel(m_data->m_globalDefaults, link_xml,logger);
}
@@ -2541,6 +2546,11 @@ class btCompoundShape* BulletMJCFImporter::convertLinkCollisionShapes( int linkI
}
break;
}
case URDF_GEOM_CDF:
{
//todo
break;
}
case URDF_GEOM_UNKNOWN:
{
break;

3
examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp
View File

@@ -2,7 +2,8 @@
#include "../../OpenGLWindow/GLInstancingRenderer.h"
#include "../../OpenGLWindow/GLInstanceGraphicsShape.h"
#include "btBulletDynamicsCommon.h"
//#include "btBulletDynamicsCommon.h"
#include "LinearMath/btVector3.h"
#include "../../OpenGLWindow/SimpleOpenGL3App.h"
#include "Wavefront2GLInstanceGraphicsShape.h"
#include "../../OpenGLWindow/GLInstancingRenderer.h"

51
examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
View File

@@ -21,6 +21,7 @@ subject to the following restrictions:
#include "../ImportSTLDemo/LoadMeshFromSTL.h"
#include "../ImportColladaDemo/LoadMeshFromCollada.h"
#include "BulletCollision/CollisionShapes/btShapeHull.h"//to create a tesselation of a generic btConvexShape
#include "BulletCollision/CollisionShapes/btSdfCollisionShape.h"
#include "../../CommonInterfaces/CommonGUIHelperInterface.h"
#include "Bullet3Common/b3FileUtils.h"
#include <string>
@@ -509,6 +510,10 @@ bool findExistingMeshFile(
{
*out_type = UrdfGeometry::FILE_OBJ;
}
else if (ext == ".cdf")
{
*out_type = UrdfGeometry::FILE_CDF;
}
else
{
b3Warning("%s: invalid mesh filename extension '%s'\n", error_message_prefix.c_str(), ext.c_str());
@@ -662,7 +667,53 @@ btCollisionShape* BulletURDFImporter::convertURDFToCollisionShape(const UrdfColl
shape ->setMargin(gUrdfDefaultCollisionMargin);
break;
}
case URDF_GEOM_CDF:
{
char relativeFileName[1024];
char pathPrefix[1024];
pathPrefix[0] = 0;
if (b3ResourcePath::findResourcePath(collision->m_geometry.m_meshFileName.c_str(), relativeFileName, 1024))
{
b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024);
btAlignedObjectArray<char> sdfData;
{
std::streampos fsize = 0;
std::ifstream file(relativeFileName, std::ios::binary);
if (file.good())
{
fsize = file.tellg();
file.seekg(0, std::ios::end);
fsize = file.tellg() - fsize;
file.seekg(0, std::ios::beg);
sdfData.resize(fsize);
int bytesRead = file.rdbuf()->sgetn(&sdfData[0], fsize);
btAssert(bytesRead == fsize);
file.close();
}
}
if (sdfData.size())
{
btSdfCollisionShape* sdfShape = new btSdfCollisionShape();
bool valid = sdfShape->initializeSDF(&sdfData[0], sdfData.size());
btAssert(valid);
if (valid)
{
shape = sdfShape;
}
else
{
delete sdfShape;
}
}
}
break;
}
case URDF_GEOM_MESH:
{
GLInstanceGraphicsShape* glmesh = 0;

221
examples/Importers/ImportURDFDemo/UrdfParser.cpp
View File

@@ -1,8 +1,9 @@
#include "UrdfParser.h"
#include "../../ThirdPartyLibs/tinyxml/tinyxml.h"
#include "../../ThirdPartyLibs/tinyxml2/tinyxml2.h"
#include "urdfStringSplit.h"
#include "urdfLexicalCast.h"
using namespace tinyxml2;
UrdfParser::UrdfParser()
:m_parseSDF(false),
@@ -74,7 +75,7 @@ static bool parseVector3(btVector3& vec3, const std::string& vector_str, ErrorLo
return true;
}
bool UrdfParser::parseMaterial(UrdfMaterial& material, TiXmlElement *config, ErrorLogger* logger)
bool UrdfParser::parseMaterial(UrdfMaterial& material, XMLElement *config, ErrorLogger* logger)
{
if (!config->Attribute("name"))
@@ -86,7 +87,7 @@ bool UrdfParser::parseMaterial(UrdfMaterial& material, TiXmlElement *config, Err
material.m_name = config->Attribute("name");
// texture
TiXmlElement *t = config->FirstChildElement("texture");
XMLElement *t = config->FirstChildElement("texture");
if (t)
{
if (t->Attribute("filename"))
@@ -102,7 +103,7 @@ bool UrdfParser::parseMaterial(UrdfMaterial& material, TiXmlElement *config, Err
// color
{
TiXmlElement *c = config->FirstChildElement("color");
XMLElement *c = config->FirstChildElement("color");
if (c)
{
if (c->Attribute("rgba"))
@@ -118,7 +119,7 @@ bool UrdfParser::parseMaterial(UrdfMaterial& material, TiXmlElement *config, Err
{
// specular (non-standard)
TiXmlElement *s = config->FirstChildElement("specular");
XMLElement *s = config->FirstChildElement("specular");
if (s)
{
if (s->Attribute("rgb"))
@@ -133,7 +134,7 @@ bool UrdfParser::parseMaterial(UrdfMaterial& material, TiXmlElement *config, Err
}
bool UrdfParser::parseTransform(btTransform& tr, TiXmlElement* xml, ErrorLogger* logger, bool parseSDF )
bool UrdfParser::parseTransform(btTransform& tr, XMLElement* xml, ErrorLogger* logger, bool parseSDF )
{
tr.setIdentity();
@@ -207,13 +208,13 @@ bool UrdfParser::parseTransform(btTransform& tr, TiXmlElement* xml, ErrorLogger*
return true;
}
bool UrdfParser::parseInertia(UrdfInertia& inertia, TiXmlElement* config, ErrorLogger* logger)
bool UrdfParser::parseInertia(UrdfInertia& inertia, XMLElement* config, ErrorLogger* logger)
{
inertia.m_linkLocalFrame.setIdentity();
inertia.m_mass = 0.f;
if(m_parseSDF)
{
TiXmlElement* pose = config->FirstChildElement("pose");
XMLElement* pose = config->FirstChildElement("pose");
if (pose)
{
parseTransform(inertia.m_linkLocalFrame, pose,logger,m_parseSDF);
@@ -223,7 +224,7 @@ bool UrdfParser::parseInertia(UrdfInertia& inertia, TiXmlElement* config, ErrorL
// Origin
TiXmlElement *o = config->FirstChildElement("origin");
XMLElement *o = config->FirstChildElement("origin");
if (o)
{
if (!parseTransform(inertia.m_linkLocalFrame,o,logger))
@@ -232,7 +233,7 @@ bool UrdfParser::parseInertia(UrdfInertia& inertia, TiXmlElement* config, ErrorL
}
}
TiXmlElement *mass_xml = config->FirstChildElement("mass");
XMLElement *mass_xml = config->FirstChildElement("mass");
if (!mass_xml)
{
logger->reportError("Inertial element must have a mass element");
@@ -252,7 +253,7 @@ bool UrdfParser::parseInertia(UrdfInertia& inertia, TiXmlElement* config, ErrorL
inertia.m_mass = urdfLexicalCast<double>(mass_xml->Attribute("value"));
}
TiXmlElement *inertia_xml = config->FirstChildElement("inertia");
XMLElement *inertia_xml = config->FirstChildElement("inertia");
if (!inertia_xml)
{
logger->reportError("Inertial element must have inertia element");
@@ -260,12 +261,12 @@ bool UrdfParser::parseInertia(UrdfInertia& inertia, TiXmlElement* config, ErrorL
}
if (m_parseSDF)
{
TiXmlElement* ixx = inertia_xml->FirstChildElement("ixx");
TiXmlElement* ixy = inertia_xml->FirstChildElement("ixy");
TiXmlElement* ixz = inertia_xml->FirstChildElement("ixz");
TiXmlElement* iyy = inertia_xml->FirstChildElement("iyy");
TiXmlElement* iyz = inertia_xml->FirstChildElement("iyz");
TiXmlElement* izz = inertia_xml->FirstChildElement("izz");
XMLElement* ixx = inertia_xml->FirstChildElement("ixx");
XMLElement* ixy = inertia_xml->FirstChildElement("ixy");
XMLElement* ixz = inertia_xml->FirstChildElement("ixz");
XMLElement* iyy = inertia_xml->FirstChildElement("iyy");
XMLElement* iyz = inertia_xml->FirstChildElement("iyz");
XMLElement* izz = inertia_xml->FirstChildElement("izz");
if (ixx && ixy && ixz && iyy && iyz && izz)
{
inertia.m_ixx = urdfLexicalCast<double>(ixx->GetText());
@@ -324,20 +325,21 @@ bool UrdfParser::parseInertia(UrdfInertia& inertia, TiXmlElement* config, ErrorL
}
bool UrdfParser::parseGeometry(UrdfGeometry& geom, TiXmlElement* g, ErrorLogger* logger)
bool UrdfParser::parseGeometry(UrdfGeometry& geom, XMLElement* g, ErrorLogger* logger)
{
// btAssert(g);
if (g==0)
return false;
TiXmlElement *shape = g->FirstChildElement();
XMLElement *shape = g->FirstChildElement();
if (!shape)
{
logger->reportError("Geometry tag contains no child element.");
return false;
}
const std::string type_name = shape->ValueTStr().c_str();
//const std::string type_name = shape->ValueTStr().c_str();
const std::string type_name = shape->Value();
if (type_name == "sphere")
{
geom.m_type = URDF_GEOM_SPHERE;
@@ -355,7 +357,7 @@ bool UrdfParser::parseGeometry(UrdfGeometry& geom, TiXmlElement* g, ErrorLogger*
geom.m_type = URDF_GEOM_BOX;
if (m_parseSDF)
{
TiXmlElement* size = shape->FirstChildElement("size");
XMLElement* size = shape->FirstChildElement("size");
if (0==size)
{
logger->reportError("box requires a size child element");
@@ -386,12 +388,12 @@ bool UrdfParser::parseGeometry(UrdfGeometry& geom, TiXmlElement* g, ErrorLogger*
if (m_parseSDF)
{
if (TiXmlElement* scale = shape->FirstChildElement("radius"))
if (XMLElement* scale = shape->FirstChildElement("radius"))
{
parseVector3(geom.m_meshScale,scale->GetText(),logger);
geom.m_capsuleRadius = m_urdfScaling * urdfLexicalCast<double>(scale->GetText());
}
if (TiXmlElement* scale = shape->FirstChildElement("length"))
if (XMLElement* scale = shape->FirstChildElement("length"))
{
parseVector3(geom.m_meshScale,scale->GetText(),logger);
geom.m_capsuleHeight = m_urdfScaling * urdfLexicalCast<double>(scale->GetText());
@@ -413,12 +415,12 @@ bool UrdfParser::parseGeometry(UrdfGeometry& geom, TiXmlElement* g, ErrorLogger*
geom.m_hasFromTo = false;
if (m_parseSDF)
{
if (TiXmlElement* scale = shape->FirstChildElement("radius"))
if (XMLElement* scale = shape->FirstChildElement("radius"))
{
parseVector3(geom.m_meshScale,scale->GetText(),logger);
geom.m_capsuleRadius = m_urdfScaling * urdfLexicalCast<double>(scale->GetText());
}
if (TiXmlElement* scale = shape->FirstChildElement("length"))
if (XMLElement* scale = shape->FirstChildElement("length"))
{
parseVector3(geom.m_meshScale,scale->GetText(),logger);
geom.m_capsuleHeight = m_urdfScaling * urdfLexicalCast<double>(scale->GetText());
@@ -434,19 +436,26 @@ bool UrdfParser::parseGeometry(UrdfGeometry& geom, TiXmlElement* g, ErrorLogger*
geom.m_capsuleHeight = m_urdfScaling * urdfLexicalCast<double>(shape->Attribute("length"));
}
}
else if (type_name == "mesh")
else if ((type_name == "mesh") || (type_name == "cdf"))
{
geom.m_type = URDF_GEOM_MESH;
if ((type_name == "cdf"))
{
geom.m_type = URDF_GEOM_CDF;
}
else
{
geom.m_type = URDF_GEOM_MESH;
}
geom.m_meshScale.setValue(1,1,1);
std::string fn;
if (m_parseSDF)
{
if (TiXmlElement* scale = shape->FirstChildElement("scale"))
if (XMLElement* scale = shape->FirstChildElement("scale"))
{
parseVector3(geom.m_meshScale,scale->GetText(),logger);
}
if (TiXmlElement* filename = shape->FirstChildElement("uri"))
if (XMLElement* filename = shape->FirstChildElement("uri"))
{
fn = filename->GetText();
}
@@ -498,8 +507,8 @@ bool UrdfParser::parseGeometry(UrdfGeometry& geom, TiXmlElement* g, ErrorLogger*
geom.m_type = URDF_GEOM_PLANE;
if (this->m_parseSDF)
{
TiXmlElement *n = shape->FirstChildElement("normal");
TiXmlElement *s = shape->FirstChildElement("size");
XMLElement *n = shape->FirstChildElement("normal");
XMLElement *s = shape->FirstChildElement("size");
if ((0==n)||(0==s))
{
@@ -535,14 +544,14 @@ bool UrdfParser::parseGeometry(UrdfGeometry& geom, TiXmlElement* g, ErrorLogger*
}
bool UrdfParser::parseCollision(UrdfCollision& collision, TiXmlElement* config, ErrorLogger* logger)
bool UrdfParser::parseCollision(UrdfCollision& collision, XMLElement* config, ErrorLogger* logger)
{
collision.m_linkLocalFrame.setIdentity();
if(m_parseSDF)
{
TiXmlElement* pose = config->FirstChildElement("pose");
XMLElement* pose = config->FirstChildElement("pose");
if (pose)
{
parseTransform(collision.m_linkLocalFrame, pose,logger,m_parseSDF);
@@ -550,14 +559,14 @@ bool UrdfParser::parseCollision(UrdfCollision& collision, TiXmlElement* config,
}
// Origin
TiXmlElement *o = config->FirstChildElement("origin");
XMLElement *o = config->FirstChildElement("origin");
if (o)
{
if (!parseTransform(collision.m_linkLocalFrame, o,logger))
return false;
}
// Geometry
TiXmlElement *geom = config->FirstChildElement("geometry");
XMLElement *geom = config->FirstChildElement("geometry");
if (!parseGeometry(collision.m_geometry,geom,logger))
{
return false;
@@ -575,12 +584,12 @@ bool UrdfParser::parseCollision(UrdfCollision& collision, TiXmlElement* config,
return true;
}
bool UrdfParser::parseVisual(UrdfModel& model, UrdfVisual& visual, TiXmlElement* config, ErrorLogger* logger)
bool UrdfParser::parseVisual(UrdfModel& model, UrdfVisual& visual, XMLElement* config, ErrorLogger* logger)
{
visual.m_linkLocalFrame.setIdentity();
if(m_parseSDF)
{
TiXmlElement* pose = config->FirstChildElement("pose");
XMLElement* pose = config->FirstChildElement("pose");
if (pose)
{
parseTransform(visual.m_linkLocalFrame, pose,logger,m_parseSDF);
@@ -588,14 +597,14 @@ bool UrdfParser::parseVisual(UrdfModel& model, UrdfVisual& visual, TiXmlElement*
}
// Origin
TiXmlElement *o = config->FirstChildElement("origin");
XMLElement *o = config->FirstChildElement("origin");
if (o)
{
if (!parseTransform(visual.m_linkLocalFrame, o,logger))
return false;
}
// Geometry
TiXmlElement *geom = config->FirstChildElement("geometry");
XMLElement *geom = config->FirstChildElement("geometry");
if (!parseGeometry(visual.m_geometry,geom,logger))
{
return false;
@@ -609,7 +618,7 @@ bool UrdfParser::parseVisual(UrdfModel& model, UrdfVisual& visual, TiXmlElement*
visual.m_geometry.m_hasLocalMaterial = false;
// Material
TiXmlElement *mat = config->FirstChildElement("material");
XMLElement *mat = config->FirstChildElement("material");
//todo(erwincoumans) skip materials in SDF for now (due to complexity)
if (mat)
{
@@ -630,7 +639,7 @@ bool UrdfParser::parseVisual(UrdfModel& model, UrdfVisual& visual, TiXmlElement*
}
model.m_materials.insert(matPtr->m_name.c_str(),matPtr);
{
TiXmlElement *diffuse = mat->FirstChildElement("diffuse");
XMLElement *diffuse = mat->FirstChildElement("diffuse");
if (diffuse) {
std::string diffuseText = diffuse->GetText();
btVector4 rgba(1,0,0,1);
@@ -642,7 +651,7 @@ bool UrdfParser::parseVisual(UrdfModel& model, UrdfVisual& visual, TiXmlElement*
}
}
{
TiXmlElement *specular = mat->FirstChildElement("specular");
XMLElement *specular = mat->FirstChildElement("specular");
if (specular) {
std::string specularText = specular->GetText();
btVector3 rgba(1,1,1);
@@ -665,9 +674,9 @@ bool UrdfParser::parseVisual(UrdfModel& model, UrdfVisual& visual, TiXmlElement*
// try to parse material element in place
TiXmlElement *t = mat->FirstChildElement("texture");
TiXmlElement *c = mat->FirstChildElement("color");
TiXmlElement *s = mat->FirstChildElement("specular");
XMLElement *t = mat->FirstChildElement("texture");
XMLElement *c = mat->FirstChildElement("color");
XMLElement *s = mat->FirstChildElement("specular");
if (t||c||s)
{
if (parseMaterial(visual.m_geometry.m_localMaterial, mat,logger))
@@ -692,7 +701,7 @@ bool UrdfParser::parseVisual(UrdfModel& model, UrdfVisual& visual, TiXmlElement*
return true;
}
bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *config, ErrorLogger* logger)
bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, XMLElement *config, ErrorLogger* logger)
{
const char* linkName = config->Attribute("name");
if (!linkName)
@@ -705,7 +714,7 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *confi
if (m_parseSDF) {
TiXmlElement* pose = config->FirstChildElement("pose");
XMLElement* pose = config->FirstChildElement("pose");
if (0==pose)
{
link.m_linkTransformInWorld.setIdentity();
@@ -730,7 +739,7 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *confi
</contact>
</audio_source>
#endif
TiXmlElement* ci = config->FirstChildElement("audio_source");
XMLElement* ci = config->FirstChildElement("audio_source");
if (ci)
{
link.m_audioSource.m_flags |= SDFAudioSource::SDFAudioSourceValid;
@@ -741,41 +750,41 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *confi
link.m_audioSource.m_uri = fn;
} else
{
if (TiXmlElement* filename_xml = ci->FirstChildElement("uri"))
if (XMLElement* filename_xml = ci->FirstChildElement("uri"))
{
link.m_audioSource.m_uri = filename_xml->GetText();
}
}
if (TiXmlElement* pitch_xml = ci->FirstChildElement("pitch"))
if (XMLElement* pitch_xml = ci->FirstChildElement("pitch"))
{
link.m_audioSource.m_pitch = urdfLexicalCast<double>(pitch_xml->GetText());
}
if (TiXmlElement* gain_xml = ci->FirstChildElement("gain"))
if (XMLElement* gain_xml = ci->FirstChildElement("gain"))
{
link.m_audioSource.m_gain = urdfLexicalCast<double>(gain_xml->GetText());
}
if (TiXmlElement* attack_rate_xml = ci->FirstChildElement("attack_rate"))
if (XMLElement* attack_rate_xml = ci->FirstChildElement("attack_rate"))
{
link.m_audioSource.m_attackRate = urdfLexicalCast<double>(attack_rate_xml->GetText());
}
if (TiXmlElement* decay_rate_xml = ci->FirstChildElement("decay_rate"))
if (XMLElement* decay_rate_xml = ci->FirstChildElement("decay_rate"))
{
link.m_audioSource.m_decayRate = urdfLexicalCast<double>(decay_rate_xml->GetText());
}
if (TiXmlElement* sustain_level_xml = ci->FirstChildElement("sustain_level"))
if (XMLElement* sustain_level_xml = ci->FirstChildElement("sustain_level"))
{
link.m_audioSource.m_sustainLevel = urdfLexicalCast<double>(sustain_level_xml->GetText());
}
if (TiXmlElement* release_rate_xml = ci->FirstChildElement("release_rate"))
if (XMLElement* release_rate_xml = ci->FirstChildElement("release_rate"))
{
link.m_audioSource.m_releaseRate = urdfLexicalCast<double>(release_rate_xml->GetText());
}
if (TiXmlElement* loop_xml = ci->FirstChildElement("loop"))
if (XMLElement* loop_xml = ci->FirstChildElement("loop"))
{
std::string looptxt = loop_xml->GetText();
if (looptxt == "true")
@@ -783,7 +792,7 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *confi
link.m_audioSource.m_flags |= SDFAudioSource::SDFAudioSourceLooping;
}
}
if (TiXmlElement* forceThreshold_xml = ci->FirstChildElement("collision_force_threshold"))
if (XMLElement* forceThreshold_xml = ci->FirstChildElement("collision_force_threshold"))
{
link.m_audioSource.m_collisionForceThreshold= urdfLexicalCast<double>(forceThreshold_xml->GetText());
}
@@ -794,11 +803,11 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *confi
}
{
//optional 'contact' parameters
TiXmlElement* ci = config->FirstChildElement("contact");
XMLElement* ci = config->FirstChildElement("contact");
if (ci)
{
TiXmlElement *damping_xml = ci->FirstChildElement("inertia_scaling");
XMLElement *damping_xml = ci->FirstChildElement("inertia_scaling");
if (damping_xml)
{
if (m_parseSDF)
@@ -819,7 +828,7 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *confi
}
}
{
TiXmlElement *friction_xml = ci->FirstChildElement("lateral_friction");
XMLElement *friction_xml = ci->FirstChildElement("lateral_friction");
if (friction_xml)
{
if (m_parseSDF)
@@ -839,7 +848,7 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *confi
}
{
TiXmlElement *rolling_xml = ci->FirstChildElement("rolling_friction");
XMLElement *rolling_xml = ci->FirstChildElement("rolling_friction");
if (rolling_xml)
{
if (m_parseSDF)
@@ -862,7 +871,7 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *confi
}
{
TiXmlElement *restitution_xml = ci->FirstChildElement("restitution");
XMLElement *restitution_xml = ci->FirstChildElement("restitution");
if (restitution_xml)
{
if (m_parseSDF)
@@ -886,7 +895,7 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *confi
}
{
TiXmlElement *spinning_xml = ci->FirstChildElement("spinning_friction");
XMLElement *spinning_xml = ci->FirstChildElement("spinning_friction");
if (spinning_xml)
{
if (m_parseSDF)
@@ -908,7 +917,7 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *confi
}
}
{
TiXmlElement *friction_anchor = ci->FirstChildElement("friction_anchor");
XMLElement *friction_anchor = ci->FirstChildElement("friction_anchor");
if (friction_anchor)
{
link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_FRICTION_ANCHOR;
@@ -916,7 +925,7 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *confi
}
{
TiXmlElement *stiffness_xml = ci->FirstChildElement("stiffness");
XMLElement *stiffness_xml = ci->FirstChildElement("stiffness");
if (stiffness_xml)
{
if (m_parseSDF)
@@ -939,7 +948,7 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *confi
}
{
TiXmlElement *damping_xml = ci->FirstChildElement("damping");
XMLElement *damping_xml = ci->FirstChildElement("damping");
if (damping_xml)
{
if (m_parseSDF)
@@ -964,7 +973,7 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *confi
}
// Inertial (optional)
TiXmlElement *i = config->FirstChildElement("inertial");
XMLElement *i = config->FirstChildElement("inertial");
if (i)
{
if (!parseInertia(link.m_inertia, i,logger))
@@ -997,7 +1006,7 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *confi
}
// Multiple Visuals (optional)
for (TiXmlElement* vis_xml = config->FirstChildElement("visual"); vis_xml; vis_xml = vis_xml->NextSiblingElement("visual"))
for (XMLElement* vis_xml = config->FirstChildElement("visual"); vis_xml; vis_xml = vis_xml->NextSiblingElement("visual"))
{
UrdfVisual visual;
visual.m_sourceFileLocation = sourceFileLocation(vis_xml);
@@ -1017,7 +1026,7 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *confi
// Multiple Collisions (optional)
for (TiXmlElement* col_xml = config->FirstChildElement("collision"); col_xml; col_xml = col_xml->NextSiblingElement("collision"))
for (XMLElement* col_xml = config->FirstChildElement("collision"); col_xml; col_xml = col_xml->NextSiblingElement("collision"))
{
UrdfCollision col;
col.m_sourceFileLocation = sourceFileLocation(col_xml);
@@ -1036,7 +1045,7 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *confi
return true;
}
bool UrdfParser::parseJointLimits(UrdfJoint& joint, TiXmlElement* config, ErrorLogger* logger)
bool UrdfParser::parseJointLimits(UrdfJoint& joint, XMLElement* config, ErrorLogger* logger)
{
joint.m_lowerLimit = 0.f;
joint.m_upperLimit = -1.f;
@@ -1047,22 +1056,22 @@ bool UrdfParser::parseJointLimits(UrdfJoint& joint, TiXmlElement* config, ErrorL
if (m_parseSDF)
{
TiXmlElement *lower_xml = config->FirstChildElement("lower");
XMLElement *lower_xml = config->FirstChildElement("lower");
if (lower_xml) {
joint.m_lowerLimit = urdfLexicalCast<double>(lower_xml->GetText());
}
TiXmlElement *upper_xml = config->FirstChildElement("upper");
XMLElement *upper_xml = config->FirstChildElement("upper");
if (upper_xml) {
joint.m_upperLimit = urdfLexicalCast<double>(upper_xml->GetText());
}
TiXmlElement *effort_xml = config->FirstChildElement("effort");
XMLElement *effort_xml = config->FirstChildElement("effort");
if (effort_xml) {
joint.m_effortLimit = urdfLexicalCast<double>(effort_xml->GetText());
}
TiXmlElement *velocity_xml = config->FirstChildElement("velocity");
XMLElement *velocity_xml = config->FirstChildElement("velocity");
if (velocity_xml) {
joint.m_velocityLimit = urdfLexicalCast<double>(velocity_xml->GetText());
}
@@ -1105,18 +1114,18 @@ bool UrdfParser::parseJointLimits(UrdfJoint& joint, TiXmlElement* config, ErrorL
return true;
}
bool UrdfParser::parseJointDynamics(UrdfJoint& joint, TiXmlElement* config, ErrorLogger* logger)
bool UrdfParser::parseJointDynamics(UrdfJoint& joint, XMLElement* config, ErrorLogger* logger)
{
joint.m_jointDamping = 0;
joint.m_jointFriction = 0;
if (m_parseSDF) {
TiXmlElement *damping_xml = config->FirstChildElement("damping");
XMLElement *damping_xml = config->FirstChildElement("damping");
if (damping_xml) {
joint.m_jointDamping = urdfLexicalCast<double>(damping_xml->GetText());
}
TiXmlElement *friction_xml = config->FirstChildElement("friction");
XMLElement *friction_xml = config->FirstChildElement("friction");
if (friction_xml) {
joint.m_jointFriction = urdfLexicalCast<double>(friction_xml->GetText());
}
@@ -1153,7 +1162,7 @@ bool UrdfParser::parseJointDynamics(UrdfJoint& joint, TiXmlElement* config, Erro
return true;
}
bool UrdfParser::parseJoint(UrdfJoint& joint, TiXmlElement *config, ErrorLogger* logger)
bool UrdfParser::parseJoint(UrdfJoint& joint, XMLElement *config, ErrorLogger* logger)
{
@@ -1168,7 +1177,7 @@ bool UrdfParser::parseJoint(UrdfJoint& joint, TiXmlElement *config, ErrorLogger*
joint.m_parentLinkToJointTransform.setIdentity();
// Get transform from Parent Link to Joint Frame
TiXmlElement *origin_xml = config->FirstChildElement("origin");
XMLElement *origin_xml = config->FirstChildElement("origin");
if (origin_xml)
{
if (!parseTransform(joint.m_parentLinkToJointTransform, origin_xml,logger))
@@ -1180,7 +1189,7 @@ bool UrdfParser::parseJoint(UrdfJoint& joint, TiXmlElement *config, ErrorLogger*
}
// Get Parent Link
TiXmlElement *parent_xml = config->FirstChildElement("parent");
XMLElement *parent_xml = config->FirstChildElement("parent");
if (parent_xml)
{
if (m_parseSDF)
@@ -1204,7 +1213,7 @@ bool UrdfParser::parseJoint(UrdfJoint& joint, TiXmlElement *config, ErrorLogger*
}
// Get Child Link
TiXmlElement *child_xml = config->FirstChildElement("child");
XMLElement *child_xml = config->FirstChildElement("child");
if (child_xml)
{
if (m_parseSDF)
@@ -1263,14 +1272,14 @@ bool UrdfParser::parseJoint(UrdfJoint& joint, TiXmlElement *config, ErrorLogger*
if (joint.m_type != URDFFloatingJoint && joint.m_type != URDFFixedJoint)
{
// axis
TiXmlElement *axis_xml = config->FirstChildElement("axis");
XMLElement *axis_xml = config->FirstChildElement("axis");
if (!axis_xml){
logger->reportWarning("urdfdom: no axis elemement for Joint, defaulting to (1,0,0) axis");
logger->reportWarning(joint.m_name.c_str());
joint.m_localJointAxis.setValue(1,0,0);
}
else{
TiXmlElement *xyz_xml = axis_xml->FirstChildElement("xyz");
XMLElement *xyz_xml = axis_xml->FirstChildElement("xyz");
if (xyz_xml) {
if (!parseVector3(joint.m_localJointAxis,std::string(xyz_xml->GetText()),logger))
{
@@ -1282,7 +1291,7 @@ bool UrdfParser::parseJoint(UrdfJoint& joint, TiXmlElement *config, ErrorLogger*
}
}
TiXmlElement *limit_xml = axis_xml->FirstChildElement("limit");
XMLElement *limit_xml = axis_xml->FirstChildElement("limit");
if (limit_xml)
{
if (joint.m_type != URDFContinuousJoint)
@@ -1308,7 +1317,7 @@ bool UrdfParser::parseJoint(UrdfJoint& joint, TiXmlElement *config, ErrorLogger*
return false;
}
TiXmlElement *prop_xml = axis_xml->FirstChildElement("dynamics");
XMLElement *prop_xml = axis_xml->FirstChildElement("dynamics");
if (prop_xml)
{
if (!parseJointDynamics(joint, prop_xml,logger))
@@ -1327,7 +1336,7 @@ bool UrdfParser::parseJoint(UrdfJoint& joint, TiXmlElement *config, ErrorLogger*
if (joint.m_type != URDFFloatingJoint && joint.m_type != URDFFixedJoint)
{
// axis
TiXmlElement *axis_xml = config->FirstChildElement("axis");
XMLElement *axis_xml = config->FirstChildElement("axis");
if (!axis_xml){
logger->reportWarning("urdfdom: no axis elemement for Joint, defaulting to (1,0,0) axis");
logger->reportWarning(joint.m_name.c_str());
@@ -1349,7 +1358,7 @@ bool UrdfParser::parseJoint(UrdfJoint& joint, TiXmlElement *config, ErrorLogger*
}
// Get limit
TiXmlElement *limit_xml = config->FirstChildElement("limit");
XMLElement *limit_xml = config->FirstChildElement("limit");
if (limit_xml)
{
if (!parseJointLimits(joint, limit_xml,logger))
@@ -1376,7 +1385,7 @@ bool UrdfParser::parseJoint(UrdfJoint& joint, TiXmlElement *config, ErrorLogger*
joint.m_jointFriction = 0;
// Get Dynamics
TiXmlElement *prop_xml = config->FirstChildElement("dynamics");
XMLElement *prop_xml = config->FirstChildElement("dynamics");
if (prop_xml)
{
@@ -1491,16 +1500,16 @@ bool UrdfParser::initTreeAndRoot(UrdfModel& model, ErrorLogger* logger)
bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceFixedBase)
{
TiXmlDocument xml_doc;
XMLDocument xml_doc;
xml_doc.Parse(urdfText);
if (xml_doc.Error())
{
logger->reportError(xml_doc.ErrorDesc());
logger->reportError(xml_doc.ErrorStr());
xml_doc.ClearError();
return false;
}
TiXmlElement *robot_xml = xml_doc.FirstChildElement("robot");
XMLElement *robot_xml = xml_doc.FirstChildElement("robot");
if (!robot_xml)
{
logger->reportError("expected a robot element");
@@ -1519,7 +1528,7 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF
// Get all Material elements
for (TiXmlElement* material_xml = robot_xml->FirstChildElement("material"); material_xml; material_xml = material_xml->NextSiblingElement("material"))
for (XMLElement* material_xml = robot_xml->FirstChildElement("material"); material_xml; material_xml = material_xml->NextSiblingElement("material"))
{
UrdfMaterial* material = new UrdfMaterial;
@@ -1543,7 +1552,7 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF
// logger->printMessage(msg);
for (TiXmlElement* link_xml = robot_xml->FirstChildElement("link"); link_xml; link_xml = link_xml->NextSiblingElement("link"))
for (XMLElement* link_xml = robot_xml->FirstChildElement("link"); link_xml; link_xml = link_xml->NextSiblingElement("link"))
{
UrdfLink* link = new UrdfLink;
@@ -1592,7 +1601,7 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF
}
// Get all Joint elements
for (TiXmlElement* joint_xml = robot_xml->FirstChildElement("joint"); joint_xml; joint_xml = joint_xml->NextSiblingElement("joint"))
for (XMLElement* joint_xml = robot_xml->FirstChildElement("joint"); joint_xml; joint_xml = joint_xml->NextSiblingElement("joint"))
{
UrdfJoint* joint = new UrdfJoint;
@@ -1651,16 +1660,16 @@ void UrdfParser::activateModel(int modelIndex)
bool UrdfParser::loadSDF(const char* sdfText, ErrorLogger* logger)
{
TiXmlDocument xml_doc;
XMLDocument xml_doc;
xml_doc.Parse(sdfText);
if (xml_doc.Error())
{
logger->reportError(xml_doc.ErrorDesc());
logger->reportError(xml_doc.ErrorStr());
xml_doc.ClearError();
return false;
}
TiXmlElement *sdf_xml = xml_doc.FirstChildElement("sdf");
XMLElement *sdf_xml = xml_doc.FirstChildElement("sdf");
if (!sdf_xml)
{
logger->reportError("expected an sdf element");
@@ -1668,9 +1677,9 @@ bool UrdfParser::loadSDF(const char* sdfText, ErrorLogger* logger)
}
//apparently, SDF doesn't require a "world" element, optional? URDF does.
TiXmlElement *world_xml = sdf_xml->FirstChildElement("world");
XMLElement *world_xml = sdf_xml->FirstChildElement("world");
TiXmlElement* robot_xml = 0;
XMLElement* robot_xml = 0;
if (!world_xml)
{
@@ -1687,7 +1696,7 @@ bool UrdfParser::loadSDF(const char* sdfText, ErrorLogger* logger)
UrdfModel* localModel = new UrdfModel;
m_tmpModels.push_back(localModel);
TiXmlElement* stat = robot_xml->FirstChildElement("static");
XMLElement* stat = robot_xml->FirstChildElement("static");
if (0!=stat)
{
int val = int(atof(stat->GetText()));
@@ -1707,7 +1716,7 @@ bool UrdfParser::loadSDF(const char* sdfText, ErrorLogger* logger)
}
localModel->m_name = name;
TiXmlElement* pose_xml = robot_xml->FirstChildElement("pose");
XMLElement* pose_xml = robot_xml->FirstChildElement("pose");
if (0==pose_xml)
{
localModel->m_rootTransformInWorld.setIdentity();
@@ -1718,7 +1727,7 @@ bool UrdfParser::loadSDF(const char* sdfText, ErrorLogger* logger)
}
// Get all Material elements
for (TiXmlElement* material_xml = robot_xml->FirstChildElement("material"); material_xml; material_xml = material_xml->NextSiblingElement("material"))
for (XMLElement* material_xml = robot_xml->FirstChildElement("material"); material_xml; material_xml = material_xml->NextSiblingElement("material"))
{
UrdfMaterial* material = new UrdfMaterial;
@@ -1742,7 +1751,7 @@ bool UrdfParser::loadSDF(const char* sdfText, ErrorLogger* logger)
// logger->printMessage(msg);
for (TiXmlElement* link_xml = robot_xml->FirstChildElement("link"); link_xml; link_xml = link_xml->NextSiblingElement("link"))
for (XMLElement* link_xml = robot_xml->FirstChildElement("link"); link_xml; link_xml = link_xml->NextSiblingElement("link"))
{
UrdfLink* link = new UrdfLink;
@@ -1791,7 +1800,7 @@ bool UrdfParser::loadSDF(const char* sdfText, ErrorLogger* logger)
}
// Get all Joint elements
for (TiXmlElement* joint_xml = robot_xml->FirstChildElement("joint"); joint_xml; joint_xml = joint_xml->NextSiblingElement("joint"))
for (XMLElement* joint_xml = robot_xml->FirstChildElement("joint"); joint_xml; joint_xml = joint_xml->NextSiblingElement("joint"))
{
UrdfJoint* joint = new UrdfJoint;
@@ -1828,7 +1837,7 @@ bool UrdfParser::loadSDF(const char* sdfText, ErrorLogger* logger)
return true;
}
std::string UrdfParser::sourceFileLocation(TiXmlElement* e)
std::string UrdfParser::sourceFileLocation(XMLElement* e)
{
#if 0
//no C++11 etc, no snprintf
@@ -1838,7 +1847,7 @@ std::string UrdfParser::sourceFileLocation(TiXmlElement* e)
return buf;
#else
char row[1024];
sprintf(row,"%d",e->Row());
sprintf(row,"%d",e->GetLineNum());
std::string str = m_urdf2Model.m_sourceFile.c_str() + std::string(":") + std::string(row);
return str;
#endif

32
examples/Importers/ImportURDFDemo/UrdfParser.h
View File

@@ -54,7 +54,8 @@ enum UrdfGeomTypes
URDF_GEOM_CYLINDER,
URDF_GEOM_MESH,
URDF_GEOM_PLANE,
URDF_GEOM_CAPSULE, //non-standard URDF?
URDF_GEOM_CAPSULE, //non-standard URDF
URDF_GEOM_CDF,//signed-distance-field, non-standard URDF
URDF_GEOM_UNKNOWN,
};
@@ -79,6 +80,8 @@ struct UrdfGeometry
FILE_STL =1,
FILE_COLLADA =2,
FILE_OBJ =3,
FILE_CDF = 4,
};
int m_meshFileType;
std::string m_meshFileName;
@@ -240,6 +243,11 @@ struct UrdfModel
}
};
namespace tinyxml2
{
class XMLElement;
};
class UrdfParser
{
protected:
@@ -252,17 +260,17 @@ protected:
int m_activeSdfModel;
btScalar m_urdfScaling;
bool parseTransform(btTransform& tr, class TiXmlElement* xml, ErrorLogger* logger, bool parseSDF = false);
bool parseInertia(UrdfInertia& inertia, class TiXmlElement* config, ErrorLogger* logger);
bool parseGeometry(UrdfGeometry& geom, class TiXmlElement* g, ErrorLogger* logger);
bool parseVisual(UrdfModel& model, UrdfVisual& visual, class TiXmlElement* config, ErrorLogger* logger);
bool parseCollision(UrdfCollision& collision, class TiXmlElement* config, ErrorLogger* logger);
bool parseTransform(btTransform& tr, tinyxml2::XMLElement* xml, ErrorLogger* logger, bool parseSDF = false);
bool parseInertia(UrdfInertia& inertia, tinyxml2::XMLElement* config, ErrorLogger* logger);
bool parseGeometry(UrdfGeometry& geom, tinyxml2::XMLElement* g, ErrorLogger* logger);
bool parseVisual(UrdfModel& model, UrdfVisual& visual, tinyxml2::XMLElement* config, ErrorLogger* logger);
bool parseCollision(UrdfCollision& collision, tinyxml2::XMLElement* config, ErrorLogger* logger);
bool initTreeAndRoot(UrdfModel& model, ErrorLogger* logger);
bool parseMaterial(UrdfMaterial& material, class TiXmlElement *config, ErrorLogger* logger);
bool parseJointLimits(UrdfJoint& joint, TiXmlElement* config, ErrorLogger* logger);
bool parseJointDynamics(UrdfJoint& joint, TiXmlElement* config, ErrorLogger* logger);
bool parseJoint(UrdfJoint& link, TiXmlElement *config, ErrorLogger* logger);
bool parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *config, ErrorLogger* logger);
bool parseMaterial(UrdfMaterial& material, tinyxml2::XMLElement *config, ErrorLogger* logger);
bool parseJointLimits(UrdfJoint& joint, tinyxml2::XMLElement* config, ErrorLogger* logger);
bool parseJointDynamics(UrdfJoint& joint, tinyxml2::XMLElement* config, ErrorLogger* logger);
bool parseJoint(UrdfJoint& joint, tinyxml2::XMLElement *config, ErrorLogger* logger);
bool parseLink(UrdfModel& model, UrdfLink& link, tinyxml2::XMLElement *config, ErrorLogger* logger);
public:
@@ -333,7 +341,7 @@ public:
return m_urdf2Model;
}
std::string sourceFileLocation(TiXmlElement* e);
std::string sourceFileLocation(tinyxml2::XMLElement* e);
void setSourceFile(const std::string& sourceFile)
{

261
examples/MultiThreadedDemo/CommonRigidBodyMTBase.cpp
View File

@@ -29,17 +29,17 @@ class btCollisionShape;
#include "BulletCollision/CollisionDispatch/btCollisionDispatcherMt.h"
#include "BulletDynamics/Dynamics/btSimulationIslandManagerMt.h" // for setSplitIslands()
#include "BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.h"
#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.h"
#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
#include "BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.h"
#include "BulletDynamics/MLCPSolvers/btMLCPSolver.h"
#include "BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h"
#include "BulletDynamics/MLCPSolvers/btDantzigSolver.h"
#include "BulletDynamics/MLCPSolvers/btLemkeSolver.h"
#include "../MultiThreading/btTaskScheduler.h"
static int gNumIslands = 0;
bool gAllowNestedParallelForLoops = false;
class Profiler
{
@@ -52,6 +52,10 @@ public:
kRecordPredictUnconstrainedMotion,
kRecordCreatePredictiveContacts,
kRecordIntegrateTransforms,
kRecordSolverTotal,
kRecordSolverSetup,
kRecordSolverIterations,
kRecordSolverFinish,
kRecordCount
};
@@ -139,6 +143,41 @@ static void profileEndCallback( btDynamicsWorld *world, btScalar timeStep )
}
class MySequentialImpulseConstraintSolverMt : public btSequentialImpulseConstraintSolverMt
{
typedef btSequentialImpulseConstraintSolverMt ParentClass;
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
MySequentialImpulseConstraintSolverMt() {}
// for profiling
virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) BT_OVERRIDE
{
ProfileHelper prof(Profiler::kRecordSolverSetup);
btScalar ret = ParentClass::solveGroupCacheFriendlySetup(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer );
return ret;
}
virtual btScalar solveGroupCacheFriendlyIterations( btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer ) BT_OVERRIDE
{
ProfileHelper prof(Profiler::kRecordSolverIterations);
btScalar ret = ParentClass::solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer );
return ret;
}
virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal) BT_OVERRIDE
{
ProfileHelper prof(Profiler::kRecordSolverFinish);
btScalar ret = ParentClass::solveGroupCacheFriendlyFinish(bodies, numBodies, infoGlobal);
return ret;
}
virtual btScalar solveGroup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher) BT_OVERRIDE
{
ProfileHelper prof(Profiler::kRecordSolverTotal);
btScalar ret = ParentClass::solveGroup(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer, dispatcher);
return ret;
}
};
///
/// MyCollisionDispatcher -- subclassed for profiling purposes
///
@@ -161,11 +200,11 @@ public:
///
/// myParallelIslandDispatch -- wrap default parallel dispatch for profiling and to get the number of simulation islands
//
void myParallelIslandDispatch( btAlignedObjectArray<btSimulationIslandManagerMt::Island*>* islandsPtr, btSimulationIslandManagerMt::IslandCallback* callback )
void myParallelIslandDispatch( btAlignedObjectArray<btSimulationIslandManagerMt::Island*>* islandsPtr, const btSimulationIslandManagerMt::SolverParams& solverParams)
{
ProfileHelper prof( Profiler::kRecordDispatchIslands );
gNumIslands = islandsPtr->size();
btSimulationIslandManagerMt::parallelIslandDispatch( islandsPtr, callback );
btSimulationIslandManagerMt::parallelIslandDispatch( islandsPtr, solverParams );
}
@@ -200,9 +239,10 @@ public:
MyDiscreteDynamicsWorld( btDispatcher* dispatcher,
btBroadphaseInterface* pairCache,
btConstraintSolverPoolMt* constraintSolver,
btSequentialImpulseConstraintSolverMt* constraintSolverMt,
btCollisionConfiguration* collisionConfiguration
) :
btDiscreteDynamicsWorldMt( dispatcher, pairCache, constraintSolver, collisionConfiguration )
btDiscreteDynamicsWorldMt( dispatcher, pairCache, constraintSolver, constraintSolverMt, collisionConfiguration )
{
btSimulationIslandManagerMt* islandMgr = static_cast<btSimulationIslandManagerMt*>( m_islandManager );
islandMgr->setIslandDispatchFunction( myParallelIslandDispatch );
@@ -218,6 +258,8 @@ btConstraintSolver* createSolverByType( SolverType t )
{
case SOLVER_TYPE_SEQUENTIAL_IMPULSE:
return new btSequentialImpulseConstraintSolver();
case SOLVER_TYPE_SEQUENTIAL_IMPULSE_MT:
return new MySequentialImpulseConstraintSolverMt();
case SOLVER_TYPE_NNCG:
return new btNNCGConstraintSolver();
case SOLVER_TYPE_MLCP_PGS:
@@ -253,7 +295,7 @@ public:
{
addTaskScheduler( btGetSequentialTaskScheduler() );
#if BT_THREADSAFE
if ( btITaskScheduler* ts = createDefaultTaskScheduler() )
if ( btITaskScheduler* ts = btCreateDefaultTaskScheduler() )
{
m_allocatedTaskSchedulers.push_back( ts );
addTaskScheduler( ts );
@@ -306,11 +348,12 @@ static btTaskSchedulerManager gTaskSchedulerMgr;
#if BT_THREADSAFE
static bool gMultithreadedWorld = true;
static bool gDisplayProfileInfo = true;
static SolverType gSolverType = SOLVER_TYPE_SEQUENTIAL_IMPULSE_MT;
#else
static bool gMultithreadedWorld = false;
static bool gDisplayProfileInfo = false;
#endif
static SolverType gSolverType = SOLVER_TYPE_SEQUENTIAL_IMPULSE;
#endif
static int gSolverMode = SOLVER_SIMD |
SOLVER_USE_WARMSTARTING |
// SOLVER_RANDMIZE_ORDER |
@@ -318,9 +361,11 @@ static int gSolverMode = SOLVER_SIMD |
// SOLVER_USE_2_FRICTION_DIRECTIONS |
0;
static btScalar gSliderSolverIterations = 10.0f; // should be int
static btScalar gSliderNumThreads = 1.0f; // should be int
static btScalar gSliderIslandBatchingThreshold = 0.0f; // should be int
static btScalar gSliderMinBatchSize = btScalar(btSequentialImpulseConstraintSolverMt::s_minBatchSize); // should be int
static btScalar gSliderMaxBatchSize = btScalar(btSequentialImpulseConstraintSolverMt::s_maxBatchSize); // should be int
static btScalar gSliderLeastSquaresResidualThreshold = 0.0f;
////////////////////////////////////
CommonRigidBodyMTBase::CommonRigidBodyMTBase( struct GUIHelperInterface* helper )
@@ -419,6 +464,23 @@ void setTaskSchedulerComboBoxCallback(int combobox, const char* item, void* user
}
void setBatchingMethodComboBoxCallback(int combobox, const char* item, void* userPointer)
{
#if BT_THREADSAFE
const char** items = static_cast<const char**>( userPointer );
for ( int i = 0; i < btBatchedConstraints::BATCHING_METHOD_COUNT; ++i )
{
if ( strcmp( item, items[ i ] ) == 0 )
{
// change the task scheduler
btSequentialImpulseConstraintSolverMt::s_contactBatchingMethod = static_cast<btBatchedConstraints::BatchingMethod>( i );
break;
}
}
#endif // #if BT_THREADSAFE
}
static void setThreadCountCallback(float val, void* userPtr)
{
#if BT_THREADSAFE
@@ -435,13 +497,43 @@ static void setSolverIterationCountCallback(float val, void* userPtr)
}
}
static void setLargeIslandManifoldCountCallback( float val, void* userPtr )
{
btSequentialImpulseConstraintSolverMt::s_minimumContactManifoldsForBatching = int( gSliderIslandBatchingThreshold );
}
static void setMinBatchSizeCallback( float val, void* userPtr )
{
gSliderMaxBatchSize = (std::max)(gSliderMinBatchSize, gSliderMaxBatchSize);
btSequentialImpulseConstraintSolverMt::s_minBatchSize = int(gSliderMinBatchSize);
btSequentialImpulseConstraintSolverMt::s_maxBatchSize = int(gSliderMaxBatchSize);
}
static void setMaxBatchSizeCallback( float val, void* userPtr )
{
gSliderMinBatchSize = (std::min)(gSliderMinBatchSize, gSliderMaxBatchSize);
btSequentialImpulseConstraintSolverMt::s_minBatchSize = int(gSliderMinBatchSize);
btSequentialImpulseConstraintSolverMt::s_maxBatchSize = int(gSliderMaxBatchSize);
}
static void setLeastSquaresResidualThresholdCallback( float val, void* userPtr )
{
if (btDiscreteDynamicsWorld* world = reinterpret_cast<btDiscreteDynamicsWorld*>(userPtr))
{
world->getSolverInfo().m_leastSquaresResidualThreshold = gSliderLeastSquaresResidualThreshold;
}
}
void CommonRigidBodyMTBase::createEmptyDynamicsWorld()
{
gNumIslands = 0;
m_solverType = gSolverType;
#if BT_THREADSAFE && (BT_USE_OPENMP || BT_USE_PPL || BT_USE_TBB)
#if BT_THREADSAFE
btAssert( btGetTaskScheduler() != NULL );
m_multithreadCapable = true;
if (NULL != btGetTaskScheduler() && gTaskSchedulerMgr.getNumTaskSchedulers() > 1)
{
m_multithreadCapable = true;
}
#endif
if ( gMultithreadedWorld )
{
@@ -457,16 +549,28 @@ void CommonRigidBodyMTBase::createEmptyDynamicsWorld()
btConstraintSolverPoolMt* solverPool;
{
SolverType poolSolverType = m_solverType;
if (poolSolverType == SOLVER_TYPE_SEQUENTIAL_IMPULSE_MT)
{
// pool solvers shouldn't be parallel solvers, we don't allow that kind of
// nested parallelism because of performance issues
poolSolverType = SOLVER_TYPE_SEQUENTIAL_IMPULSE;
}
btConstraintSolver* solvers[ BT_MAX_THREAD_COUNT ];
int maxThreadCount = BT_MAX_THREAD_COUNT;
for ( int i = 0; i < maxThreadCount; ++i )
{
solvers[ i ] = createSolverByType( m_solverType );
solvers[ i ] = createSolverByType( poolSolverType );
}
solverPool = new btConstraintSolverPoolMt( solvers, maxThreadCount );
m_solver = solverPool;
}
btDiscreteDynamicsWorld* world = new MyDiscreteDynamicsWorld( m_dispatcher, m_broadphase, solverPool, m_collisionConfiguration );
btSequentialImpulseConstraintSolverMt* solverMt = NULL;
if ( m_solverType == SOLVER_TYPE_SEQUENTIAL_IMPULSE_MT )
{
solverMt = new MySequentialImpulseConstraintSolverMt();
}
btDiscreteDynamicsWorld* world = new MyDiscreteDynamicsWorld( m_dispatcher, m_broadphase, solverPool, solverMt, m_collisionConfiguration );
m_dynamicsWorld = world;
m_multithreadedWorld = true;
btAssert( btGetTaskScheduler() != NULL );
@@ -486,7 +590,14 @@ void CommonRigidBodyMTBase::createEmptyDynamicsWorld()
m_broadphase = new btDbvtBroadphase();
m_solver = createSolverByType( m_solverType );
SolverType solverType = m_solverType;
if ( solverType == SOLVER_TYPE_SEQUENTIAL_IMPULSE_MT )
{
// using the parallel solver with the single-threaded world works, but is
// disabled here to avoid confusion
solverType = SOLVER_TYPE_SEQUENTIAL_IMPULSE;
}
m_solver = createSolverByType( solverType );
m_dynamicsWorld = new btDiscreteDynamicsWorld( m_dispatcher, m_broadphase, m_solver, m_collisionConfiguration );
}
@@ -494,6 +605,7 @@ void CommonRigidBodyMTBase::createEmptyDynamicsWorld()
m_dynamicsWorld->setInternalTickCallback( profileEndCallback, NULL, false );
m_dynamicsWorld->setGravity( btVector3( 0, -10, 0 ) );
m_dynamicsWorld->getSolverInfo().m_solverMode = gSolverMode;
m_dynamicsWorld->getSolverInfo().m_numIterations = btMax(1, int(gSliderSolverIterations));
createDefaultParameters();
}
@@ -504,16 +616,18 @@ void CommonRigidBodyMTBase::createDefaultParameters()
{
// create a button to toggle multithreaded world
ButtonParams button( "Multithreaded world enable", 0, true );
button.m_initialState = gMultithreadedWorld;
button.m_userPointer = &gMultithreadedWorld;
bool* ptr = &gMultithreadedWorld;
button.m_initialState = *ptr;
button.m_userPointer = ptr;
button.m_callback = boolPtrButtonCallback;
m_guiHelper->getParameterInterface()->registerButtonParameter( button );
}
{
// create a button to toggle profile printing
ButtonParams button( "Display solver info", 0, true );
button.m_initialState = gDisplayProfileInfo;
button.m_userPointer = &gDisplayProfileInfo;
bool* ptr = &gDisplayProfileInfo;
button.m_initialState = *ptr;
button.m_userPointer = ptr;
button.m_callback = boolPtrButtonCallback;
m_guiHelper->getParameterInterface()->registerButtonParameter( button );
}
@@ -544,6 +658,16 @@ void CommonRigidBodyMTBase::createDefaultParameters()
slider.m_clampToIntegers = true;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter( slider );
}
{
// a slider for the solver leastSquaresResidualThreshold (used to run fewer solver iterations when convergence is good)
SliderParams slider( "Solver residual thresh", &gSliderLeastSquaresResidualThreshold );
slider.m_minVal = 0.0f;
slider.m_maxVal = 0.25f;
slider.m_callback = setLeastSquaresResidualThresholdCallback;
slider.m_userPointer = m_dynamicsWorld;
slider.m_clampToIntegers = false;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter( slider );
}
{
ButtonParams button( "Solver use SIMD", 0, true );
button.m_buttonId = SOLVER_SIMD;
@@ -618,20 +742,86 @@ void CommonRigidBodyMTBase::createDefaultParameters()
m_guiHelper->getParameterInterface()->registerComboBox( comboParams );
}
{
// create a slider to set the number of threads to use
int numThreads = btGetTaskScheduler()->getNumThreads();
// if slider has not been set yet (by another demo),
if ( gSliderNumThreads <= 1.0f )
{
// create a slider to set the number of threads to use
int numThreads = btGetTaskScheduler()->getNumThreads();
gSliderNumThreads = float( numThreads );
}
int maxNumThreads = btGetTaskScheduler()->getMaxNumThreads();
SliderParams slider("Thread count", &gSliderNumThreads);
slider.m_minVal = 1.0f;
slider.m_maxVal = float( BT_MAX_THREAD_COUNT );
slider.m_maxVal = float( maxNumThreads );
slider.m_callback = setThreadCountCallback;
slider.m_clampToIntegers = true;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter( slider );
}
{
// a slider for the number of manifolds an island needs to be too large for parallel dispatch
if (gSliderIslandBatchingThreshold < 1.0)
{
gSliderIslandBatchingThreshold = float(btSequentialImpulseConstraintSolverMt::s_minimumContactManifoldsForBatching);
}
SliderParams slider( "IslandBatchThresh", &gSliderIslandBatchingThreshold );
slider.m_minVal = 1.0f;
slider.m_maxVal = 2000.0f;
slider.m_callback = setLargeIslandManifoldCountCallback;
slider.m_userPointer = NULL;
slider.m_clampToIntegers = true;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter( slider );
}
{
// create a combo box for selecting the batching method
static const char* sBatchingMethodComboBoxItems[ btBatchedConstraints::BATCHING_METHOD_COUNT ];
{
sBatchingMethodComboBoxItems[ btBatchedConstraints::BATCHING_METHOD_SPATIAL_GRID_2D ] = "Batching: 2D Grid";
sBatchingMethodComboBoxItems[ btBatchedConstraints::BATCHING_METHOD_SPATIAL_GRID_3D ] = "Batching: 3D Grid";
};
ComboBoxParams comboParams;
comboParams.m_userPointer = sBatchingMethodComboBoxItems;
comboParams.m_numItems = btBatchedConstraints::BATCHING_METHOD_COUNT;
comboParams.m_startItem = static_cast<int>(btSequentialImpulseConstraintSolverMt::s_contactBatchingMethod);
comboParams.m_items = sBatchingMethodComboBoxItems;
comboParams.m_callback = setBatchingMethodComboBoxCallback;
m_guiHelper->getParameterInterface()->registerComboBox( comboParams );
}
{
// a slider for the sequentialImpulseConstraintSolverMt min batch size (when batching)
SliderParams slider( "Min batch size", &gSliderMinBatchSize );
slider.m_minVal = 1.0f;
slider.m_maxVal = 1000.0f;
slider.m_callback = setMinBatchSizeCallback;
slider.m_userPointer = NULL;
slider.m_clampToIntegers = true;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter( slider );
}
{
// a slider for the sequentialImpulseConstraintSolverMt max batch size (when batching)
SliderParams slider( "Max batch size", &gSliderMaxBatchSize );
slider.m_minVal = 1.0f;
slider.m_maxVal = 1000.0f;
slider.m_callback = setMaxBatchSizeCallback;
slider.m_userPointer = NULL;
slider.m_clampToIntegers = true;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter( slider );
}
{
// create a button to toggle debug drawing of batching visualization
ButtonParams button( "Visualize batching", 0, true );
bool* ptr = &btBatchedConstraints::s_debugDrawBatches;
button.m_initialState = *ptr;
button.m_userPointer = ptr;
button.m_callback = boolPtrButtonCallback;
m_guiHelper->getParameterInterface()->registerButtonParameter( button );
}
{
ButtonParams button( "Allow Nested ParallelFor", 0, true );
button.m_initialState = btSequentialImpulseConstraintSolverMt::s_allowNestedParallelForLoops;
button.m_userPointer = &btSequentialImpulseConstraintSolverMt::s_allowNestedParallelForLoops;
button.m_callback = boolPtrButtonCallback;
m_guiHelper->getParameterInterface()->registerButtonParameter( button );
}
#endif // #if BT_THREADSAFE
}
}
@@ -643,6 +833,7 @@ void CommonRigidBodyMTBase::drawScreenText()
int xCoord = 400;
int yCoord = 30;
int yStep = 30;
int indent = 30;
if (m_solverType != gSolverType)
{
sprintf( msg, "restart example to change solver type" );
@@ -721,6 +912,34 @@ void CommonRigidBodyMTBase::drawScreenText()
m_guiHelper->getAppInterface()->drawText( msg, xCoord, yCoord, 0.4f );
yCoord += yStep;
sprintf( msg,
"SolverTotal %5.3f ms",
gProfiler.getAverageTime( Profiler::kRecordSolverTotal )*0.001f
);
m_guiHelper->getAppInterface()->drawText( msg, xCoord, yCoord, 0.4f );
yCoord += yStep;
sprintf( msg,
"SolverSetup %5.3f ms",
gProfiler.getAverageTime( Profiler::kRecordSolverSetup )*0.001f
);
m_guiHelper->getAppInterface()->drawText( msg, xCoord + indent, yCoord, 0.4f );
yCoord += yStep;
sprintf( msg,
"SolverIterations %5.3f ms",
gProfiler.getAverageTime( Profiler::kRecordSolverIterations )*0.001f
);
m_guiHelper->getAppInterface()->drawText( msg, xCoord + indent, yCoord, 0.4f );
yCoord += yStep;
sprintf( msg,
"SolverFinish %5.3f ms",
gProfiler.getAverageTime( Profiler::kRecordSolverFinish )*0.001f
);
m_guiHelper->getAppInterface()->drawText( msg, xCoord + indent, yCoord, 0.4f );
yCoord += yStep;
sprintf( msg,
"PredictUnconstrainedMotion %5.3f ms",
gProfiler.getAverageTime( Profiler::kRecordPredictUnconstrainedMotion )*0.001f

2
examples/MultiThreadedDemo/CommonRigidBodyMTBase.h
View File

@@ -14,6 +14,7 @@
enum SolverType
{
SOLVER_TYPE_SEQUENTIAL_IMPULSE,
SOLVER_TYPE_SEQUENTIAL_IMPULSE_MT,
SOLVER_TYPE_NNCG,
SOLVER_TYPE_MLCP_PGS,
SOLVER_TYPE_MLCP_DANTZIG,
@@ -27,6 +28,7 @@ inline const char* getSolverTypeName( SolverType t )
switch (t)
{
case SOLVER_TYPE_SEQUENTIAL_IMPULSE: return "SequentialImpulse";
case SOLVER_TYPE_SEQUENTIAL_IMPULSE_MT: return "SequentialImpulseMt";
case SOLVER_TYPE_NNCG: return "NNCG";
case SOLVER_TYPE_MLCP_PGS: return "MLCP ProjectedGaussSeidel";
case SOLVER_TYPE_MLCP_DANTZIG: return "MLCP Dantzig";

31
examples/MultiThreadedDemo/MultiThreadedDemo.cpp
View File

@@ -25,10 +25,10 @@ subject to the following restrictions:
static btScalar gSliderStackRows = 8.0f;
static btScalar gSliderStackColumns = 6.0f;
static btScalar gSliderStackHeight = 10.0f;
static btScalar gSliderStackWidth = 1.0f;
static btScalar gSliderStackRows = 1.0f;
static btScalar gSliderStackColumns = 1.0f;
static btScalar gSliderStackHeight = 15.0f;
static btScalar gSliderStackWidth = 8.0f;
static btScalar gSliderGroundHorizontalAmplitude = 0.0f;
static btScalar gSliderGroundVerticalAmplitude = 0.0f;
static btScalar gSliderGroundTilt = 0.0f;
@@ -75,6 +75,21 @@ public:
btScalar tilt = gSliderGroundTilt * SIMD_2_PI / 360.0f;
return btQuaternion( btVector3( 1.0f, 0.0f, 0.0f ), tilt );
}
struct TestSumBody : public btIParallelSumBody
{
virtual btScalar sumLoop( int iBegin, int iEnd ) const BT_OVERRIDE
{
btScalar sum = 0.0f;
for (int i = iBegin; i < iEnd; ++i)
{
if (i > 0)
{
sum += 1.0f / btScalar(i);
}
}
return sum;
}
};
virtual void stepSimulation( float deltaTime ) BT_OVERRIDE
{
if ( m_dynamicsWorld )
@@ -115,6 +130,14 @@ public:
// always step by 1/60 for benchmarking
m_dynamicsWorld->stepSimulation( 1.0f / 60.0f, 0 );
}
#if 0
{
// test parallelSum
TestSumBody testSumBody;
float testSum = btParallelSum( 1, 10000000, 10000, testSumBody );
printf( "sum = %f\n", testSum );
}
#endif
}
virtual void initPhysics() BT_OVERRIDE;

11
examples/MultiThreading/MultiThreadingExample.cpp
View File

@@ -21,7 +21,7 @@
void SampleThreadFunc(void* userPtr,void* lsMemory);
void* SamplelsMemoryFunc();
void SamplelsMemoryReleaseFunc(void* ptr);
#include <stdio.h>
//#include "BulletMultiThreaded/PlatformDefinitions.h"
@@ -34,6 +34,7 @@ b3ThreadSupportInterface* createThreadSupport(int numThreads)
b3PosixThreadSupport::ThreadConstructionInfo constructionInfo("testThreads",
SampleThreadFunc,
SamplelsMemoryFunc,
SamplelsMemoryReleaseFunc,
numThreads);
b3ThreadSupportInterface* threadSupport = new b3PosixThreadSupport(constructionInfo);
@@ -47,7 +48,7 @@ b3ThreadSupportInterface* createThreadSupport(int numThreads)
b3ThreadSupportInterface* createThreadSupport(int numThreads)
{
b3Win32ThreadSupport::Win32ThreadConstructionInfo threadConstructionInfo("testThreads",SampleThreadFunc,SamplelsMemoryFunc,numThreads);
b3Win32ThreadSupport::Win32ThreadConstructionInfo threadConstructionInfo("testThreads",SampleThreadFunc,SamplelsMemoryFunc,SamplelsMemoryReleaseFunc,numThreads);
b3Win32ThreadSupport* threadSupport = new b3Win32ThreadSupport(threadConstructionInfo);
return threadSupport;
@@ -155,7 +156,11 @@ void* SamplelsMemoryFunc()
return new SampleThreadLocalStorage;
}
void SamplelsMemoryReleaseFunc(void* ptr)
{
SampleThreadLocalStorage* p = (SampleThreadLocalStorage*) ptr;
delete p;
}

10
examples/MultiThreading/b3PosixThreadSupport.cpp
View File

@@ -251,7 +251,8 @@ void b3PosixThreadSupport::startThreads(ThreadConstructionInfo& threadConstructi
spuStatus.m_mainSemaphore = m_mainSemaphore;
spuStatus.m_lsMemory = threadConstructionInfo.m_lsMemoryFunc();
spuStatus.m_userThreadFunc = threadConstructionInfo.m_userThreadFunc;
spuStatus.threadUsed = 0;
spuStatus.m_lsMemoryReleaseFunc = threadConstructionInfo.m_lsMemoryReleaseFunc;
spuStatus.threadUsed = 0;
printf("started thread %d \n",i);
@@ -277,8 +278,13 @@ void b3PosixThreadSupport::stopThreads()
destroySem(spuStatus.startSemaphore);
printf("semaphore destroyed\n");
checkPThreadFunction(pthread_join(spuStatus.thread,0));
if (spuStatus.m_lsMemoryReleaseFunc)
{
spuStatus.m_lsMemoryReleaseFunc( spuStatus.m_lsMemory);
}
}
printf("destroy main semaphore\n");
destroySem(m_mainSemaphore);
printf("main semaphore destroyed\n");

7
examples/MultiThreading/b3PosixThreadSupport.h
View File

@@ -35,6 +35,7 @@ subject to the following restrictions:
typedef void (*b3PosixThreadFunc)(void* userPtr,void* lsMemory);
typedef void* (*b3PosixlsMemorySetupFunc)();
typedef void (*b3PosixlsMemoryReleaseFunc)(void* ptr);
// b3PosixThreadSupport helps to initialize/shutdown libspe2, start/stop SPU tasks and communication
class b3PosixThreadSupport : public b3ThreadSupportInterface
@@ -55,6 +56,8 @@ public:
b3PosixThreadFunc m_userThreadFunc;
void* m_userPtr; //for taskDesc etc
b3PosixlsMemoryReleaseFunc m_lsMemoryReleaseFunc;
void* m_lsMemory; //initialized using PosixLocalStoreMemorySetupFunc
pthread_t thread;
@@ -83,12 +86,14 @@ public:
ThreadConstructionInfo(const char* uniqueName,
b3PosixThreadFunc userThreadFunc,
b3PosixlsMemorySetupFunc lsMemoryFunc,
b3PosixlsMemoryReleaseFunc lsMemoryReleaseFunc,
int numThreads=1,
int threadStackSize=65535
)
:m_uniqueName(uniqueName),
m_userThreadFunc(userThreadFunc),
m_lsMemoryFunc(lsMemoryFunc),
m_lsMemoryReleaseFunc(lsMemoryReleaseFunc),
m_numThreads(numThreads),
m_threadStackSize(threadStackSize)
{
@@ -98,6 +103,8 @@ public:
const char* m_uniqueName;
b3PosixThreadFunc m_userThreadFunc;
b3PosixlsMemorySetupFunc m_lsMemoryFunc;
b3PosixlsMemoryReleaseFunc m_lsMemoryReleaseFunc;
int m_numThreads;
int m_threadStackSize;

8
examples/MultiThreading/b3Win32ThreadSupport.cpp
View File

@@ -289,6 +289,7 @@ void b3Win32ThreadSupport::startThreads(const Win32ThreadConstructionInfo& threa
threadStatus.m_threadHandle = handle;
threadStatus.m_lsMemory = threadConstructionInfo.m_lsMemoryFunc();
threadStatus.m_userThreadFunc = threadConstructionInfo.m_userThreadFunc;
threadStatus.m_lsMemoryReleaseFunc = threadConstructionInfo.m_lsMemoryReleaseFunc;
printf("started %s thread %d with threadHandle %p\n",threadConstructionInfo.m_uniqueName,i,handle);
@@ -312,9 +313,12 @@ void b3Win32ThreadSupport::stopThreads()
{
WaitForSingleObject(threadStatus.m_eventCompletetHandle, INFINITE);
}
delete threadStatus.m_lsMemory;
if (threadStatus.m_lsMemoryReleaseFunc)
{
threadStatus.m_lsMemoryReleaseFunc(threadStatus.m_lsMemory);
}
threadStatus.m_userPtr = 0;
SetEvent(threadStatus.m_eventStartHandle);
WaitForSingleObject(threadStatus.m_eventCompletetHandle, INFINITE);

6
examples/MultiThreading/b3Win32ThreadSupport.h
View File

@@ -26,6 +26,7 @@ subject to the following restrictions:
typedef void (*b3Win32ThreadFunc)(void* userPtr,void* lsMemory);
typedef void* (*b3Win32lsMemorySetupFunc)();
typedef void (*b3Win32lsMemoryReleaseFunc)(void*);
///b3Win32ThreadSupport helps to initialize/shutdown libspe2, start/stop SPU tasks and communication
@@ -43,6 +44,8 @@ public:
void* m_userPtr; //for taskDesc etc
void* m_lsMemory; //initialized using Win32LocalStoreMemorySetupFunc
b3Win32lsMemoryReleaseFunc m_lsMemoryReleaseFunc;
void* m_threadHandle; //this one is calling 'Win32ThreadFunc'
void* m_eventStartHandle;
@@ -67,12 +70,14 @@ public:
Win32ThreadConstructionInfo(const char* uniqueName,
b3Win32ThreadFunc userThreadFunc,
b3Win32lsMemorySetupFunc lsMemoryFunc,
b3Win32lsMemoryReleaseFunc lsMemoryReleaseFunc,
int numThreads=1,
int threadStackSize=65535
)
:m_uniqueName(uniqueName),
m_userThreadFunc(userThreadFunc),
m_lsMemoryFunc(lsMemoryFunc),
m_lsMemoryReleaseFunc(lsMemoryReleaseFunc),
m_numThreads(numThreads),
m_threadStackSize(threadStackSize),
m_priority(0)
@@ -83,6 +88,7 @@ public:
const char* m_uniqueName;
b3Win32ThreadFunc m_userThreadFunc;
b3Win32lsMemorySetupFunc m_lsMemoryFunc;
b3Win32lsMemoryReleaseFunc m_lsMemoryReleaseFunc;
int m_numThreads;
int m_threadStackSize;
int m_priority;

448
examples/MultiThreading/btTaskScheduler.cpp
View File

@@ -1,448 +0,0 @@
#include "LinearMath/btTransform.h"
#include "../Utils/b3Clock.h"
#include "LinearMath/btAlignedObjectArray.h"
#include "LinearMath/btThreads.h"
#include "LinearMath/btQuickprof.h"
#include <stdio.h>
#include <algorithm>
typedef void( *btThreadFunc )( void* userPtr, void* lsMemory );
typedef void* ( *btThreadLocalStorageFunc )();
#if BT_THREADSAFE
#if defined( _WIN32 )
#include "b3Win32ThreadSupport.h"
b3ThreadSupportInterface* createThreadSupport( int numThreads, btThreadFunc threadFunc, btThreadLocalStorageFunc localStoreFunc, const char* uniqueName )
{
b3Win32ThreadSupport::Win32ThreadConstructionInfo constructionInfo( uniqueName, threadFunc, localStoreFunc, numThreads );
//constructionInfo.m_priority = 0; // highest priority (the default) -- can cause erratic performance when numThreads > numCores
// we don't want worker threads to be higher priority than the main thread or the main thread could get
// totally shut out and unable to tell the workers to stop
constructionInfo.m_priority = -1; // normal priority
b3Win32ThreadSupport* threadSupport = new b3Win32ThreadSupport( constructionInfo );
return threadSupport;
}
#else // #if defined( _WIN32 )
#include "b3PosixThreadSupport.h"
b3ThreadSupportInterface* createThreadSupport( int numThreads, btThreadFunc threadFunc, btThreadLocalStorageFunc localStoreFunc, const char* uniqueName)
{
b3PosixThreadSupport::ThreadConstructionInfo constructionInfo( uniqueName, threadFunc, localStoreFunc, numThreads );
b3ThreadSupportInterface* threadSupport = new b3PosixThreadSupport( constructionInfo );
return threadSupport;
}
#endif // #else // #if defined( _WIN32 )
///
/// getNumHardwareThreads()
///
///
/// https://stackoverflow.com/questions/150355/programmatically-find-the-number-of-cores-on-a-machine
///
#if __cplusplus >= 201103L
#include <thread>
int getNumHardwareThreads()
{
return std::thread::hardware_concurrency();
}
#elif defined( _WIN32 )
#define WIN32_LEAN_AND_MEAN
#include <windows.h>
int getNumHardwareThreads()
{
// caps out at 32
SYSTEM_INFO info;
GetSystemInfo( &info );
return info.dwNumberOfProcessors;
}
#else
int getNumHardwareThreads()
{
return 0; // don't know
}
#endif
struct WorkerThreadStatus
{
enum Type
{
kInvalid,
kWaitingForWork,
kWorking,
kSleeping,
};
};
struct IJob
{
virtual void executeJob() = 0;
};
class ParallelForJob : public IJob
{
const btIParallelForBody* mBody;
int mBegin;
int mEnd;
public:
ParallelForJob()
{
mBody = NULL;
mBegin = 0;
mEnd = 0;
}
void init( int iBegin, int iEnd, const btIParallelForBody& body )
{
mBody = &body;
mBegin = iBegin;
mEnd = iEnd;
}
virtual void executeJob() BT_OVERRIDE
{
BT_PROFILE( "executeJob" );
// call the functor body to do the work
mBody->forLoop( mBegin, mEnd );
}
};
struct JobContext
{
JobContext()
{
m_queueLock = NULL;
m_headIndex = 0;
m_tailIndex = 0;
m_workersShouldCheckQueue = false;
m_useSpinMutex = false;
}
b3CriticalSection* m_queueLock;
btSpinMutex m_mutex;
volatile bool m_workersShouldCheckQueue;
btAlignedObjectArray<IJob*> m_jobQueue;
bool m_queueIsEmpty;
int m_tailIndex;
int m_headIndex;
bool m_useSpinMutex;
void lockQueue()
{
if ( m_useSpinMutex )
{
m_mutex.lock();
}
else
{
m_queueLock->lock();
}
}
void unlockQueue()
{
if ( m_useSpinMutex )
{
m_mutex.unlock();
}
else
{
m_queueLock->unlock();
}
}
void clearQueue()
{
lockQueue();
m_headIndex = 0;
m_tailIndex = 0;
m_queueIsEmpty = true;
unlockQueue();
m_jobQueue.resizeNoInitialize( 0 );
}
void submitJob( IJob* job )
{
m_jobQueue.push_back( job );
lockQueue();
m_tailIndex++;
m_queueIsEmpty = false;
unlockQueue();
}
IJob* consumeJob()
{
if ( m_queueIsEmpty )
{
// lock free path. even if this is taken erroneously it isn't harmful
return NULL;
}
IJob* job = NULL;
lockQueue();
if ( !m_queueIsEmpty )
{
job = m_jobQueue[ m_headIndex++ ];
if ( m_headIndex == m_tailIndex )
{
m_queueIsEmpty = true;
}
}
unlockQueue();
return job;
}
};
struct WorkerThreadLocalStorage
{
int threadId;
WorkerThreadStatus::Type status;
};
static void WorkerThreadFunc( void* userPtr, void* lsMemory )
{
BT_PROFILE( "WorkerThreadFunc" );
WorkerThreadLocalStorage* localStorage = (WorkerThreadLocalStorage*) lsMemory;
localStorage->status = WorkerThreadStatus::kWaitingForWork;
//printf( "WorkerThreadFunc: worker %d start working\n", localStorage->threadId );
JobContext* jobContext = (JobContext*) userPtr;
while ( jobContext->m_workersShouldCheckQueue )
{
if ( IJob* job = jobContext->consumeJob() )
{
localStorage->status = WorkerThreadStatus::kWorking;
job->executeJob();
localStorage->status = WorkerThreadStatus::kWaitingForWork;
}
else
{
// todo: spin wait a bit to avoid hammering the empty queue
}
}
//printf( "WorkerThreadFunc stop working\n" );
localStorage->status = WorkerThreadStatus::kSleeping;
// go idle
}
static void* WorkerThreadAllocFunc()
{
return new WorkerThreadLocalStorage;
}
class btTaskSchedulerDefault : public btITaskScheduler
{
JobContext m_jobContext;
b3ThreadSupportInterface* m_threadSupport;
btAlignedObjectArray<ParallelForJob> m_jobs;
btSpinMutex m_antiNestingLock; // prevent nested parallel-for
int m_numThreads;
int m_numWorkerThreads;
int m_numWorkersRunning;
public:
btTaskSchedulerDefault() : btITaskScheduler("ThreadSupport")
{
m_threadSupport = NULL;
m_numThreads = getNumHardwareThreads();
// if can't detect number of cores,
if ( m_numThreads == 0 )
{
// take a guess
m_numThreads = 4;
}
m_numWorkerThreads = m_numThreads - 1;
m_numWorkersRunning = 0;
}
virtual ~btTaskSchedulerDefault()
{
shutdown();
}
void init()
{
int maxNumWorkerThreads = BT_MAX_THREAD_COUNT - 1;
m_threadSupport = createThreadSupport( maxNumWorkerThreads, WorkerThreadFunc, WorkerThreadAllocFunc, "TaskScheduler" );
m_jobContext.m_queueLock = m_threadSupport->createCriticalSection();
for ( int i = 0; i < maxNumWorkerThreads; i++ )
{
WorkerThreadLocalStorage* storage = (WorkerThreadLocalStorage*) m_threadSupport->getThreadLocalMemory( i );
btAssert( storage );
storage->threadId = i;
storage->status = WorkerThreadStatus::kSleeping;
}
setWorkersActive( false ); // no work for them yet
}
virtual void shutdown()
{
setWorkersActive( false );
waitForWorkersToSleep();
m_threadSupport->deleteCriticalSection( m_jobContext.m_queueLock );
m_jobContext.m_queueLock = NULL;
delete m_threadSupport;
m_threadSupport = NULL;
}
void setWorkersActive( bool active )
{
m_jobContext.m_workersShouldCheckQueue = active;
}
virtual int getMaxNumThreads() const BT_OVERRIDE
{
return BT_MAX_THREAD_COUNT;
}
virtual int getNumThreads() const BT_OVERRIDE
{
return m_numThreads;
}
virtual void setNumThreads( int numThreads ) BT_OVERRIDE
{
m_numThreads = btMax( btMin(numThreads, int(BT_MAX_THREAD_COUNT)), 1 );
m_numWorkerThreads = m_numThreads - 1;
}
void waitJobs()
{
BT_PROFILE( "waitJobs" );
// have the main thread work until the job queue is empty
for ( ;; )
{
if ( IJob* job = m_jobContext.consumeJob() )
{
job->executeJob();
}
else
{
break;
}
}
// done with jobs for now, tell workers to rest
setWorkersActive( false );
waitForWorkersToSleep();
}
void wakeWorkers()
{
BT_PROFILE( "wakeWorkers" );
btAssert( m_jobContext.m_workersShouldCheckQueue );
// tell each worker thread to start working
for ( int i = 0; i < m_numWorkerThreads; i++ )
{
m_threadSupport->runTask( B3_THREAD_SCHEDULE_TASK, &m_jobContext, i );
m_numWorkersRunning++;
}
}
void waitForWorkersToSleep()
{
BT_PROFILE( "waitForWorkersToSleep" );
while ( m_numWorkersRunning > 0 )
{
int iThread;
int threadStatus;
m_threadSupport->waitForResponse( &iThread, &threadStatus ); // wait for worker threads to finish working
m_numWorkersRunning--;
}
//m_threadSupport->waitForAllTasksToComplete();
for ( int i = 0; i < m_numWorkerThreads; i++ )
{
//m_threadSupport->waitForTaskCompleted( i );
WorkerThreadLocalStorage* storage = (WorkerThreadLocalStorage*) m_threadSupport->getThreadLocalMemory( i );
btAssert( storage );
btAssert( storage->status == WorkerThreadStatus::kSleeping );
}
}
virtual void parallelFor( int iBegin, int iEnd, int grainSize, const btIParallelForBody& body ) BT_OVERRIDE
{
BT_PROFILE( "parallelFor_ThreadSupport" );
btAssert( iEnd >= iBegin );
btAssert( grainSize >= 1 );
int iterationCount = iEnd - iBegin;
if ( iterationCount > grainSize && m_numWorkerThreads > 0 && m_antiNestingLock.tryLock() )
{
int jobCount = ( iterationCount + grainSize - 1 ) / grainSize;
btAssert( jobCount >= 2 ); // need more than one job for multithreading
if ( jobCount > m_jobs.size() )
{
m_jobs.resize( jobCount );
}
if ( jobCount > m_jobContext.m_jobQueue.capacity() )
{
m_jobContext.m_jobQueue.reserve( jobCount );
}
m_jobContext.clearQueue();
// prepare worker threads for incoming work
setWorkersActive( true );
wakeWorkers();
// submit all of the jobs
int iJob = 0;
for ( int i = iBegin; i < iEnd; i += grainSize )
{
btAssert( iJob < jobCount );
int iE = btMin( i + grainSize, iEnd );
ParallelForJob& job = m_jobs[ iJob ];
job.init( i, iE, body );
m_jobContext.submitJob( &job );
iJob++;
}
// put the main thread to work on emptying the job queue and then wait for all workers to finish
waitJobs();
m_antiNestingLock.unlock();
}
else
{
BT_PROFILE( "parallelFor_mainThread" );
// just run on main thread
body.forLoop( iBegin, iEnd );
}
}
};
btITaskScheduler* createDefaultTaskScheduler()
{
btTaskSchedulerDefault* ts = new btTaskSchedulerDefault();
ts->init();
return ts;
}
#else // #if BT_THREADSAFE
btITaskScheduler* createDefaultTaskScheduler()
{
return NULL;
}
#endif // #else // #if BT_THREADSAFE

26
examples/MultiThreading/btTaskScheduler.h
View File

@@ -1,26 +0,0 @@
/*
Copyright (c) 2003-2014 Erwin Coumans http://bullet.googlecode.com
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.
*/
#ifndef BT_TASK_SCHEDULER_H
#define BT_TASK_SCHEDULER_H
class btITaskScheduler;
btITaskScheduler* createDefaultTaskScheduler();
#endif // BT_TASK_SCHEDULER_H

2
examples/OpenGLWindow/EGLOpenGLWindow.cpp
View File

@@ -38,7 +38,7 @@
#include "OpenGLInclude.h"
#include "third_party/GL/EGL/egl.h"
#include "third_party/GL/gl/include/EGL/egl.h"
#include "third_party/GL/gl/include/EGL/eglext.h"
#include "third_party/GL/gl/include/GL/gl.h"

88
examples/OpenGLWindow/GLInstancingRenderer.cpp
View File

@@ -361,7 +361,8 @@ GLInstancingRenderer::GLInstancingRenderer(int maxNumObjectCapacity, int maxShap
m_textureinitialized(false),
m_screenWidth(0),
m_screenHeight(0),
m_upAxis(1)
m_upAxis(1),
m_planeReflectionShapeIndex(-1)
{
m_data = new InternalDataRenderer;
@@ -508,6 +509,16 @@ void GLInstancingRenderer::writeSingleInstanceColorToCPU(const double* color, in
b3Assert(pg);
int srcIndex = pg->m_internalInstanceIndex;
int shapeIndex = pg->m_shapeIndex;
b3GraphicsInstance* gfxObj = m_graphicsInstances[shapeIndex];
if (color[3]<1)
{
gfxObj->m_flags |= eGfxTransparency;
} else
{
gfxObj->m_flags &= ~eGfxTransparency;
}
m_data->m_instance_colors_ptr[srcIndex*4+0]=float(color[0]);
m_data->m_instance_colors_ptr[srcIndex*4+1]=float(color[1]);
m_data->m_instance_colors_ptr[srcIndex*4+2]=float(color[2]);
@@ -519,6 +530,16 @@ void GLInstancingRenderer::writeSingleInstanceColorToCPU(const float* color, int
b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(srcIndex2);
b3Assert(pg);
int srcIndex = pg->m_internalInstanceIndex;
int shapeIndex = pg->m_shapeIndex;
b3GraphicsInstance* gfxObj = m_graphicsInstances[shapeIndex];
if (color[3]<1)
{
gfxObj->m_flags |= eGfxTransparency;
} else
{
gfxObj->m_flags &= ~eGfxTransparency;
}
m_data->m_instance_colors_ptr[srcIndex*4+0]=color[0];
m_data->m_instance_colors_ptr[srcIndex*4+1]=color[1];
@@ -1597,13 +1618,44 @@ void GLInstancingRenderer::renderScene()
{
renderSceneInternal(B3_CREATE_SHADOWMAP_RENDERMODE);
//glClear(GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT);
//renderSceneInternal(B3_USE_SHADOWMAP_RENDERMODE_REFLECTION);
glClear(GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT|GL_STENCIL_BUFFER_BIT);
if (m_planeReflectionShapeIndex>=0)
{
/* Don't update color or depth. */
glDisable(GL_DEPTH_TEST);
glColorMask(GL_FALSE, GL_FALSE, GL_FALSE, GL_FALSE);
/* Draw 1 into the stencil buffer. */
glEnable(GL_STENCIL_TEST);
glStencilOp(GL_REPLACE, GL_REPLACE, GL_REPLACE);
glStencilFunc(GL_ALWAYS, 1, 0xffffffff);
/* Now render floor; floor pixels just get their stencil set to 1. */
renderSceneInternal(B3_USE_SHADOWMAP_RENDERMODE_REFLECTION_PLANE);
/* Re-enable update of color and depth. */
glColorMask(GL_TRUE, GL_TRUE, GL_TRUE, GL_TRUE);
glEnable(GL_DEPTH_TEST);
/* Now, only render where stencil is set to 1. */
glStencilFunc(GL_EQUAL, 1, 0xffffffff); /* draw if ==1 */
glStencilOp(GL_KEEP, GL_KEEP, GL_KEEP);
//draw the reflection objects
renderSceneInternal(B3_USE_SHADOWMAP_RENDERMODE_REFLECTION);
glDisable(GL_STENCIL_TEST);
}
renderSceneInternal(B3_USE_SHADOWMAP_RENDERMODE);
}
else if (m_data->m_useProjectiveTexture)
{
renderSceneInternal(B3_USE_PROJECTIVE_TEXTURE_RENDERMODE);
}
else
@@ -2040,6 +2092,13 @@ void GLInstancingRenderer::renderSceneInternal(int orgRenderMode)
{
int renderMode=orgRenderMode;
bool reflectionPass = false;
bool reflectionPlanePass = false;
if (orgRenderMode==B3_USE_SHADOWMAP_RENDERMODE_REFLECTION_PLANE)
{
reflectionPlanePass = true;
renderMode = B3_USE_SHADOWMAP_RENDERMODE;
}
if (orgRenderMode==B3_USE_SHADOWMAP_RENDERMODE_REFLECTION)
{
reflectionPass = true;
@@ -2085,8 +2144,8 @@ void GLInstancingRenderer::renderSceneInternal(int orgRenderMode)
//GLfloat depthModelViewMatrix2[4][4];
// For projective texture mapping
float textureProjectionMatrix[4][4];
GLfloat textureModelViewMatrix[4][4];
//float textureProjectionMatrix[4][4];
//GLfloat textureModelViewMatrix[4][4];
// Compute the MVP matrix from the light's point of view
if (renderMode==B3_CREATE_SHADOWMAP_RENDERMODE)
@@ -2254,6 +2313,7 @@ b3Assert(glGetError() ==GL_NO_ERROR);
for (int obj=0;obj<m_graphicsInstances.size();obj++)
{
b3GraphicsInstance* gfxObj = m_graphicsInstances[obj];
if (gfxObj->m_numGraphicsInstances)
{
SortableTransparentInstance inst;
@@ -2300,9 +2360,16 @@ b3Assert(glGetError() ==GL_NO_ERROR);
{
for (int i=0;i<transparentInstances.size();i++)
{
int shapeIndex = transparentInstances[i].m_shapeIndex;
//during a reflectionPlanePass, only draw the plane, nothing else
if (reflectionPlanePass)
{
if (shapeIndex!=m_planeReflectionShapeIndex)
continue;
}
b3GraphicsInstance* gfxObj = m_graphicsInstances[shapeIndex];
//only draw stuff (opaque/transparent) if it is the right pass
@@ -2340,7 +2407,7 @@ b3Assert(glGetError() ==GL_NO_ERROR);
curBindTexture = m_data->m_defaultTexturehandle;
}
//disable lazy evaluation, it just leads to bugs
//disable lazy evaluation, it just leads to bugs
//if (lastBindTexture != curBindTexture)
{
glBindTexture(GL_TEXTURE_2D,curBindTexture);
@@ -2503,6 +2570,8 @@ b3Assert(glGetError() ==GL_NO_ERROR);
float MVP[16];
if (reflectionPass)
{
//todo: create an API to select this reflection matrix, to allow
//reflection planes different from Z-axis up through (0,0,0)
float tmp[16];
float reflectionMatrix[16] = {1,0,0,0,
0,1,0,0,
@@ -2669,6 +2738,11 @@ void GLInstancingRenderer::CleanupShaders()
{
}
void GLInstancingRenderer::setPlaneReflectionShapeIndex(int index)
{
m_planeReflectionShapeIndex = index;
}
void GLInstancingRenderer::enableShadowMap()
{
glActiveTexture(GL_TEXTURE0);

4
examples/OpenGLWindow/GLInstancingRenderer.h
View File

@@ -39,6 +39,8 @@ class GLInstancingRenderer : public CommonRenderInterface
int m_screenHeight;
int m_upAxis;
int m_planeReflectionShapeIndex;
int registerGraphicsInstanceInternal(int shapeIndex, const float* position, const float* quaternion, const float* color, const float* scaling);
@@ -151,6 +153,8 @@ public:
virtual int getTotalNumInstances() const;
virtual void enableShadowMap();
virtual void setPlaneReflectionShapeIndex(int index);
virtual void clearZBuffer();

8
examples/OpenGLWindow/OpenGL2Include.h
View File

@@ -17,12 +17,20 @@ subject to the following restrictions:
#ifndef __OPENGL_INCLUDE_H
#define __OPENGL_INCLUDE_H
#ifdef BT_NO_GLAD
#include "third_party/GL/gl/include/EGL/egl.h"
#include "third_party/GL/gl/include/EGL/eglext.h"
#include "third_party/GL/gl/include/GL/gl.h"
#else
#ifdef B3_USE_GLFW
#include "glad/glad.h"
#include <GLFW/glfw3.h>
#else
#include "glad/glad.h"
#endif //B3_USE_GLFW
#endif //BT_NO_GLAD
//disable glGetError
//#undef glGetError
//#define glGetError MyGetError

7
examples/OpenGLWindow/OpenGLInclude.h
View File

@@ -17,12 +17,17 @@ subject to the following restrictions:
#ifndef __OPENGL_INCLUDE_H
#define __OPENGL_INCLUDE_H
#ifdef BT_NO_GLAD
#include "third_party/GL/gl/include/EGL/egl.h"
#include "third_party/GL/gl/include/EGL/eglext.h"
#include "third_party/GL/gl/include/GL/gl.h"
#else
#ifdef B3_USE_GLFW
#include <glad/glad.h>
#include <GLFW/glfw3.h>
#else
#include "glad/glad.h"
#endif //B3_USE_GLFW
#endif //BT_NO_GLAD
#endif //__OPENGL_INCLUDE_H

4
examples/OpenGLWindow/SimpleOpenGL3App.cpp
View File

@@ -295,7 +295,7 @@ static void printGLString(const char *name, GLenum s) {
bool sOpenGLVerbose = true;
SimpleOpenGL3App::SimpleOpenGL3App( const char* title, int width,int height, bool allowRetina)
SimpleOpenGL3App::SimpleOpenGL3App(const char* title, int width, int height, bool allowRetina, int maxNumObjectCapacity, int maxShapeCapacityInBytes)
{
gApp = this;
@@ -369,7 +369,7 @@ SimpleOpenGL3App::SimpleOpenGL3App( const char* title, int width,int height, boo
b3Assert(glGetError() ==GL_NO_ERROR);
m_instancingRenderer = new GLInstancingRenderer(128*1024,128*1024*1024);
m_instancingRenderer = new GLInstancingRenderer(maxNumObjectCapacity, maxShapeCapacityInBytes);
m_primRenderer = new GLPrimitiveRenderer(width,height);

3
examples/OpenGLWindow/SimpleOpenGL3App.h
View File

@@ -16,7 +16,8 @@ struct SimpleOpenGL3App : public CommonGraphicsApp
class GLInstancingRenderer* m_instancingRenderer;
virtual void setBackgroundColor(float red, float green, float blue);
SimpleOpenGL3App(const char* title, int width,int height, bool allowRetina=true);
SimpleOpenGL3App(const char* title, int width,int height, bool allowRetina=true, int maxNumObjectCapacity = 128 * 1024, int maxShapeCapacityInBytes = 128 * 1024 * 1024);
virtual ~SimpleOpenGL3App();
virtual int registerCubeShape(float halfExtentsX=1.f,float halfExtentsY=1.f, float halfExtentsZ = 1.f, int textureIndex = -1, float textureScaling = 1);

5
examples/OpenGLWindow/X11OpenGLWindow.cpp
View File

@@ -6,11 +6,6 @@
#include<stdio.h>
#include<stdlib.h>
#ifdef GLEW_STATIC
#include "glad/glad.h"
#else
#include <GL/glew.h>
#endif//GLEW_STATIC
#ifdef GLEW_DYNAMIC_LOAD_ALL_GLX_FUNCTIONS
#include "glad/glad_glx.h"

5
examples/RobotSimulator/premake4.lua
View File

@@ -43,10 +43,7 @@ myfiles =
"../../examples/Utils/b3ResourcePath.h",
"../../examples/Utils/RobotLoggingUtil.cpp",
"../../examples/Utils/RobotLoggingUtil.h",
"../../examples/ThirdPartyLibs/tinyxml/tinystr.cpp",
"../../examples/ThirdPartyLibs/tinyxml/tinyxml.cpp",
"../../examples/ThirdPartyLibs/tinyxml/tinyxmlerror.cpp",
"../../examples/ThirdPartyLibs/tinyxml/tinyxmlparser.cpp",
"../../examples/ThirdPartyLibs/tinyxml2/tinyxml2.cpp",
"../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp",
"../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h",
"../../examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp",

83
examples/SharedMemory/CMakeLists.txt
View File

@@ -77,10 +77,7 @@ SharedMemoryCommands.h
../Importers/ImportColladaDemo/LoadMeshFromCollada.cpp
../Importers/ImportColladaDemo/ColladaGraphicsInstance.h
../ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp
../ThirdPartyLibs/tinyxml/tinystr.cpp
../ThirdPartyLibs/tinyxml/tinyxml.cpp
../ThirdPartyLibs/tinyxml/tinyxmlerror.cpp
../ThirdPartyLibs/tinyxml/tinyxmlparser.cpp
../ThirdPartyLibs/tinyxml2/tinyxml2.cpp
../Importers/ImportMeshUtility/b3ImportMeshUtility.cpp
../ThirdPartyLibs/stb_image/stb_image.cpp
../MultiThreading/b3ThreadSupportInterface.cpp
@@ -214,7 +211,7 @@ IF (INTERNAL_ADD_POSTFIX_EXECUTABLE_NAMES)
ENDIF(INTERNAL_ADD_POSTFIX_EXECUTABLE_NAMES)
#VR/OpenVR only on Windows and Mac OSX for now
#VR/OpenVR on Windows and Mac OSX
IF (WIN32 OR APPLE)
INCLUDE_DIRECTORIES(
@@ -328,4 +325,80 @@ IF (INTERNAL_ADD_POSTFIX_EXECUTABLE_NAMES)
SET_TARGET_PROPERTIES(App_PhysicsServer_SharedMemory_VR PROPERTIES RELWITHDEBINFO_POSTFIX "_RelWithDebugInfo")
ENDIF(INTERNAL_ADD_POSTFIX_EXECUTABLE_NAMES)
#VR/OpenVR on Linux
ELSE(WIN32 OR APPLE)
IF(CMAKE_SIZEOF_VOID_P MATCHES 8)
LINK_DIRECTORIES(${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs/openvr/bin/linux64)
ELSE()
LINK_DIRECTORIES(${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs/openvr/bin/linux32)
ENDIF()
ADD_EXECUTABLE(App_PhysicsServer_SharedMemory_VR
${SharedMemory_SRCS}
../StandaloneMain/hellovr_opengl_main.cpp
../ExampleBrowser/OpenGLGuiHelper.cpp
../ExampleBrowser/GL_ShapeDrawer.cpp
../ExampleBrowser/CollisionShape2TriangleMesh.cpp
../RenderingExamples/TinyVRGui.cpp
../RenderingExamples/TinyVRGui.h
../RenderingExamples/TimeSeriesCanvas.cpp
../RenderingExamples/TimeSeriesFontData.cpp
../MultiThreading/b3PosixThreadSupport.cpp
../MultiThreading/b3PosixThreadSupport.h
../ThirdPartyLibs/openvr/samples/shared/lodepng.cpp
../ThirdPartyLibs/openvr/samples/shared/lodepng.h
../ThirdPartyLibs/openvr/samples/shared/Matrices.cpp
../ThirdPartyLibs/openvr/samples/shared/Matrices.h
../ThirdPartyLibs/openvr/samples/shared/pathtools.cpp
../ThirdPartyLibs/openvr/samples/shared/pathtools.h
../ThirdPartyLibs/openvr/samples/shared/strtools.cpp
../ThirdPartyLibs/openvr/samples/shared/strtools.h
../ThirdPartyLibs/openvr/samples/shared/Vectors.h
)
target_include_directories(App_PhysicsServer_SharedMemory_VR PRIVATE
${BULLET_PHYSICS_SOURCE_DIR}/src
${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs
${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs/Glew
${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs/openvr/headers
${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs/openvr/samples
${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs/openvr/samples/shared
)
target_compile_definitions(App_PhysicsServer_SharedMemory_VR PRIVATE
POSIX
LINUX
BT_ENABLE_VR
GLEW_STATIC
GLEW_INIT_OPENGL11_FUNCTIONS=1
GLEW_DYNAMIC_LOAD_ALL_GLX_FUNCTIONS=1
)
target_compile_options(App_PhysicsServer_SharedMemory_VR PRIVATE
-std=c++11
)
target_link_libraries(App_PhysicsServer_SharedMemory_VR PRIVATE
openvr_api
pthread
${DL}
${OPENGL_gl_LIBRARY}
${OPENGL_glu_LIBRARY}
Bullet3Common
BulletWorldImporter
BulletInverseDynamicsUtils
BulletInverseDynamics
BulletDynamics
BulletCollision
LinearMath
BussIK
OpenGLWindow
)
IF (INTERNAL_ADD_POSTFIX_EXECUTABLE_NAMES)
SET_TARGET_PROPERTIES(App_PhysicsServer_SharedMemory_VR PROPERTIES DEBUG_POSTFIX "_Debug")
SET_TARGET_PROPERTIES(App_PhysicsServer_SharedMemory_VR PROPERTIES MINSIZEREL_POSTFIX "_MinsizeRel")
SET_TARGET_PROPERTIES(App_PhysicsServer_SharedMemory_VR PROPERTIES RELWITHDEBINFO_POSTFIX "_RelWithDebugInfo")
ENDIF(INTERNAL_ADD_POSTFIX_EXECUTABLE_NAMES)
ENDIF(WIN32 OR APPLE)

10
examples/SharedMemory/PhysicsClientUDP.cpp
View File

@@ -11,6 +11,8 @@
#include "../MultiThreading/b3ThreadSupportInterface.h"
void UDPThreadFunc(void* userPtr, void* lsMemory);
void* UDPlsMemoryFunc();
void UDPlsMemoryReleaseFunc(void* ptr);
bool gVerboseNetworkMessagesClient = false;
#ifndef _WIN32
@@ -21,6 +23,7 @@ b3ThreadSupportInterface* createUDPThreadSupport(int numThreads)
b3PosixThreadSupport::ThreadConstructionInfo constructionInfo("UDPThread",
UDPThreadFunc,
UDPlsMemoryFunc,
UDPlsMemoryReleaseFunc,
numThreads);
b3ThreadSupportInterface* threadSupport = new b3PosixThreadSupport(constructionInfo);
@@ -34,7 +37,7 @@ b3ThreadSupportInterface* createUDPThreadSupport(int numThreads)
b3ThreadSupportInterface* createUDPThreadSupport(int numThreads)
{
b3Win32ThreadSupport::Win32ThreadConstructionInfo threadConstructionInfo("UDPThread", UDPThreadFunc, UDPlsMemoryFunc, numThreads);
b3Win32ThreadSupport::Win32ThreadConstructionInfo threadConstructionInfo("UDPThread", UDPThreadFunc, UDPlsMemoryFunc,UDPlsMemoryReleaseFunc, numThreads);
b3Win32ThreadSupport* threadSupport = new b3Win32ThreadSupport(threadConstructionInfo);
return threadSupport;
@@ -436,6 +439,11 @@ void* UDPlsMemoryFunc()
return new UDPThreadLocalStorage;
}
void UDPlsMemoryReleaseFunc(void* ptr)
{
UDPThreadLocalStorage* p = (UDPThreadLocalStorage*) ptr;
delete p;
}

231
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -150,16 +150,16 @@ struct InternalVisualShapeData
int m_tinyRendererVisualShapeIndex;
int m_OpenGLGraphicsIndex;
UrdfVisual m_visualShape;
btTransform m_localInertiaFrame;
std::string m_pathPrefix;
b3AlignedObjectArray<UrdfVisual> m_visualShapes;
b3AlignedObjectArray<std::string> m_pathPrefixes;
void clear()
{
m_tinyRendererVisualShapeIndex = 0;
m_OpenGLGraphicsIndex = 0;
m_localInertiaFrame.setIdentity();
m_pathPrefix = "";
m_tinyRendererVisualShapeIndex = -1;
m_OpenGLGraphicsIndex = -1;
m_visualShapes.clear();
m_pathPrefixes.clear();
}
};
@@ -482,10 +482,10 @@ struct CommandLogPlayback
SharedMemoryCommand unused;
#endif//BACKWARD_COMPAT
bool result = false;
size_t s = 0;
if (m_file)
{
size_t s = 0;
int commandType = -1;
if (m_fileIs64bit)
@@ -518,8 +518,8 @@ struct CommandLogPlayback
#ifdef BACKWARD_COMPAT
cmd->m_mjcfArguments = unused.m_mjcfArguments;
#else
fread(&cmd->m_updateFlags,sizeof(int),1,m_file);
fread(&cmd->m_mjcfArguments,sizeof(MjcfArgs),1,m_file);
s=fread(&cmd->m_updateFlags,sizeof(int),1,m_file);
s = fread(&cmd->m_mjcfArguments,sizeof(MjcfArgs),1,m_file);
#endif
result=true;
break;
@@ -529,8 +529,8 @@ struct CommandLogPlayback
#ifdef BACKWARD_COMPAT
cmd->m_sdfRequestInfoArgs = unused.m_sdfRequestInfoArgs;
#else
fread(&cmd->m_updateFlags,sizeof(int),1,m_file);
fread(&cmd->m_sdfRequestInfoArgs,sizeof(SdfRequestInfoArgs),1,m_file);
s = fread(&cmd->m_updateFlags,sizeof(int),1,m_file);
s = fread(&cmd->m_sdfRequestInfoArgs,sizeof(SdfRequestInfoArgs),1,m_file);
#endif
result=true;
break;
@@ -540,8 +540,8 @@ struct CommandLogPlayback
#ifdef BACKWARD_COMPAT
cmd->m_requestVisualShapeDataArguments = unused.m_requestVisualShapeDataArguments;
#else
fread(&cmd->m_updateFlags,sizeof(int),1,m_file);
fread(&cmd->m_requestVisualShapeDataArguments,sizeof(RequestVisualShapeDataArgs),1,m_file);
s = fread(&cmd->m_updateFlags,sizeof(int),1,m_file);
s = fread(&cmd->m_requestVisualShapeDataArguments,sizeof(RequestVisualShapeDataArgs),1,m_file);
#endif
result=true;
break;
@@ -551,8 +551,8 @@ struct CommandLogPlayback
#ifdef BACKWARD_COMPAT
cmd->m_urdfArguments = unused.m_urdfArguments;
#else
fread(&cmd->m_updateFlags,sizeof(int),1,m_file);
fread(&cmd->m_urdfArguments,sizeof(UrdfArgs),1,m_file);
s = fread(&cmd->m_updateFlags,sizeof(int),1,m_file);
s = fread(&cmd->m_urdfArguments,sizeof(UrdfArgs),1,m_file);
#endif
result=true;
break;
@@ -562,8 +562,8 @@ struct CommandLogPlayback
#ifdef BACKWARD_COMPAT
cmd->m_initPoseArgs = unused.m_initPoseArgs;
#else
fread(&cmd->m_updateFlags,sizeof(int),1,m_file);
fread(&cmd->m_initPoseArgs,sizeof(InitPoseArgs),1,m_file);
s = fread(&cmd->m_updateFlags,sizeof(int),1,m_file);
s = fread(&cmd->m_initPoseArgs,sizeof(InitPoseArgs),1,m_file);
#endif
result=true;
@@ -574,8 +574,8 @@ struct CommandLogPlayback
#ifdef BACKWARD_COMPAT
cmd->m_requestActualStateInformationCommandArgument = unused.m_requestActualStateInformationCommandArgument;
#else
fread(&cmd->m_updateFlags,sizeof(int),1,m_file);
fread(&cmd->m_requestActualStateInformationCommandArgument,sizeof(RequestActualStateArgs),1,m_file);
s = fread(&cmd->m_updateFlags,sizeof(int),1,m_file);
s = fread(&cmd->m_requestActualStateInformationCommandArgument,sizeof(RequestActualStateArgs),1,m_file);
#endif
result=true;
break;
@@ -585,8 +585,8 @@ struct CommandLogPlayback
#ifdef BACKWARD_COMPAT
cmd->m_sendDesiredStateCommandArgument = unused.m_sendDesiredStateCommandArgument;
#else
fread(&cmd->m_updateFlags,sizeof(int),1,m_file);
fread(&cmd->m_sendDesiredStateCommandArgument ,sizeof(SendDesiredStateArgs),1,m_file);
s = fread(&cmd->m_updateFlags,sizeof(int),1,m_file);
s = fread(&cmd->m_sendDesiredStateCommandArgument ,sizeof(SendDesiredStateArgs),1,m_file);
#endif
result = true;
@@ -597,8 +597,8 @@ struct CommandLogPlayback
#ifdef BACKWARD_COMPAT
cmd->m_physSimParamArgs = unused.m_physSimParamArgs;
#else
fread(&cmd->m_updateFlags,sizeof(int),1,m_file);
fread(&cmd->m_physSimParamArgs ,sizeof(b3PhysicsSimulationParameters),1,m_file);
s = fread(&cmd->m_updateFlags,sizeof(int),1,m_file);
s = fread(&cmd->m_physSimParamArgs ,sizeof(b3PhysicsSimulationParameters),1,m_file);
#endif
result = true;
@@ -609,8 +609,8 @@ struct CommandLogPlayback
#ifdef BACKWARD_COMPAT
cmd->m_requestContactPointArguments = unused.m_requestContactPointArguments;
#else
fread(&cmd->m_updateFlags,sizeof(int),1,m_file);
fread(&cmd->m_requestContactPointArguments ,sizeof(RequestContactDataArgs),1,m_file);
s = fread(&cmd->m_updateFlags,sizeof(int),1,m_file);
s = fread(&cmd->m_requestContactPointArguments ,sizeof(RequestContactDataArgs),1,m_file);
#endif
result = true;
@@ -1921,10 +1921,13 @@ struct ProgrammaticUrdfInterface : public URDFImporterInterface
const InternalVisualShapeHandle* visHandle = m_data->m_userVisualShapeHandles.getHandle(m_createBodyArgs.m_linkVisualShapeUniqueIds[linkIndex]);
if (visHandle)
{
if (visHandle->m_visualShape.m_geometry.m_hasLocalMaterial)
for (int i=0;i<visHandle->m_visualShapes.size();i++)
{
matCol = visHandle->m_visualShape.m_geometry.m_localMaterial.m_matColor;
return true;
if (visHandle->m_visualShapes[i].m_geometry.m_hasLocalMaterial)
{
matCol = visHandle->m_visualShapes[i].m_geometry.m_localMaterial.m_matColor;
return true;
}
}
}
}
@@ -2092,39 +2095,91 @@ struct ProgrammaticUrdfInterface : public URDFImporterInterface
b3Assert(0);
}
///quick hack: need to rethink the API/dependencies of this
virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const
virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const
{
int graphicsIndex = -1;
double globalScaling = 1.f;//todo!
int flags=0;
BulletURDFImporter u2b(m_data->m_guiHelper, m_data->m_pluginManager.getRenderInterface(), globalScaling, flags);
u2b.setEnableTinyRenderer(m_data->m_enableTinyRenderer);
btAlignedObjectArray<GLInstanceVertex> vertices;
btAlignedObjectArray<int> indices;
btTransform startTrans; startTrans.setIdentity();
btAlignedObjectArray<BulletURDFTexture> textures;
if (m_createBodyArgs.m_linkVisualShapeUniqueIds[linkIndex]>=0)
{
const InternalVisualShapeHandle* visHandle = m_data->m_userVisualShapeHandles.getHandle(m_createBodyArgs.m_linkVisualShapeUniqueIds[linkIndex]);
InternalVisualShapeHandle* visHandle = m_data->m_userVisualShapeHandles.getHandle(m_createBodyArgs.m_linkVisualShapeUniqueIds[linkIndex]);
if (visHandle)
{
return visHandle->m_OpenGLGraphicsIndex;
if (visHandle->m_OpenGLGraphicsIndex>=0)
{
//instancing. assume the inertial frame is identical
graphicsIndex = visHandle->m_OpenGLGraphicsIndex;
} else
{
for (int v = 0;v<visHandle->m_visualShapes.size();v++)
{
u2b.convertURDFToVisualShapeInternal(&visHandle->m_visualShapes[v], pathPrefix, localInertiaFrame.inverse()*visHandle->m_visualShapes[v].m_linkLocalFrame, vertices, indices, textures);
}
if (vertices.size() && indices.size())
{
if (1)
{
int textureIndex = -1;
if (textures.size())
{
textureIndex = m_data->m_guiHelper->registerTexture(textures[0].textureData1, textures[0].m_width, textures[0].m_height);
}
{
B3_PROFILE("registerGraphicsShape");
graphicsIndex = m_data->m_guiHelper->registerGraphicsShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size(), B3_GL_TRIANGLES, textureIndex);
visHandle->m_OpenGLGraphicsIndex = graphicsIndex;
}
}
}
}
}
}
return -1;
return graphicsIndex;
}
virtual void convertLinkVisualShapes2(int linkIndex, int urdfIndex, const char* pathPrefix, const btTransform& localInertiaFrame, class btCollisionObject* colObj, int bodyUniqueId) const
{
//if there is a visual, use it, otherwise convert collision shape back into UrdfCollision...
UrdfModel model;// = m_data->m_urdfParser.getModel();
UrdfLink link;
int colShapeUniqueId = m_createBodyArgs.m_linkCollisionShapeUniqueIds[urdfIndex];
if (colShapeUniqueId>=0)
if (m_createBodyArgs.m_linkVisualShapeUniqueIds[linkIndex]>=0)
{
InternalCollisionShapeHandle* handle = m_data->m_userCollisionShapeHandles.getHandle(colShapeUniqueId);
if (handle)
const InternalVisualShapeHandle* visHandle = m_data->m_userVisualShapeHandles.getHandle(m_createBodyArgs.m_linkVisualShapeUniqueIds[linkIndex]);
if (visHandle)
{
for (int i=0;i<handle->m_urdfCollisionObjects.size();i++)
for (int i=0;i<visHandle->m_visualShapes.size();i++)
{
link.m_collisionArray.push_back(handle->m_urdfCollisionObjects[i]);
link.m_visualArray.push_back(visHandle->m_visualShapes[i]);
}
}
}
if (link.m_visualArray.size()==0)
{
int colShapeUniqueId = m_createBodyArgs.m_linkCollisionShapeUniqueIds[urdfIndex];
if (colShapeUniqueId>=0)
{
InternalCollisionShapeHandle* handle = m_data->m_userCollisionShapeHandles.getHandle(colShapeUniqueId);
if (handle)
{
for (int i=0;i<handle->m_urdfCollisionObjects.size();i++)
{
link.m_collisionArray.push_back(handle->m_urdfCollisionObjects[i]);
}
}
}
}
@@ -3968,19 +4023,16 @@ bool PhysicsServerCommandProcessor::processCreateVisualShapeCommand(const struct
u2b.setEnableTinyRenderer(m_data->m_enableTinyRenderer);
btTransform localInertiaFrame;
localInertiaFrame.setIdentity();
btTransform childTrans;
childTrans.setIdentity();
const char* pathPrefix = "";
btAlignedObjectArray<GLInstanceVertex> vertices;
btAlignedObjectArray<int> indices;
btTransform startTrans; startTrans.setIdentity();
btAlignedObjectArray<BulletURDFTexture> textures;
int visualShapeUniqueId = -1;
UrdfVisual visualShape;
for (int userShapeIndex = 0; userShapeIndex< clientCmd.m_createUserShapeArgs.m_numUserShapes; userShapeIndex++)
{
btTransform childTrans;
childTrans.setIdentity();
visualShape.m_geometry.m_type = (UrdfGeomTypes)clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_type;
char relativeFileName[1024];
char pathPrefix[1024];
@@ -4106,41 +4158,23 @@ bool PhysicsServerCommandProcessor::processCreateVisualShapeCommand(const struct
}
u2b.convertURDFToVisualShapeInternal(&visualShape, pathPrefix, localInertiaFrame.inverse()*childTrans, vertices, indices, textures);
}
if (vertices.size() && indices.size())
{
if (1)
if (visualShapeUniqueId<0)
{
int textureIndex = -1;
if (textures.size())
{
textureIndex = m_data->m_guiHelper->registerTexture(textures[0].textureData1, textures[0].m_width, textures[0].m_height);
}
int graphicsIndex = -1;
{
B3_PROFILE("registerGraphicsShape");
graphicsIndex = m_data->m_guiHelper->registerGraphicsShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size(), B3_GL_TRIANGLES, textureIndex);
if (graphicsIndex >= 0)
{
int visualShapeUniqueId = m_data->m_userVisualShapeHandles.allocHandle();
InternalVisualShapeHandle* visualHandle = m_data->m_userVisualShapeHandles.getHandle(visualShapeUniqueId);
visualHandle->m_OpenGLGraphicsIndex = graphicsIndex;
visualHandle->m_tinyRendererVisualShapeIndex = -1;
//tinyrenderer doesn't separate shape versus instance, so create it when creating the multibody instance
//store needed info for tinyrenderer
visualHandle->m_localInertiaFrame = localInertiaFrame;
visualHandle->m_visualShape = visualShape;
visualHandle->m_pathPrefix = pathPrefix[0] ? pathPrefix : "";
serverStatusOut.m_createUserShapeResultArgs.m_userShapeUniqueId = visualShapeUniqueId;
serverStatusOut.m_type = CMD_CREATE_VISUAL_SHAPE_COMPLETED;
}
}
visualShapeUniqueId = m_data->m_userVisualShapeHandles.allocHandle();
}
InternalVisualShapeHandle* visualHandle = m_data->m_userVisualShapeHandles.getHandle(visualShapeUniqueId);
visualHandle->m_OpenGLGraphicsIndex = -1;
visualHandle->m_tinyRendererVisualShapeIndex = -1;
//tinyrenderer doesn't separate shape versus instance, so create it when creating the multibody instance
//store needed info for tinyrenderer
visualShape.m_linkLocalFrame = childTrans;
visualHandle->m_visualShapes.push_back(visualShape);
visualHandle->m_pathPrefixes.push_back(pathPrefix[0] ? pathPrefix : "");
serverStatusOut.m_createUserShapeResultArgs.m_userShapeUniqueId = visualShapeUniqueId;
serverStatusOut.m_type = CMD_CREATE_VISUAL_SHAPE_COMPLETED;
}
return hasStatus;
@@ -4760,6 +4794,9 @@ bool PhysicsServerCommandProcessor::processSendDesiredStateCommand(const struct
}
if (hasDesiredVelocity)
{
//disable velocity clamp in velocity mode
motor->setRhsClamp(SIMD_INFINITY);
btScalar maxImp = 1000000.f*m_data->m_physicsDeltaTime;
if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex]&SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0)
{
@@ -4973,9 +5010,7 @@ bool PhysicsServerCommandProcessor::processSendDesiredStateCommand(const struct
{
con->enableMotor(3+limitAxis,true);
con->setTargetVelocity(3+limitAxis, qdotTarget);
//this is max motor force impulse
btScalar torqueImpulse = torque*m_data->m_dynamicsWorld->getSolverInfo().m_timeStep;
con->setMaxMotorForce(3+limitAxis,torqueImpulse);
con->setMaxMotorForce(3+limitAxis, torque);
}
break;
}
@@ -4988,9 +5023,7 @@ bool PhysicsServerCommandProcessor::processSendDesiredStateCommand(const struct
//next one is the maximum velocity to reach target position.
//the maximum velocity is limited by maxMotorForce
con->setTargetVelocity(3+limitAxis, 100);
//this is max motor force impulse
btScalar torqueImpulse = torque*m_data->m_dynamicsWorld->getSolverInfo().m_timeStep;
con->setMaxMotorForce(3+limitAxis,torqueImpulse);
con->setMaxMotorForce(3+limitAxis, torque);
con->enableMotor(3+limitAxis,true);
}
break;
@@ -5026,9 +5059,7 @@ bool PhysicsServerCommandProcessor::processSendDesiredStateCommand(const struct
{
con->enableMotor(limitAxis,true);
con->setTargetVelocity(limitAxis, -qdotTarget);
//this is max motor force impulse
btScalar torqueImpulse = torque*m_data->m_dynamicsWorld->getSolverInfo().m_timeStep;
con->setMaxMotorForce(limitAxis,torqueImpulse);
con->setMaxMotorForce(limitAxis, torque);
break;
}
case CONTROL_MODE_POSITION_VELOCITY_PD:
@@ -5038,9 +5069,7 @@ bool PhysicsServerCommandProcessor::processSendDesiredStateCommand(const struct
//next one is the maximum velocity to reach target position.
//the maximum velocity is limited by maxMotorForce
con->setTargetVelocity(limitAxis, 100);
//this is max motor force impulse
btScalar torqueImpulse = torque*m_data->m_dynamicsWorld->getSolverInfo().m_timeStep;
con->setMaxMotorForce(limitAxis,torqueImpulse);
con->setMaxMotorForce(limitAxis, torque);
con->enableMotor(limitAxis,true);
break;
}
@@ -9665,6 +9694,12 @@ void PhysicsServerCommandProcessor::resetSimulation()
{
//clean up all data
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
if (m_data && m_data->m_dynamicsWorld)
{
m_data->m_dynamicsWorld->getWorldInfo().m_sparsesdf.Reset();
}
#endif
if (m_data && m_data->m_guiHelper)
{
m_data->m_guiHelper->removeAllGraphicsInstances();

16
examples/SharedMemory/PhysicsServerExample.cpp
View File

@@ -92,6 +92,7 @@ static void saveCurrentSettingsVR(const btVector3& VRTeleportPos1)
bool gDebugRenderToggle = false;
void MotionThreadFunc(void* userPtr,void* lsMemory);
void* MotionlsMemoryFunc();
void MotionlsMemoryReleaseFunc(void* ptr);
#define MAX_MOTION_NUM_THREADS 1
enum
{
@@ -150,6 +151,7 @@ b3ThreadSupportInterface* createMotionThreadSupport(int numThreads)
b3PosixThreadSupport::ThreadConstructionInfo constructionInfo("MotionThreads",
MotionThreadFunc,
MotionlsMemoryFunc,
MotionlsMemoryReleaseFunc,
numThreads);
b3ThreadSupportInterface* threadSupport = new b3PosixThreadSupport(constructionInfo);
@@ -163,7 +165,7 @@ b3ThreadSupportInterface* createMotionThreadSupport(int numThreads)
b3ThreadSupportInterface* createMotionThreadSupport(int numThreads)
{
b3Win32ThreadSupport::Win32ThreadConstructionInfo threadConstructionInfo("MotionThreads",MotionThreadFunc,MotionlsMemoryFunc,numThreads);
b3Win32ThreadSupport::Win32ThreadConstructionInfo threadConstructionInfo("MotionThreads",MotionThreadFunc,MotionlsMemoryFunc,MotionlsMemoryReleaseFunc,numThreads);
b3Win32ThreadSupport* threadSupport = new b3Win32ThreadSupport(threadConstructionInfo);
return threadSupport;
@@ -486,6 +488,12 @@ void* MotionlsMemoryFunc()
return new MotionThreadLocalStorage;
}
void MotionlsMemoryReleaseFunc(void* ptr)
{
MotionThreadLocalStorage* p = (MotionThreadLocalStorage*) ptr;
delete p;
}
struct UserDebugDrawLine
@@ -756,6 +764,12 @@ public:
delete m_debugDraw;
m_debugDraw = 0;
}
for (int i=0;i<m_userDebugParams.size();i++)
{
delete m_userDebugParams[i];
}
m_userDebugParams.clear();
}
void setCriticalSection(b3CriticalSection* cs)

2
examples/SharedMemory/PhysicsServerSharedMemory.cpp
View File

@@ -170,7 +170,7 @@ bool PhysicsServerSharedMemory::connectSharedMemory( struct GUIHelperInterface*
}
} else
{
b3Error("Cannot connect to shared memory");
//b3Error("Cannot connect to shared memory");
m_data->m_areConnected[block] = false;
}
} while (counter++ < 10 && !m_data->m_areConnected[block]);

6
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -23,7 +23,11 @@
typedef unsigned long long int smUint64_t;
#endif
#define SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE (8*1024*1024)
#ifdef __APPLE__
#define SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE (512*1024)
#else
#define SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE (8*1024*1024)
#endif
#define SHARED_MEMORY_SERVER_TEST_C
#define MAX_DEGREE_OF_FREEDOM 128

1
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -648,6 +648,7 @@ enum b3ConfigureDebugVisualizerEnum
COV_ENABLE_RGB_BUFFER_PREVIEW,
COV_ENABLE_DEPTH_BUFFER_PREVIEW,
COV_ENABLE_SEGMENTATION_MARK_PREVIEW,
COV_ENABLE_PLANAR_REFLECTION,
};

4
examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp
View File

@@ -593,7 +593,7 @@ void TinyRendererVisualShapeConverter::convertVisualShapes(
colorIndex &=3;
btVector4 color;
color = sColors[colorIndex];
float rgbaColor[4] = {color[0],color[1],color[2],color[3]};
float rgbaColor[4] = {(float)color[0],(float)color[1],(float)color[2],(float)color[3]};
//if (colObj->getCollisionShape()->getShapeType()==STATIC_PLANE_PROXYTYPE)
//{
// color.setValue(1,1,1,1);
@@ -773,7 +773,7 @@ void TinyRendererVisualShapeConverter::changeRGBAColor(int bodyUniqueId, int lin
TinyRendererObjectArray** ptrptr = m_data->m_swRenderInstances.getAtIndex(i);
if (ptrptr && *ptrptr)
{
float rgba[4] = {rgbaColor[0], rgbaColor[1], rgbaColor[2], rgbaColor[3]};
float rgba[4] = {(float)rgbaColor[0], (float)rgbaColor[1], (float)rgbaColor[2], (float)rgbaColor[3]};
TinyRendererObjectArray* visuals = *ptrptr;
if ((bodyUniqueId == visuals->m_objectUniqueId) && (linkIndex == visuals->m_linkIndex))
{

8
examples/SharedMemory/plugins/tinyRendererPlugin/premake4.lua
View File

@@ -39,12 +39,8 @@ project ("pybullet_tinyRendererPlugin")
"../../../ThirdPartyLibs/Wavefront/tiny_obj_loader.h",
"../../../ThirdPartyLibs/stb_image/stb_image.cpp",
"../../../ThirdPartyLibs/stb_image/stb_image.h",
"../../../ThirdPartyLibs/tinyxml/tinystr.cpp",
"../../../ThirdPartyLibs/tinyxml/tinystr.h",
"../../../ThirdPartyLibs/tinyxml/tinyxml.cpp",
"../../../ThirdPartyLibs/tinyxml/tinyxml.h",
"../../../ThirdPartyLibs/tinyxml/tinyxmlerror.cpp",
"../../../ThirdPartyLibs/tinyxml/tinyxmlparser.cpp",
"../../../ThirdPartyLibs/tinyxml2/tinyxml2.cpp",
"../../../ThirdPartyLibs/tinyxml2/tinyxml2.h",
"../../../OpenGLWindow/SimpleCamera.cpp",
"../../../OpenGLWindow/SimpleCamera.h",
"../../../Utils/b3Clock.cpp",

5
examples/SharedMemory/premake4.lua
View File

@@ -91,10 +91,7 @@ myfiles =
"../Importers/ImportColladaDemo/LoadMeshFromCollada.cpp",
"../Importers/ImportColladaDemo/ColladaGraphicsInstance.h",
"../ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp",
"../ThirdPartyLibs/tinyxml/tinystr.cpp",
"../ThirdPartyLibs/tinyxml/tinyxml.cpp",
"../ThirdPartyLibs/tinyxml/tinyxmlerror.cpp",
"../ThirdPartyLibs/tinyxml/tinyxmlparser.cpp",
"../ThirdPartyLibs/tinyxml2/tinyxml2.cpp",
"../Importers/ImportMeshUtility/b3ImportMeshUtility.cpp",
"../ThirdPartyLibs/stb_image/stb_image.cpp",

5
examples/SharedMemory/tcp/premake4.lua
View File

@@ -132,10 +132,7 @@ myfiles =
"../../Importers/ImportColladaDemo/LoadMeshFromCollada.cpp",
"../../Importers/ImportColladaDemo/ColladaGraphicsInstance.h",
"../../ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp",
"../../ThirdPartyLibs/tinyxml/tinystr.cpp",
"../../ThirdPartyLibs/tinyxml/tinyxml.cpp",
"../../ThirdPartyLibs/tinyxml/tinyxmlerror.cpp",
"../../ThirdPartyLibs/tinyxml/tinyxmlparser.cpp",
"../../ThirdPartyLibs/tinyxml2/tinyxml2.cpp",
"../../Importers/ImportMeshUtility/b3ImportMeshUtility.cpp",
"../../ThirdPartyLibs/stb_image/stb_image.cpp",
}

5
examples/SharedMemory/udp/premake4.lua
View File

@@ -126,10 +126,7 @@ myfiles =
"../../Importers/ImportColladaDemo/LoadMeshFromCollada.cpp",
"../../Importers/ImportColladaDemo/ColladaGraphicsInstance.h",
"../../ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp",
"../../ThirdPartyLibs/tinyxml/tinystr.cpp",
"../../ThirdPartyLibs/tinyxml/tinyxml.cpp",
"../../ThirdPartyLibs/tinyxml/tinyxmlerror.cpp",
"../../ThirdPartyLibs/tinyxml/tinyxmlparser.cpp",
"../../ThirdPartyLibs/tinyxml2/tinyxml2.cpp",
"../../Importers/ImportMeshUtility/b3ImportMeshUtility.cpp",
"../../ThirdPartyLibs/stb_image/stb_image.cpp",
}

2
examples/SoftDemo/SoftDemo.cpp
View File

@@ -169,7 +169,7 @@ public:
for ( int i=0;i<softWorld->getSoftBodyArray().size();i++)
{
btSoftBody* psb=(btSoftBody*)softWorld->getSoftBodyArray()[i];
if (softWorld->getDebugDrawer() && !(softWorld->getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe)))
//if (softWorld->getDebugDrawer() && !(softWorld->getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe)))
{
btSoftBodyHelpers::DrawFrame(psb,softWorld->getDebugDrawer());
btSoftBodyHelpers::Draw(psb,softWorld->getDebugDrawer(),softWorld->getDrawFlags());

29
examples/StandaloneMain/hellovr_opengl_main.cpp
View File

@@ -28,9 +28,12 @@
#include "LinearMath/btIDebugDraw.h"
int gSharedMemoryKey = -1;
int gDebugDrawFlags = 0;
bool gDisplayDistortion = false;
bool gDisableDesktopGL = false;
static int gDebugDrawFlags = 0;
static bool gDisplayDistortion = false;
static bool gDisableDesktopGL = false;
static int maxNumObjectCapacity = 128 * 1024;
static int maxShapeCapacityInBytes = 128 * 1024 * 1024;
#include <stdio.h>
@@ -59,6 +62,9 @@ static vr::VRControllerState_t sPrevStates[vr::k_unMaxTrackedDeviceCount] = { 0
#ifdef _WIN32
#include <Windows.h>
#endif
#ifdef __linux__
#define APIENTRY
#endif
void ThreadSleep( unsigned long nMilliseconds )
{
@@ -474,7 +480,8 @@ bool CMainApplication::BInit()
SDL_GL_SetAttribute( SDL_GL_CONTEXT_FLAGS, SDL_GL_CONTEXT_DEBUG_FLAG );
*/
m_app = new SimpleOpenGL3App("SimpleOpenGL3App",m_nWindowWidth,m_nWindowHeight,true);
m_app = new SimpleOpenGL3App("SimpleOpenGL3App",m_nWindowWidth,m_nWindowHeight,true, maxNumObjectCapacity, maxShapeCapacityInBytes);
sGuiPtr = new OpenGLGuiHelper(m_app,false);
@@ -1804,13 +1811,11 @@ void CMainApplication::RenderStereoTargets()
}
glBindFramebuffer( GL_FRAMEBUFFER, leftEyeDesc.m_nRenderFramebufferId );
glViewport(0, 0, m_nRenderWidth, m_nRenderHeight );
m_app->m_window->startRendering();
glViewport(0, 0, m_nRenderWidth, m_nRenderHeight );
RenderScene( vr::Eye_Left );
@@ -1863,9 +1868,9 @@ void CMainApplication::RenderStereoTargets()
}
glBindFramebuffer( GL_FRAMEBUFFER, rightEyeDesc.m_nRenderFramebufferId );
glViewport(0, 0, m_nRenderWidth, m_nRenderHeight );
m_app->m_window->startRendering();
glViewport(0, 0, m_nRenderWidth, m_nRenderHeight );
RenderScene( vr::Eye_Right );
@@ -2081,6 +2086,7 @@ void CMainApplication::UpdateHMDMatrixPose()
case vr::TrackedDeviceClass_HMD: m_rDevClassChar[nDevice] = 'H'; break;
case vr::TrackedDeviceClass_Invalid: m_rDevClassChar[nDevice] = 'I'; break;
case vr::TrackedDeviceClass_TrackingReference: m_rDevClassChar[nDevice] = 'T'; break;
case vr::TrackedDeviceClass_GenericTracker: m_rDevClassChar[nDevice] = 'G'; break;
default: m_rDevClassChar[nDevice] = '?'; break;
}
}
@@ -2352,7 +2358,10 @@ int main(int argc, char *argv[])
b3ChromeUtilsEnableProfiling();
}
args.GetCmdLineArgument("max_num_object_capacity", maxNumObjectCapacity);
args.GetCmdLineArgument("max_shape_capacity_in_bytes", maxShapeCapacityInBytes);
args.GetCmdLineArgument("shared_memory_key", gSharedMemoryKey);
#ifdef BT_USE_CUSTOM_PROFILER
b3SetCustomEnterProfileZoneFunc(dcEnter);
b3SetCustomLeaveProfileZoneFunc(dcLeave);

6
examples/ThirdPartyLibs/Gwen/Renderers/OpenGL_DebugFont.cpp
View File

@@ -22,7 +22,13 @@
#include "third_party/GL/gl/include/GL/gl.h"
#include "third_party/GL/gl/include/GL/glext.h"
#else
#ifdef BT_NO_GLAD
#include <GL/glew.h>
#else
#include "glad/glad.h"
#endif
#endif //NO_GLEW
#endif //GLEW_STATIC
#endif//(__APPLE__)

6
examples/ThirdPartyLibs/openvr/samples/shared/strtools.cpp
View File

@@ -4,6 +4,12 @@
#include <stdio.h>
#include <stdlib.h>
#ifdef __linux__
#define stricmp strcasecmp
#define strnicmp strncasecmp
#endif
//-----------------------------------------------------------------------------
// Purpose:
//-----------------------------------------------------------------------------

111
examples/ThirdPartyLibs/tinyxml/tinystr.cpp
View File

@@ -1,111 +0,0 @@
/*
www.sourceforge.net/projects/tinyxml
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.
*/
#ifndef TIXML_USE_STL
#include "tinystr.h"
// Error value for find primitive
const TiXmlString::size_type TiXmlString::npos = static_cast< TiXmlString::size_type >(-1);
// Null rep.
TiXmlString::Rep TiXmlString::nullrep_ = { 0, 0, { '\0' } };
void TiXmlString::reserve (size_type cap)
{
if (cap > capacity())
{
TiXmlString tmp;
tmp.init(length(), cap);
memcpy(tmp.start(), data(), length());
swap(tmp);
}
}
TiXmlString& TiXmlString::assign(const char* str, size_type len)
{
size_type cap = capacity();
if (len > cap || cap > 3*(len + 8))
{
TiXmlString tmp;
tmp.init(len);
memcpy(tmp.start(), str, len);
swap(tmp);
}
else
{
memmove(start(), str, len);
set_size(len);
}
return *this;
}
TiXmlString& TiXmlString::append(const char* str, size_type len)
{
size_type newsize = length() + len;
if (newsize > capacity())
{
reserve (newsize + capacity());
}
memmove(finish(), str, len);
set_size(newsize);
return *this;
}
TiXmlString operator + (const TiXmlString & a, const TiXmlString & b)
{
TiXmlString tmp;
tmp.reserve(a.length() + b.length());
tmp += a;
tmp += b;
return tmp;
}
TiXmlString operator + (const TiXmlString & a, const char* b)
{
TiXmlString tmp;
TiXmlString::size_type b_len = static_cast<TiXmlString::size_type>( strlen(b) );
tmp.reserve(a.length() + b_len);
tmp += a;
tmp.append(b, b_len);
return tmp;
}
TiXmlString operator + (const char* a, const TiXmlString & b)
{
TiXmlString tmp;
TiXmlString::size_type a_len = static_cast<TiXmlString::size_type>( strlen(a) );
tmp.reserve(a_len + b.length());
tmp.append(a, a_len);
tmp += b;
return tmp;
}
#endif // TIXML_USE_STL

305
examples/ThirdPartyLibs/tinyxml/tinystr.h
View File

@@ -1,305 +0,0 @@
/*
www.sourceforge.net/projects/tinyxml
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.
*/
#ifndef TIXML_USE_STL
#ifndef TIXML_STRING_INCLUDED
#define TIXML_STRING_INCLUDED
#include <assert.h>
#include <string.h>
/* The support for explicit isn't that universal, and it isn't really
required - it is used to check that the TiXmlString class isn't incorrectly
used. Be nice to old compilers and macro it here:
*/
#if defined(_MSC_VER) && (_MSC_VER >= 1200 )
// Microsoft visual studio, version 6 and higher.
#define TIXML_EXPLICIT explicit
#elif defined(__GNUC__) && (__GNUC__ >= 3 )
// GCC version 3 and higher.s
#define TIXML_EXPLICIT explicit
#else
#define TIXML_EXPLICIT
#endif
/*
TiXmlString is an emulation of a subset of the std::string template.
Its purpose is to allow compiling TinyXML on compilers with no or poor STL support.
Only the member functions relevant to the TinyXML project have been implemented.
The buffer allocation is made by a simplistic power of 2 like mechanism : if we increase
a string and there's no more room, we allocate a buffer twice as big as we need.
*/
class TiXmlString
{
public :
// The size type used
typedef size_t size_type;
// Error value for find primitive
static const size_type npos; // = -1;
// TiXmlString empty constructor
TiXmlString () : rep_(&nullrep_)
{
}
// TiXmlString copy constructor
TiXmlString ( const TiXmlString & copy) : rep_(0)
{
init(copy.length());
memcpy(start(), copy.data(), length());
}
// TiXmlString constructor, based on a string
TIXML_EXPLICIT TiXmlString ( const char * copy) : rep_(0)
{
init( static_cast<size_type>( strlen(copy) ));
memcpy(start(), copy, length());
}
// TiXmlString constructor, based on a string
TIXML_EXPLICIT TiXmlString ( const char * str, size_type len) : rep_(0)
{
init(len);
memcpy(start(), str, len);
}
// TiXmlString destructor
~TiXmlString ()
{
quit();
}
TiXmlString& operator = (const char * copy)
{
return assign( copy, (size_type)strlen(copy));
}
TiXmlString& operator = (const TiXmlString & copy)
{
return assign(copy.start(), copy.length());
}
// += operator. Maps to append
TiXmlString& operator += (const char * suffix)
{
return append(suffix, static_cast<size_type>( strlen(suffix) ));
}
// += operator. Maps to append
TiXmlString& operator += (char single)
{
return append(&single, 1);
}
// += operator. Maps to append
TiXmlString& operator += (const TiXmlString & suffix)
{
return append(suffix.data(), suffix.length());
}
// Convert a TiXmlString into a null-terminated char *
const char * c_str () const { return rep_->str; }
// Convert a TiXmlString into a char * (need not be null terminated).
const char * data () const { return rep_->str; }
// Return the length of a TiXmlString
size_type length () const { return rep_->size; }
// Alias for length()
size_type size () const { return rep_->size; }
// Checks if a TiXmlString is empty
bool empty () const { return rep_->size == 0; }
// Return capacity of string
size_type capacity () const { return rep_->capacity; }
// single char extraction
const char& at (size_type index) const
{
assert( index < length() );
return rep_->str[ index ];
}
// [] operator
char& operator [] (size_type index) const
{
assert( index < length() );
return rep_->str[ index ];
}
// find a char in a string. Return TiXmlString::npos if not found
size_type find (char lookup) const
{
return find(lookup, 0);
}
// find a char in a string from an offset. Return TiXmlString::npos if not found
size_type find (char tofind, size_type offset) const
{
if (offset >= length()) return npos;
for (const char* p = c_str() + offset; *p != '\0'; ++p)
{
if (*p == tofind) return static_cast< size_type >( p - c_str() );
}
return npos;
}
void clear ()
{
//Lee:
//The original was just too strange, though correct:
// TiXmlString().swap(*this);
//Instead use the quit & re-init:
quit();
init(0,0);
}
/* Function to reserve a big amount of data when we know we'll need it. Be aware that this
function DOES NOT clear the content of the TiXmlString if any exists.
*/
void reserve (size_type cap);
TiXmlString& assign (const char* str, size_type len);
TiXmlString& append (const char* str, size_type len);
void swap (TiXmlString& other)
{
Rep* r = rep_;
rep_ = other.rep_;
other.rep_ = r;
}
private:
void init(size_type sz) { init(sz, sz); }
void set_size(size_type sz) { rep_->str[ rep_->size = sz ] = '\0'; }
char* start() const { return rep_->str; }
char* finish() const { return rep_->str + rep_->size; }
struct Rep
{
size_type size, capacity;
char str[1];
};
void init(size_type sz, size_type cap)
{
if (cap)
{
// Lee: the original form:
// rep_ = static_cast<Rep*>(operator new(sizeof(Rep) + cap));
// doesn't work in some cases of new being overloaded. Switching
// to the normal allocation, although use an 'int' for systems
// that are overly picky about structure alignment.
const size_type bytesNeeded = sizeof(Rep) + cap;
const size_type intsNeeded = ( bytesNeeded + sizeof(int) - 1 ) / sizeof( int );
rep_ = reinterpret_cast<Rep*>( new int[ intsNeeded ] );
rep_->str[ rep_->size = sz ] = '\0';
rep_->capacity = cap;
}
else
{
rep_ = &nullrep_;
}
}
void quit()
{
if (rep_ != &nullrep_)
{
// The rep_ is really an array of ints. (see the allocator, above).
// Cast it back before delete, so the compiler won't incorrectly call destructors.
delete [] ( reinterpret_cast<int*>( rep_ ) );
}
}
Rep * rep_;
static Rep nullrep_;
} ;
inline bool operator == (const TiXmlString & a, const TiXmlString & b)
{
return ( a.length() == b.length() ) // optimization on some platforms
&& ( strcmp(a.c_str(), b.c_str()) == 0 ); // actual compare
}
inline bool operator < (const TiXmlString & a, const TiXmlString & b)
{
return strcmp(a.c_str(), b.c_str()) < 0;
}
inline bool operator != (const TiXmlString & a, const TiXmlString & b) { return !(a == b); }
inline bool operator > (const TiXmlString & a, const TiXmlString & b) { return b < a; }
inline bool operator <= (const TiXmlString & a, const TiXmlString & b) { return !(b < a); }
inline bool operator >= (const TiXmlString & a, const TiXmlString & b) { return !(a < b); }
inline bool operator == (const TiXmlString & a, const char* b) { return strcmp(a.c_str(), b) == 0; }
inline bool operator == (const char* a, const TiXmlString & b) { return b == a; }
inline bool operator != (const TiXmlString & a, const char* b) { return !(a == b); }
inline bool operator != (const char* a, const TiXmlString & b) { return !(b == a); }
TiXmlString operator + (const TiXmlString & a, const TiXmlString & b);
TiXmlString operator + (const TiXmlString & a, const char* b);
TiXmlString operator + (const char* a, const TiXmlString & b);
/*
TiXmlOutStream is an emulation of std::ostream. It is based on TiXmlString.
Only the operators that we need for TinyXML have been developped.
*/
class TiXmlOutStream : public TiXmlString
{
public :
// TiXmlOutStream << operator.
TiXmlOutStream & operator << (const TiXmlString & in)
{
*this += in;
return *this;
}
// TiXmlOutStream << operator.
TiXmlOutStream & operator << (const char * in)
{
*this += in;
return *this;
}
} ;
#endif // TIXML_STRING_INCLUDED
#endif // TIXML_USE_STL

1886
examples/ThirdPartyLibs/tinyxml/tinyxml.cpp
View File

File diff suppressed because it is too large Load Diff

1805
examples/ThirdPartyLibs/tinyxml/tinyxml.h
View File

File diff suppressed because it is too large Load Diff

52
examples/ThirdPartyLibs/tinyxml/tinyxmlerror.cpp
View File

@@ -1,52 +0,0 @@
/*
www.sourceforge.net/projects/tinyxml
Original code (2.0 and earlier )copyright (c) 2000-2006 Lee Thomason (www.grinninglizard.com)
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 "tinyxml.h"
// The goal of the seperate error file is to make the first
// step towards localization. tinyxml (currently) only supports
// english error messages, but the could now be translated.
//
// It also cleans up the code a bit.
//
const char* TiXmlBase::errorString[ TiXmlBase::TIXML_ERROR_STRING_COUNT ] =
{
"No error",
"Error",
"Failed to open file",
"Error parsing Element.",
"Failed to read Element name",
"Error reading Element value.",
"Error reading Attributes.",
"Error: empty tag.",
"Error reading end tag.",
"Error parsing Unknown.",
"Error parsing Comment.",
"Error parsing Declaration.",
"Error document empty.",
"Error null (0) or unexpected EOF found in input stream.",
"Error parsing CDATA.",
"Error when TiXmlDocument added to document, because TiXmlDocument can only be at the root.",
};

1638
examples/ThirdPartyLibs/tinyxml/tinyxmlparser.cpp
View File

File diff suppressed because it is too large Load Diff

2826
examples/ThirdPartyLibs/tinyxml2/tinyxml2.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

2314
examples/ThirdPartyLibs/tinyxml2/tinyxml2.h Normal file
View File

File diff suppressed because it is too large Load Diff

8
examples/Tutorial/Dof6ConstraintTutorial.cpp
View File

@@ -313,11 +313,11 @@ void Dof6ConstraintTutorial::initPhysics()
#ifdef USE_6DOF2
constraint->enableMotor(5,true);
constraint->setTargetVelocity(5,3.f);
constraint->setMaxMotorForce(5,10.f);
constraint->setMaxMotorForce(5,600.f);
#else
constraint->getRotationalLimitMotor(2)->m_enableMotor = true;
constraint->getRotationalLimitMotor(2)->m_targetVelocity = 3.f;
constraint->getRotationalLimitMotor(2)->m_maxMotorForce = 10;
constraint->getRotationalLimitMotor(2)->m_maxMotorForce = 600.f;
#endif
constraint->setDbgDrawSize(btScalar(2.f));
m_dynamicsWorld->addConstraint(constraint, true);
@@ -348,13 +348,13 @@ void Dof6ConstraintTutorial::initPhysics()
#ifdef USE_6DOF2
constraint->enableMotor(5,true);
constraint->setTargetVelocity(5,3.f);
constraint->setMaxMotorForce(5,10.f);
constraint->setMaxMotorForce(5,600.f);
constraint->setServo(5,true);
constraint->setServoTarget(5, M_PI_2);
#else
constraint->getRotationalLimitMotor(2)->m_enableMotor = true;
constraint->getRotationalLimitMotor(2)->m_targetVelocity = 3.f;
constraint->getRotationalLimitMotor(2)->m_maxMotorForce = 10;
constraint->getRotationalLimitMotor(2)->m_maxMotorForce = 600.f;
//servo motor is not implemented in 6dofspring constraint
#endif
constraint->setDbgDrawSize(btScalar(2.f));

5
examples/TwoJoint/CMakeLists.txt
View File

@@ -54,10 +54,7 @@ SET(RobotSimulator_SRCS
../../examples/Utils/b3ResourcePath.h
../../examples/Utils/RobotLoggingUtil.cpp
../../examples/Utils/RobotLoggingUtil.h
../../examples/ThirdPartyLibs/tinyxml/tinystr.cpp
../../examples/ThirdPartyLibs/tinyxml/tinyxml.cpp
../../examples/ThirdPartyLibs/tinyxml/tinyxmlerror.cpp
../../examples/ThirdPartyLibs/tinyxml/tinyxmlparser.cpp
../../examples/ThirdPartyLibs/tinyxml2/tinyxml2.cpp
../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp
../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h
../../examples/ThirdPartyLibs/stb_image/stb_image.cpp

5
examples/pybullet/CMakeLists.txt
View File

@@ -64,10 +64,7 @@ SET(pybullet_SRCS
../../examples/Utils/RobotLoggingUtil.cpp
../../examples/Utils/RobotLoggingUtil.h
../../examples/ThirdPartyLibs/tinyxml/tinystr.cpp
../../examples/ThirdPartyLibs/tinyxml/tinyxml.cpp
../../examples/ThirdPartyLibs/tinyxml/tinyxmlerror.cpp
../../examples/ThirdPartyLibs/tinyxml/tinyxmlparser.cpp
../../examples/ThirdPartyLibs/tinyxml2/tinyxml2.cpp
../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp
../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h
../../examples/ThirdPartyLibs/stb_image/stb_image.cpp

75
examples/pybullet/examples/addPlanarReflection.py Normal file
View File

@@ -0,0 +1,75 @@
import pybullet as p
import time
import math
def getRayFromTo(mouseX,mouseY):
width, height, viewMat, projMat, cameraUp, camForward, horizon,vertical, _,_,dist, camTarget = p.getDebugVisualizerCamera()
camPos = [camTarget[0] - dist*camForward[0],camTarget[1] - dist*camForward[1],camTarget[2] - dist*camForward[2]]
farPlane = 10000
rayForward = [(camTarget[0]-camPos[0]),(camTarget[1]-camPos[1]),(camTarget[2]-camPos[2])]
invLen = farPlane*1./(math.sqrt(rayForward[0]*rayForward[0]+rayForward[1]*rayForward[1]+rayForward[2]*rayForward[2]))
rayForward = [invLen*rayForward[0],invLen*rayForward[1],invLen*rayForward[2]]
rayFrom = camPos
oneOverWidth = float(1)/float(width)
oneOverHeight = float(1)/float(height)
dHor = [horizon[0] * oneOverWidth,horizon[1] * oneOverWidth,horizon[2] * oneOverWidth]
dVer = [vertical[0] * oneOverHeight,vertical[1] * oneOverHeight,vertical[2] * oneOverHeight]
rayToCenter=[rayFrom[0]+rayForward[0],rayFrom[1]+rayForward[1],rayFrom[2]+rayForward[2]]
rayTo = [rayFrom[0]+rayForward[0] - 0.5 * horizon[0] + 0.5 * vertical[0]+float(mouseX)*dHor[0]-float(mouseY)*dVer[0],
rayFrom[1]+rayForward[1] - 0.5 * horizon[1] + 0.5 * vertical[1]+float(mouseX)*dHor[1]-float(mouseY)*dVer[1],
rayFrom[2]+rayForward[2] - 0.5 * horizon[2] + 0.5 * vertical[2]+float(mouseX)*dHor[2]-float(mouseY)*dVer[2]]
return rayFrom,rayTo
cid = p.connect(p.SHARED_MEMORY)
if (cid<0):
p.connect(p.GUI)
p.setPhysicsEngineParameter(numSolverIterations=10)
p.setTimeStep(1./120.)
logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "visualShapeBench.json")
#useMaximalCoordinates is much faster then the default reduced coordinates (Featherstone)
p.loadURDF("plane_transparent.urdf", useMaximalCoordinates=True)
#disable rendering during creation.
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
p.configureDebugVisualizer(p.COV_ENABLE_PLANAR_REFLECTION,1)
p.configureDebugVisualizer(p.COV_ENABLE_GUI,0)
#disable tinyrenderer, software (CPU) renderer, we don't use it here
p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER,0)
shift = [0,-0.02,0]
meshScale=[0.1,0.1,0.1]
#the visual shape and collision shape can be re-used by all createMultiBody instances (instancing)
visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,fileName="duck.obj", rgbaColor=[1,1,1,1], specularColor=[0.4,.4,0], visualFramePosition=shift, meshScale=meshScale)
collisionShapeId = p.createCollisionShape(shapeType=p.GEOM_MESH, fileName="duck_vhacd.obj", collisionFramePosition=shift,meshScale=meshScale)
rangex = 3
rangey = 3
for i in range (rangex):
for j in range (rangey ):
p.createMultiBody(baseMass=1,baseInertialFramePosition=[0,0,0],baseCollisionShapeIndex=collisionShapeId, baseVisualShapeIndex = visualShapeId, basePosition = [((-rangex/2)+i)*meshScale[0]*2,(-rangey/2+j)*meshScale[1]*2,1], useMaximalCoordinates=True)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
p.stopStateLogging(logId)
p.setGravity(0,0,-10)
p.setRealTimeSimulation(1)
colors = [[1,0,0,1],[0,1,0,1],[0,0,1,1],[1,1,1,1]]
currentColor = 0
while (1):
mouseEvents = p.getMouseEvents()
for e in mouseEvents:
if ((e[0] == 2) and (e[3]==0) and (e[4]& p.KEY_WAS_TRIGGERED)):
mouseX = e[1]
mouseY = e[2]
rayFrom,rayTo=getRayFromTo(mouseX,mouseY)
rayInfo = p.rayTest(rayFrom,rayTo)
#p.addUserDebugLine(rayFrom,rayTo,[1,0,0],3)
for l in range(len(rayInfo)):
hit = rayInfo[l]
objectUid = hit[0]
if (objectUid>=1):
#p.removeBody(objectUid)
p.changeVisualShape(objectUid,-1,rgbaColor=colors[currentColor])
currentColor+=1
if (currentColor>=len(colors)):
currentColor=0

6
examples/pybullet/examples/createVisualShape.py
View File

@@ -40,8 +40,8 @@ meshScale=[0.1,0.1,0.1]
visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,fileName="duck.obj", rgbaColor=[1,1,1,1], specularColor=[0.4,.4,0], visualFramePosition=shift, meshScale=meshScale)
collisionShapeId = p.createCollisionShape(shapeType=p.GEOM_MESH, fileName="duck_vhacd.obj", collisionFramePosition=shift,meshScale=meshScale)
rangex = 32
rangey = 32
rangex = 5
rangey = 5
for i in range (rangex):
for j in range (rangey ):
p.createMultiBody(baseMass=1,baseInertialFramePosition=[0,0,0],baseCollisionShapeIndex=collisionShapeId, baseVisualShapeIndex = visualShapeId, basePosition = [((-rangex/2)+i)*meshScale[0]*2,(-rangey/2+j)*meshScale[1]*2,1], useMaximalCoordinates=True)
@@ -65,7 +65,7 @@ while (1):
for l in range(len(rayInfo)):
hit = rayInfo[l]
objectUid = hit[0]
if (objectUid>=0):
if (objectUid>=1):
#p.removeBody(objectUid)
p.changeVisualShape(objectUid,-1,rgbaColor=colors[currentColor])
currentColor+=1

61
examples/pybullet/examples/externalTorqueControlledSphere.py Normal file
View File

@@ -0,0 +1,61 @@
import pybullet as p
import pybullet_data
import time
p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.loadSDF("stadium.sdf")
p.setGravity(0,0,-10)
objects = p.loadMJCF("mjcf/sphere.xml")
sphere = objects[0]
p.resetBasePositionAndOrientation(sphere,[0,0,1],[0,0,0,1])
p.changeDynamics(sphere,-1,linearDamping=0.9)
p.changeVisualShape(sphere,-1,rgbaColor=[1,0,0,1])
forward = 0
turn = 0
forwardVec = [2,0,0]
cameraDistance = 1
cameraYaw = 35
cameraPitch = -35
while (1):
spherePos, orn = p.getBasePositionAndOrientation(sphere)
cameraTargetPosition = spherePos
p.resetDebugVisualizerCamera(cameraDistance,cameraYaw,cameraPitch,cameraTargetPosition)
camInfo = p.getDebugVisualizerCamera()
camForward = camInfo[5]
keys = p.getKeyboardEvents()
for k,v in keys.items():
if (k == p.B3G_RIGHT_ARROW and (v&p.KEY_WAS_TRIGGERED)):
turn = -0.5
if (k == p.B3G_RIGHT_ARROW and (v&p.KEY_WAS_RELEASED)):
turn = 0
if (k == p.B3G_LEFT_ARROW and (v&p.KEY_WAS_TRIGGERED)):
turn = 0.5
if (k == p.B3G_LEFT_ARROW and (v&p.KEY_WAS_RELEASED)):
turn = 0
if (k == p.B3G_UP_ARROW and (v&p.KEY_WAS_TRIGGERED)):
forward=1
if (k == p.B3G_UP_ARROW and (v&p.KEY_WAS_RELEASED)):
forward=0
if (k == p.B3G_DOWN_ARROW and (v&p.KEY_WAS_TRIGGERED)):
forward=-1
if (k == p.B3G_DOWN_ARROW and (v&p.KEY_WAS_RELEASED)):
forward=0
force = [forward*camForward[0],forward*camForward[1],0]
cameraYaw = cameraYaw+turn
if (forward):
p.applyExternalForce(sphere,-1, force , spherePos, flags = p.WORLD_FRAME )
p.stepSimulation()
time.sleep(1./240.)

46
examples/pybullet/examples/jacobian.py
View File

@@ -8,6 +8,15 @@ def getJointStates(robot):
joint_torques = [state[3] for state in joint_states]
return joint_positions, joint_velocities, joint_torques
def getMotorJointStates(robot):
joint_states = p.getJointStates(robot, range(p.getNumJoints(robot)))
joint_infos = [p.getJointInfo(robot, i) for i in range(p.getNumJoints(robot))]
joint_states = [j for j, i in zip(joint_states, joint_infos) if i[3] > -1]
joint_positions = [state[0] for state in joint_states]
joint_velocities = [state[1] for state in joint_states]
joint_torques = [state[3] for state in joint_states]
return joint_positions, joint_velocities, joint_torques
def setJointPosition(robot, position, kp=1.0, kv=0.3):
num_joints = p.getNumJoints(robot)
zero_vec = [0.0] * num_joints
@@ -20,47 +29,60 @@ def setJointPosition(robot, position, kp=1.0, kv=0.3):
"Expected torque vector of "
"length {}, got {}".format(num_joints, len(torque)))
def multiplyJacobian(jacobian, vector):
def multiplyJacobian(robot, jacobian, vector):
result = [0.0, 0.0, 0.0]
i = 0
for c in range(len(vector)):
for r in range(3):
result[r] += jacobian[r][c] * vector[c]
if p.getJointInfo(robot, c)[3] > -1:
for r in range(3):
result[r] += jacobian[r][i] * vector[c]
i += 1
return result
clid = p.connect(p.SHARED_MEMORY)
if (clid<0):
p.connect(p.DIRECT)
time_step = 0.001
gravity_constant = -9.81
p.resetSimulation()
p.setTimeStep(time_step)
p.setGravity(0.0, 0.0, gravity_constant)
p.loadURDF("plane.urdf",[0,0,-0.3])
kukaId = p.loadURDF("kuka_iiwa/model.urdf",[0,0,0])
kukaId = p.loadURDF("TwoJointRobot_w_fixedJoints.urdf", useFixedBase=True)
#kukaId = p.loadURDF("TwoJointRobot_w_fixedJoints.urdf",[0,0,0])
#kukaId = p.loadURDF("kuka_iiwa/model.urdf",[0,0,0])
#kukaId = p.loadURDF("kuka_lwr/kuka.urdf",[0,0,0])
#kukaId = p.loadURDF("humanoid/nao.urdf",[0,0,0])
p.resetBasePositionAndOrientation(kukaId,[0,0,0],[0,0,0,1])
kukaEndEffectorIndex = 6
numJoints = p.getNumJoints(kukaId)
if (numJoints!=7):
exit()
kukaEndEffectorIndex = numJoints - 1
# Set a joint target for the position control and step the sim.
setJointPosition(kukaId, [0.1] * p.getNumJoints(kukaId))
setJointPosition(kukaId, [0.1] * numJoints)
p.stepSimulation()
# Get the joint and link state directly from Bullet.
pos, vel, torq = getJointStates(kukaId)
mpos, mvel, mtorq = getMotorJointStates(kukaId)
result = p.getLinkState(kukaId, kukaEndEffectorIndex, computeLinkVelocity=1, computeForwardKinematics=1)
link_trn, link_rot, com_trn, com_rot, frame_pos, frame_rot, link_vt, link_vr = result
# Get the Jacobians for the CoM of the end-effector link.
# Note that in this example com_rot = identity, and we would need to use com_rot.T * com_trn.
# The localPosition is always defined in terms of the link frame coordinates.
zero_vec = [0.0] * numJoints
jac_t, jac_r = p.calculateJacobian(kukaId, kukaEndEffectorIndex, com_trn, pos, zero_vec, zero_vec)
zero_vec = [0.0] * len(mpos)
jac_t, jac_r = p.calculateJacobian(kukaId, kukaEndEffectorIndex, com_trn, mpos, zero_vec, zero_vec)
print ("Link linear velocity of CoM from getLinkState:")
print (link_vt)
print ("Link linear velocity of CoM from linearJacobian * q_dot:")
print (multiplyJacobian(jac_t, vel))
print (multiplyJacobian(kukaId, jac_t, vel))
print ("Link angular velocity of CoM from getLinkState:")
print (link_vr)
print ("Link angular velocity of CoM from angularJacobian * q_dot:")
print (multiplyJacobian(jac_r, vel))
print (multiplyJacobian(kukaId, jac_r, vel))

15
examples/pybullet/examples/signedDistanceField.py Normal file
View File

@@ -0,0 +1,15 @@
import pybullet as p
import pybullet
import time
p.connect(p.GUI)
p.loadURDF("toys/concave_box.urdf")
p.setGravity(0,0,-10)
for i in range (10):
p.loadURDF("sphere_1cm.urdf",[i*0.02,0,0.5])
p.loadURDF("duck_vhacd.urdf")
timeStep = 1./240.
p.setTimeStep(timeStep)
while (1):
p.stepSimulation()
time.sleep(timeStep)

4
examples/pybullet/gym/pybullet_data/plane100.urdf
View File

@@ -1,6 +1,6 @@
<?xml version="0.0" ?>
<robot name="cube.urdf">
<link name="baseLink">
<robot name="floor">
<link name="floor">
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value=".0"/>

44
examples/pybullet/gym/pybullet_data/plane_stadium.sdf Normal file
View File

@@ -0,0 +1,44 @@
<sdf version='1.6'>
<world name='default'>
<gravity>0 0 -9.8</gravity>
<model name='floor_obj'>
<static>1</static>
<pose frame=''>0 0 0 0 0 0</pose>
<link name='floor'>
<inertial>
<mass>0</mass>
<inertia>
<ixx>0.166667</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.166667</iyy>
<iyz>0</iyz>
<izz>0.166667</izz>
</inertia>
</inertial>
<collision name='collision_1'>
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
</collision>
<visual name='visual'>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>plane100.obj</uri>
</mesh>
</geometry>
<material>
<ambient>1 1 1 1</ambient>
<diffuse>1 1 1 1</diffuse>
<specular>.5 .5 .5 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
</link>
</model>
</world>
</sdf>

54
examples/pybullet/gym/pybullet_data/policies/ppo/minitaur_reactive_env/config.yaml Normal file
View File

@@ -0,0 +1,54 @@
!!python/object/new:pybullet_envs.minitaur.agents.tools.attr_dict.AttrDict
dictitems:
algorithm: !!python/name:pybullet_envs.minitaur.agents.ppo.algorithm.PPOAlgorithm ''
discount: 0.9868209124499899
env: !!python/object/apply:functools.partial
args:
- &id001 !!python/name:pybullet_envs.minitaur.envs.minitaur_reactive_env.MinitaurReactiveEnv ''
state: !!python/tuple
- *id001
- !!python/tuple []
- accurate_motor_model_enabled: true
control_latency: 0.02
energy_weight: 0.005
env_randomizer: null
motor_kd: 0.015
num_steps_to_log: 1000
pd_latency: 0.003
remove_default_joint_damping: true
render: false
urdf_version: rainbow_dash_v0
- null
eval_episodes: 25
init_logstd: -1.1579536194508315
init_mean_factor: 0.3084392491563408
kl_cutoff_coef: 1000
kl_cutoff_factor: 2
kl_init_penalty: 1
kl_target: 0.01
logdir: /cns/ij-d/home/jietan/experiment/minitaur_vizier_study_ppo/minreact_nonexp_nr_02_186515603_186518344/333
max_length: 1000
network: !!python/name:pybullet_envs.minitaur.agents.scripts.networks.ForwardGaussianPolicy ''
network_config: {}
num_agents: 25
policy_layers: !!python/tuple
- 114
- 45
policy_lr: 0.00023516695218031146
policy_optimizer: AdamOptimizer
steps: 7000000.0
update_epochs_policy: 25
update_epochs_value: 25
update_every: 25
use_gpu: false
value_layers: !!python/tuple
- 170
- 78
value_lr: 0.00031014032715987193
value_optimizer: AdamOptimizer
weight_summaries:
all: .*
policy: .*/policy/.*
value: .*/value/.*
state:
_mutable: false

BIN
examples/pybullet/gym/pybullet_data/policies/ppo/minitaur_reactive_env/model.ckpt-14000000.data-00000-of-00001 Normal file
View File

Binary file not shown.

BIN
examples/pybullet/gym/pybullet_data/policies/ppo/minitaur_reactive_env/model.ckpt-14000000.index Normal file
View File

Binary file not shown.

BIN
examples/pybullet/gym/pybullet_data/policies/ppo/minitaur_reactive_env/model.ckpt-14000000.meta Normal file
View File

Binary file not shown.

51
examples/pybullet/gym/pybullet_data/policies/ppo/minitaur_trotting_env/config.yaml Normal file
View File

@@ -0,0 +1,51 @@
!!python/object/new:pybullet_envs.minitaur.agents.tools.attr_dict.AttrDict
dictitems:
algorithm: !!python/name:pybullet_envs.minitaur.agents.ppo.algorithm.PPOAlgorithm ''
discount: 0.9899764168788918
env: !!python/object/apply:functools.partial
args:
- &id001 !!python/name:pybullet_envs.minitaur.envs.minitaur_trotting_env.MinitaurTrottingEnv ''
state: !!python/tuple
- *id001
- !!python/tuple []
- env_randomizer: null
motor_kd: 0.015
num_steps_to_log: 1000
pd_latency: 0.003
remove_default_joint_damping: true
render: false
urdf_version: rainbow_dash_v0
- null
eval_episodes: 25
init_logstd: -0.6325707791047228
init_mean_factor: 0.6508531688665261
kl_cutoff_coef: 1000
kl_cutoff_factor: 2
kl_init_penalty: 1
kl_target: 0.01
logdir: /cns/ij-d/home/jietan/experiment/minitaur_vizier_study_ppo/mintrot_nonexp_nr_01_186515603_186518344/373
max_length: 1000
network: !!python/name:pybullet_envs.minitaur.agents.scripts.networks.ForwardGaussianPolicy ''
network_config: {}
num_agents: 25
policy_layers: !!python/tuple
- 133
- 100
policy_lr: 0.00048104185841752015
policy_optimizer: AdamOptimizer
steps: 7000000.0
update_epochs_policy: 25
update_epochs_value: 25
update_every: 25
use_gpu: false
value_layers: !!python/tuple
- 64
- 57
value_lr: 0.0012786382882055453
value_optimizer: AdamOptimizer
weight_summaries:
all: .*
policy: .*/policy/.*
value: .*/value/.*
state:
_mutable: false

BIN
examples/pybullet/gym/pybullet_data/policies/ppo/minitaur_trotting_env/model.ckpt-14000000.data-00000-of-00001 Normal file
View File

Binary file not shown.

BIN
examples/pybullet/gym/pybullet_data/policies/ppo/minitaur_trotting_env/model.ckpt-14000000.index Normal file
View File

Binary file not shown.

BIN
examples/pybullet/gym/pybullet_data/policies/ppo/minitaur_trotting_env/model.ckpt-14000000.meta Normal file
View File

Binary file not shown.

1073
examples/pybullet/gym/pybullet_data/quadruped/minitaur_derpy.urdf Normal file
View File

File diff suppressed because it is too large Load Diff

914
examples/pybullet/gym/pybullet_data/quadruped/minitaur_rainbow_dash.urdf Normal file
View File

@@ -0,0 +1,914 @@
<?xml version="1.0"?>
<robot name="quadruped">
<link name="base_chassis_link">
<visual>
<geometry>
<box size=".3 .13 .087"/>
</geometry>
<material name="black">
<color rgba=".3 .3 .3 1"/>
</material>
</visual>
<visual>
<origin rpy="0 0 0" xyz=".10 0 .08"/>
<geometry>
<box size=".08 .08 .08"/>
</geometry>
<material name="grey">
<color rgba=".65 .65 .75 1"/>
</material>
</visual>
<collision>
<geometry>
<box size=".3 .13 .087"/>
</geometry>
</collision>
<collision>
<origin rpy="0 0 0" xyz=".10 0 .08"/>
<geometry>
<box size=".08 .08 .08"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz=".05 0 0"/>
<mass value="3.353"/>
<inertia ixx=".006837" ixy=".0" ixz=".0" iyy=".027262" iyz=".0" izz=".029870"/>
</inertial>
</link>
<link name="chassis_right">
<visual>
<origin rpy="0 0 0" xyz=".2375 -.054 0"/>
<geometry>
<box size=".068 .032 .050"/>
</geometry>
<material name="grey">
<color rgba=".65 .65 .75 1"/>
</material>
</visual>
<visual>
<origin rpy="0 0 0" xyz=".2375 .054 0"/>
<geometry>
<box size=".068 .032 .050"/>
</geometry>
<material name="grey">
<color rgba=".65 .65 .75 1"/>
</material>
</visual>
<visual>
<origin rpy="0 0 0" xyz="-.2375 -.054 0"/>
<geometry>
<box size=".068 .032 .050"/>
</geometry>
<material name="grey">
<color rgba=".65 .65 .75 1"/>
</material>
</visual>
<visual>
<origin rpy="0 0 0" xyz="-.2375 .054 0"/>
<geometry>
<box size=".068 .032 .050"/>
</geometry>
<material name="grey">
<color rgba=".65 .65 .75 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz=".2375 -.054 0"/>
<geometry>
<box size=".068 .032 .050"/>
</geometry>
</collision>
<collision>
<origin rpy="0 0 0" xyz=".2375 .054 0"/>
<geometry>
<box size=".068 .032 .050"/>
</geometry>
</collision>
<collision>
<origin rpy="0 0 0" xyz="-.2375 -.054 0"/>
<geometry>
<box size=".068 .032 .050"/>
</geometry>
</collision>
<collision>
<origin rpy="0 0 0" xyz="-.2375 .054 0"/>
<geometry>
<box size=".068 .032 .050"/>
</geometry>
</collision>
<inertial>
<mass value="1.32"/>
<inertia ixx=".004147" ixy=".0" ixz=".0" iyy=".040913" iyz=".0" izz=".041840"/>
</inertial>
</link>
<joint name="chassis_right_center" type="fixed">
<axis xyz="0 0 1"/>
<parent link="base_chassis_link"/>
<child link="chassis_right"/>
<origin rpy="0 0 0" xyz="0 -.1265 -.0185"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="chassis_left">
<visual>
<origin rpy="0 0 0" xyz=".2375 -.054 0"/>
<geometry>
<box size=".068 .032 .050"/>
</geometry>
<material name="grey">
<color rgba=".65 .65 .75 1"/>
</material>
</visual>
<visual>
<origin rpy="0 0 0" xyz=".2375 .054 0"/>
<geometry>
<box size=".068 .032 .050"/>
</geometry>
<material name="grey">
<color rgba=".65 .65 .75 1"/>
</material>
</visual>
<visual>
<origin rpy="0 0 0" xyz="-.2375 -.054 0"/>
<geometry>
<box size=".068 .032 .050"/>
</geometry>
<material name="grey">
<color rgba=".65 .65 .75 1"/>
</material>
</visual>
<visual>
<origin rpy="0 0 0" xyz="-.2375 .054 0"/>
<geometry>
<box size=".068 .032 .050"/>
</geometry>
<material name="grey">
<color rgba=".65 .65 .75 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz=".2375 -.054 0"/>
<geometry>
<box size=".068 .032 .050"/>
</geometry>
</collision>
<collision>
<origin rpy="0 0 0" xyz=".2375 .054 0"/>
<geometry>
<box size=".068 .032 .050"/>
</geometry>
</collision>
<collision>
<origin rpy="0 0 0" xyz="-.2375 -.054 0"/>
<geometry>
<box size=".068 .032 .050"/>
</geometry>
</collision>
<collision>
<origin rpy="0 0 0" xyz="-.2375 .054 0"/>
<geometry>
<box size=".068 .032 .050"/>
</geometry>
</collision>
<inertial>
<mass value="1.32"/>
<inertia ixx=".004147" ixy=".0" ixz=".0" iyy=".040913" iyz=".0" izz=".041840"/>
</inertial>
</link>
<joint name="chassis_left_center" type="fixed">
<axis xyz="0 0 1"/>
<parent link="base_chassis_link"/>
<child link="chassis_left"/>
<origin rpy="0 0 0" xyz="0 .1265 -.0185"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="motor_front_rightR_link">
<visual>
<geometry>
<mesh filename="tmotor3.obj" scale="1 1 1"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length=".021" radius=".0425"/>
</geometry>
</collision>
<inertial>
<mass value=".072"/>
<inertia ixx=".00011397" ixy=".0" ixz=".0" iyy=".00011397" iyz=".0" izz=".00021765"/>
</inertial>
</link>
<joint name="motor_front_rightR_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="chassis_right"/>
<child link="motor_front_rightR_link"/>
<origin rpy="1.57075 0 0" xyz=".2375 -.0275 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="motor_front_rightL_link">
<visual>
<geometry>
<mesh filename="tmotor3.obj" scale="1 1 1"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length=".021" radius=".0425"/>
</geometry>
</collision>
<inertial>
<mass value=".072"/>
<inertia ixx=".00011397" ixy=".0" ixz=".0" iyy=".00011397" iyz=".0" izz=".00021765"/>
</inertial>
</link>
<joint name="motor_front_rightL_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="chassis_right"/>
<child link="motor_front_rightL_link"/>
<origin rpy="1.57075 0 3.141592" xyz=".2375 .0275 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="motor_front_leftL_link">
<visual>
<geometry>
<mesh filename="tmotor3.obj" scale="1 1 1"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length=".021" radius=".0425"/>
</geometry>
</collision>
<inertial>
<mass value=".072"/>
<inertia ixx=".00011397" ixy=".0" ixz=".0" iyy=".00011397" iyz=".0" izz=".00021765"/>
</inertial>
</link>
<joint name="motor_front_leftL_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="chassis_left"/>
<child link="motor_front_leftL_link"/>
<origin rpy="1.57075 0 3.141592" xyz=".2375 .0275 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="motor_front_leftR_link">
<visual>
<geometry>
<mesh filename="tmotor3.obj" scale="1 1 1"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length=".021" radius=".0425"/>
</geometry>
</collision>
<inertial>
<mass value=".072"/>
<inertia ixx=".00011397" ixy=".0" ixz=".0" iyy=".00011397" iyz=".0" izz=".00021765"/>
</inertial>
</link>
<joint name="motor_front_leftR_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="chassis_left"/>
<child link="motor_front_leftR_link"/>
<origin rpy="1.57075 0 0" xyz=".2375 -.0275 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="motor_back_rightR_link">
<visual>
<geometry>
<mesh filename="tmotor3.obj" scale="1 1 1"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length=".021" radius=".0425"/>
</geometry>
</collision>
<inertial>
<mass value=".072"/>
<inertia ixx=".00011397" ixy=".0" ixz=".0" iyy=".00011397" iyz=".0" izz=".00021765"/>
</inertial>
</link>
<joint name="motor_back_rightR_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="chassis_right"/>
<child link="motor_back_rightR_link"/>
<origin rpy="1.57075 0 0" xyz="-.2375 -.0275 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="motor_back_rightL_link">
<visual>
<geometry>
<mesh filename="tmotor3.obj" scale="1 1 1"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length=".021" radius=".0425"/>
</geometry>
</collision>
<inertial>
<mass value=".072"/>
<inertia ixx=".00011397" ixy=".0" ixz=".0" iyy=".00011397" iyz=".0" izz=".00021765"/>
</inertial>
</link>
<joint name="motor_back_rightL_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="chassis_right"/>
<child link="motor_back_rightL_link"/>
<origin rpy="1.57075 0 3.141592" xyz="-.2375 .0275 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="motor_back_leftL_link">
<visual>
<geometry>
<mesh filename="tmotor3.obj" scale="1 1 1"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length=".021" radius=".0425"/>
</geometry>
</collision>
<inertial>
<mass value=".072"/>
<inertia ixx=".00011397" ixy=".0" ixz=".0" iyy=".00011397" iyz=".0" izz=".00021765"/>
</inertial>
</link>
<joint name="motor_back_leftL_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="chassis_left"/>
<child link="motor_back_leftL_link"/>
<origin rpy="1.57075 0 3.141592" xyz="-.2375 .0275 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="motor_back_leftR_link">
<visual>
<geometry>
<mesh filename="tmotor3.obj" scale="1 1 1"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length=".021" radius=".0425"/>
</geometry>
</collision>
<inertial>
<mass value=".072"/>
<inertia ixx=".00011397" ixy=".0" ixz=".0" iyy=".00011397" iyz=".0" izz=".00021765"/>
</inertial>
</link>
<joint name="motor_back_leftR_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="chassis_left"/>
<child link="motor_back_leftR_link"/>
<origin rpy="1.57075 0 0" xyz="-.2375 -.0275 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="upper_leg_front_rightR_link">
<visual>
<geometry>
<box size=".039 .008 .129"/>
</geometry>
<material name="grey">
<color rgba=".65 .65 .75 1"/>
</material>
</visual>
<collision>
<geometry>
<box size=".039 .008 .129"/>
</geometry>
</collision>
<inertial>
<mass value=".034"/>
<inertia ixx=".000038770" ixy=".0" ixz=".0" iyy=".000042198" iyz=".0" izz=".0000036030"/>
</inertial>
</link>
<joint name="hip_front_rightR_joint" type="fixed">
<axis xyz="0 0 1"/>
<parent link="motor_front_rightR_link"/>
<child link="upper_leg_front_rightR_link"/>
<origin rpy="-1.57075 0 0" xyz="0 .045 -.0145"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="lower_leg_front_rightR_link">
<contact>
<friction_anchor/>
<stiffness value="3000.0"/>
<damping value="100.0"/>
<spinning_friction value=".3"/>
<lateral_friction value="1"/>
</contact>
<visual>
<origin rpy="0 0 0" xyz="0 0 .120"/>
<geometry>
<box size=".017 .009 .240"/>
</geometry>
<material name="grey">
<color rgba=".65 .65 .75 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 .120"/>
<geometry>
<box size=".017 .009 .240"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0 0 .120"/>
<mass value=".086"/>
<inertia ixx=".00032459" ixy=".0" ixz=".0" iyy=".00032637" iyz=".0" izz=".000002178"/>
</inertial>
</link>
<joint name="knee_front_rightR_joint" type="continuous">
<axis xyz="0 1 0"/>
<parent link="upper_leg_front_rightR_link"/>
<child link="lower_leg_front_rightR_link"/>
<origin rpy="0 0 0" xyz="0 .0085 .056"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="upper_leg_front_rightL_link">
<visual>
<geometry>
<box size=".039 .008 .129"/>
</geometry>
<material name="grey">
<color rgba=".65 .65 .75 1"/>
</material>
</visual>
<collision>
<geometry>
<box size=".039 .008 .129"/>
</geometry>
</collision>
<inertial>
<mass value=".034"/>
<inertia ixx=".000038770" ixy=".0" ixz=".0" iyy=".000042198" iyz=".0" izz=".0000036030"/>
</inertial>
</link>
<joint name="hip_front_rightL_joint" type="fixed">
<axis xyz="0 0 1"/>
<parent link="motor_front_rightL_link"/>
<child link="upper_leg_front_rightL_link"/>
<origin rpy="-1.57075 0 0" xyz="0 .045 -.0145"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="lower_leg_front_rightL_link">
<contact>
<friction_anchor/>
<stiffness value="3000.0"/>
<damping value="100.0"/>
<spinning_friction value=".3"/>
<lateral_friction value="1"/>
</contact>
<visual>
<origin rpy="0 0 0" xyz="0 0 .108"/>
<geometry>
<box size=".017 .009 .216"/>
</geometry>
<material name="grey">
<color rgba=".65 .65 .75 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 .108"/>
<geometry>
<box size=".017 .009 .216"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0 0 .1"/>
<mass value=".072"/>
<inertia ixx=".00023166" ixy=".0" ixz=".0" iyy=".00023325" iyz=".0" izz=".00000198"/>
</inertial>
</link>
<joint name="knee_front_rightL_joint" type="continuous">
<axis xyz="0 1 0"/>
<parent link="upper_leg_front_rightL_link"/>
<child link="lower_leg_front_rightL_link"/>
<origin rpy="0 0 0" xyz="0 .0085 .056"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="upper_leg_front_leftR_link">
<visual>
<geometry>
<box size=".039 .008 .129"/>
</geometry>
<material name="grey">
<color rgba=".65 .65 .75 1"/>
</material>
</visual>
<collision>
<geometry>
<box size=".039 .008 .129"/>
</geometry>
</collision>
<inertial>
<mass value=".034"/>
<inertia ixx=".000038770" ixy=".0" ixz=".0" iyy=".000042198" iyz=".0" izz=".0000036030"/>
</inertial>
</link>
<joint name="hip_front_leftR_joint" type="fixed">
<axis xyz="0 0 1"/>
<parent link="motor_front_leftR_link"/>
<child link="upper_leg_front_leftR_link"/>
<origin rpy="-1.57075 0 0" xyz="0 .045 -.0145"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="lower_leg_front_leftR_link">
<contact>
<friction_anchor/>
<stiffness value="3000.0"/>
<damping value="100.0"/>
<spinning_friction value=".3"/>
<lateral_friction value="1"/>
</contact>
<visual>
<origin rpy="0 0 0" xyz="0 0 .108"/>
<geometry>
<box size=".017 .009 .216"/>
</geometry>
<material name="grey">
<color rgba=".65 .65 .75 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 .108"/>
<geometry>
<box size=".017 .009 .216"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0 0 .1"/>
<mass value=".072"/>
<inertia ixx=".00023166" ixy=".0" ixz=".0" iyy=".00023325" iyz=".0" izz=".00000198"/>
</inertial>
</link>
<joint name="knee_front_leftR_joint" type="continuous">
<axis xyz="0 1 0"/>
<parent link="upper_leg_front_leftR_link"/>
<child link="lower_leg_front_leftR_link"/>
<origin rpy="0 0 0" xyz="0 .0085 .056"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="upper_leg_front_leftL_link">
<visual>
<geometry>
<box size=".039 .008 .129"/>
</geometry>
<material name="grey">
<color rgba=".65 .65 .75 1"/>
</material>
</visual>
<collision>
<geometry>
<box size=".039 .008 .129"/>
</geometry>
</collision>
<inertial>
<mass value=".034"/>
<inertia ixx=".000038770" ixy=".0" ixz=".0" iyy=".000042198" iyz=".0" izz=".0000036030"/>
</inertial>
</link>
<joint name="hip_front_leftL_joint" type="fixed">
<axis xyz="0 0 1"/>
<parent link="motor_front_leftL_link"/>
<child link="upper_leg_front_leftL_link"/>
<origin rpy="-1.57075 0 0" xyz="0 .045 -.0145"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="lower_leg_front_leftL_link">
<contact>
<friction_anchor/>
<stiffness value="3000.0"/>
<damping value="100.0"/>
<spinning_friction value=".3"/>
<lateral_friction value="1"/>
</contact>
<visual>
<origin rpy="0 0 0" xyz="0 0 .120"/>
<geometry>
<box size=".017 .009 .240"/>
</geometry>
<material name="grey">
<color rgba=".65 .65 .75 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 .120"/>
<geometry>
<box size=".017 .009 .240"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0 0 .120"/>
<mass value=".086"/>
<inertia ixx=".00032459" ixy=".0" ixz=".0" iyy=".00032637" iyz=".0" izz=".000002178"/>
</inertial>
</link>
<joint name="knee_front_leftL_joint" type="continuous">
<axis xyz="0 1 0"/>
<parent link="upper_leg_front_leftL_link"/>
<child link="lower_leg_front_leftL_link"/>
<origin rpy="0 0 0" xyz="0 .0085 .056"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="upper_leg_back_rightR_link">
<visual>
<geometry>
<box size=".039 .008 .129"/>
</geometry>
<material name="grey">
<color rgba=".65 .65 .75 1"/>
</material>
</visual>
<collision>
<geometry>
<box size=".039 .008 .129"/>
</geometry>
</collision>
<inertial>
<mass value=".034"/>
<inertia ixx=".000038770" ixy=".0" ixz=".0" iyy=".000042198" iyz=".0" izz=".0000036030"/>
</inertial>
</link>
<joint name="hip_back_rightR_joint" type="fixed">
<axis xyz="0 0 1"/>
<parent link="motor_back_rightR_link"/>
<child link="upper_leg_back_rightR_link"/>
<origin rpy="-1.57075 0 0" xyz="0 .045 -.0145"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="lower_leg_back_rightR_link">
<contact>
<friction_anchor/>
<stiffness value="3000.0"/>
<damping value="100.0"/>
<spinning_friction value=".3"/>
<lateral_friction value="1"/>
</contact>
<visual>
<origin rpy="0 0 0" xyz="0 0 .120"/>
<geometry>
<box size=".017 .009 .240"/>
</geometry>
<material name="grey">
<color rgba=".65 .65 .75 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 .120"/>
<geometry>
<box size=".017 .009 .240"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0 0 .120"/>
<mass value=".086"/>
<inertia ixx=".00032459" ixy=".0" ixz=".0" iyy=".00032637" iyz=".0" izz=".000002178"/>
</inertial>
</link>
<joint name="knee_back_rightR_joint" type="continuous">
<axis xyz="0 1 0"/>
<parent link="upper_leg_back_rightR_link"/>
<child link="lower_leg_back_rightR_link"/>
<origin rpy="0 0 0" xyz="0 .0085 .056"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="upper_leg_back_rightL_link">
<visual>
<geometry>
<box size=".039 .008 .129"/>
</geometry>
<material name="grey">
<color rgba=".65 .65 .75 1"/>
</material>
</visual>
<collision>
<geometry>
<box size=".039 .008 .129"/>
</geometry>
</collision>
<inertial>
<mass value=".034"/>
<inertia ixx=".000038770" ixy=".0" ixz=".0" iyy=".000042198" iyz=".0" izz=".0000036030"/>
</inertial>
</link>
<joint name="hip_back_rightL_joint" type="fixed">
<axis xyz="0 0 1"/>
<parent link="motor_back_rightL_link"/>
<child link="upper_leg_back_rightL_link"/>
<origin rpy="-1.57075 0 0" xyz="0 .045 -.0145"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="lower_leg_back_rightL_link">
<contact>
<friction_anchor/>
<stiffness value="3000.0"/>
<damping value="100.0"/>
<spinning_friction value=".3"/>
<lateral_friction value="1"/>
</contact>
<visual>
<origin rpy="0 0 0" xyz="0 0 .108"/>
<geometry>
<box size=".017 .009 .216"/>
</geometry>
<material name="grey">
<color rgba=".65 .65 .75 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 .108"/>
<geometry>
<box size=".017 .009 .216"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0 0 .1"/>
<mass value=".072"/>
<inertia ixx=".00023166" ixy=".0" ixz=".0" iyy=".00023325" iyz=".0" izz=".00000198"/>
</inertial>
</link>
<joint name="knee_back_rightL_joint" type="continuous">
<axis xyz="0 1 0"/>
<parent link="upper_leg_back_rightL_link"/>
<child link="lower_leg_back_rightL_link"/>
<origin rpy="0 0 0" xyz="0 .0085 .056"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="upper_leg_back_leftR_link">
<visual>
<geometry>
<box size=".039 .008 .129"/>
</geometry>
<material name="grey">
<color rgba=".65 .65 .75 1"/>
</material>
</visual>
<collision>
<geometry>
<box size=".039 .008 .129"/>
</geometry>
</collision>
<inertial>
<mass value=".034"/>
<inertia ixx=".000038770" ixy=".0" ixz=".0" iyy=".000042198" iyz=".0" izz=".0000036030"/>
</inertial>
</link>
<joint name="hip_back_leftR_joint" type="fixed">
<axis xyz="0 0 1"/>
<parent link="motor_back_leftR_link"/>
<child link="upper_leg_back_leftR_link"/>
<origin rpy="-1.57075 0 0" xyz="0 .045 -.0145"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="lower_leg_back_leftR_link">
<contact>
<friction_anchor/>
<stiffness value="3000.0"/>
<damping value="100.0"/>
<spinning_friction value=".3"/>
<lateral_friction value="1"/>
</contact>
<visual>
<origin rpy="0 0 0" xyz="0 0 .108"/>
<geometry>
<box size=".017 .009 .216"/>
</geometry>
<material name="grey">
<color rgba=".65 .65 .75 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 .108"/>
<geometry>
<box size=".017 .009 .216"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0 0 .1"/>
<mass value=".072"/>
<inertia ixx=".00023166" ixy=".0" ixz=".0" iyy=".00023325" iyz=".0" izz=".00000198"/>
</inertial>
</link>
<joint name="knee_back_leftR_joint" type="continuous">
<axis xyz="0 1 0"/>
<parent link="upper_leg_back_leftR_link"/>
<child link="lower_leg_back_leftR_link"/>
<origin rpy="0 0 0" xyz="0 .0085 .056"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="upper_leg_back_leftL_link">
<visual>
<geometry>
<box size=".039 .008 .129"/>
</geometry>
<material name="grey">
<color rgba=".65 .65 .75 1"/>
</material>
</visual>
<collision>
<geometry>
<box size=".039 .008 .129"/>
</geometry>
</collision>
<inertial>
<mass value=".034"/>
<inertia ixx=".000038770" ixy=".0" ixz=".0" iyy=".000042198" iyz=".0" izz=".0000036030"/>
</inertial>
</link>
<joint name="hip_back_leftL_joint" type="fixed">
<axis xyz="0 0 1"/>
<parent link="motor_back_leftL_link"/>
<child link="upper_leg_back_leftL_link"/>
<origin rpy="-1.57075 0 0" xyz="0 .045 -.0145"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="lower_leg_back_leftL_link">
<contact>
<friction_anchor/>
<stiffness value="3000.0"/>
<damping value="100.0"/>
<spinning_friction value=".3"/>
<lateral_friction value="1"/>
</contact>
<visual>
<origin rpy="0 0 0" xyz="0 0 .120"/>
<geometry>
<box size=".017 .009 .240"/>
</geometry>
<material name="grey">
<color rgba=".65 .65 .75 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 .120"/>
<geometry>
<box size=".017 .009 .240"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0 0 .120"/>
<mass value=".086"/>
<inertia ixx=".00032459" ixy=".0" ixz=".0" iyy=".00032637" iyz=".0" izz=".000002178"/>
</inertial>
</link>
<joint name="knee_back_leftL_joint" type="continuous">
<axis xyz="0 1 0"/>
<parent link="upper_leg_back_leftL_link"/>
<child link="lower_leg_back_leftL_link"/>
<origin rpy="0 0 0" xyz="0 .0085 .056"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
</robot>

980
examples/pybullet/gym/pybullet_data/quadruped/minitaur_rainbow_dash_v1.urdf Normal file
View File

@@ -0,0 +1,980 @@
<?xml version="1.0"?>
<robot name="quadruped">
<link name="base_chassis_link">
<visual>
<geometry>
<box size=".3 .13 .087"/>
</geometry>
<material name="black">
<color rgba=".3 .3 .3 1"/>
</material>
</visual>
<collision>
<geometry>
<box size=".3 .13 .087"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz=".05 0 0"/>
<mass value="3.353"/>
<inertia ixx=".006837" ixy=".0" ixz=".0" iyy=".027262" iyz=".0" izz=".029870"/>
</inertial>
</link>
<link name="chassis_right">
<visual>
<origin rpy="0 0 0" xyz=".2375 .054 0"/>
<geometry>
<box size=".068 .032 .050"/>
</geometry>
<material name="blue">
<color rgba="0 0 1 1"/>
</material>
</visual>
<visual>
<origin rpy="0 0 0" xyz="-.2375 .054 0"/>
<geometry>
<box size=".068 .032 .050"/>
</geometry>
<material name="yellow">
<color rgba="1 1 0 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz=".2375 .054 0"/>
<geometry>
<box size=".068 .032 .050"/>
</geometry>
</collision>
<collision>
<origin rpy="0 0 0" xyz="-.2375 .054 0"/>
<geometry>
<box size=".068 .032 .050"/>
</geometry>
</collision>
<inertial>
<mass value="0.322"/>
<inertia ixx=".004147" ixy=".0" ixz=".0" iyy=".040913" iyz=".0" izz=".041840"/>
</inertial>
</link>
<joint name="chassis_right_center" type="fixed">
<axis xyz="0 0 1"/>
<parent link="base_chassis_link"/>
<child link="chassis_right"/>
<origin rpy="0 0 0" xyz="0 -.1265 -.0185"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="chassis_left">
<visual>
<origin rpy="0 0 0" xyz=".2375 -.054 0"/>
<geometry>
<box size=".068 .032 .050"/>
</geometry>
<material name="green">
<color rgba="0 1 0 1"/>
</material>
</visual>
<visual>
<origin rpy="0 0 0" xyz="-.2375 -.054 0"/>
<geometry>
<box size=".068 .032 .050"/>
</geometry>
<material name="green"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz=".2375 -.054 0"/>
<geometry>
<box size=".068 .032 .050"/>
</geometry>
</collision>
<collision>
<origin rpy="0 0 0" xyz="-.2375 -.054 0"/>
<geometry>
<box size=".068 .032 .050"/>
</geometry>
</collision>
<inertial>
<mass value="0.322"/>
<inertia ixx=".004147" ixy=".0" ixz=".0" iyy=".040913" iyz=".0" izz=".041840"/>
</inertial>
</link>
<joint name="chassis_left_center" type="fixed">
<axis xyz="0 0 1"/>
<parent link="base_chassis_link"/>
<child link="chassis_left"/>
<origin rpy="0 0 0" xyz="0 .1265 -.0185"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="motor_front_rightR_bracket_link">
<visual>
<origin rpy="0 0 0" xyz="0 -0.02 0"/>
<geometry>
<box size=".068 .032 .050"/>
</geometry>
<material name="red">
<color rgba="1 0 0 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 -0.02 0"/>
<geometry>
<box size=".068 .032 .050"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0 -0.03 0"/>
<mass value=".16"/>
<inertia ixx=".00011397" ixy=".0" ixz=".0" iyy=".00011397" iyz=".0" izz=".00021765"/>
</inertial>
</link>
<joint name="motor_front_rightR_bracket_joint" type="prismatic">
<axis xyz="0 0 1"/>
<parent link="base_chassis_link"/>
<child link="motor_front_rightR_bracket_link"/>
<origin rpy="0 0 0" xyz=".2375 -0.154 -.0185"/>
<limit effort="100" lower="-0.02" upper="0.02" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="motor_front_rightR_link">
<visual>
<geometry>
<mesh filename="tmotor3.obj" scale="1 1 1"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length=".021" radius=".0425"/>
</geometry>
</collision>
<inertial>
<mass value=".241"/>
<inertia ixx=".00011397" ixy=".0" ixz=".0" iyy=".00011397" iyz=".0" izz=".00021765"/>
</inertial>
</link>
<joint name="motor_front_rightR_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="motor_front_rightR_bracket_link"/>
<child link="motor_front_rightR_link"/>
<origin rpy="1.57075 0 0" xyz="0 0 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="motor_front_rightL_link">
<visual>
<geometry>
<mesh filename="tmotor3.obj" scale="1 1 1"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length=".021" radius=".0425"/>
</geometry>
</collision>
<inertial>
<mass value=".241"/>
<inertia ixx=".00011397" ixy=".0" ixz=".0" iyy=".00011397" iyz=".0" izz=".00021765"/>
</inertial>
</link>
<joint name="motor_front_rightL_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="chassis_right"/>
<child link="motor_front_rightL_link"/>
<origin rpy="1.57075 0 3.141592" xyz=".2375 .0275 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="motor_front_leftL_bracket_link">
<visual>
<origin rpy="0 0 0" xyz="0 0.02 0"/>
<geometry>
<box size=".068 .032 .050"/>
</geometry>
<material name="red">
<color rgba="1 0 0 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0.02 0"/>
<geometry>
<box size=".068 .032 .050"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0 0.02 0"/>
<mass value=".16"/>
<inertia ixx=".00011397" ixy=".0" ixz=".0" iyy=".00011397" iyz=".0" izz=".00021765"/>
</inertial>
</link>
<joint name="motor_front_leftL_bracket_joint" type="prismatic">
<axis xyz="0 0 1"/>
<parent link="base_chassis_link"/>
<child link="motor_front_leftL_bracket_link"/>
<origin rpy="0 0 0" xyz=".2375 0.154 -.0185"/>
<limit effort="100" lower="-0.02" upper="0.02" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="motor_front_leftL_link">
<visual>
<geometry>
<mesh filename="tmotor3.obj" scale="1 1 1"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length=".021" radius=".0425"/>
</geometry>
</collision>
<inertial>
<mass value=".241"/>
<inertia ixx=".00011397" ixy=".0" ixz=".0" iyy=".00011397" iyz=".0" izz=".00021765"/>
</inertial>
</link>
<joint name="motor_front_leftL_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="motor_front_leftL_bracket_link"/>
<child link="motor_front_leftL_link"/>
<origin rpy="1.57075 0 3.141592" xyz="0 0 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="motor_front_leftR_link">
<visual>
<geometry>
<mesh filename="tmotor3.obj" scale="1 1 1"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length=".021" radius=".0425"/>
</geometry>
</collision>
<inertial>
<mass value=".241"/>
<inertia ixx=".00011397" ixy=".0" ixz=".0" iyy=".00011397" iyz=".0" izz=".00021765"/>
</inertial>
</link>
<joint name="motor_front_leftR_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="chassis_left"/>
<child link="motor_front_leftR_link"/>
<origin rpy="1.57075 0 0" xyz=".2375 -.0275 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="motor_back_rightR_bracket_link">
<visual>
<origin rpy="0 0 0" xyz="0 -0.02 0"/>
<geometry>
<box size=".068 .032 .050"/>
</geometry>
<material name="red">
<color rgba="1 0 0 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 -0.02 0"/>
<geometry>
<box size=".068 .032 .050"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0 -0.03 0"/>
<mass value=".16"/>
<inertia ixx=".00011397" ixy=".0" ixz=".0" iyy=".00011397" iyz=".0" izz=".00021765"/>
</inertial>
</link>
<joint name="motor_back_rightR_bracket_joint" type="prismatic">
<axis xyz="0 0 1"/>
<parent link="base_chassis_link"/>
<child link="motor_back_rightR_bracket_link"/>
<origin rpy="0 0 0" xyz="-.2375 -0.154 -.0185"/>
<limit effort="100" lower="-0.02" upper="0.02" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="motor_back_rightR_link">
<visual>
<geometry>
<mesh filename="tmotor3.obj" scale="1 1 1"/>
</geometry>
<material name="white1">
<color rgba="1 0 0 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length=".021" radius=".0425"/>
</geometry>
</collision>
<inertial>
<mass value=".241"/>
<inertia ixx=".00011397" ixy=".0" ixz=".0" iyy=".00011397" iyz=".0" izz=".00021765"/>
</inertial>
</link>
<joint name="motor_back_rightR_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="motor_back_rightR_bracket_link"/>
<child link="motor_back_rightR_link"/>
<origin rpy="1.57075 0 0" xyz="0 0 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="motor_back_rightL_link">
<visual>
<geometry>
<mesh filename="tmotor3.obj" scale="1 1 1"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length=".021" radius=".0425"/>
</geometry>
</collision>
<inertial>
<mass value=".241"/>
<inertia ixx=".00011397" ixy=".0" ixz=".0" iyy=".00011397" iyz=".0" izz=".00021765"/>
</inertial>
</link>
<joint name="motor_back_rightL_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="chassis_right"/>
<child link="motor_back_rightL_link"/>
<origin rpy="1.57075 0 3.141592" xyz="-.2375 .0275 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="motor_back_leftL_bracket_link">
<visual>
<origin rpy="0 0 0" xyz="0 0.02 0"/>
<geometry>
<box size=".068 .032 .050"/>
</geometry>
<material name="red">
<color rgba="1 0 0 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0.02 0"/>
<geometry>
<box size=".068 .032 .050"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0 0.02 0"/>
<mass value=".16"/>
<inertia ixx=".00011397" ixy=".0" ixz=".0" iyy=".00011397" iyz=".0" izz=".00021765"/>
</inertial>
</link>
<joint name="motor_back_leftL_bracket_joint" type="prismatic">
<axis xyz="0 0 1"/>
<parent link="base_chassis_link"/>
<child link="motor_back_leftL_bracket_link"/>
<origin rpy="0 0 0" xyz="-.2375 0.154 -.0185"/>
<limit effort="100" lower="-0.02" upper="0.02" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="motor_back_leftL_link">
<visual>
<geometry>
<mesh filename="tmotor3.obj" scale="1 1 1"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length=".021" radius=".0425"/>
</geometry>
</collision>
<inertial>
<mass value=".241"/>
<inertia ixx=".00011397" ixy=".0" ixz=".0" iyy=".00011397" iyz=".0" izz=".00021765"/>
</inertial>
</link>
<joint name="motor_back_leftL_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="motor_back_leftL_bracket_link"/>
<child link="motor_back_leftL_link"/>
<origin rpy="1.57075 0 3.141592" xyz="0 0 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="motor_back_leftR_link">
<visual>
<geometry>
<mesh filename="tmotor3.obj" scale="1 1 1"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length=".021" radius=".0425"/>
</geometry>
</collision>
<inertial>
<mass value=".241"/>
<inertia ixx=".00011397" ixy=".0" ixz=".0" iyy=".00011397" iyz=".0" izz=".00021765"/>
</inertial>
</link>
<joint name="motor_back_leftR_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="chassis_left"/>
<child link="motor_back_leftR_link"/>
<origin rpy="1.57075 0 0" xyz="-.2375 -.0275 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="upper_leg_front_rightR_link">
<visual>
<geometry>
<box size=".039 .008 .129"/>
</geometry>
<material name="grey">
<color rgba=".65 .65 .75 1"/>
</material>
</visual>
<collision>
<geometry>
<box size=".039 .008 .129"/>
</geometry>
</collision>
<inertial>
<mass value=".034"/>
<inertia ixx=".000038770" ixy=".0" ixz=".0" iyy=".000042198" iyz=".0" izz=".0000036030"/>
</inertial>
</link>
<joint name="hip_front_rightR_joint" type="fixed">
<axis xyz="0 0 1"/>
<parent link="motor_front_rightR_link"/>
<child link="upper_leg_front_rightR_link"/>
<origin rpy="-1.57075 0 0" xyz="0 .045 -.0145"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="lower_leg_front_rightR_link">
<contact>
<friction_anchor/>
<stiffness value="3000.0"/>
<damping value="100.0"/>
<spinning_friction value=".05"/>
<lateral_friction value="1"/>
</contact>
<visual>
<origin rpy="0 0 0" xyz="0 0 .120"/>
<geometry>
<box size=".017 .009 .240"/>
</geometry>
<material name="grey">
<color rgba=".65 .65 .75 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 .120"/>
<geometry>
<box size=".017 .009 .240"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0 0 .120"/>
<mass value=".086"/>
<inertia ixx=".00032459" ixy=".0" ixz=".0" iyy=".00032637" iyz=".0" izz=".000002178"/>
</inertial>
</link>
<joint name="knee_front_rightR_joint" type="continuous">
<axis xyz="0 1 0"/>
<parent link="upper_leg_front_rightR_link"/>
<child link="lower_leg_front_rightR_link"/>
<origin rpy="0 0 0" xyz="0 .0085 .056"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="upper_leg_front_rightL_link">
<visual>
<geometry>
<box size=".039 .008 .129"/>
</geometry>
<material name="grey">
<color rgba=".65 .65 .75 1"/>
</material>
</visual>
<collision>
<geometry>
<box size=".039 .008 .129"/>
</geometry>
</collision>
<inertial>
<mass value=".034"/>
<inertia ixx=".000038770" ixy=".0" ixz=".0" iyy=".000042198" iyz=".0" izz=".0000036030"/>
</inertial>
</link>
<joint name="hip_front_rightL_joint" type="fixed">
<axis xyz="0 0 1"/>
<parent link="motor_front_rightL_link"/>
<child link="upper_leg_front_rightL_link"/>
<origin rpy="-1.57075 0 0" xyz="0 .045 -.0145"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="lower_leg_front_rightL_link">
<contact>
<friction_anchor/>
<stiffness value="3000.0"/>
<damping value="100.0"/>
<spinning_friction value=".05"/>
<lateral_friction value="1"/>
</contact>
<visual>
<origin rpy="0 0 0" xyz="0 0 .108"/>
<geometry>
<box size=".017 .009 .216"/>
</geometry>
<material name="grey">
<color rgba=".65 .65 .75 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 .108"/>
<geometry>
<box size=".017 .009 .216"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0 0 .1"/>
<mass value=".072"/>
<inertia ixx=".00023166" ixy=".0" ixz=".0" iyy=".00023325" iyz=".0" izz=".00000198"/>
</inertial>
</link>
<joint name="knee_front_rightL_joint" type="continuous">
<axis xyz="0 1 0"/>
<parent link="upper_leg_front_rightL_link"/>
<child link="lower_leg_front_rightL_link"/>
<origin rpy="0 0 0" xyz="0 .0085 .056"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="upper_leg_front_leftR_link">
<visual>
<geometry>
<box size=".039 .008 .129"/>
</geometry>
<material name="grey">
<color rgba=".65 .65 .75 1"/>
</material>
</visual>
<collision>
<geometry>
<box size=".039 .008 .129"/>
</geometry>
</collision>
<inertial>
<mass value=".034"/>
<inertia ixx=".000038770" ixy=".0" ixz=".0" iyy=".000042198" iyz=".0" izz=".0000036030"/>
</inertial>
</link>
<joint name="hip_front_leftR_joint" type="fixed">
<axis xyz="0 0 1"/>
<parent link="motor_front_leftR_link"/>
<child link="upper_leg_front_leftR_link"/>
<origin rpy="-1.57075 0 0" xyz="0 .045 -.0145"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="lower_leg_front_leftR_link">
<contact>
<friction_anchor/>
<stiffness value="3000.0"/>
<damping value="100.0"/>
<spinning_friction value=".05"/>
<lateral_friction value="1"/>
</contact>
<visual>
<origin rpy="0 0 0" xyz="0 0 .108"/>
<geometry>
<box size=".017 .009 .216"/>
</geometry>
<material name="grey">
<color rgba=".65 .65 .75 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 .108"/>
<geometry>
<box size=".017 .009 .216"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0 0 .1"/>
<mass value=".072"/>
<inertia ixx=".00023166" ixy=".0" ixz=".0" iyy=".00023325" iyz=".0" izz=".00000198"/>
</inertial>
</link>
<joint name="knee_front_leftR_joint" type="continuous">
<axis xyz="0 1 0"/>
<parent link="upper_leg_front_leftR_link"/>
<child link="lower_leg_front_leftR_link"/>
<origin rpy="0 0 0" xyz="0 .0085 .056"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="upper_leg_front_leftL_link">
<visual>
<geometry>
<box size=".039 .008 .129"/>
</geometry>
<material name="grey">
<color rgba=".65 .65 .75 1"/>
</material>
</visual>
<collision>
<geometry>
<box size=".039 .008 .129"/>
</geometry>
</collision>
<inertial>
<mass value=".034"/>
<inertia ixx=".000038770" ixy=".0" ixz=".0" iyy=".000042198" iyz=".0" izz=".0000036030"/>
</inertial>
</link>
<joint name="hip_front_leftL_joint" type="fixed">
<axis xyz="0 0 1"/>
<parent link="motor_front_leftL_link"/>
<child link="upper_leg_front_leftL_link"/>
<origin rpy="-1.57075 0 0" xyz="0 .045 -.0145"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="lower_leg_front_leftL_link">
<contact>
<friction_anchor/>
<stiffness value="3000.0"/>
<damping value="100.0"/>
<spinning_friction value=".05"/>
<lateral_friction value="1"/>
</contact>
<visual>
<origin rpy="0 0 0" xyz="0 0 .120"/>
<geometry>
<box size=".017 .009 .240"/>
</geometry>
<material name="grey">
<color rgba=".65 .65 .75 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 .120"/>
<geometry>
<box size=".017 .009 .240"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0 0 .120"/>
<mass value=".086"/>
<inertia ixx=".00032459" ixy=".0" ixz=".0" iyy=".00032637" iyz=".0" izz=".000002178"/>
</inertial>
</link>
<joint name="knee_front_leftL_joint" type="continuous">
<axis xyz="0 1 0"/>
<parent link="upper_leg_front_leftL_link"/>
<child link="lower_leg_front_leftL_link"/>
<origin rpy="0 0 0" xyz="0 .0085 .056"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="upper_leg_back_rightR_link">
<visual>
<geometry>
<box size=".039 .008 .129"/>
</geometry>
<material name="grey">
<color rgba=".65 .65 .75 1"/>
</material>
</visual>
<collision>
<geometry>
<box size=".039 .008 .129"/>
</geometry>
</collision>
<inertial>
<mass value=".034"/>
<inertia ixx=".000038770" ixy=".0" ixz=".0" iyy=".000042198" iyz=".0" izz=".0000036030"/>
</inertial>
</link>
<joint name="hip_back_rightR_joint" type="fixed">
<axis xyz="0 0 1"/>
<parent link="motor_back_rightR_link"/>
<child link="upper_leg_back_rightR_link"/>
<origin rpy="-1.57075 0 0" xyz="0 .045 -.0145"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="lower_leg_back_rightR_link">
<contact>
<friction_anchor/>
<stiffness value="3000.0"/>
<damping value="100.0"/>
<spinning_friction value=".05"/>
<lateral_friction value="1"/>
</contact>
<visual>
<origin rpy="0 0 0" xyz="0 0 .120"/>
<geometry>
<box size=".017 .009 .240"/>
</geometry>
<material name="grey">
<color rgba=".65 .65 .75 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 .120"/>
<geometry>
<box size=".017 .009 .240"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0 0 .120"/>
<mass value=".086"/>
<inertia ixx=".00032459" ixy=".0" ixz=".0" iyy=".00032637" iyz=".0" izz=".000002178"/>
</inertial>
</link>
<joint name="knee_back_rightR_joint" type="continuous">
<axis xyz="0 1 0"/>
<parent link="upper_leg_back_rightR_link"/>
<child link="lower_leg_back_rightR_link"/>
<origin rpy="0 0 0" xyz="0 .0085 .056"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="upper_leg_back_rightL_link">
<visual>
<geometry>
<box size=".039 .008 .129"/>
</geometry>
<material name="grey">
<color rgba=".65 .65 .75 1"/>
</material>
</visual>
<collision>
<geometry>
<box size=".039 .008 .129"/>
</geometry>
</collision>
<inertial>
<mass value=".034"/>
<inertia ixx=".000038770" ixy=".0" ixz=".0" iyy=".000042198" iyz=".0" izz=".0000036030"/>
</inertial>
</link>
<joint name="hip_back_rightL_joint" type="fixed">
<axis xyz="0 0 1"/>
<parent link="motor_back_rightL_link"/>
<child link="upper_leg_back_rightL_link"/>
<origin rpy="-1.57075 0 0" xyz="0 .045 -.0145"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="lower_leg_back_rightL_link">
<contact>
<friction_anchor/>
<stiffness value="3000.0"/>
<damping value="100.0"/>
<spinning_friction value=".05"/>
<lateral_friction value="1"/>
</contact>
<visual>
<origin rpy="0 0 0" xyz="0 0 .108"/>
<geometry>
<box size=".017 .009 .216"/>
</geometry>
<material name="grey">
<color rgba=".65 .65 .75 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 .108"/>
<geometry>
<box size=".017 .009 .216"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0 0 .1"/>
<mass value=".072"/>
<inertia ixx=".00023166" ixy=".0" ixz=".0" iyy=".00023325" iyz=".0" izz=".00000198"/>
</inertial>
</link>
<joint name="knee_back_rightL_joint" type="continuous">
<axis xyz="0 1 0"/>
<parent link="upper_leg_back_rightL_link"/>
<child link="lower_leg_back_rightL_link"/>
<origin rpy="0 0 0" xyz="0 .0085 .056"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="upper_leg_back_leftR_link">
<visual>
<geometry>
<box size=".039 .008 .129"/>
</geometry>
<material name="grey">
<color rgba=".65 .65 .75 1"/>
</material>
</visual>
<collision>
<geometry>
<box size=".039 .008 .129"/>
</geometry>
</collision>
<inertial>
<mass value=".034"/>
<inertia ixx=".000038770" ixy=".0" ixz=".0" iyy=".000042198" iyz=".0" izz=".0000036030"/>
</inertial>
</link>
<joint name="hip_back_leftR_joint" type="fixed">
<axis xyz="0 0 1"/>
<parent link="motor_back_leftR_link"/>
<child link="upper_leg_back_leftR_link"/>
<origin rpy="-1.57075 0 0" xyz="0 .045 -.0145"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="lower_leg_back_leftR_link">
<contact>
<friction_anchor/>
<stiffness value="3000.0"/>
<damping value="100.0"/>
<spinning_friction value=".05"/>
<lateral_friction value="1"/>
</contact>
<visual>
<origin rpy="0 0 0" xyz="0 0 .108"/>
<geometry>
<box size=".017 .009 .216"/>
</geometry>
<material name="grey">
<color rgba=".65 .65 .75 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 .108"/>
<geometry>
<box size=".017 .009 .216"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0 0 .1"/>
<mass value=".072"/>
<inertia ixx=".00023166" ixy=".0" ixz=".0" iyy=".00023325" iyz=".0" izz=".00000198"/>
</inertial>
</link>
<joint name="knee_back_leftR_joint" type="continuous">
<axis xyz="0 1 0"/>
<parent link="upper_leg_back_leftR_link"/>
<child link="lower_leg_back_leftR_link"/>
<origin rpy="0 0 0" xyz="0 .0085 .056"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="upper_leg_back_leftL_link">
<visual>
<geometry>
<box size=".039 .008 .129"/>
</geometry>
<material name="grey">
<color rgba=".65 .65 .75 1"/>
</material>
</visual>
<collision>
<geometry>
<box size=".039 .008 .129"/>
</geometry>
</collision>
<inertial>
<mass value=".034"/>
<inertia ixx=".000038770" ixy=".0" ixz=".0" iyy=".000042198" iyz=".0" izz=".0000036030"/>
</inertial>
</link>
<joint name="hip_back_leftL_joint" type="fixed">
<axis xyz="0 0 1"/>
<parent link="motor_back_leftL_link"/>
<child link="upper_leg_back_leftL_link"/>
<origin rpy="-1.57075 0 0" xyz="0 .045 -.0145"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="lower_leg_back_leftL_link">
<contact>
<friction_anchor/>
<stiffness value="3000.0"/>
<damping value="100.0"/>
<spinning_friction value=".05"/>
<lateral_friction value="1"/>
</contact>
<visual>
<origin rpy="0 0 0" xyz="0 0 .120"/>
<geometry>
<box size=".017 .009 .240"/>
</geometry>
<material name="grey">
<color rgba=".65 .65 .75 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 .120"/>
<geometry>
<box size=".017 .009 .240"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0 0 .120"/>
<mass value=".086"/>
<inertia ixx=".00032459" ixy=".0" ixz=".0" iyy=".00032637" iyz=".0" izz=".000002178"/>
</inertial>
</link>
<joint name="knee_back_leftL_joint" type="continuous">
<axis xyz="0 1 0"/>
<parent link="upper_leg_back_leftL_link"/>
<child link="lower_leg_back_leftL_link"/>
<origin rpy="0 0 0" xyz="0 .0085 .056"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
</robot>

203
examples/pybullet/gym/pybullet_data/quadruped/minitaur_single_motor.urdf Normal file
View File

@@ -0,0 +1,203 @@
<?xml version="0.0" ?>
<!-- ======================================================================= -->
<!--LICENSE: -->
<!--Copyright (c) 2017, Erwin Coumans -->
<!--Google 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: -->
<!-- -->
<!--1. Redistributions or derived work must retain this copyright notice, -->
<!-- this list of conditions and the following disclaimer. -->
<!-- -->
<!--2. 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. -->
<!-- -->
<!--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 HOLDER 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. -->
<robot name="quadruped">
<link name="base_chassis_link">
<visual>
<geometry>
<box size=".33 0.10 .07"/>
</geometry>
<material name="black">
<color rgba="0.3 0.3 0.3 1"/>
</material>
</visual>
<visual>
<origin rpy="0 0 0" xyz="0 0.10 0"/>
<geometry>
<box size=".17 0.10 .05"/>
</geometry>
<material name="black">
<color rgba="0.3 0.3 0.3 1"/>
</material>
</visual>
<visual>
<origin rpy="0 0 0" xyz="0 -0.10 0"/>
<geometry>
<box size=".17 0.10 .05"/>
</geometry>
<material name="black">
<color rgba="0.3 0.3 0.3 1"/>
</material>
</visual>
<collision>
<geometry>
<box size=".33 0.10 .07"/>
</geometry>
</collision>
<collision>
<origin rpy="0 0 0" xyz="0 0.10 0"/>
<geometry>
<box size=".17 0.10 .05"/>
</geometry>
</collision>
<collision>
<origin rpy="0 0 0" xyz="0 -0.10 0"/>
<geometry>
<box size=".17 0.10 .05"/>
</geometry>
</collision>
<inertial>
<mass value="3"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<link name="chassis_right">
<visual>
<origin rpy="0 0 0" xyz="0 0.035 0"/>
<geometry>
<box size=".34 0.01 .04"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material> </visual>
<visual>
<origin rpy="0 0 0" xyz="0 -0.035 0"/>
<geometry>
<box size=".34 0.01 .04"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material> </visual>
<collision>
<origin rpy="0 0 0" xyz="0 0.035 0"/>
<geometry>
<box size=".34 0.01 .04"/>
</geometry>
</collision>
<collision>
<origin rpy="0 0 0" xyz="0 -0.035 0"/>
<geometry>
<box size=".34 0.01 .04"/>
</geometry>
</collision>
<inertial>
<mass value=".1"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="chassis_right_center" type="fixed">
<axis xyz="0 0 1"/>
<parent link="base_chassis_link"/>
<child link="chassis_right"/>
<origin rpy="-0.0872665 0 0" xyz="0.0 -0.10 0.0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="chassis_left">
<visual>
<origin rpy="0 0 0" xyz="0 0.035 0"/>
<geometry>
<box size=".34 0.01 .04"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material> </visual>
<visual>
<origin rpy="0 0 0" xyz="0 -0.035 0"/>
<geometry>
<box size=".34 0.01 .04"/>
</geometry>
<material name="grey">
<color rgba="0.65 0.65 0.75 1"/>
</material> </visual>
<collision>
<origin rpy="0 0 0" xyz="0 0.035 0"/>
<geometry>
<box size=".34 0.01 .04"/>
</geometry>
</collision>
<collision>
<origin rpy="0 0 0" xyz="0 -0.035 0"/>
<geometry>
<box size=".34 0.01 .04"/>
</geometry>
</collision>
<inertial>
<mass value=".1"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="chassis_left_center" type="fixed">
<axis xyz="0 0 1"/>
<parent link="base_chassis_link"/>
<child link="chassis_left"/>
<origin rpy="0.0872665 0 0" xyz="0.0 0.10 0.0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="motor_front_rightR_link">
<visual>
<geometry>
<mesh filename="tmotor3.obj" scale="1 1 1"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length="0.026" radius="0.0434"/>
</geometry>
</collision>
<inertial>
<mass value="0.25"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="motor_front_rightR_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="chassis_right"/>
<child link="motor_front_rightR_link"/>
<origin rpy="1.57075 0 0" xyz="0.21 -0.025 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
</robot>

33
examples/pybullet/gym/pybullet_data/sphere_with_restitution.urdf Normal file
View File

@@ -0,0 +1,33 @@
<?xml version="0.0" ?>
<robot name="urdf_robot">
<link name="baseLink">
<contact>
<restitution value="0.5" />
<rolling_friction value="0.001"/>
<spinning_friction value="0.001"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="10.0"/>
<inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="textured_sphere_smooth.obj" scale="0.5 0.5 0.5"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.5"/>
</geometry>
</collision>
</link>
</robot>

397
examples/pybullet/gym/pybullet_envs/ARS/ars.py Normal file
View File

@@ -0,0 +1,397 @@
"""Internal implementation of the Augmented Random Search method."""
from __future__ import absolute_import
from __future__ import division
from __future__ import print_function
import os, inspect
currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe())))
os.sys.path.insert(0,currentdir)
from concurrent import futures
import copy
import os
import time
import gym
import numpy as np
import logz
import utils
import optimizers
#from google3.pyglib import gfile
import policies
import shared_noise
import utility
class Worker(object):
"""Object class for parallel rollout generation."""
def __init__(self,
env_seed,
env_callback,
policy_params=None,
deltas=None,
rollout_length=1000,
delta_std=0.02):
# initialize OpenAI environment for each worker
self.env = env_callback()
self.env.seed(env_seed)
# each worker gets access to the shared noise table
# with independent random streams for sampling
# from the shared noise table.
self.deltas = shared_noise.SharedNoiseTable(deltas, env_seed + 7)
self.policy_params = policy_params
if policy_params['type'] == 'linear':
self.policy = policies.LinearPolicy(policy_params)
else:
raise NotImplementedError
self.delta_std = delta_std
self.rollout_length = rollout_length
def get_weights_plus_stats(self):
"""
Get current policy weights and current statistics of past states.
"""
assert self.policy_params['type'] == 'linear'
return self.policy.get_weights_plus_stats()
def rollout(self, shift=0., rollout_length=None):
"""Performs one rollout of maximum length rollout_length.
At each time-step it substracts shift from the reward.
"""
if rollout_length is None:
rollout_length = self.rollout_length
total_reward = 0.
steps = 0
ob = self.env.reset()
for i in range(rollout_length):
action = self.policy.act(ob)
ob, reward, done, _ = self.env.step(action)
steps += 1
total_reward += (reward - shift)
if done:
break
return total_reward, steps
def do_rollouts(self, w_policy, num_rollouts=1, shift=1, evaluate=False):
"""
Generate multiple rollouts with a policy parametrized by w_policy.
"""
print('Doing {} rollouts'.format(num_rollouts))
rollout_rewards, deltas_idx = [], []
steps = 0
for i in range(num_rollouts):
if evaluate:
self.policy.update_weights(w_policy)
deltas_idx.append(-1)
# set to false so that evaluation rollouts are not used for updating state statistics
self.policy.update_filter = False
# for evaluation we do not shift the rewards (shift = 0) and we use the
# default rollout length (1000 for the MuJoCo locomotion tasks)
reward, r_steps = self.rollout(
shift=0., rollout_length=self.rollout_length)
rollout_rewards.append(reward)
else:
idx, delta = self.deltas.get_delta(w_policy.size)
delta = (self.delta_std * delta).reshape(w_policy.shape)
deltas_idx.append(idx)
# set to true so that state statistics are updated
self.policy.update_filter = True
# compute reward and number of timesteps used for positive perturbation rollout
self.policy.update_weights(w_policy + delta)
pos_reward, pos_steps = self.rollout(shift=shift)
# compute reward and number of timesteps used for negative pertubation rollout
self.policy.update_weights(w_policy - delta)
neg_reward, neg_steps = self.rollout(shift=shift)
steps += pos_steps + neg_steps
rollout_rewards.append([pos_reward, neg_reward])
return {
'deltas_idx': deltas_idx,
'rollout_rewards': rollout_rewards,
'steps': steps
}
def stats_increment(self):
self.policy.observation_filter.stats_increment()
return
def get_weights(self):
return self.policy.get_weights()
def get_filter(self):
return self.policy.observation_filter
def sync_filter(self, other):
self.policy.observation_filter.sync(other)
return
class ARSLearner(object):
"""
Object class implementing the ARS algorithm.
"""
def __init__(self,
env_callback,
policy_params=None,
num_workers=32,
num_deltas=320,
deltas_used=320,
delta_std=0.02,
logdir=None,
rollout_length=1000,
step_size=0.01,
shift='constant zero',
params=None,
seed=123):
logz.configure_output_dir(logdir)
# params_to_save = copy.deepcopy(params)
# params_to_save['env'] = None
# logz.save_params(params_to_save)
utility.save_config(params, logdir)
env = env_callback()
self.timesteps = 0
self.action_size = env.action_space.shape[0]
self.ob_size = env.observation_space.shape[0]
self.num_deltas = num_deltas
self.deltas_used = deltas_used
self.rollout_length = rollout_length
self.step_size = step_size
self.delta_std = delta_std
self.logdir = logdir
self.shift = shift
self.params = params
self.max_past_avg_reward = float('-inf')
self.num_episodes_used = float('inf')
# create shared table for storing noise
print('Creating deltas table.')
deltas = shared_noise.create_shared_noise()
self.deltas = shared_noise.SharedNoiseTable(deltas, seed=seed + 3)
print('Created deltas table.')
# initialize workers with different random seeds
print('Initializing workers.')
self.num_workers = num_workers
self.workers = [
Worker(
seed + 7 * i,
env_callback=env_callback,
policy_params=policy_params,
deltas=deltas,
rollout_length=rollout_length,
delta_std=delta_std) for i in range(num_workers)
]
# initialize policy
if policy_params['type'] == 'linear':
self.policy = policies.LinearPolicy(policy_params)
self.w_policy = self.policy.get_weights()
else:
raise NotImplementedError
# initialize optimization algorithm
self.optimizer = optimizers.SGD(self.w_policy, self.step_size)
print('Initialization of ARS complete.')
def aggregate_rollouts(self, num_rollouts=None, evaluate=False):
"""
Aggregate update step from rollouts generated in parallel.
"""
if num_rollouts is None:
num_deltas = self.num_deltas
else:
num_deltas = num_rollouts
results_one = [] #rollout_ids_one
results_two = [] #rollout_ids_two
t1 = time.time()
num_rollouts = int(num_deltas / self.num_workers)
# if num_rollouts > 0:
# with futures.ThreadPoolExecutor(
# max_workers=self.num_workers) as executor:
# workers = [
# executor.submit(
# worker.do_rollouts,
# self.w_policy,
# num_rollouts=num_rollouts,
# shift=self.shift,
# evaluate=evaluate) for worker in self.workers
# ]
# for worker in futures.as_completed(workers):
# results_one.append(worker.result())
#
# workers = [
# executor.submit(
# worker.do_rollouts,
# self.w_policy,
# num_rollouts=1,
# shift=self.shift,
# evaluate=evaluate)
# for worker in self.workers[:(num_deltas % self.num_workers)]
# ]
# for worker in futures.as_completed(workers):
# results_two.append(worker.result())
# parallel generation of rollouts
rollout_ids_one = [
worker.do_rollouts(
self.w_policy,
num_rollouts=num_rollouts,
shift=self.shift,
evaluate=evaluate) for worker in self.workers
]
rollout_ids_two = [
worker.do_rollouts(
self.w_policy, num_rollouts=1, shift=self.shift, evaluate=evaluate)
for worker in self.workers[:(num_deltas % self.num_workers)]
]
results_one = rollout_ids_one
results_two = rollout_ids_two
# gather results
rollout_rewards, deltas_idx = [], []
for result in results_one:
if not evaluate:
self.timesteps += result['steps']
deltas_idx += result['deltas_idx']
rollout_rewards += result['rollout_rewards']
for result in results_two:
if not evaluate:
self.timesteps += result['steps']
deltas_idx += result['deltas_idx']
rollout_rewards += result['rollout_rewards']
deltas_idx = np.array(deltas_idx)
rollout_rewards = np.array(rollout_rewards, dtype=np.float64)
print('Maximum reward of collected rollouts:', rollout_rewards.max())
info_dict = {
"max_reward": rollout_rewards.max()
}
t2 = time.time()
print('Time to generate rollouts:', t2 - t1)
if evaluate:
return rollout_rewards
# select top performing directions if deltas_used < num_deltas
max_rewards = np.max(rollout_rewards, axis=1)
if self.deltas_used > self.num_deltas:
self.deltas_used = self.num_deltas
idx = np.arange(max_rewards.size)[max_rewards >= np.percentile(
max_rewards, 100 * (1 - (self.deltas_used / self.num_deltas)))]
deltas_idx = deltas_idx[idx]
rollout_rewards = rollout_rewards[idx, :]
# normalize rewards by their standard deviation
rollout_rewards /= np.std(rollout_rewards)
t1 = time.time()
# aggregate rollouts to form g_hat, the gradient used to compute SGD step
g_hat, count = utils.batched_weighted_sum(
rollout_rewards[:, 0] - rollout_rewards[:, 1],
(self.deltas.get(idx, self.w_policy.size) for idx in deltas_idx),
batch_size=500)
g_hat /= deltas_idx.size
t2 = time.time()
print('time to aggregate rollouts', t2 - t1)
return g_hat, info_dict
def train_step(self):
"""
Perform one update step of the policy weights.
"""
g_hat, info_dict = self.aggregate_rollouts()
print('Euclidean norm of update step:', np.linalg.norm(g_hat))
self.w_policy -= self.optimizer._compute_step(g_hat).reshape(
self.w_policy.shape)
return info_dict
def train(self, num_iter):
start = time.time()
for i in range(num_iter):
t1 = time.time()
info_dict = self.train_step()
t2 = time.time()
print('total time of one step', t2 - t1)
print('iter ', i, ' done')
# record statistics every 10 iterations
if ((i) % 10 == 0):
rewards = self.aggregate_rollouts(num_rollouts=8, evaluate=True)
w = self.workers[0].get_weights_plus_stats()
checkpoint_filename = os.path.join(
self.logdir, 'lin_policy_plus_{:03d}.npz'.format(i))
print('Save checkpoints to {}...', checkpoint_filename)
checkpoint_file = open(checkpoint_filename, 'w')
np.savez(checkpoint_file, w)
print('End save checkpoints.')
print(sorted(self.params.items()))
logz.log_tabular('Time', time.time() - start)
logz.log_tabular('Iteration', i + 1)
logz.log_tabular('AverageReward', np.mean(rewards))
logz.log_tabular('StdRewards', np.std(rewards))
logz.log_tabular('MaxRewardRollout', np.max(rewards))
logz.log_tabular('MinRewardRollout', np.min(rewards))
logz.log_tabular('timesteps', self.timesteps)
logz.dump_tabular()
t1 = time.time()
# get statistics from all workers
for j in range(self.num_workers):
self.policy.observation_filter.update(self.workers[j].get_filter())
self.policy.observation_filter.stats_increment()
# make sure master filter buffer is clear
self.policy.observation_filter.clear_buffer()
# sync all workers
#filter_id = ray.put(self.policy.observation_filter)
setting_filters_ids = [
worker.sync_filter(self.policy.observation_filter)
for worker in self.workers
]
# waiting for sync of all workers
#ray.get(setting_filters_ids)
increment_filters_ids = [
worker.stats_increment() for worker in self.workers
]
# waiting for increment of all workers
#ray.get(increment_filters_ids)
t2 = time.time()
print('Time to sync statistics:', t2 - t1)
return info_dict

62
examples/pybullet/gym/pybullet_envs/ARS/ars_server.py Normal file
View File

@@ -0,0 +1,62 @@
"""
blaze build -c opt //experimental/users/jietan/ARS:ars_server
blaze-bin/experimental/users/jietan/ARS/ars_server \
--config_name=MINITAUR_GYM_CONFIG
"""
from __future__ import absolute_import
from __future__ import division
from __future__ import print_function
import time
from absl import app
from absl import flags
from concurrent import futures
import grpc
from grpc import loas2
from google3.robotics.reinforcement_learning.minitaur.envs import minitaur_gym_env
from google3.robotics.reinforcement_learning.minitaur.envs import minitaur_reactive_env
from google3.robotics.reinforcement_learning.minitaur.envs.env_randomizers import minitaur_env_randomizer
from google3.robotics.reinforcement_learning.minitaur.envs.env_randomizers import minitaur_env_randomizer_from_config as randomizer_config_lib
from google3.experimental.users.jietan.ARS import ars_evaluation_service_pb2_grpc
from google3.experimental.users.jietan.ARS import ars_evaluation_service
FLAGS = flags.FLAGS
flags.DEFINE_integer("server_id", 0, "number of servers")
flags.DEFINE_integer("port", 20000, "port number.")
flags.DEFINE_string("config_name", None, "The name of the config dictionary.")
flags.DEFINE_bool('run_on_borg', False,
'Whether the servers are running on borg.')
_ONE_DAY_IN_SECONDS = 60 * 60 * 24
def main(unused_argv):
servers = []
server_creds = loas2.loas2_server_credentials()
port = FLAGS.port
if not FLAGS.run_on_borg:
port = 20000 + FLAGS.server_id
server = grpc.server(
futures.ThreadPoolExecutor(max_workers=10), ports=(port,))
servicer = ars_evaluation_service.ParameterEvaluationServicer(
FLAGS.config_name, worker_id=FLAGS.server_id)
ars_evaluation_service_pb2_grpc.add_EvaluationServicer_to_server(
servicer, server)
server.add_secure_port("[::]:{}".format(port), server_creds)
servers.append(server)
server.start()
print("Start server {}".format(FLAGS.server_id))
# prevent the main thread from exiting
try:
while True:
time.sleep(_ONE_DAY_IN_SECONDS)
except KeyboardInterrupt:
for server in servers:
server.stop(0)
if __name__ == "__main__":
app.run(main)

83
examples/pybullet/gym/pybullet_envs/ARS/config_ars.py Normal file
View File

@@ -0,0 +1,83 @@
from __future__ import absolute_import
from __future__ import division
from __future__ import print_function
import functools
from pybullet_envs.minitaur.envs import minitaur_gym_env
from pybullet_envs.minitaur.envs import minitaur_reactive_env
from pybullet_envs.minitaur.envs.env_randomizers import minitaur_env_randomizer
from pybullet_envs.minitaur.envs.env_randomizers import minitaur_env_randomizer_from_config as randomizer_config_lib
MAX_LENGTH = 1000
def merge_two_dicts(x, y):
"""Given two dicts, merge them into a new dict as a shallow copy."""
z = dict(x)
z.update(y)
return z
# The default configurations.
DEFAULT_CONFIG = dict(
num_workers=8,
num_directions=8,
num_iterations=1000,
deltas_used=8,
step_size=0.02,
delta_std=0.03,
rollout_length=MAX_LENGTH,
shift=0,
seed=237,
policy_type="linear",
filter="MeanStdFilter",
)
# Configuration specific to minitaur_gym_env.MinitaurGymEnv class.
MINITAUR_GYM_CONFIG_ADDITIONS = dict(
env=functools.partial(
minitaur_gym_env.MinitaurGymEnv,
urdf_version=minitaur_gym_env.DERPY_V0_URDF_VERSION,
accurate_motor_model_enabled=True,
motor_overheat_protection=True,
pd_control_enabled=True,
env_randomizer=None,#minitaur_env_randomizer.MinitaurEnvRandomizer(),
render=False,
num_steps_to_log=MAX_LENGTH))
MINITAUR_GYM_CONFIG = merge_two_dicts(DEFAULT_CONFIG,
MINITAUR_GYM_CONFIG_ADDITIONS)
# Configuration specific to MinitaurReactiveEnv class.
MINITAUR_REACTIVE_CONFIG_ADDITIONS = dict(
env=functools.partial(
minitaur_reactive_env.MinitaurReactiveEnv,
urdf_version=minitaur_gym_env.RAINBOW_DASH_V0_URDF_VERSION,
energy_weight=0.005,
accurate_motor_model_enabled=True,
pd_latency=0.003,
control_latency=0.02,
motor_kd=0.015,
remove_default_joint_damping=True,
env_randomizer=None,
render=False,
num_steps_to_log=MAX_LENGTH))
MINITAUR_REACTIVE_CONFIG = merge_two_dicts(DEFAULT_CONFIG,
MINITAUR_REACTIVE_CONFIG_ADDITIONS)
# Configuration specific to MinitaurReactiveEnv class with randomizer.
MINITAUR_REACTIVE_RANDOMIZER_CONFIG_ADDITIONS = dict(
env=functools.partial(
minitaur_reactive_env.MinitaurReactiveEnv,
urdf_version=minitaur_gym_env.RAINBOW_DASH_V0_URDF_VERSION,
energy_weight=0.005,
accurate_motor_model_enabled=True,
pd_latency=0.003,
control_latency=0.02,
motor_kd=0.015,
remove_default_joint_damping=True,
env_randomizer=randomizer_config_lib.MinitaurEnvRandomizerFromConfig(),
render=False,
num_steps_to_log=MAX_LENGTH))
MINITAUR_REACTIVE_RANDOMIZER_CONFIG = merge_two_dicts(
DEFAULT_CONFIG, MINITAUR_REACTIVE_RANDOMIZER_CONFIG_ADDITIONS)

99
examples/pybullet/gym/pybullet_envs/ARS/eval_ars.py Normal file
View File

@@ -0,0 +1,99 @@
"""
blaze run -c opt //experimental/users/jietan/ARS:eval_ars -- \
--logdir=/cns/ij-d/home/jietan/experiment/ARS/ars_react_nr01.191950338.191950550/ \
--checkpoint=lin_policy_plus_990.npz \
--num_rollouts=10
"""
from __future__ import absolute_import
from __future__ import division
from __future__ import print_function
import os, inspect
import time
currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe())))
os.sys.path.insert(0,currentdir)
from absl import app
from absl import flags
import pdb
import os
import numpy as np
import gym
import config_ars
import utility
import policies
FLAGS = flags.FLAGS
flags.DEFINE_string('logdir', None, 'The path of the checkpoint.')
flags.DEFINE_string('checkpoint', None, 'The file name of the checkpoint.')
flags.DEFINE_integer('num_rollouts', 1, 'The number of rollouts.')
def main(argv):
del argv # Unused.
print('loading and building expert policy')
checkpoint_file = os.path.join(FLAGS.logdir, FLAGS.checkpoint)
lin_policy = np.load(checkpoint_file, encoding='bytes')
lin_policy = lin_policy.items()[0][1]
M = lin_policy[0]
# mean and std of state vectors estimated online by ARS.
mean = lin_policy[1]
std = lin_policy[2]
config = utility.load_config(FLAGS.logdir)
print("config=",config)
env = config['env'](hard_reset=True, render=True)
ob_dim = env.observation_space.shape[0]
ac_dim = env.action_space.shape[0]
# set policy parameters. Possible filters: 'MeanStdFilter' for v2, 'NoFilter' for v1.
policy_params = {
'type': 'linear',
'ob_filter': config['filter'],
'ob_dim': ob_dim,
'ac_dim': ac_dim,
"weights": M,
"mean": mean,
"std": std,
}
policy = policies.LinearPolicy(policy_params, update_filter=False)
returns = []
observations = []
actions = []
for i in range(FLAGS.num_rollouts):
print('iter', i)
obs = env.reset()
done = False
totalr = 0.
steps = 0
while not done:
action = policy.act(obs)
observations.append(obs)
actions.append(action)
obs, r, done, _ = env.step(action)
time.sleep(1./100.)
totalr += r
steps += 1
if steps % 100 == 0:
print('%i/%i' % (steps, config['rollout_length']))
if steps >= config['rollout_length']:
break
returns.append(totalr)
print('returns', returns)
print('mean return', np.mean(returns))
print('std of return', np.std(returns))
if __name__ == '__main__':
flags.mark_flag_as_required('logdir')
flags.mark_flag_as_required('checkpoint')
app.run(main)

280
examples/pybullet/gym/pybullet_envs/ARS/filter.py Normal file
View File

@@ -0,0 +1,280 @@
# Code in this file is copied and adapted from
# https://github.com/ray-project/ray/blob/master/python/ray/rllib/utils/filter.py
from __future__ import absolute_import
from __future__ import division
from __future__ import print_function
import numpy as np
class Filter(object):
"""Processes input, possibly statefully."""
def update(self, other, *args, **kwargs):
"""Updates self with "new state" from other filter."""
raise NotImplementedError
def copy(self):
"""Creates a new object with same state as self.
Returns:
copy (Filter): Copy of self"""
raise NotImplementedError
def sync(self, other):
"""Copies all state from other filter to self."""
raise NotImplementedError
class NoFilter(Filter):
def __init__(self, *args):
pass
def __call__(self, x, update=True):
return np.asarray(x, dtype = np.float64)
def update(self, other, *args, **kwargs):
pass
def copy(self):
return self
def sync(self, other):
pass
def stats_increment(self):
pass
def clear_buffer(self):
pass
def get_stats(self):
return 0, 1
@property
def mean(self):
return 0
@property
def var(self):
return 1
@property
def std(self):
return 1
# http://www.johndcook.com/blog/standard_deviation/
class RunningStat(object):
def __init__(self, shape=None):
self._n = 0
self._M = np.zeros(shape, dtype = np.float64)
self._S = np.zeros(shape, dtype = np.float64)
self._M2 = np.zeros(shape, dtype = np.float64)
def copy(self):
other = RunningStat()
other._n = self._n
other._M = np.copy(self._M)
other._S = np.copy(self._S)
return other
def push(self, x):
x = np.asarray(x)
# Unvectorized update of the running statistics.
assert x.shape == self._M.shape, ("x.shape = {}, self.shape = {}"
.format(x.shape, self._M.shape))
n1 = self._n
self._n += 1
if self._n == 1:
self._M[...] = x
else:
delta = x - self._M
deltaM2 = np.square(x) - self._M2
self._M[...] += delta / self._n
self._S[...] += delta * delta * n1 / self._n
def update(self, other):
n1 = self._n
n2 = other._n
n = n1 + n2
delta = self._M - other._M
delta2 = delta * delta
M = (n1 * self._M + n2 * other._M) / n
S = self._S + other._S + delta2 * n1 * n2 / n
self._n = n
self._M = M
self._S = S
def __repr__(self):
return '(n={}, mean_mean={}, mean_std={})'.format(
self.n, np.mean(self.mean), np.mean(self.std))
@property
def n(self):
return self._n
@property
def mean(self):
return self._M
@property
def var(self):
return self._S / (self._n - 1) if self._n > 1 else np.square(self._M)
@property
def std(self):
return np.sqrt(self.var)
@property
def shape(self):
return self._M.shape
class MeanStdFilter(Filter):
"""Keeps track of a running mean for seen states"""
def __init__(self, shape, demean=True, destd=True):
self.shape = shape
self.demean = demean
self.destd = destd
self.rs = RunningStat(shape)
# In distributed rollouts, each worker sees different states.
# The buffer is used to keep track of deltas amongst all the
# observation filters.
self.buffer = RunningStat(shape)
self.mean = np.zeros(shape, dtype = np.float64)
self.std = np.ones(shape, dtype = np.float64)
def clear_buffer(self):
self.buffer = RunningStat(self.shape)
return
def update(self, other, copy_buffer=False):
"""Takes another filter and only applies the information from the
buffer.
Using notation `F(state, buffer)`
Given `Filter1(x1, y1)` and `Filter2(x2, yt)`,
`update` modifies `Filter1` to `Filter1(x1 + yt, y1)`
If `copy_buffer`, then `Filter1` is modified to
`Filter1(x1 + yt, yt)`.
"""
self.rs.update(other.buffer)
if copy_buffer:
self.buffer = other.buffer.copy()
return
def copy(self):
"""Returns a copy of Filter."""
other = MeanStdFilter(self.shape)
other.demean = self.demean
other.destd = self.destd
other.rs = self.rs.copy()
other.buffer = self.buffer.copy()
return other
def sync(self, other):
"""Syncs all fields together from other filter.
Using notation `F(state, buffer)`
Given `Filter1(x1, y1)` and `Filter2(x2, yt)`,
`sync` modifies `Filter1` to `Filter1(x2, yt)`
"""
assert other.shape == self.shape, "Shapes don't match!"
self.demean = other.demean
self.destd = other.destd
self.rs = other.rs.copy()
self.buffer = other.buffer.copy()
return
def __call__(self, x, update=True):
x = np.asarray(x, dtype = np.float64)
if update:
if len(x.shape) == len(self.rs.shape) + 1:
# The vectorized case.
for i in range(x.shape[0]):
self.rs.push(x[i])
self.buffer.push(x[i])
else:
# The unvectorized case.
self.rs.push(x)
self.buffer.push(x)
if self.demean:
x = x - self.mean
if self.destd:
x = x / (self.std + 1e-8)
return x
def stats_increment(self):
self.mean = self.rs.mean
self.std = self.rs.std
# Set values for std less than 1e-7 to +inf to avoid
# dividing by zero. State elements with zero variance
# are set to zero as a result.
self.std[self.std < 1e-7] = float("inf")
return
def get_stats(self):
return self.rs.mean, (self.rs.std + 1e-8)
def __repr__(self):
return 'MeanStdFilter({}, {}, {}, {}, {}, {})'.format(
self.shape, self.demean,
self.rs, self.buffer)
def get_filter(filter_config, shape = None):
if filter_config == "MeanStdFilter":
return MeanStdFilter(shape)
elif filter_config == "NoFilter":
return NoFilter()
else:
raise Exception("Unknown observation_filter: " +
str(filter_config))
def test_running_stat():
for shp in ((), (3,), (3, 4)):
li = []
rs = RunningStat(shp)
for _ in range(5):
val = np.random.randn(*shp)
rs.push(val)
li.append(val)
m = np.mean(li, axis=0)
assert np.allclose(rs.mean, m)
v = np.square(m) if (len(li) == 1) else np.var(li, ddof=1, axis=0)
assert np.allclose(rs.var, v)
def test_combining_stat():
for shape in [(), (3,), (3, 4)]:
li = []
rs1 = RunningStat(shape)
rs2 = RunningStat(shape)
rs = RunningStat(shape)
for _ in range(5):
val = np.random.randn(*shape)
rs1.push(val)
rs.push(val)
li.append(val)
for _ in range(9):
rs2.push(val)
rs.push(val)
li.append(val)
rs1.update(rs2)
assert np.allclose(rs.mean, rs1.mean)
assert np.allclose(rs.std, rs1.std)
test_running_stat()
test_combining_stat()

29
examples/pybullet/gym/pybullet_envs/ARS/log/config.yaml Normal file
View File

@@ -0,0 +1,29 @@
delta_std: 0.03
deltas_used: 8
env: !!python/object/apply:functools.partial
args:
- &id001 !!python/name:pybullet_envs.minitaur.envs.minitaur_reactive_env.MinitaurReactiveEnv ''
state: !!python/tuple
- *id001
- !!python/tuple []
- accurate_motor_model_enabled: true
control_latency: 0.02
energy_weight: 0.005
env_randomizer: null
motor_kd: 0.015
num_steps_to_log: 1000
pd_latency: 0.003
remove_default_joint_damping: true
render: false
urdf_version: rainbow_dash_v0
- null
filter: MeanStdFilter
num_directions: 8
num_iterations: 1000
num_workers: 8
policy_type: linear
rollout_length: 1000
seed: 237
shift: 0
step_size: 0.02

BIN
examples/pybullet/gym/pybullet_envs/ARS/log/lin_policy_plus_990_bla.npz Normal file
View File

Binary file not shown.

104
examples/pybullet/gym/pybullet_envs/ARS/logz.py Normal file
View File

@@ -0,0 +1,104 @@
# Code in this file is copied and adapted from
# https://github.com/berkeleydeeprlcourse
import json
"""
Some simple logging functionality, inspired by rllab's logging.
Assumes that each diagnostic gets logged each iteration
Call logz.configure_output_dir() to start logging to a
tab-separated-values file (some_folder_name/log.txt)
"""
import os.path as osp, shutil, time, atexit, os, subprocess
color2num = dict(
gray=30,
red=31,
green=32,
yellow=33,
blue=34,
magenta=35,
cyan=36,
white=37,
crimson=38
)
def colorize(string, color, bold=False, highlight=False):
attr = []
num = color2num[color]
if highlight: num += 10
attr.append(str(num))
if bold: attr.append('1')
return '\x1b[%sm%s\x1b[0m' % (';'.join(attr), string)
class G(object):
output_dir = None
output_file = None
first_row = True
log_headers = []
log_current_row = {}
def configure_output_dir(d=None):
"""
Set output directory to d, or to /tmp/somerandomnumber if d is None
"""
G.first_row = True
G.log_headers = []
G.log_current_row = {}
G.output_dir = d or "/tmp/experiments/%i"%int(time.time())
if not osp.exists(G.output_dir):
os.makedirs(G.output_dir)
G.output_file = open(osp.join(G.output_dir, "log.txt"), 'w')
atexit.register(G.output_file.close)
print(colorize("Logging data to %s"%G.output_file.name, 'green', bold=True))
def log_tabular(key, val):
"""
Log a value of some diagnostic
Call this once for each diagnostic quantity, each iteration
"""
if G.first_row:
G.log_headers.append(key)
else:
assert key in G.log_headers, "Trying to introduce a new key %s that you didn't include in the first iteration"%key
assert key not in G.log_current_row, "You already set %s this iteration. Maybe you forgot to call dump_tabular()"%key
G.log_current_row[key] = val
def save_params(params):
with open(osp.join(G.output_dir, "params.json"), 'w') as out:
out.write(json.dumps(params, separators=(',\n','\t:\t'), sort_keys=True))
def dump_tabular():
"""
Write all of the diagnostics from the current iteration
"""
vals = []
key_lens = [len(key) for key in G.log_headers]
max_key_len = max(15,max(key_lens))
keystr = '%'+'%d'%max_key_len
fmt = "| " + keystr + "s | %15s |"
n_slashes = 22 + max_key_len
print("-"*n_slashes)
for key in G.log_headers:
val = G.log_current_row.get(key, "")
if hasattr(val, "__float__"): valstr = "%8.3g"%val
else: valstr = val
print(fmt%(key, valstr))
vals.append(val)
print("-"*n_slashes)
if G.output_file is not None:
if G.first_row:
G.output_file.write("\t".join(G.log_headers))
G.output_file.write("\n")
G.output_file.write("\t".join(map(str,vals)))
G.output_file.write("\n")
G.output_file.flush()
G.log_current_row.clear()
G.first_row=False

35
examples/pybullet/gym/pybullet_envs/ARS/optimizers.py Normal file
View File

@@ -0,0 +1,35 @@
# Code in this file is copied and adapted from
# https://github.com/openai/evolution-strategies-starter.
from __future__ import absolute_import
from __future__ import division
from __future__ import print_function
import numpy as np
# OPTIMIZERS FOR MINIMIZING OBJECTIVES
class Optimizer(object):
def __init__(self, w_policy):
self.w_policy = w_policy.flatten()
self.dim = w_policy.size
self.t = 0
def update(self, globalg):
self.t += 1
step = self._compute_step(globalg)
ratio = np.linalg.norm(step) / (np.linalg.norm(self.w_policy) + 1e-5)
return self.w_policy + step, ratio
def _compute_step(self, globalg):
raise NotImplementedError
class SGD(Optimizer):
def __init__(self, pi, stepsize):
Optimizer.__init__(self, pi)
self.stepsize = stepsize
def _compute_step(self, globalg):
step = -self.stepsize * globalg
return step

72
examples/pybullet/gym/pybullet_envs/ARS/policies.py Normal file
View File

@@ -0,0 +1,72 @@
"""
Policy class for computing action from weights and observation vector.
Horia Mania --- hmania@berkeley.edu
Aurelia Guy
Benjamin Recht
"""
from __future__ import absolute_import
from __future__ import division
from __future__ import print_function
import numpy as np
import filter
class Policy(object):
def __init__(self, policy_params):
self.ob_dim = policy_params['ob_dim']
self.ac_dim = policy_params['ac_dim']
self.weights = np.empty(0)
# a filter for updating statistics of the observations and normalizing
# inputs to the policies
self.observation_filter = filter.get_filter(
policy_params['ob_filter'], shape=(self.ob_dim,))
self.update_filter = True
def update_weights(self, new_weights):
self.weights[:] = new_weights[:]
return
def get_weights(self):
return self.weights
def get_observation_filter(self):
return self.observation_filter
def act(self, ob):
raise NotImplementedError
def copy(self):
raise NotImplementedError
class LinearPolicy(Policy):
"""
Linear policy class that computes action as <w, ob>.
"""
def __init__(self, policy_params, update_filter=True):
Policy.__init__(self, policy_params)
self.weights = np.zeros(self.ac_dim * self.ob_dim, dtype=np.float64)
if "weights" in policy_params:
self.weights = policy_params["weights"]
if "mean" in policy_params:
self.observation_filter.mean = policy_params["mean"]
if "std" in policy_params:
self.observation_filter.std = policy_params["std"]
self.update_filter = update_filter
def act(self, ob):
ob = self.observation_filter(ob, update=self.update_filter)
matrix_weights = np.reshape(self.weights, (self.ac_dim, self.ob_dim))
return np.clip(np.dot(matrix_weights, ob), -1.0, 1.0)
def get_weights_plus_stats(self):
mu, std = self.observation_filter.get_stats()
aux = np.asarray([self.weights, mu, std])
return aux

41
examples/pybullet/gym/pybullet_envs/ARS/shared_noise.py Normal file
View File

@@ -0,0 +1,41 @@
"""TODO(jietan): DO NOT SUBMIT without one-line documentation for shared_noise.
Code in this file is copied and adapted from
https://github.com/ray-project/ray/tree/master/python/ray/rllib/es
TODO(jietan): DO NOT SUBMIT without a detailed description of shared_noise.
"""
from __future__ import absolute_import
from __future__ import division
from __future__ import print_function
import numpy as np
def create_shared_noise():
"""
Create a large array of noise to be shared by all workers. Used
for avoiding the communication of the random perturbations delta.
"""
seed = 12345
count = 250000000
noise = np.random.RandomState(seed).randn(count).astype(np.float64)
return noise
class SharedNoiseTable(object):
def __init__(self, noise, seed = 11):
self.rg = np.random.RandomState(seed)
self.noise = noise
assert self.noise.dtype == np.float64
def get(self, i, dim):
return self.noise[i:i + dim]
def sample_index(self, dim):
return self.rg.randint(0, len(self.noise) - dim + 1)
def get_delta(self, dim):
idx = self.sample_index(dim)
return idx, self.get(idx, dim)

28
examples/pybullet/gym/pybullet_envs/ARS/start_ars_servers.py Normal file
View File

@@ -0,0 +1,28 @@
"""TODO(jietan): DO NOT SUBMIT without one-line documentation for start_ars_servers.
blaze build -c opt //experimental/users/jietan/ARS:start_ars_servers
blaze-bin/experimental/users/jietan/ARS/start_ars_servers
TODO(jietan): DO NOT SUBMIT without a detailed description of start_ars_servers.
"""
from __future__ import absolute_import
from __future__ import division
from __future__ import print_function
import time
import subprocess
from absl import app
from absl import flags
FLAGS = flags.FLAGS
flags.DEFINE_integer("num_servers", 8, "number of servers")
def main(argv):
del argv # Unused.
for server_id in xrange(FLAGS.num_servers):
args = ["blaze-bin/experimental/users/jietan/ARS/ars_server", "--config_name=MINITAUR_GYM_CONFIG", "--server_id={}".format(server_id), "--run_on_borg=False"]
subprocess.Popen(args)
if __name__ == '__main__':
app.run(main)

93
examples/pybullet/gym/pybullet_envs/ARS/train_ars.borg Normal file
View File

@@ -0,0 +1,93 @@
// Example borg file to do a parameter sweep.
//
// To run:
// echo `srcfs get_readonly`-`g4 p | head -1 | awk '{print $2}'`
// blaze build -c opt experimental/users/jietan/ARS:ars_server.par
// blaze build -c opt experimental/users/jietan/ARS:ars_client.par
// borgcfg --skip_confirmation --vars 'base_cl=191950338,my_cl=191950550,label=ars_react_nr01,config=MINITAUR_REACTIVE_CONFIG' experimental/users/jietan/ARS/train_ars.borg reload
// borgcfg --skip_confirmation --vars 'base_cl=191950338,my_cl=191950550,label=ars_react_rd01,config=MINITAUR_REACTIVE_RANDOMIZER_CONFIG' experimental/users/jietan/ARS/train_ars.borg reload
import '//production/borg/templates/lambda/buildtool_support.borg' as build
import '//production/borg/templates/lambda/dnsname.borg' as dns
vars = {
cell = 'atlanta'
charged_user = 'robotics'
base_cl = 0
my_cl = 0
label = external
user = real_username()
workers = 8
config = external
cns_home = "/cns/ij-d/home/%user%"
logdir = "%cns_home%/experiment/ARS/%label%.%base_cl%.%my_cl%/"
}
service augmented_random_search {
runtime {
cell = vars.cell
}
scheduling = {
priority = 100
batch_quota = {
strategy = 'RUN_SOON'
}
deadline = 3600 * 24
}
accounting = {
charged_user = vars.charged_user
}
requirements {
autopilot = true
}
params = {
mygoogle3 = build.google3dir(myfilename())
experiment_dir = 'experimental/users/jietan/ARS/'
}
job ars_server = {
runtime {
cell = vars.cell
}
name = real_username() + '_server_' + vars.label
replicas = vars.workers
binary_path = build.binfile_v2(params.mygoogle3,
params.experiment_dir + 'ars_server')
runfiles = binary_path + '.runfiles/google3/'
packages = {
package third_party = {
directory = runfiles + 'third_party/'
}
}
binary = build.binfile(params.mygoogle3,
params.experiment_dir + 'ars_server.par')
args = {
server_id = '%task%'
config_name = vars.config
port = '%port%'
run_on_borg = true
}
}
job ars_client = {
name = real_username() + '_client_' + vars.label
binary_path = build.binfile_v2(params.mygoogle3,
params.experiment_dir + 'ars_client')
runfiles = binary_path + '.runfiles/google3/'
packages = {
package third_party = {
directory = runfiles + 'third_party/'
}
}
binary = build.binfile(params.mygoogle3,
params.experiment_dir + 'ars_client.par')
args = {
server_address = dns.borg_dns_name(ars_server)
num_servers = vars.workers
config_name = vars.config
logdir = vars.logdir
run_on_borg = true
}
}
}

64
examples/pybullet/gym/pybullet_envs/ARS/train_ars.py Normal file
View File

@@ -0,0 +1,64 @@
"""TODO(jietan): DO NOT SUBMIT without one-line documentation for train_ars.
blaze build -c opt //experimental/users/jietan/ARS:train_ars
blaze-bin/experimental/users/jietan/ARS/train_ars \
--logdir=/cns/ij-d/home/jietan/experiment/ARS/test1 \
--config_name=MINITAUR_GYM_CONFIG
"""
from __future__ import absolute_import
from __future__ import division
from __future__ import print_function
import os
from absl import app
from absl import flags
import ars
import config_ars
FLAGS = flags.FLAGS
flags.DEFINE_string('logdir', None, 'The directory to write the log file.')
flags.DEFINE_string('config_name', None, 'The name of the config dictionary')
def run_ars(config, logdir):
env = config["env"]()
ob_dim = env.observation_space.shape[0]
ac_dim = env.action_space.shape[0]
# set policy parameters. Possible filters: 'MeanStdFilter' for v2, 'NoFilter' for v1.
policy_params = {
'type': 'linear',
'ob_filter': config['filter'],
'ob_dim': ob_dim,
'ac_dim': ac_dim
}
ARS = ars.ARSLearner(
env_callback=config['env'],
policy_params=policy_params,
num_deltas=config['num_directions'],
deltas_used=config['deltas_used'],
step_size=config['step_size'],
delta_std=config['delta_std'],
logdir=logdir,
rollout_length=config['rollout_length'],
shift=config['shift'],
params=config,
seed=config['seed'])
return ARS.train(config['num_iterations'])
def main(argv):
del argv # Unused.
config = getattr(config_ars, FLAGS.config_name)
run_ars(config=config, logdir=FLAGS.logdir)
if __name__ == '__main__':
flags.mark_flag_as_required('logdir')
flags.mark_flag_as_required('config_name')
app.run(main)

29
examples/pybullet/gym/pybullet_envs/ARS/train_ars_test.py Normal file
View File

@@ -0,0 +1,29 @@
"""Tests for google3.experimental.users.jietan.ARS.train_ars.
blaze build -c opt //experimental/users/jietan/ARS:train_ars_test
blaze-bin/experimental/users/jietan/ARS/train_ars_test
"""
from __future__ import absolute_import
from __future__ import division
from __future__ import print_function
from absl import flags
from google3.testing.pybase import googletest
from google3.experimental.users.jietan.ARS import train_ars
from google3.experimental.users.jietan.ARS import config_ars
FLAGS = flags.FLAGS
MAX_RETURN_AFTER_TWO_ITEATIONS = 0.0890905394617
class TrainArsTest(googletest.TestCase):
def testArsTwoStepResult(self):
config = getattr(config_ars, "MINITAUR_REACTIVE_CONFIG")
config['num_iterations'] = 2
info = train_ars.run_ars(config=config, logdir=FLAGS.test_tmpdir)
print (info)
self.assertAlmostEqual(info["max_reward"], MAX_RETURN_AFTER_TWO_ITEATIONS)
if __name__ == '__main__':
googletest.main()

52
examples/pybullet/gym/pybullet_envs/ARS/utility.py Normal file
View File

@@ -0,0 +1,52 @@
from __future__ import absolute_import
from __future__ import division
from __future__ import print_function
import os
import ruamel.yaml as yaml
def save_config(config, logdir):
"""Save a new configuration by name.
If a logging directory is specified, is will be created and the configuration
will be stored there. Otherwise, a log message will be printed.
Args:
config: Configuration object.
logdir: Location for writing summaries and checkpoints if specified.
Returns:
Configuration object.
"""
message = 'Start a new run and write summaries and checkpoints to {}.'
print(message.format(logdir))
config_path = os.path.join(logdir, 'config.yaml')
yaml.dump(config, config_path, default_flow_style=False)
return config
def load_config(logdir):
"""Load a configuration from the log directory.
Args:
logdir: The logging directory containing the configuration file.
Raises:
IOError: The logging directory does not contain a configuration file.
Returns:
Configuration object.
"""
config_path = logdir and os.path.join(logdir, 'config.yaml')
if not config_path:
message = (
'Cannot resume an existing run since the logging directory does not '
'contain a configuration file.')
raise IOError(message)
print("config_path=",config_path)
stream = open(config_path, 'r')
config = yaml.load(stream)
message = 'Resume run and write summaries and checkpoints to {}.'
print(message.format(logdir))
return config

28
examples/pybullet/gym/pybullet_envs/ARS/utils.py Normal file
View File

@@ -0,0 +1,28 @@
# Code in this file is copied and adapted from
# https://github.com/openai/evolution-strategies-starter.
import numpy as np
def itergroups(items, group_size):
assert group_size >= 1
group = []
for x in items:
group.append(x)
if len(group) == group_size:
yield tuple(group)
del group[:]
if group:
yield tuple(group)
def batched_weighted_sum(weights, vecs, batch_size):
total = 0
num_items_summed = 0
for batch_weights, batch_vecs in zip(itergroups(weights, batch_size),
itergroups(vecs, batch_size)):
assert len(batch_weights) == len(batch_vecs) <= batch_size
total += np.dot(np.asarray(batch_weights, dtype=np.float64),
np.asarray(batch_vecs, dtype=np.float64))
num_items_summed += len(batch_weights)
return total, num_items_summed

Some files were not shown because too many files have changed in this diff Show More