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

Merge pull request #1935 from erwincoumans/master

Browse Source 
fix fileIO issue with \r\n in lines
This commit is contained in:
erwincoumans
2018-10-14 16:48:22 -07:00
committed by GitHub
parent 11ef1bf0cd b0a3b1b962
commit 79f6cc901e
26 changed files with 1059 additions and 75 deletions
Download Patch File Download Diff File Expand all files Collapse all files

122
data/threecubes/newsdf.sdf Normal file
View File

@@ -0,0 +1,122 @@
<sdf version='1.6'>
<world name='default'>
<gravity>0 0 -9.8</gravity>
<model name='part0.obj'>
<static>1</static>
<pose frame=''>0 0 0 0 0 0</pose>
<link name='link_d0'>
<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_0'>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>part0.obj</uri>
</mesh>
</geometry>
</collision>
<visual name='visual'>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>part0.obj</uri>
</mesh>
</geometry>
<material>
<ambient>1 0 0 1</ambient>
<diffuse>0.000000 0.640000 0.000000 1.000000</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
</link>
</model>
<model name='part1.obj'>
<static>1</static>
<pose frame=''>0 0 0 0 0 0</pose>
<link name='link_d1'>
<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>
<mesh>
<scale>1 1 1</scale>
<uri>part1.obj</uri>
</mesh>
</geometry>
</collision>
<visual name='visual'>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>part1.obj</uri>
</mesh>
</geometry>
<material>
<ambient>1 0 0 1</ambient>
<diffuse>0.000000 0.000000 0.640000 1.000000</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
</link>
</model>
<model name='part2.obj'>
<static>1</static>
<pose frame=''>0 0 0 0 0 0</pose>
<link name='link_d2'>
<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_2'>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>part2.obj</uri>
</mesh>
</geometry>
</collision>
<visual name='visual'>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>part2.obj</uri>
</mesh>
</geometry>
<material>
<ambient>1 0 0 1</ambient>
<diffuse>0.640000 0.000000 0.000000 1.000000</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
</link>
</model>
</world>
</sdf>

60
data/threecubes/part0.obj Normal file
View File

@@ -0,0 +1,60 @@
# Exported using automatic converter by Erwin Coumans
mtllib three_cubes.mtl
#object Cube.001
v -4.474365 4.513344 -0.488734
v -4.474365 2.513344 -0.488735
v -4.474365 2.513344 1.511265
v -2.474365 4.513344 -0.488734
v -2.474365 2.513344 -0.488735
v -4.474365 2.513344 -0.488735
v -2.474365 4.513343 1.511266
v -2.474365 2.513344 1.511265
v -2.474365 2.513344 -0.488735
v -4.474365 4.513343 1.511266
v -4.474365 2.513344 1.511265
v -2.474365 2.513344 1.511265
v -4.474365 2.513344 -0.488735
v -2.474365 4.513343 1.511266
v -2.474365 4.513344 -0.488734
v -4.474365 4.513343 1.511266
v -4.474365 4.513344 -0.488734
v -2.474365 4.513344 -0.488734
v -2.474365 4.513343 1.511266
v -4.474365 2.513344 1.511265
usemtl Material.001
vt 1.000000 0.000000
vt 1.000000 1.000000
vt 0.000000 1.000000
vt 1.000000 0.000000
vt 1.000000 1.000000
vt 0.000000 1.000000
vt 1.000000 0.000000
vt 1.000000 1.000000
vt 0.000000 1.000000
vt 1.000000 0.000000
vt 1.000000 1.000000
vt 0.000000 1.000000
vt 1.000000 0.000000
vt 1.000000 1.000000
vt 0.000000 1.000000
vt 0.000000 0.000000
vt 0.000000 0.000000
vt 0.000000 0.000000
vt 0.000000 0.000000
vt 0.000000 0.000000
s off
f 1/1/1 2/2/2 3/3/3
f 4/4/4 5/5/5 6/6/6
f 7/7/7 8/8/8 9/9/9
f 10/10/10 11/11/11 12/12/12
f 13/13/13 5/5/5 12/12/12
f 10/10/10 14/14/14 15/15/15
f 16/16/16 1/1/1 3/3/3
f 17/17/17 4/4/4 6/6/6
f 18/18/18 7/7/7 9/9/9
f 19/19/19 10/10/10 12/12/12
f 20/20/20 13/13/13 12/12/12
f 17/17/17 10/10/10 15/15/15

60
data/threecubes/part1.obj Normal file
View File

@@ -0,0 +1,60 @@
# Exported using automatic converter by Erwin Coumans
mtllib three_cubes.mtl
#object Cube.002
v -4.474365 4.513344 -3.663786
v -4.474365 2.513345 -3.663786
v -4.474365 2.513344 -1.663787
v -2.474365 4.513344 -3.663786
v -2.474365 2.513345 -3.663786
v -4.474365 2.513345 -3.663786
v -2.474365 4.513344 -1.663786
v -2.474365 2.513344 -1.663787
v -2.474365 2.513345 -3.663786
v -4.474365 4.513344 -1.663786
v -4.474365 2.513344 -1.663787
v -2.474365 2.513344 -1.663787
v -4.474365 2.513345 -3.663786
v -2.474365 4.513344 -1.663786
v -2.474365 4.513344 -3.663786
v -4.474365 4.513344 -1.663786
v -4.474365 4.513344 -3.663786
v -2.474365 4.513344 -3.663786
v -2.474365 4.513344 -1.663786
v -4.474365 2.513344 -1.663787
usemtl Material.002
vt 1.000000 0.000000
vt 1.000000 1.000000
vt 0.000000 1.000000
vt 1.000000 0.000000
vt 1.000000 1.000000
vt 0.000000 1.000000
vt 1.000000 0.000000
vt 1.000000 1.000000
vt 0.000000 1.000000
vt 1.000000 0.000000
vt 1.000000 1.000000
vt 0.000000 1.000000
vt 1.000000 0.000000
vt 1.000000 1.000000
vt 0.000000 1.000000
vt 0.000000 0.000000
vt 0.000000 0.000000
vt 0.000000 0.000000
vt 0.000000 0.000000
vt 0.000000 0.000000
s off
f 1/1/1 2/2/2 3/3/3
f 4/4/4 5/5/5 6/6/6
f 7/7/7 8/8/8 9/9/9
f 10/10/10 11/11/11 12/12/12
f 13/13/13 5/5/5 12/12/12
f 10/10/10 14/14/14 15/15/15
f 16/16/16 1/1/1 3/3/3
f 17/17/17 4/4/4 6/6/6
f 18/18/18 7/7/7 9/9/9
f 19/19/19 10/10/10 12/12/12
f 20/20/20 13/13/13 12/12/12
f 17/17/17 10/10/10 15/15/15

60
data/threecubes/part2.obj Normal file
View File

@@ -0,0 +1,60 @@
# Exported using automatic converter by Erwin Coumans
mtllib three_cubes.mtl
#object Cube
v -4.474365 4.513343 2.535691
v -4.474365 2.513344 2.535691
v -4.474365 2.513343 4.535690
v -2.474365 4.513343 2.535691
v -2.474365 2.513344 2.535691
v -4.474365 2.513344 2.535691
v -2.474365 4.513343 4.535691
v -2.474365 2.513343 4.535690
v -2.474365 2.513344 2.535691
v -4.474365 4.513343 4.535691
v -4.474365 2.513343 4.535690
v -2.474365 2.513343 4.535690
v -4.474365 2.513344 2.535691
v -2.474365 4.513343 4.535691
v -2.474365 4.513343 2.535691
v -4.474365 4.513343 4.535691
v -4.474365 4.513343 2.535691
v -2.474365 4.513343 2.535691
v -2.474365 4.513343 4.535691
v -4.474365 2.513343 4.535690
usemtl Material
vt 1.000000 0.000000
vt 1.000000 1.000000
vt 0.000000 1.000000
vt 1.000000 0.000000
vt 1.000000 1.000000
vt 0.000000 1.000000
vt 1.000000 0.000000
vt 1.000000 1.000000
vt 0.000000 1.000000
vt 1.000000 0.000000
vt 1.000000 1.000000
vt 0.000000 1.000000
vt 1.000000 0.000000
vt 1.000000 1.000000
vt 0.000000 1.000000
vt 0.000000 0.000000
vt 0.000000 0.000000
vt 0.000000 0.000000
vt 0.000000 0.000000
vt 0.000000 0.000000
s off
f 1/1/1 2/2/2 3/3/3
f 4/4/4 5/5/5 6/6/6
f 7/7/7 8/8/8 9/9/9
f 10/10/10 11/11/11 12/12/12
f 13/13/13 5/5/5 12/12/12
f 10/10/10 14/14/14 15/15/15
f 16/16/16 1/1/1 3/3/3
f 17/17/17 4/4/4 6/6/6
f 18/18/18 7/7/7 9/9/9
f 19/19/19 10/10/10 12/12/12
f 20/20/20 13/13/13 12/12/12
f 17/17/17 10/10/10 15/15/15

31
data/threecubes/three_cubes.mtl Normal file
View File

@@ -0,0 +1,31 @@
# Blender MTL File: 'None'
# Material Count: 3
newmtl Material
Ns 92.156863
Ka 0.000000 0.000000 0.000000
Kd 0.640000 0.000000 0.000000
Ks 0.500000 0.500000 0.500000
Ni 1.000000
d 1.000000
illum 2
newmtl Material.001
Ns 92.156863
Ka 0.000000 0.000000 0.000000
Kd 0.000000 0.640000 0.000000
Ks 0.500000 0.500000 0.500000
Ni 1.000000
d 1.000000
illum 2
newmtl Material.002
Ns 92.156863
Ka 0.000000 0.000000 0.000000
Kd 0.000000 0.000000 0.640000
Ks 0.500000 0.500000 0.500000
Ni 1.000000
d 1.000000
illum 2

84
data/threecubes/three_cubes.obj Normal file
View File

@@ -0,0 +1,84 @@
# Blender v2.71 (sub 0) OBJ File: ''
# www.blender.org
mtllib three_cubes.mtl
o Cube.001
v -4.474365 4.513343 1.511266
v -4.474365 4.513344 -0.488734
v -4.474365 2.513344 -0.488735
v -4.474365 2.513344 1.511265
v -2.474365 4.513344 -0.488734
v -2.474365 2.513344 -0.488735
v -2.474365 4.513343 1.511266
v -2.474365 2.513344 1.511265
vt 1.000000 0.000000
vt 1.000000 1.000000
vt 0.000000 1.000000
vt 0.000000 0.000000
usemtl Material.001
s off
f 2/1 3/2 4/3
f 5/1 6/2 3/3
f 7/1 8/2 6/3
f 1/1 4/2 8/3
f 3/1 6/2 8/3
f 1/1 7/2 5/3
f 1/4 2/1 4/3
f 2/4 5/1 3/3
f 5/4 7/1 6/3
f 7/4 1/1 8/3
f 4/4 3/1 8/3
f 2/4 1/1 5/3
o Cube.002
v -4.474365 4.513344 -1.663786
v -4.474365 4.513344 -3.663786
v -4.474365 2.513345 -3.663786
v -4.474365 2.513344 -1.663787
v -2.474365 4.513344 -3.663786
v -2.474365 2.513345 -3.663786
v -2.474365 4.513344 -1.663786
v -2.474365 2.513344 -1.663787
vt 1.000000 0.000000
vt 1.000000 1.000000
vt 0.000000 1.000000
vt 0.000000 0.000000
usemtl Material.002
s off
f 10/5 11/6 12/7
f 13/5 14/6 11/7
f 15/5 16/6 14/7
f 9/5 12/6 16/7
f 11/5 14/6 16/7
f 9/5 15/6 13/7
f 9/8 10/5 12/7
f 10/8 13/5 11/7
f 13/8 15/5 14/7
f 15/8 9/5 16/7
f 12/8 11/5 16/7
f 10/8 9/5 13/7
o Cube
v -4.474365 4.513343 4.535691
v -4.474365 4.513343 2.535691
v -4.474365 2.513344 2.535691
v -4.474365 2.513343 4.535690
v -2.474365 4.513343 2.535691
v -2.474365 2.513344 2.535691
v -2.474365 4.513343 4.535691
v -2.474365 2.513343 4.535690
vt 1.000000 0.000000
vt 1.000000 1.000000
vt 0.000000 1.000000
vt 0.000000 0.000000
usemtl Material
s off
f 18/9 19/10 20/11
f 21/9 22/10 19/11
f 23/9 24/10 22/11
f 17/9 20/10 24/11
f 19/9 22/10 24/11
f 17/9 23/10 21/11
f 17/12 18/9 20/11
f 18/12 21/9 19/11
f 21/12 23/9 22/11
f 23/12 17/9 24/11
f 20/12 19/9 24/11
f 18/12 17/9 21/11

8
data/threecubes/threecubes.py Normal file
View File

@@ -0,0 +1,8 @@
import pybullet as p
p.connect(p.DIRECT)
p.loadPlugin("eglRendererPlugin")
p.loadSDF("newsdf.sdf")
while (1):
p.getCameraImage(320,240, flags=p.ER_NO_SEGMENTATION_MASK)
p.stepSimulation()

32
data/threecubes/threecubes.urdf Normal file
View File

@@ -0,0 +1,32 @@
<?xml version="1.0" ?>
<robot name="cube">
<link name="baseLink">
<contact>
<lateral_friction value="1.0"/>
<rolling_friction value="0.0"/>
<contact_cfm value="0.0"/>
<contact_erp value="1.0"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="1.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="three_cubes.obj" scale="1 1 1"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="three_cubes.obj" scale="1 1 1"/>
</geometry>
</collision>
</link>
</robot>

22
data/threecubes/threecubes_org.py Normal file
View File

@@ -0,0 +1,22 @@
import pybullet as p
useEGL = False
useEGLGUI = False
if useEGL:
if useEGLGUI:
p.connect(p.GUI, "window_backend=2")
else:
p.connect(p.DIRECT)
p.loadPlugin("eglRendererPlugin")
else:
p.connect(p.GUI)
p.loadURDF("threecubes.urdf", flags=p.URDF_USE_MATERIAL_COLORS_FROM_MTL)
while (1):
viewmat= [0.642787516117096, -0.4393851161003113, 0.6275069713592529, 0.0, 0.766044557094574, 0.36868777871131897, -0.5265407562255859, 0.0, -0.0, 0.8191521167755127, 0.5735764503479004, 0.0, 2.384185791015625e-07, 2.384185791015625e-07, -5.000000476837158, 1.0]
projmat= [0.7499999403953552, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, -1.0000200271606445, -1.0, 0.0, 0.0, -0.02000020071864128, 0.0]
p.getCameraImage(64,64, viewMatrix=viewmat, projectionMatrix=projmat, flags=p.ER_NO_SEGMENTATION_MASK )
p.stepSimulation()

13
examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp
View File

@@ -53,6 +53,7 @@ bool b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(const std::string&
meshData.m_textureImage1 = 0; meshData.m_textureImage1 = 0;
meshData.m_textureHeight = 0; meshData.m_textureHeight = 0;
meshData.m_textureWidth = 0; meshData.m_textureWidth = 0;
meshData.m_flags = 0;
meshData.m_isCached = false; meshData.m_isCached = false;
char relativeFileName[1024]; char relativeFileName[1024];
@@ -78,6 +79,18 @@ bool b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(const std::string&
for (int i = 0; meshData.m_textureImage1 == 0 && i < shapes.size(); i++) for (int i = 0; meshData.m_textureImage1 == 0 && i < shapes.size(); i++)
{ {
const tinyobj::shape_t& shape = shapes[i]; const tinyobj::shape_t& shape = shapes[i];
meshData.m_rgbaColor[0] = shape.material.diffuse[0];
meshData.m_rgbaColor[1] = shape.material.diffuse[1];
meshData.m_rgbaColor[2] = shape.material.diffuse[2];
meshData.m_rgbaColor[3] = shape.material.transparency;
meshData.m_flags |= B3_IMPORT_MESH_HAS_RGBA_COLOR;
meshData.m_specularColor[0] = shape.material.specular[0];
meshData.m_specularColor[1] = shape.material.specular[1];
meshData.m_specularColor[2] = shape.material.specular[2];
meshData.m_specularColor[3] = 1;
meshData.m_flags |= B3_IMPORT_MESH_HAS_SPECULAR_COLOR;
if (shape.material.diffuse_texname.length() > 0) if (shape.material.diffuse_texname.length() > 0)
{ {
int width, height, n; int width, height, n;

20
examples/Importers/ImportMeshUtility/b3ImportMeshUtility.h
View File

@@ -3,6 +3,12 @@
#include <string> #include <string>
enum b3ImportMeshDataFlags
{
B3_IMPORT_MESH_HAS_RGBA_COLOR=1,
B3_IMPORT_MESH_HAS_SPECULAR_COLOR=2,
};
struct b3ImportMeshData struct b3ImportMeshData
{ {
struct GLInstanceGraphicsShape* m_gfxShape; struct GLInstanceGraphicsShape* m_gfxShape;
@@ -11,6 +17,20 @@ struct b3ImportMeshData
bool m_isCached; bool m_isCached;
int m_textureWidth; int m_textureWidth;
int m_textureHeight; int m_textureHeight;
double m_rgbaColor[4];
double m_specularColor[4];
int m_flags;
b3ImportMeshData()
:m_gfxShape(0),
m_textureImage1(0),
m_isCached(false),
m_textureWidth(0),
m_textureHeight(0),
m_flags(0)
{
}
}; };
class b3ImportMeshUtility class b3ImportMeshUtility

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

@@ -228,24 +228,36 @@ bool BulletURDFImporter::loadSDF(const char* fileName, bool forceFixedBase)
} }
else else
{ {
char path[1024]; char path[1024];
fu.extractPath(relativeFileName, path, sizeof(path)); fu.extractPath(relativeFileName, path, sizeof(path));
m_data->setSourceFile(relativeFileName, path); m_data->setSourceFile(relativeFileName, path);
std::fstream xml_file(relativeFileName, std::fstream::in); //read file
while (xml_file.good()) int fileId = m_data->m_fileIO->fileOpen(relativeFileName,"r");
char destBuffer[8192];
char* line = 0;
do
{ {
std::string line; line = m_data->m_fileIO->readLine(fileId, destBuffer, 8192);
std::getline(xml_file, line); if (line)
xml_string += (line + "\n"); {
xml_string += (std::string(destBuffer) + "\n");
}
} }
xml_file.close(); while (line);
m_data->m_fileIO->fileClose(fileId);
} }
BulletErrorLogger loggie; BulletErrorLogger loggie;
//todo: quick test to see if we can re-use the URDF parser for SDF or not //todo: quick test to see if we can re-use the URDF parser for SDF or not
m_data->m_urdfParser.setParseSDF(true); m_data->m_urdfParser.setParseSDF(true);
bool result = m_data->m_urdfParser.loadSDF(xml_string.c_str(), &loggie); bool result = false;
if (xml_string.length())
{
result = m_data->m_urdfParser.loadSDF(xml_string.c_str(), &loggie);
}
return result; return result;
} }
@@ -861,7 +873,7 @@ btCollisionShape* BulletURDFImporter::convertURDFToCollisionShape(const UrdfColl
return shape; return shape;
} }
void BulletURDFImporter::convertURDFToVisualShapeInternal(const UrdfVisual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut, btAlignedObjectArray<BulletURDFTexture>& texturesOut) const void BulletURDFImporter::convertURDFToVisualShapeInternal(const UrdfVisual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut, btAlignedObjectArray<BulletURDFTexture>& texturesOut, struct b3ImportMeshData& meshData) const
{ {
BT_PROFILE("convertURDFToVisualShapeInternal"); BT_PROFILE("convertURDFToVisualShapeInternal");
@@ -920,7 +932,7 @@ void BulletURDFImporter::convertURDFToVisualShapeInternal(const UrdfVisual* visu
{ {
case UrdfGeometry::FILE_OBJ: case UrdfGeometry::FILE_OBJ:
{ {
b3ImportMeshData meshData;
if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(visual->m_geometry.m_meshFileName, meshData, m_data->m_fileIO)) if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(visual->m_geometry.m_meshFileName, meshData, m_data->m_fileIO))
{ {
if (meshData.m_textureImage1) if (meshData.m_textureImage1)
@@ -1150,16 +1162,49 @@ int BulletURDFImporter::convertLinkVisualShapes(int linkIndex, const char* pathP
btTransform childTrans = vis.m_linkLocalFrame; btTransform childTrans = vis.m_linkLocalFrame;
btHashString matName(vis.m_materialName.c_str()); btHashString matName(vis.m_materialName.c_str());
UrdfMaterial* const* matPtr = model.m_materials[matName]; UrdfMaterial* const* matPtr = model.m_materials[matName];
if (matPtr) b3ImportMeshData meshData;
convertURDFToVisualShapeInternal(&vis, pathPrefix, localInertiaFrame.inverse() * childTrans, vertices, indices, textures,meshData);
if (m_data->m_flags&CUF_USE_MATERIAL_COLORS_FROM_MTL)
{ {
UrdfMaterial* const mat = *matPtr; if ((meshData.m_flags & B3_IMPORT_MESH_HAS_RGBA_COLOR) &&
//printf("UrdfMaterial %s, rgba = %f,%f,%f,%f\n",mat->m_name.c_str(),mat->m_rgbaColor[0],mat->m_rgbaColor[1],mat->m_rgbaColor[2],mat->m_rgbaColor[3]); (meshData.m_flags & B3_IMPORT_MESH_HAS_SPECULAR_COLOR))
UrdfMaterialColor matCol; {
matCol.m_rgbaColor = mat->m_matColor.m_rgbaColor; UrdfMaterialColor matCol;
matCol.m_specularColor = mat->m_matColor.m_specularColor;
m_data->m_linkColors.insert(linkIndex, matCol); if (m_data->m_flags&CUF_USE_MATERIAL_TRANSPARANCY_FROM_MTL)
{
matCol.m_rgbaColor.setValue(meshData.m_rgbaColor[0],
meshData.m_rgbaColor[1],
meshData.m_rgbaColor[2],
meshData.m_rgbaColor[3]);
} else
{
matCol.m_rgbaColor.setValue(meshData.m_rgbaColor[0],
meshData.m_rgbaColor[1],
meshData.m_rgbaColor[2],
1);
}
matCol.m_specularColor.setValue(meshData.m_specularColor[0],
meshData.m_specularColor[1],
meshData.m_specularColor[2]);
m_data->m_linkColors.insert(linkIndex, matCol);
}
} else
{
if (matPtr)
{
UrdfMaterial* const mat = *matPtr;
//printf("UrdfMaterial %s, rgba = %f,%f,%f,%f\n",mat->m_name.c_str(),mat->m_rgbaColor[0],mat->m_rgbaColor[1],mat->m_rgbaColor[2],mat->m_rgbaColor[3]);
UrdfMaterialColor matCol;
matCol.m_rgbaColor = mat->m_matColor.m_rgbaColor;
matCol.m_specularColor = mat->m_matColor.m_specularColor;
m_data->m_linkColors.insert(linkIndex, matCol);
}
} }
convertURDFToVisualShapeInternal(&vis, pathPrefix, localInertiaFrame.inverse() * childTrans, vertices, indices, textures);
} }
} }
if (vertices.size() && indices.size()) if (vertices.size() && indices.size())
@@ -1266,6 +1311,7 @@ void BulletURDFImporter::convertLinkVisualShapes2(int linkIndex, int urdfIndex,
UrdfLink* const* linkPtr = model.m_links.getAtIndex(urdfIndex); UrdfLink* const* linkPtr = model.m_links.getAtIndex(urdfIndex);
if (linkPtr) if (linkPtr)
{ {
m_data->m_customVisualShapesConverter->setFlags(m_data->m_flags);
m_data->m_customVisualShapesConverter->convertVisualShapes(linkIndex, pathPrefix, localInertiaFrame, *linkPtr, &model, colObj->getBroadphaseHandle()->getUid(), bodyUniqueId, m_data->m_fileIO); m_data->m_customVisualShapesConverter->convertVisualShapes(linkIndex, pathPrefix, localInertiaFrame, *linkPtr, &model, colObj->getBroadphaseHandle()->getUid(), bodyUniqueId, m_data->m_fileIO);
} }
} }

2
examples/Importers/ImportURDFDemo/BulletUrdfImporter.h
View File

@@ -88,7 +88,7 @@ public:
virtual int getAllocatedTexture(int index) const; virtual int getAllocatedTexture(int index) const;
virtual void setEnableTinyRenderer(bool enable); virtual void setEnableTinyRenderer(bool enable);
void convertURDFToVisualShapeInternal(const struct UrdfVisual* visual, const char* urdfPathPrefix, const class btTransform& visualTransform, btAlignedObjectArray<struct GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut, btAlignedObjectArray<struct BulletURDFTexture>& texturesOut) const; void convertURDFToVisualShapeInternal(const struct UrdfVisual* visual, const char* urdfPathPrefix, const class btTransform& visualTransform, btAlignedObjectArray<struct GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut, btAlignedObjectArray<struct BulletURDFTexture>& texturesOut, struct b3ImportMeshData& meshData) const;
}; };
#endif //BULLET_URDF_IMPORTER_H #endif //BULLET_URDF_IMPORTER_H

2
examples/Importers/ImportURDFDemo/URDF2Bullet.h
View File

@@ -32,6 +32,8 @@ enum ConvertURDFFlags
CUF_INITIALIZE_SAT_FEATURES = 4096, CUF_INITIALIZE_SAT_FEATURES = 4096,
CUF_USE_SELF_COLLISION_INCLUDE_PARENT = 8192, CUF_USE_SELF_COLLISION_INCLUDE_PARENT = 8192,
CUF_PARSE_SENSORS = 16384, CUF_PARSE_SENSORS = 16384,
CUF_USE_MATERIAL_COLORS_FROM_MTL = 32768,
CUF_USE_MATERIAL_TRANSPARANCY_FROM_MTL = 64738,
}; };
struct UrdfVisualShapeCache struct UrdfVisualShapeCache

22
examples/Importers/ImportURDFDemo/UrdfFindMeshFile.h
View File

@@ -74,16 +74,8 @@ static bool UrdfFindMeshFile(
std::string existing_file; std::string existing_file;
{
std::string attempt = fn;
int f = fileIO->fileOpen(attempt.c_str(), "rb");
if (f>=0)
{
existing_file = attempt;
fileIO->fileClose(f);
}
}
if (existing_file.empty())
{ {
for (std::list<std::string>::iterator x = shorter.begin(); x != shorter.end(); ++x) for (std::list<std::string>::iterator x = shorter.begin(); x != shorter.end(); ++x)
{ {
@@ -100,6 +92,16 @@ static bool UrdfFindMeshFile(
break; break;
} }
} }
if (existing_file.empty())
{
std::string attempt = fn;
int f = fileIO->fileOpen(attempt.c_str(), "rb");
if (f>=0)
{
existing_file = attempt;
fileIO->fileClose(f);
}
}
if (existing_file.empty()) if (existing_file.empty())
{ {

76
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -9,7 +9,7 @@
#include "../Extras/InverseDynamics/btMultiBodyTreeCreator.hpp" #include "../Extras/InverseDynamics/btMultiBodyTreeCreator.hpp"
#include "BulletCollision/CollisionDispatch/btInternalEdgeUtility.h" #include "BulletCollision/CollisionDispatch/btInternalEdgeUtility.h"
#include "../Importers/ImportMeshUtility/b3ImportMeshUtility.h"
#include "BulletDynamics/MLCPSolvers/btDantzigSolver.h" #include "BulletDynamics/MLCPSolvers/btDantzigSolver.h"
#include "BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h" #include "BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h"
#include "BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h" #include "BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h"
@@ -2016,11 +2016,13 @@ struct ProgrammaticUrdfInterface : public URDFImporterInterface
const b3CreateMultiBodyArgs& m_createBodyArgs; const b3CreateMultiBodyArgs& m_createBodyArgs;
mutable b3AlignedObjectArray<btCollisionShape*> m_allocatedCollisionShapes; mutable b3AlignedObjectArray<btCollisionShape*> m_allocatedCollisionShapes;
PhysicsServerCommandProcessorInternalData* m_data; PhysicsServerCommandProcessorInternalData* m_data;
int m_flags;
ProgrammaticUrdfInterface(const b3CreateMultiBodyArgs& bodyArgs, PhysicsServerCommandProcessorInternalData* data) ProgrammaticUrdfInterface(const b3CreateMultiBodyArgs& bodyArgs, PhysicsServerCommandProcessorInternalData* data, int flags)
: m_bodyUniqueId(-1), : m_bodyUniqueId(-1),
m_createBodyArgs(bodyArgs), m_createBodyArgs(bodyArgs),
m_data(data) m_data(data),
m_flags(flags)
{ {
} }
@@ -2068,19 +2070,38 @@ struct ProgrammaticUrdfInterface : public URDFImporterInterface
return false; return false;
} }
mutable btHashMap<btHashInt, UrdfMaterialColor> m_linkColors;
virtual bool getLinkColor2(int linkIndex, struct UrdfMaterialColor& matCol) const virtual bool getLinkColor2(int linkIndex, struct UrdfMaterialColor& matCol) const
{ {
if (m_createBodyArgs.m_linkVisualShapeUniqueIds[linkIndex] >= 0)
if (m_flags & URDF_USE_MATERIAL_COLORS_FROM_MTL)
{ {
const InternalVisualShapeHandle* visHandle = m_data->m_userVisualShapeHandles.getHandle(m_createBodyArgs.m_linkVisualShapeUniqueIds[linkIndex]); const UrdfMaterialColor* matColPtr = m_linkColors[linkIndex];
if (visHandle) if (matColPtr)
{ {
for (int i = 0; i < visHandle->m_visualShapes.size(); i++) matCol = *matColPtr;
if ((m_flags&CUF_USE_MATERIAL_TRANSPARANCY_FROM_MTL)==0)
{ {
if (visHandle->m_visualShapes[i].m_geometry.m_hasLocalMaterial) matCol.m_rgbaColor[3] = 1;
}
return true;
}
} else
{
if (m_createBodyArgs.m_linkVisualShapeUniqueIds[linkIndex] >= 0)
{
const InternalVisualShapeHandle* visHandle = m_data->m_userVisualShapeHandles.getHandle(m_createBodyArgs.m_linkVisualShapeUniqueIds[linkIndex]);
if (visHandle)
{
for (int i = 0; i < visHandle->m_visualShapes.size(); i++)
{ {
matCol = visHandle->m_visualShapes[i].m_geometry.m_localMaterial.m_matColor; if (visHandle->m_visualShapes[i].m_geometry.m_hasLocalMaterial)
return true; {
matCol = visHandle->m_visualShapes[i].m_geometry.m_localMaterial.m_matColor;
return true;
}
} }
} }
} }
@@ -2274,7 +2295,21 @@ struct ProgrammaticUrdfInterface : public URDFImporterInterface
{ {
for (int v = 0; v < visHandle->m_visualShapes.size(); v++) 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); b3ImportMeshData meshData;
u2b.convertURDFToVisualShapeInternal(&visHandle->m_visualShapes[v], pathPrefix, localInertiaFrame.inverse() * visHandle->m_visualShapes[v].m_linkLocalFrame, vertices, indices, textures, meshData);
if ((meshData.m_flags & B3_IMPORT_MESH_HAS_RGBA_COLOR) &&
(meshData.m_flags & B3_IMPORT_MESH_HAS_SPECULAR_COLOR))
{
UrdfMaterialColor matCol;
matCol.m_rgbaColor.setValue(meshData.m_rgbaColor[0],
meshData.m_rgbaColor[1],
meshData.m_rgbaColor[2],
meshData.m_rgbaColor[3]);
matCol.m_specularColor.setValue(meshData.m_specularColor[0],
meshData.m_specularColor[1],
meshData.m_specularColor[2]);
m_linkColors.insert(linkIndex, matCol);
}
} }
if (vertices.size() && indices.size()) if (vertices.size() && indices.size())
@@ -3713,6 +3748,10 @@ bool PhysicsServerCommandProcessor::processRequestCameraImageCommand(const struc
m_data->m_pluginManager.getRenderInterface()->setProjectiveTexture(false); m_data->m_pluginManager.getRenderInterface()->setProjectiveTexture(false);
} }
if ((flags & ER_NO_SEGMENTATION_MASK) != 0)
{
segmentationMaskBuffer = 0;
}
m_data->m_pluginManager.getRenderInterface()->copyCameraImageData(pixelRGBA, numRequestedPixels, m_data->m_pluginManager.getRenderInterface()->copyCameraImageData(pixelRGBA, numRequestedPixels,
depthBuffer, numRequestedPixels, depthBuffer, numRequestedPixels,
@@ -4462,6 +4501,7 @@ bool PhysicsServerCommandProcessor::processCreateVisualShapeCommand(const struct
} }
else else
{ {
visualShape.m_geometry.m_localMaterial.m_matColor.m_rgbaColor.setValue(1,1,1,1);
} }
if (hasSpecular) if (hasSpecular)
{ {
@@ -6641,7 +6681,14 @@ bool PhysicsServerCommandProcessor::processCreateMultiBodyCommand(const struct S
{ {
m_data->m_sdfRecentLoadedBodies.clear(); m_data->m_sdfRecentLoadedBodies.clear();
ProgrammaticUrdfInterface u2b(clientCmd.m_createMultiBodyArgs, m_data); int flags = 0;
if (clientCmd.m_updateFlags & MULT_BODY_HAS_FLAGS)
{
flags = clientCmd.m_createMultiBodyArgs.m_flags;
}
ProgrammaticUrdfInterface u2b(clientCmd.m_createMultiBodyArgs, m_data, flags);
bool useMultiBody = true; bool useMultiBody = true;
if (clientCmd.m_updateFlags & MULT_BODY_USE_MAXIMAL_COORDINATES) if (clientCmd.m_updateFlags & MULT_BODY_USE_MAXIMAL_COORDINATES)
@@ -6649,12 +6696,7 @@ bool PhysicsServerCommandProcessor::processCreateMultiBodyCommand(const struct S
useMultiBody = false; useMultiBody = false;
} }
int flags = 0;
if (clientCmd.m_updateFlags & MULT_BODY_HAS_FLAGS)
{
flags = clientCmd.m_createMultiBodyArgs.m_flags;
}
bool ok = processImportedObjects("memory", bufferServerToClient, bufferSizeInBytes, useMultiBody, flags, u2b); bool ok = processImportedObjects("memory", bufferServerToClient, bufferSizeInBytes, useMultiBody, flags, u2b);

2
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -819,6 +819,8 @@ enum eURDF_Flags
URDF_INITIALIZE_SAT_FEATURES = 4096, URDF_INITIALIZE_SAT_FEATURES = 4096,
URDF_USE_SELF_COLLISION_INCLUDE_PARENT = 8192, URDF_USE_SELF_COLLISION_INCLUDE_PARENT = 8192,
URDF_PARSE_SENSORS = 16384, URDF_PARSE_SENSORS = 16384,
URDF_USE_MATERIAL_COLORS_FROM_MTL = 32768,
URDF_USE_MATERIAL_TRANSPARANCY_FROM_MTL = 65536,
}; };
enum eUrdfGeomTypes //sync with UrdfParser UrdfGeomTypes enum eUrdfGeomTypes //sync with UrdfParser UrdfGeomTypes

23
examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.cpp
View File

@@ -270,13 +270,14 @@ void EGLRendererVisualShapeConverter::setLightSpecularCoeff(float specularCoeff)
} }
///todo: merge into single file with TinyRendererVisualShapeConverter ///todo: merge into single file with TinyRendererVisualShapeConverter
static void convertURDFToVisualShape2(const UrdfShape* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut, btAlignedObjectArray<MyTexture3>& texturesOut, b3VisualShapeData& visualShapeOut, struct CommonFileIOInterface* fileIO) static void convertURDFToVisualShape2(const UrdfShape* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut, btAlignedObjectArray<MyTexture3>& texturesOut, b3VisualShapeData& visualShapeOut, struct CommonFileIOInterface* fileIO, int flags)
{ {
visualShapeOut.m_visualGeometryType = visual->m_geometry.m_type; visualShapeOut.m_visualGeometryType = visual->m_geometry.m_type;
visualShapeOut.m_dimensions[0] = 0; visualShapeOut.m_dimensions[0] = 0;
visualShapeOut.m_dimensions[1] = 0; visualShapeOut.m_dimensions[1] = 0;
visualShapeOut.m_dimensions[2] = 0; visualShapeOut.m_dimensions[2] = 0;
memset(visualShapeOut.m_meshAssetFileName, 0, sizeof(visualShapeOut.m_meshAssetFileName)); memset(visualShapeOut.m_meshAssetFileName, 0, sizeof(visualShapeOut.m_meshAssetFileName));
#if 0
if (visual->m_geometry.m_hasLocalMaterial) if (visual->m_geometry.m_hasLocalMaterial)
{ {
visualShapeOut.m_rgbaColor[0] = visual->m_geometry.m_localMaterial.m_matColor.m_rgbaColor[0]; visualShapeOut.m_rgbaColor[0] = visual->m_geometry.m_localMaterial.m_matColor.m_rgbaColor[0];
@@ -284,6 +285,7 @@ static void convertURDFToVisualShape2(const UrdfShape* visual, const char* urdfP
visualShapeOut.m_rgbaColor[2] = visual->m_geometry.m_localMaterial.m_matColor.m_rgbaColor[2]; visualShapeOut.m_rgbaColor[2] = visual->m_geometry.m_localMaterial.m_matColor.m_rgbaColor[2];
visualShapeOut.m_rgbaColor[3] = visual->m_geometry.m_localMaterial.m_matColor.m_rgbaColor[3]; visualShapeOut.m_rgbaColor[3] = visual->m_geometry.m_localMaterial.m_matColor.m_rgbaColor[3];
} }
#endif
GLInstanceGraphicsShape* glmesh = 0; GLInstanceGraphicsShape* glmesh = 0;
@@ -412,6 +414,23 @@ static void convertURDFToVisualShape2(const UrdfShape* visual, const char* urdfP
b3ImportMeshData meshData; b3ImportMeshData meshData;
if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(visual->m_geometry.m_meshFileName, meshData, fileIO)) if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(visual->m_geometry.m_meshFileName, meshData, fileIO))
{ {
if (flags&URDF_USE_MATERIAL_COLORS_FROM_MTL)
{
if (meshData.m_flags & B3_IMPORT_MESH_HAS_RGBA_COLOR)
{
visualShapeOut.m_rgbaColor[0] = meshData.m_rgbaColor[0];
visualShapeOut.m_rgbaColor[1] = meshData.m_rgbaColor[1];
visualShapeOut.m_rgbaColor[2] = meshData.m_rgbaColor[2];
if (flags&URDF_USE_MATERIAL_TRANSPARANCY_FROM_MTL)
{
visualShapeOut.m_rgbaColor[3] = meshData.m_rgbaColor[3];
} else
{
visualShapeOut.m_rgbaColor[3] = 1;
}
}
}
if (meshData.m_textureImage1) if (meshData.m_textureImage1)
{ {
MyTexture3 texData; MyTexture3 texData;
@@ -752,7 +771,7 @@ void EGLRendererVisualShapeConverter::convertVisualShapes(
visualShape.m_rgbaColor[3] = rgbaColor[3]; visualShape.m_rgbaColor[3] = rgbaColor[3];
{ {
B3_PROFILE("convertURDFToVisualShape2"); B3_PROFILE("convertURDFToVisualShape2");
convertURDFToVisualShape2(vis, pathPrefix, localInertiaFrame.inverse() * childTrans, vertices, indices, textures, visualShape, fileIO); convertURDFToVisualShape2(vis, pathPrefix, localInertiaFrame.inverse() * childTrans, vertices, indices, textures, visualShape, fileIO, m_data->m_flags);
} }
m_data->m_visualShapes.push_back(visualShape); m_data->m_visualShapes.push_back(visualShape);

336
examples/SharedMemory/plugins/fileIOPlugin/fileIOPlugin.cpp
View File

@@ -6,8 +6,8 @@
#include <stdio.h> #include <stdio.h>
#include "../../../CommonInterfaces/CommonFileIOInterface.h" #include "../../../CommonInterfaces/CommonFileIOInterface.h"
#include "../../../Utils/b3ResourcePath.h" #include "../../../Utils/b3ResourcePath.h"
#include "Bullet3Common/b3HashMap.h"
#include <string.h> //memcpy/strlen
#ifndef B3_EXCLUDE_DEFAULT_FILEIO #ifndef B3_EXCLUDE_DEFAULT_FILEIO
#include "../../../Utils/b3BulletDefaultFileIO.h" #include "../../../Utils/b3BulletDefaultFileIO.h"
#endif //B3_EXCLUDE_DEFAULT_FILEIO #endif //B3_EXCLUDE_DEFAULT_FILEIO
@@ -30,13 +30,258 @@ struct WrapperFileHandle
int m_childFileHandle; int m_childFileHandle;
}; };
struct InMemoryFile
{
char* m_buffer;
int m_fileSize;
};
struct InMemoryFileAccessor
{
InMemoryFile* m_file;
int m_curPos;
};
struct InMemoryFileIO : public CommonFileIOInterface
{
b3HashMap<b3HashString,InMemoryFile*> m_fileCache;
InMemoryFileAccessor m_fileHandles[B3_MAX_FILEIO_INTERFACES];
InMemoryFileIO()
{
for (int i=0;i<B3_FILEIO_MAX_FILES;i++)
{
m_fileHandles[i].m_curPos = 0;
m_fileHandles[i].m_file = 0;
}
}
virtual ~InMemoryFileIO()
{
clearCache();
}
void clearCache()
{
for (int i=0;i<m_fileCache.size();i++)
{
InMemoryFile** memPtr = m_fileCache.getAtIndex(i);
if (memPtr && *memPtr)
{
InMemoryFile* mem = *memPtr;
freeBuffer(mem->m_buffer);
delete (mem);
}
}
}
char* allocateBuffer(int len)
{
char* buffer = 0;
if (len)
{
buffer = new char[len];
}
return buffer;
}
void freeBuffer(char* buffer)
{
delete[] buffer;
}
virtual int registerFile(const char* fileName, char* buffer, int len)
{
InMemoryFile* f = new InMemoryFile();
f->m_buffer = buffer;
f->m_fileSize = len;
b3HashString key(fileName);
m_fileCache.insert(key,f);
return 0;
}
void removeFileFromCache(const char* fileName)
{
InMemoryFile* f = getInMemoryFile(fileName);
if (f)
{
m_fileCache.remove(fileName);
freeBuffer(f->m_buffer);
delete (f);
}
}
InMemoryFile* getInMemoryFile(const char* fileName)
{
InMemoryFile** fPtr = m_fileCache[fileName];
if (fPtr && *fPtr)
{
return *fPtr;
}
return 0;
}
virtual int fileOpen(const char* fileName, const char* mode)
{
//search a free slot
int slot = -1;
for (int i=0;i<B3_FILEIO_MAX_FILES;i++)
{
if (m_fileHandles[i].m_file==0)
{
slot=i;
break;
}
}
if (slot>=0)
{
InMemoryFile* f = getInMemoryFile(fileName);
if (f)
{
m_fileHandles[slot].m_curPos = 0;
m_fileHandles[slot].m_file = f;
} else
{
slot=-1;
}
}
//printf("InMemoryFileIO fileOpen %s, %d\n", fileName, slot);
return slot;
}
virtual int fileRead(int fileHandle, char* destBuffer, int numBytes)
{
if (fileHandle>=0 && fileHandle < B3_FILEIO_MAX_FILES)
{
InMemoryFileAccessor& f = m_fileHandles[fileHandle];
if (f.m_file)
{
//if (numBytes>1)
// printf("curPos = %d\n", f.m_curPos);
if (f.m_curPos+numBytes <= f.m_file->m_fileSize)
{
memcpy(destBuffer,f.m_file->m_buffer+f.m_curPos,numBytes);
f.m_curPos+=numBytes;
//if (numBytes>1)
// printf("read %d bytes, now curPos = %d\n", numBytes, f.m_curPos);
return numBytes;
} else
{
if (numBytes!=1)
{
printf("InMemoryFileIO::fileRead Attempt to read beyond end of file\n");
}
}
}
}
return 0;
}
virtual int fileWrite(int fileHandle,const char* sourceBuffer, int numBytes)
{
return 0;
}
virtual void fileClose(int fileHandle)
{
if (fileHandle>=0 && fileHandle < B3_FILEIO_MAX_FILES)
{
InMemoryFileAccessor& f = m_fileHandles[fileHandle];
if (f.m_file)
{
m_fileHandles[fileHandle].m_file = 0;
m_fileHandles[fileHandle].m_curPos = 0;
//printf("InMemoryFileIO fileClose %d\n", fileHandle);
}
}
}
virtual bool findResourcePath(const char* fileName, char* resourcePathOut, int resourcePathMaxNumBytes)
{
InMemoryFile* f = getInMemoryFile(fileName);
int fileNameLen = strlen(fileName);
if (f && fileNameLen<(resourcePathMaxNumBytes-1))
{
memcpy(resourcePathOut, fileName, fileNameLen);
resourcePathOut[fileNameLen]=0;
return true;
}
return false;
}
virtual char* readLine(int fileHandle, char* destBuffer, int numBytes)
{
int numRead = 0;
int endOfFile = 0;
if (fileHandle>=0 && fileHandle < B3_FILEIO_MAX_FILES )
{
InMemoryFileAccessor& f = m_fileHandles[fileHandle];
if (f.m_file)
{
//return ::fgets(destBuffer, numBytes, m_fileHandles[fileHandle]);
char c = 0;
do
{
int bytesRead = fileRead(fileHandle,&c,1);
if (bytesRead != 1)
{
endOfFile = 1;
c=0;
}
if (c && c!='\n')
{
char a='\r';
if (c!=13)
{
destBuffer[numRead++]=c;
} else
{
destBuffer[numRead++]=0;
}
}
} while (c != 0 && c != '\n' && numRead<(numBytes-1));
}
}
if (numRead==0 && endOfFile)
{
return 0;
}
if (numRead<numBytes)
{
if (numRead >=0)
{
destBuffer[numRead]=0;
}
return &destBuffer[0];
} else
{
if (endOfFile==0)
{
printf("InMemoryFileIO::readLine readLine warning: numRead=%d, numBytes=%d\n", numRead, numBytes);
}
}
return 0;
}
virtual int getFileSize(int fileHandle)
{
if (fileHandle>=0 && fileHandle < B3_FILEIO_MAX_FILES )
{
InMemoryFileAccessor& f = m_fileHandles[fileHandle];
if (f.m_file)
{
return f.m_file->m_fileSize;
}
}
return 0;
}
};
struct WrapperFileIO : public CommonFileIOInterface struct WrapperFileIO : public CommonFileIOInterface
{ {
CommonFileIOInterface* m_availableFileIOInterfaces[B3_MAX_FILEIO_INTERFACES]; CommonFileIOInterface* m_availableFileIOInterfaces[B3_MAX_FILEIO_INTERFACES];
int m_numWrapperInterfaces; int m_numWrapperInterfaces;
WrapperFileHandle m_wrapperFileHandles[B3_MAX_FILEIO_INTERFACES]; WrapperFileHandle m_wrapperFileHandles[B3_MAX_FILEIO_INTERFACES];
InMemoryFileIO m_cachedFiles;
WrapperFileIO() WrapperFileIO()
:m_numWrapperInterfaces(0) :m_numWrapperInterfaces(0)
@@ -47,6 +292,7 @@ struct WrapperFileIO : public CommonFileIOInterface
m_wrapperFileHandles[i].childFileIO=0; m_wrapperFileHandles[i].childFileIO=0;
m_wrapperFileHandles[i].m_childFileHandle=0; m_wrapperFileHandles[i].m_childFileHandle=0;
} }
//addFileIOInterface(&m_cachedFiles);
} }
virtual ~WrapperFileIO() virtual ~WrapperFileIO()
@@ -86,6 +332,7 @@ struct WrapperFileIO : public CommonFileIOInterface
virtual int fileOpen(const char* fileName, const char* mode) virtual int fileOpen(const char* fileName, const char* mode)
{ {
//find an available wrapperFileHandle slot //find an available wrapperFileHandle slot
int wrapperFileHandle=-1; int wrapperFileHandle=-1;
int slot = -1; int slot = -1;
@@ -99,20 +346,76 @@ struct WrapperFileIO : public CommonFileIOInterface
} }
if (slot>=0) if (slot>=0)
{ {
//figure out what wrapper interface to use //first check the cache
//use the first one that can open the file int childHandle = m_cachedFiles.fileOpen(fileName, mode);
for (int i=0;i<B3_MAX_FILEIO_INTERFACES;i++) if (childHandle<0)
{ {
CommonFileIOInterface* childFileIO=m_availableFileIOInterfaces[i]; for (int i=0;i<B3_MAX_FILEIO_INTERFACES;i++)
if (childFileIO)
{ {
int childHandle = childFileIO->fileOpen(fileName, mode); CommonFileIOInterface* childFileIO=m_availableFileIOInterfaces[i];
if (childHandle>=0) if (childFileIO)
{ {
wrapperFileHandle = slot; int childHandle = childFileIO->fileOpen(fileName, mode);
m_wrapperFileHandles[slot].childFileIO = childFileIO; if (childHandle>=0)
m_wrapperFileHandles[slot].m_childFileHandle = childHandle; {
break; int fileSize = childFileIO->getFileSize(childHandle);
char* buffer = 0;
if (fileSize)
{
buffer = m_cachedFiles.allocateBuffer(fileSize);
if (buffer)
{
int readBytes = childFileIO->fileRead(childHandle, buffer, fileSize);
if (readBytes!=fileSize)
{
if (readBytes<fileSize)
{
fileSize = readBytes;
} else
{
printf("WrapperFileIO error: reading more bytes (%d) then reported file size (%d) of file %s.\n", readBytes, fileSize, fileName);
}
}
} else
{
fileSize=0;
}
}
//potentially register a zero byte file, or files that only can be read partially
m_cachedFiles.registerFile(fileName,buffer, fileSize);
childFileIO->fileClose(childHandle);
break;
}
}
}
}
{
int childHandle = m_cachedFiles.fileOpen(fileName, mode);
if (childHandle>=0)
{
wrapperFileHandle = slot;
m_wrapperFileHandles[slot].childFileIO = &m_cachedFiles;
m_wrapperFileHandles[slot].m_childFileHandle = childHandle;
} else
{
//figure out what wrapper interface to use
//use the first one that can open the file
for (int i=0;i<B3_MAX_FILEIO_INTERFACES;i++)
{
CommonFileIOInterface* childFileIO=m_availableFileIOInterfaces[i];
if (childFileIO)
{
int childHandle = childFileIO->fileOpen(fileName, mode);
if (childHandle>=0)
{
wrapperFileHandle = slot;
m_wrapperFileHandles[slot].childFileIO = childFileIO;
m_wrapperFileHandles[slot].m_childFileHandle = childHandle;
break;
}
}
} }
} }
} }
@@ -155,6 +458,9 @@ struct WrapperFileIO : public CommonFileIOInterface
} }
virtual bool findResourcePath(const char* fileName, char* resourcePathOut, int resourcePathMaxNumBytes) virtual bool findResourcePath(const char* fileName, char* resourcePathOut, int resourcePathMaxNumBytes)
{ {
if (m_cachedFiles.findResourcePath(fileName, resourcePathOut, resourcePathMaxNumBytes))
return true;
bool found = false; bool found = false;
for (int i=0;i<B3_MAX_FILEIO_INTERFACES;i++) for (int i=0;i<B3_MAX_FILEIO_INTERFACES;i++)
{ {
@@ -273,7 +579,7 @@ B3_SHARED_API int executePluginCommand_fileIOPlugin(struct b3PluginContext* cont
#ifdef B3_USE_ZIPFILE_FILEIO #ifdef B3_USE_ZIPFILE_FILEIO
if (arguments->m_text) if (arguments->m_text)
{ {
obj->m_fileIO.addFileIOInterface(new ZipFileIO(arguments->m_text)); obj->m_fileIO.addFileIOInterface(new ZipFileIO(arguments->m_text, &obj->m_fileIO));
} }
#else #else
printf("eZipFileIO is not enabled in this build.\n"); printf("eZipFileIO is not enabled in this build.\n");

16
examples/SharedMemory/plugins/fileIOPlugin/zipFileIO.h
View File

@@ -10,7 +10,7 @@ struct ZipFileIO : public CommonFileIOInterface
unzFile m_fileHandles[B3_ZIP_FILEIO_MAX_FILES ]; unzFile m_fileHandles[B3_ZIP_FILEIO_MAX_FILES ];
int m_numFileHandles; int m_numFileHandles;
ZipFileIO(const char* zipfileName) ZipFileIO(const char* zipfileName, CommonFileIOInterface* wrapperFileIO)
:m_zipfileName(zipfileName), :m_zipfileName(zipfileName),
m_numFileHandles(0) m_numFileHandles(0)
{ {
@@ -37,6 +37,7 @@ struct ZipFileIO : public CommonFileIOInterface
virtual int fileOpen(const char* fileName, const char* mode) virtual int fileOpen(const char* fileName, const char* mode)
{ {
//search a free slot //search a free slot
int slot = -1; int slot = -1;
for (int i=0;i<B3_ZIP_FILEIO_MAX_FILES ;i++) for (int i=0;i<B3_ZIP_FILEIO_MAX_FILES ;i++)
@@ -58,6 +59,7 @@ struct ZipFileIO : public CommonFileIOInterface
slot = -1; slot = -1;
} else } else
{ {
int result = 0; int result = 0;
result = unzGetGlobalInfo(zipfile, &m_global_info ); result = unzGetGlobalInfo(zipfile, &m_global_info );
if (result != UNZ_OK) if (result != UNZ_OK)
@@ -66,9 +68,6 @@ struct ZipFileIO : public CommonFileIOInterface
unzClose(zipfile); unzClose(zipfile);
zipfile = 0; zipfile = 0;
slot = -1; slot = -1;
} else
{
m_fileHandles[slot] = zipfile;
} }
} }
if (slot >=0) if (slot >=0)
@@ -82,20 +81,28 @@ struct ZipFileIO : public CommonFileIOInterface
{ {
printf("unzGetCurrentFileInfo() != UNZ_OK (%d)\n", result); printf("unzGetCurrentFileInfo() != UNZ_OK (%d)\n", result);
slot=-1; slot=-1;
unzClose(zipfile);
zipfile = 0;
} }
else else
{ {
result = unzOpenCurrentFile(zipfile); result = unzOpenCurrentFile(zipfile);
if (result == UNZ_OK) if (result == UNZ_OK)
{ {
printf("zipFile::fileOpen %s in mode %s in fileHandle %d\n", fileName, mode, slot);
m_fileHandles[slot] = zipfile;
} else } else
{ {
slot=-1; slot=-1;
unzClose(zipfile);
zipfile = 0;
} }
} }
} else } else
{ {
slot=-1; slot=-1;
unzClose(zipfile);
zipfile = 0;
} }
} }
} }
@@ -137,6 +144,7 @@ struct ZipFileIO : public CommonFileIOInterface
unzFile f = m_fileHandles[fileHandle]; unzFile f = m_fileHandles[fileHandle];
if (f) if (f)
{ {
printf("zipFile::fileClose slot %d\n", fileHandle);
unzClose(f); unzClose(f);
m_fileHandles[fileHandle]=0; m_fileHandles[fileHandle]=0;
} }

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

@@ -182,13 +182,14 @@ void TinyRendererVisualShapeConverter::setLightSpecularCoeff(float specularCoeff
m_data->m_hasLightSpecularCoeff = true; m_data->m_hasLightSpecularCoeff = true;
} }
static void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut, btAlignedObjectArray<MyTexture2>& texturesOut, b3VisualShapeData& visualShapeOut, struct CommonFileIOInterface* fileIO) static void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut, btAlignedObjectArray<MyTexture2>& texturesOut, b3VisualShapeData& visualShapeOut, struct CommonFileIOInterface* fileIO, int flags)
{ {
visualShapeOut.m_visualGeometryType = visual->m_geometry.m_type; visualShapeOut.m_visualGeometryType = visual->m_geometry.m_type;
visualShapeOut.m_dimensions[0] = 0; visualShapeOut.m_dimensions[0] = 0;
visualShapeOut.m_dimensions[1] = 0; visualShapeOut.m_dimensions[1] = 0;
visualShapeOut.m_dimensions[2] = 0; visualShapeOut.m_dimensions[2] = 0;
memset(visualShapeOut.m_meshAssetFileName, 0, sizeof(visualShapeOut.m_meshAssetFileName)); memset(visualShapeOut.m_meshAssetFileName, 0, sizeof(visualShapeOut.m_meshAssetFileName));
#if 0
if (visual->m_geometry.m_hasLocalMaterial) if (visual->m_geometry.m_hasLocalMaterial)
{ {
visualShapeOut.m_rgbaColor[0] = visual->m_geometry.m_localMaterial.m_matColor.m_rgbaColor[0]; visualShapeOut.m_rgbaColor[0] = visual->m_geometry.m_localMaterial.m_matColor.m_rgbaColor[0];
@@ -196,6 +197,7 @@ static void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPa
visualShapeOut.m_rgbaColor[2] = visual->m_geometry.m_localMaterial.m_matColor.m_rgbaColor[2]; visualShapeOut.m_rgbaColor[2] = visual->m_geometry.m_localMaterial.m_matColor.m_rgbaColor[2];
visualShapeOut.m_rgbaColor[3] = visual->m_geometry.m_localMaterial.m_matColor.m_rgbaColor[3]; visualShapeOut.m_rgbaColor[3] = visual->m_geometry.m_localMaterial.m_matColor.m_rgbaColor[3];
} }
#endif
GLInstanceGraphicsShape* glmesh = 0; GLInstanceGraphicsShape* glmesh = 0;
@@ -322,8 +324,26 @@ static void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPa
{ {
//glmesh = LoadMeshFromObj(fullPath,visualPathPrefix); //glmesh = LoadMeshFromObj(fullPath,visualPathPrefix);
b3ImportMeshData meshData; b3ImportMeshData meshData;
if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(visual->m_geometry.m_meshFileName, meshData, fileIO)) if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(visual->m_geometry.m_meshFileName, meshData, fileIO))
{ {
if (flags&URDF_USE_MATERIAL_COLORS_FROM_MTL)
{
if (meshData.m_flags & B3_IMPORT_MESH_HAS_RGBA_COLOR)
{
visualShapeOut.m_rgbaColor[0] = meshData.m_rgbaColor[0];
visualShapeOut.m_rgbaColor[1] = meshData.m_rgbaColor[1];
visualShapeOut.m_rgbaColor[2] = meshData.m_rgbaColor[2];
if (flags&URDF_USE_MATERIAL_TRANSPARANCY_FROM_MTL)
{
visualShapeOut.m_rgbaColor[3] = meshData.m_rgbaColor[3];
} else
{
visualShapeOut.m_rgbaColor[3] = 1;
}
}
}
if (meshData.m_textureImage1) if (meshData.m_textureImage1)
{ {
MyTexture2 texData; MyTexture2 texData;
@@ -665,9 +685,14 @@ void TinyRendererVisualShapeConverter::convertVisualShapes(
{ {
B3_PROFILE("convertURDFToVisualShape"); B3_PROFILE("convertURDFToVisualShape");
convertURDFToVisualShape(vis, pathPrefix, localInertiaFrame.inverse() * childTrans, vertices, indices, textures, visualShape, fileIO); convertURDFToVisualShape(vis, pathPrefix, localInertiaFrame.inverse() * childTrans, vertices, indices, textures, visualShape, fileIO, m_data->m_flags);
} }
rgbaColor[0] = visualShape.m_rgbaColor[0];
rgbaColor[1] = visualShape.m_rgbaColor[1];
rgbaColor[2] = visualShape.m_rgbaColor[2];
rgbaColor[3] = visualShape.m_rgbaColor[3];
if (vertices.size() && indices.size()) if (vertices.size() && indices.size())
{ {
TinyRenderObjectData* tinyObj = new TinyRenderObjectData(m_data->m_rgbColorBuffer, m_data->m_depthBuffer, &m_data->m_shadowBuffer, &m_data->m_segmentationMaskBuffer, bodyUniqueId, linkIndex); TinyRenderObjectData* tinyObj = new TinyRenderObjectData(m_data->m_rgbColorBuffer, m_data->m_depthBuffer, &m_data->m_shadowBuffer, &m_data->m_segmentationMaskBuffer, bodyUniqueId, linkIndex);

3
examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp
View File

@@ -407,7 +407,7 @@ std::string LoadMtl(
} }
if (linebuf.size() > 0) if (linebuf.size() > 0)
{ {
if (linebuf[linebuf.size() - 1] == '\n') linebuf.erase(linebuf.size() - 1); if (linebuf[linebuf.size() - 1] == '\r') linebuf.erase(linebuf.size() - 1);
} }
// Skip if empty line. // Skip if empty line.
@@ -417,7 +417,6 @@ std::string LoadMtl(
} }
linebuf = linebuf.substr(0, linebuf.find_last_not_of(" \t") + 1); linebuf = linebuf.substr(0, linebuf.find_last_not_of(" \t") + 1);
// Skip leading space. // Skip leading space.
const char* token = linebuf.c_str(); const char* token = linebuf.c_str();
token += strspn(token, " \t"); token += strspn(token, " \t");

11
examples/Utils/b3BulletDefaultFileIO.h
View File

@@ -148,7 +148,16 @@ struct b3BulletDefaultFileIO : public CommonFileIOInterface
FILE* f = m_fileHandles[fileHandle]; FILE* f = m_fileHandles[fileHandle];
if (f) if (f)
{ {
return ::fgets(destBuffer, numBytes, m_fileHandles[fileHandle]); char* txt = ::fgets(destBuffer, numBytes, m_fileHandles[fileHandle]);
for (int i=0;i<numBytes;i++)
{
if (destBuffer[i]=='\r'||destBuffer[i]=='\n' || destBuffer[i]==0)
{
destBuffer[i] = 0;
break;
}
}
return txt;
} }
} }
return 0; return 0;

10
examples/pybullet/gym/pybullet_envs/env_bases.py
View File

@@ -111,6 +111,16 @@ class MJCFBaseBulletEnv(gym.Env):
# backwards compatibility for gym >= v0.9.x # backwards compatibility for gym >= v0.9.x
# for extension of this class. # for extension of this class.
def step(self, *args, **kwargs): def step(self, *args, **kwargs):
if self.isRender:
base_pos=[0,0,0]
if (hasattr(self,'robot')):
if (hasattr(self.robot,'body_xyz')):
base_pos = self.robot.body_xyz
# Keep the previous orientation of the camera set by the user.
#[yaw, pitch, dist] = self._p.getDebugVisualizerCamera()[8:11]
self._p.resetDebugVisualizerCamera(3,0,0, base_pos)
return self._step(*args, **kwargs) return self._step(*args, **kwargs)
if parse_version(gym.__version__)>=parse_version('0.9.6'): if parse_version(gym.__version__)>=parse_version('0.9.6'):

2
examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_gym_env.py
View File

@@ -271,7 +271,7 @@ class MinitaurGymEnv(gym.Env):
"%s/plane.urdf" % self._urdf_root) "%s/plane.urdf" % self._urdf_root)
if (self._reflection): if (self._reflection):
self._pybullet_client.changeVisualShape(self._ground_id,-1,rgbaColor=[1,1,1,0.8]) self._pybullet_client.changeVisualShape(self._ground_id,-1,rgbaColor=[1,1,1,0.8])
#self._pybullet_client.configureDebugVisualizer(self._pybullet_client.COV_ENABLE_PLANAR_REFLECTION,1) self._pybullet_client.configureDebugVisualizer(self._pybullet_client.COV_ENABLE_PLANAR_REFLECTION,self._ground_id)
self._pybullet_client.setGravity(0, 0, -10) self._pybullet_client.setGravity(0, 0, -10)
acc_motor = self._accurate_motor_model_enabled acc_motor = self._accurate_motor_model_enabled
motor_protect = self._motor_overheat_protection motor_protect = self._motor_overheat_protection

2
examples/pybullet/pybullet.c
View File

@@ -9850,6 +9850,8 @@ initpybullet(void)
PyModule_AddIntConstant(m, "URDF_ENABLE_CACHED_GRAPHICS_SHAPES", URDF_ENABLE_CACHED_GRAPHICS_SHAPES); PyModule_AddIntConstant(m, "URDF_ENABLE_CACHED_GRAPHICS_SHAPES", URDF_ENABLE_CACHED_GRAPHICS_SHAPES);
PyModule_AddIntConstant(m, "URDF_ENABLE_SLEEPING", URDF_ENABLE_SLEEPING); PyModule_AddIntConstant(m, "URDF_ENABLE_SLEEPING", URDF_ENABLE_SLEEPING);
PyModule_AddIntConstant(m, "URDF_INITIALIZE_SAT_FEATURES", URDF_INITIALIZE_SAT_FEATURES); PyModule_AddIntConstant(m, "URDF_INITIALIZE_SAT_FEATURES", URDF_INITIALIZE_SAT_FEATURES);
PyModule_AddIntConstant(m, "URDF_USE_MATERIAL_COLORS_FROM_MTL", URDF_USE_MATERIAL_COLORS_FROM_MTL);
PyModule_AddIntConstant(m, "URDF_USE_MATERIAL_TRANSPARANCY_FROM_MTL", URDF_USE_MATERIAL_TRANSPARANCY_FROM_MTL);
PyModule_AddIntConstant(m, "ACTIVATION_STATE_ENABLE_SLEEPING", eActivationStateEnableSleeping); PyModule_AddIntConstant(m, "ACTIVATION_STATE_ENABLE_SLEEPING", eActivationStateEnableSleeping);
PyModule_AddIntConstant(m, "ACTIVATION_STATE_DISABLE_SLEEPING", eActivationStateDisableSleeping); PyModule_AddIntConstant(m, "ACTIVATION_STATE_DISABLE_SLEEPING", eActivationStateDisableSleeping);