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

Merge pull request #2083 from erwincoumans/master

Browse Source 
some PyBullet gym env fixes
This commit is contained in:
erwincoumans
2019-01-29 13:38:06 -08:00
committed by GitHub
parent c97d1041ed 9392b05d53
commit c6cf5cb0c4
17 changed files with 590 additions and 61 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -969,6 +969,53 @@ void BulletURDFImporter::convertURDFToVisualShapeInternal(const UrdfVisual* visu
{
switch (visual->m_geometry.m_meshFileType)
{
case UrdfGeometry::MEMORY_VERTICES:
{
glmesh = new GLInstanceGraphicsShape;
// int index = 0;
glmesh->m_indices = new b3AlignedObjectArray<int>();
glmesh->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>();
glmesh->m_vertices->resize(visual->m_geometry.m_vertices.size());
glmesh->m_indices->resize(visual->m_geometry.m_indices.size());
for (int i = 0; i < visual->m_geometry.m_vertices.size(); i++)
{
glmesh->m_vertices->at(i).xyzw[0] = visual->m_geometry.m_vertices[i].x();
glmesh->m_vertices->at(i).xyzw[1] = visual->m_geometry.m_vertices[i].y();
glmesh->m_vertices->at(i).xyzw[2] = visual->m_geometry.m_vertices[i].z();
glmesh->m_vertices->at(i).xyzw[3] = 1;
btVector3 normal(visual->m_geometry.m_vertices[i]);
if (visual->m_geometry.m_normals.size() == visual->m_geometry.m_vertices.size())
{
normal = visual->m_geometry.m_normals[i];
}
else
{
normal.safeNormalize();
}
btVector3 uv(0.5, 0.5, 0);
if (visual->m_geometry.m_uvs.size() == visual->m_geometry.m_vertices.size())
{
uv = visual->m_geometry.m_uvs[i];
}
glmesh->m_vertices->at(i).normal[0] = normal[0];
glmesh->m_vertices->at(i).normal[1] = normal[1];
glmesh->m_vertices->at(i).normal[2] = normal[2];
glmesh->m_vertices->at(i).uv[0] = uv[0];
glmesh->m_vertices->at(i).uv[1] = uv[1];
}
for (int i = 0; i < visual->m_geometry.m_indices.size(); i++)
{
glmesh->m_indices->at(i) = visual->m_geometry.m_indices[i];
}
glmesh->m_numIndices = visual->m_geometry.m_indices.size();
glmesh->m_numvertices = visual->m_geometry.m_vertices.size();
break;
}
case UrdfGeometry::FILE_OBJ:
{

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

@@ -87,6 +87,12 @@ struct UrdfGeometry
std::string m_meshFileName;
btVector3 m_meshScale;
btArray<btVector3> m_vertices;
btArray<btVector3> m_uvs;
btArray<btVector3> m_normals;
btArray<int> m_indices;
UrdfMaterial m_localMaterial;
bool m_hasLocalMaterial;

79
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -1377,6 +1377,9 @@ B3_SHARED_API int b3CreateCollisionShapeAddConcaveMesh(b3PhysicsClientHandle phy
{
indexUpload[i]=indices[i];
}
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_numNormals = 0;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_numUVs = 0;
command->m_createUserShapeArgs.m_numUserShapes++;
cl->uploadBulletFileToSharedMemory(data, totalUploadSizeInBytes);
delete [] data;
@@ -1386,6 +1389,82 @@ B3_SHARED_API int b3CreateCollisionShapeAddConcaveMesh(b3PhysicsClientHandle phy
return -1;
}
B3_SHARED_API int b3CreateVisualShapeAddMesh2(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, const double meshScale[/*3*/], const double* vertices, int numVertices, const int* indices, int numIndices, const double* normals, int numNormals, const double* uvs, int numUVs)
{
if (numUVs == 0 && numNormals == 0)
{
return b3CreateCollisionShapeAddConcaveMesh(physClient, commandHandle, meshScale, vertices, numVertices, indices, numIndices);
}
PhysicsClient* cl = (PhysicsClient*)physClient;
b3Assert(cl);
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
b3Assert(command);
b3Assert((command->m_type == CMD_CREATE_COLLISION_SHAPE) || (command->m_type == CMD_CREATE_VISUAL_SHAPE));
if ((command->m_type == CMD_CREATE_COLLISION_SHAPE) || (command->m_type == CMD_CREATE_VISUAL_SHAPE))
{
int shapeIndex = command->m_createUserShapeArgs.m_numUserShapes;
if (shapeIndex < MAX_COMPOUND_COLLISION_SHAPES && numVertices >= 0 && numIndices >= 0)
{
int i = 0;
if (numVertices>B3_MAX_NUM_VERTICES)
numVertices = B3_MAX_NUM_VERTICES;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_type = GEOM_MESH;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_collisionFlags = GEOM_FORCE_CONCAVE_TRIMESH;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_visualFlags = 0;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_hasChildTransform = 0;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshScale[0] = meshScale[0];
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshScale[1] = meshScale[1];
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshScale[2] = meshScale[2];
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshFileType = 0;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshFileName[0] = 0;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_numVertices = numVertices;
int totalUploadSizeInBytes = numVertices * sizeof(double) * 3 + numIndices * sizeof(int) + numNormals*sizeof(double)*3+numUVs*sizeof(double)*2;
char* data = new char[totalUploadSizeInBytes];
double* vertexUpload = (double*)data;
int* indexUpload = (int*)(data + numVertices * sizeof(double) * 3);
double* normalUpload = (double*)(data + numVertices * sizeof(double) * 3 + numIndices * sizeof(int));
double* uvUpload = (double*)(data + numVertices * sizeof(double) * 3 + numIndices * sizeof(int)+ numNormals * sizeof(double) * 3);
for (i = 0; i<numVertices; i++)
{
vertexUpload[i * 3 + 0] = vertices[i * 3 + 0];
vertexUpload[i * 3 + 1] = vertices[i * 3 + 1];
vertexUpload[i * 3 + 2] = vertices[i * 3 + 2];
}
if (numIndices>B3_MAX_NUM_INDICES)
numIndices = B3_MAX_NUM_INDICES;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_numIndices = numIndices;
for (i = 0; i<numIndices; i++)
{
indexUpload[i] = indices[i];
}
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_numNormals = numNormals;
for (i = 0; i < numNormals; i++)
{
normalUpload[i * 3 + 0] = normals[i * 3 + 0];
normalUpload[i * 3 + 1] = normals[i * 3 + 1];
normalUpload[i * 3 + 2] = normals[i * 3 + 2];
}
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_numUVs = numUVs;
for (i = 0; i < numUVs; i++)
{
uvUpload[i * 2 + 0] = uvs[i * 2 + 0];
uvUpload[i * 2 + 1] = uvs[i * 2 + 1];
}
command->m_createUserShapeArgs.m_numUserShapes++;
cl->uploadBulletFileToSharedMemory(data, totalUploadSizeInBytes);
delete[] data;
return shapeIndex;
}
}
return -1;
}
B3_SHARED_API int b3CreateVisualShapeAddMesh(b3SharedMemoryCommandHandle commandHandle, const char* fileName, const double meshScale[/*3*/])
{
return b3CreateCollisionShapeAddMesh(commandHandle, fileName, meshScale);

3
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -486,6 +486,9 @@ extern "C"
B3_SHARED_API int b3CreateVisualShapeAddCylinder(b3SharedMemoryCommandHandle commandHandle, double radius, double height);
B3_SHARED_API int b3CreateVisualShapeAddPlane(b3SharedMemoryCommandHandle commandHandle, const double planeNormal[/*3*/], double planeConstant);
B3_SHARED_API int b3CreateVisualShapeAddMesh(b3SharedMemoryCommandHandle commandHandle, const char* fileName, const double meshScale[/*3*/]);
B3_SHARED_API int b3CreateVisualShapeAddMesh2(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, const double meshScale[/*3*/], const double* vertices, int numVertices, const int* indices, int numIndices, const double* normals, int numNormals, const double* uvs, int numUVs);
B3_SHARED_API void b3CreateVisualSetFlag(b3SharedMemoryCommandHandle commandHandle, int shapeIndex, int flags);
B3_SHARED_API void b3CreateVisualShapeSetChildTransform(b3SharedMemoryCommandHandle commandHandle, int shapeIndex, const double childPosition[/*3*/], const double childOrientation[/*4*/]);
B3_SHARED_API void b3CreateVisualShapeSetSpecularColor(b3SharedMemoryCommandHandle commandHandle, int shapeIndex, const double specularColor[/*3*/]);

69
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -2520,7 +2520,8 @@ struct ProgrammaticUrdfInterface : public URDFImporterInterface
if (m_data->m_pluginManager.getRenderInterface())
{
CommonFileIOInterface* fileIO = m_data->m_pluginManager.getFileIOInterface();
m_data->m_pluginManager.getRenderInterface()->convertVisualShapes(linkIndex, pathPrefix, localInertiaFrame, &link, &model, colObj->getBroadphaseHandle()->getUid(), bodyUniqueId, fileIO);
int visualShapeUniqueid = m_data->m_pluginManager.getRenderInterface()->convertVisualShapes(linkIndex, pathPrefix, localInertiaFrame, &link, &model, colObj->getBroadphaseHandle()->getUid(), bodyUniqueId, fileIO);
colObj->setUserIndex3(visualShapeUniqueid);
}
}
virtual void setBodyUniqueId(int bodyId)
@@ -4706,18 +4707,68 @@ bool PhysicsServerCommandProcessor::processCreateVisualShapeCommand(const struct
case URDF_GEOM_MESH:
{
std::string fileName = clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_meshFileName;
const std::string& error_message_prefix = "";
std::string out_found_filename;
int out_type;
if (fileIO->findResourcePath(fileName.c_str(), relativeFileName, 1024))
if (fileName.length())
{
b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024);
const std::string& error_message_prefix = "";
std::string out_found_filename;
int out_type;
if (fileIO->findResourcePath(fileName.c_str(), relativeFileName, 1024))
{
b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024);
}
bool foundFile = UrdfFindMeshFile(fileIO, pathPrefix, relativeFileName, error_message_prefix, &out_found_filename, &out_type);
if (foundFile)
{
visualShape.m_geometry.m_meshFileType = out_type;
visualShape.m_geometry.m_meshFileName = fileName;
}
else
{
}
}
else
{
visualShape.m_geometry.m_meshFileType = UrdfGeometry::MEMORY_VERTICES;
int numVertices = clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_numVertices;
int numIndices = clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_numIndices;
int numUVs = clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_numUVs;
int numNormals = clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_numNormals;
bool foundFile = UrdfFindMeshFile(fileIO, pathPrefix, relativeFileName, error_message_prefix, &out_found_filename, &out_type);
visualShape.m_geometry.m_meshFileType = out_type;
visualShape.m_geometry.m_meshFileName = fileName;
if (numVertices > 0 && numIndices >0)
{
char* data = bufferServerToClient;
double* vertexUpload = (double*)data;
int* indexUpload = (int*)(data + numVertices * sizeof(double) * 3);
double* normalUpload = (double*)(data + numVertices * sizeof(double) * 3 + numIndices * sizeof(int));
double* uvUpload = (double*)(data + numVertices * sizeof(double) * 3 + numIndices * sizeof(int)+numNormals*sizeof(double)*3);
for (int i = 0; i < numIndices; i++)
{
visualShape.m_geometry.m_indices.push_back(indexUpload[i]);
}
for (int i = 0; i < numVertices; i++)
{
btVector3 v0(vertexUpload[i * 3 + 0],
vertexUpload[i * 3 + 1],
vertexUpload[i * 3 + 2]);
visualShape.m_geometry.m_vertices.push_back(v0);
}
for (int i = 0; i < numNormals; i++)
{
btVector3 normal(normalUpload[i * 3 + 0],
normalUpload[i * 3 + 1],
normalUpload[i * 3 + 2]);
visualShape.m_geometry.m_normals.push_back(normal);
}
for (int i = 0; i < numUVs; i++)
{
btVector3 uv(uvUpload[i * 2 + 0], uvUpload[i * 2 + 1],0);
visualShape.m_geometry.m_uvs.push_back(uv);
}
}
}
visualShape.m_geometry.m_meshScale.setValue(clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_meshScale[0],
clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_meshScale[1],
clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_meshScale[2]);

2
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -932,6 +932,8 @@ struct b3CreateUserShapeData
int m_visualFlags;
int m_numVertices;
int m_numIndices;
int m_numUVs;
int m_numNormals;
double m_rgbaColor[4];
double m_specularColor[3];
};

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

@@ -361,6 +361,49 @@ static void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPa
{
case UrdfGeometry::MEMORY_VERTICES:
{
glmesh = new GLInstanceGraphicsShape;
// int index = 0;
glmesh->m_indices = new b3AlignedObjectArray<int>();
glmesh->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>();
glmesh->m_vertices->resize(visual->m_geometry.m_vertices.size());
glmesh->m_indices->resize(visual->m_geometry.m_indices.size());
for (int i = 0; i < visual->m_geometry.m_vertices.size(); i++)
{
glmesh->m_vertices->at(i).xyzw[0] = visual->m_geometry.m_vertices[i].x();
glmesh->m_vertices->at(i).xyzw[1] = visual->m_geometry.m_vertices[i].y();
glmesh->m_vertices->at(i).xyzw[2] = visual->m_geometry.m_vertices[i].z();
glmesh->m_vertices->at(i).xyzw[3] = 1;
btVector3 normal(visual->m_geometry.m_vertices[i]);
if (visual->m_geometry.m_normals.size() == visual->m_geometry.m_vertices.size())
{
normal = visual->m_geometry.m_normals[i];
}
else
{
normal.safeNormalize();
}
btVector3 uv(0.5, 0.5, 0);
if (visual->m_geometry.m_uvs.size() == visual->m_geometry.m_vertices.size())
{
uv = visual->m_geometry.m_uvs[i];
}
glmesh->m_vertices->at(i).normal[0] = normal[0];
glmesh->m_vertices->at(i).normal[1] = normal[1];
glmesh->m_vertices->at(i).normal[2] = normal[2];
glmesh->m_vertices->at(i).uv[0] = uv[0];
glmesh->m_vertices->at(i).uv[1] = uv[1];
}
for (int i = 0; i < visual->m_geometry.m_indices.size(); i++)
{
glmesh->m_indices->at(i) = visual->m_geometry.m_indices[i];
}
glmesh->m_numIndices = visual->m_geometry.m_indices.size();
glmesh->m_numvertices = visual->m_geometry.m_vertices.size();
break;
}
case UrdfGeometry::FILE_OBJ:

168
examples/pybullet/examples/createTexturedMeshVisualShape.py Normal file
View File

@@ -0,0 +1,168 @@
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("plane100.urdf", useMaximalCoordinates=True)
#disable rendering during creation.
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
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]
vertices=[
[-1.000000,-1.000000,1.000000],
[1.000000,-1.000000,1.000000],
[1.000000,1.000000,1.000000],
[-1.000000,1.000000,1.000000],
[-1.000000,-1.000000,-1.000000],
[1.000000,-1.000000,-1.000000],
[1.000000,1.000000,-1.000000],
[-1.000000,1.000000,-1.000000],
[-1.000000,-1.000000,-1.000000],
[-1.000000,1.000000,-1.000000],
[-1.000000,1.000000,1.000000],
[-1.000000,-1.000000,1.000000],
[1.000000,-1.000000,-1.000000],
[1.000000,1.000000,-1.000000],
[1.000000,1.000000,1.000000],
[1.000000,-1.000000,1.000000],
[-1.000000,-1.000000,-1.000000],
[-1.000000,-1.000000,1.000000],
[1.000000,-1.000000,1.000000],
[1.000000,-1.000000,-1.000000],
[-1.000000,1.000000,-1.000000],
[-1.000000,1.000000,1.000000],
[1.000000,1.000000,1.000000],
[1.000000,1.000000,-1.000000]
]
normals=[
[0.000000,0.000000,1.000000],
[0.000000,0.000000,1.000000],
[0.000000,0.000000,1.000000],
[0.000000,0.000000,1.000000],
[0.000000,0.000000,-1.000000],
[0.000000,0.000000,-1.000000],
[0.000000,0.000000,-1.000000],
[0.000000,0.000000,-1.000000],
[-1.000000,0.000000,0.000000],
[-1.000000,0.000000,0.000000],
[-1.000000,0.000000,0.000000],
[-1.000000,0.000000,0.000000],
[1.000000,0.000000,0.000000],
[1.000000,0.000000,0.000000],
[1.000000,0.000000,0.000000],
[1.000000,0.000000,0.000000],
[0.000000,-1.000000,0.000000],
[0.000000,-1.000000,0.000000],
[0.000000,-1.000000,0.000000],
[0.000000,-1.000000,0.000000],
[0.000000,1.000000,0.000000],
[0.000000,1.000000,0.000000],
[0.000000,1.000000,0.000000],
[0.000000,1.000000,0.000000]
]
uvs=[
[0.750000,0.250000],
[1.000000,0.250000],
[1.000000,0.000000],
[0.750000,0.000000],
[0.500000,0.250000],
[0.250000,0.250000],
[0.250000,0.000000],
[0.500000,0.000000],
[0.500000,0.000000],
[0.750000,0.000000],
[0.750000,0.250000],
[0.500000,0.250000],
[0.250000,0.500000],
[0.250000,0.250000],
[0.000000,0.250000],
[0.000000,0.500000],
[0.250000,0.500000],
[0.250000,0.250000],
[0.500000,0.250000],
[0.500000,0.500000],
[0.000000,0.000000],
[0.000000,0.250000],
[0.250000,0.250000],
[0.250000,0.000000]
]
indices=[ 0, 1, 2, 0, 2, 3, #//ground face
6, 5, 4, 7, 6, 4, #//top face
10, 9, 8, 11, 10, 8,
12, 13, 14, 12, 14, 15,
18, 17, 16, 19, 18, 16,
20, 21, 22, 20, 22, 23]
#the visual shape and collision shape can be re-used by all createMultiBody instances (instancing)
visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,rgbaColor=[1,1,1,1], specularColor=[0.4,.4,0], visualFramePosition=shift, meshScale=[1,1,1], vertices=vertices, indices=indices, uvs=uvs, normals=normals)
#visualShapeId = p.createVisualShape(shapeType=p.GEOM_BOX,rgbaColor=[1,1,1,1], halfExtents=[0.5,0.5,0.5],specularColor=[0.4,.4,0], visualFramePosition=shift, meshScale=[1,1,1], vertices=vertices, indices=indices)
#visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,rgbaColor=[1,1,1,1], specularColor=[0.4,.4,0], visualFramePosition=shift, meshScale=meshScale, fileName="duck.obj")
collisionShapeId = p.createCollisionShape(shapeType=p.GEOM_MESH, vertices=vertices, collisionFramePosition=shift,meshScale=meshScale)
texUid = p.loadTexture("tex256.png")
rangex = 1
rangey = 1
for i in range (rangex):
for j in range (rangey ):
bodyUid = 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.changeVisualShape(bodyUid,-1, textureUniqueId = texUid)
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):
p.getCameraImage(320,200)
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

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

@@ -69,7 +69,7 @@ class MJCFBaseBulletEnv(gym.Env):
self.potential = self.robot.calc_potential()
return s
def render(self, mode='human'):
def render(self, mode='human', close=False):
if mode == "human":
self.isRender = True
if mode != "rgb_array":
@@ -125,7 +125,6 @@ class MJCFBaseBulletEnv(gym.Env):
_render = render
_reset = reset
_seed = seed
_step = step
class Camera:
def __init__(self):

80
examples/pybullet/gym/pybullet_envs/examples/laikago.py
View File

@@ -10,23 +10,23 @@ plane = p.loadURDF("plane.urdf")
p.setGravity(0,0,-9.8)
p.setTimeStep(1./500)
#p.setDefaultContactERP(0)
#urdfFlags = p.URDF_USE_SELF_COLLISION+p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS
#urdfFlags = p.URDF_USE_SELF_COLLISION+p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS
urdfFlags = p.URDF_USE_SELF_COLLISION
quadruped = p.loadURDF("laikago/laikago.urdf",[0,0,.5],[0,0.5,0.5,0], flags = urdfFlags,useFixedBase=False)
#enable collision between lower legs
for j in range (p.getNumJoints(quadruped)):
print(p.getJointInfo(quadruped,j))
print(p.getJointInfo(quadruped,j))
#2,5,8 and 11 are the lower legs
lower_legs = [2,5,8,11]
for l0 in lower_legs:
for l1 in lower_legs:
if (l1>l0):
enableCollision = 1
print("collision for pair",l0,l1, p.getJointInfo(quadruped,l0)[12],p.getJointInfo(quadruped,l1)[12], "enabled=",enableCollision)
p.setCollisionFilterPair(quadruped, quadruped, 2,5,enableCollision)
for l1 in lower_legs:
if (l1>l0):
enableCollision = 1
print("collision for pair",l0,l1, p.getJointInfo(quadruped,l0)[12],p.getJointInfo(quadruped,l1)[12], "enabled=",enableCollision)
p.setCollisionFilterPair(quadruped, quadruped, 2,5,enableCollision)
jointIds=[]
paramIds=[]
@@ -35,9 +35,9 @@ jointDirections=[-1,1,1,1,1,1,-1,1,1,1,1,1]
jointAngles=[0,0,0,0,0,0,0,0,0,0,0,0]
for i in range (4):
jointOffsets.append(0)
jointOffsets.append(-0.7)
jointOffsets.append(0.7)
jointOffsets.append(0)
jointOffsets.append(-0.7)
jointOffsets.append(0.7)
maxForceId = p.addUserDebugParameter("maxForce",0,100,20)
@@ -50,35 +50,35 @@ for j in range (p.getNumJoints(quadruped)):
if (jointType==p.JOINT_PRISMATIC or jointType==p.JOINT_REVOLUTE):
jointIds.append(j)
p.getCameraImage(480,320)
p.setRealTimeSimulation(0)
joints=[]
with open(pd.getDataPath()+"/laikago/data1.txt","r") as filestream:
for line in filestream:
print("line=",line)
maxForce = p.readUserDebugParameter(maxForceId)
currentline = line.split(",")
#print (currentline)
#print("-----")
frame = currentline[0]
t = currentline[1]
#print("frame[",frame,"]")
joints=currentline[2:14]
#print("joints=",joints)
for j in range (12):
targetPos = float(joints[j])
p.setJointMotorControl2(quadruped,jointIds[j],p.POSITION_CONTROL,jointDirections[j]*targetPos+jointOffsets[j], force=maxForce)
p.stepSimulation()
for lower_leg in lower_legs:
#print("points for ", quadruped, " link: ", lower_leg)
pts = p.getContactPoints(quadruped,-1, lower_leg)
#print("num points=",len(pts))
#for pt in pts:
# print(pt[9])
time.sleep(1./500.)
for line in filestream:
print("line=",line)
maxForce = p.readUserDebugParameter(maxForceId)
currentline = line.split(",")
#print (currentline)
#print("-----")
frame = currentline[0]
t = currentline[1]
#print("frame[",frame,"]")
joints=currentline[2:14]
#print("joints=",joints)
for j in range (12):
targetPos = float(joints[j])
p.setJointMotorControl2(quadruped,jointIds[j],p.POSITION_CONTROL,jointDirections[j]*targetPos+jointOffsets[j], force=maxForce)
p.stepSimulation()
for lower_leg in lower_legs:
#print("points for ", quadruped, " link: ", lower_leg)
pts = p.getContactPoints(quadruped,-1, lower_leg)
#print("num points=",len(pts))
#for pt in pts:
# print(pt[9])
time.sleep(1./500.)
for j in range (p.getNumJoints(quadruped)):
@@ -95,10 +95,10 @@ for j in range (p.getNumJoints(quadruped)):
p.setRealTimeSimulation(1)
while (1):
for i in range(len(paramIds)):
c = paramIds[i]
targetPos = p.readUserDebugParameter(c)
maxForce = p.readUserDebugParameter(maxForceId)
p.setJointMotorControl2(quadruped,jointIds[i],p.POSITION_CONTROL,jointDirections[i]*targetPos+jointOffsets[i], force=maxForce)
for i in range(len(paramIds)):
c = paramIds[i]
targetPos = p.readUserDebugParameter(c)
maxForce = p.readUserDebugParameter(maxForceId)
p.setJointMotorControl2(quadruped,jointIds[i],p.POSITION_CONTROL,jointDirections[i]*targetPos+jointOffsets[i], force=maxForce)

2
examples/pybullet/gym/pybullet_envs/examples/testBike.py
View File

@@ -24,7 +24,7 @@ for i in range (1):
p.setJointMotorControl2(bike,1,p.VELOCITY_CONTROL,targetVelocity=5, force=0)
p.setJointMotorControl2(bike,2,p.VELOCITY_CONTROL,targetVelocity=15, force=20)
p.changeDynamics(plane,-1, mass=20,lateralFriction=1, linearDamping=0, angularDamping=0)
p.changeDynamics(plane,-1, mass=0,lateralFriction=1, linearDamping=0, angularDamping=0)
p.changeDynamics(bike,1,lateralFriction=1,linearDamping=0, angularDamping=0)
p.changeDynamics(bike,2,lateralFriction=1,linearDamping=0, angularDamping=0)
#p.resetJointState(bike,1,0,100)

7
examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_gym_env_example.py
View File

@@ -4,10 +4,11 @@
import csv
import math
import os, inspect
import os
import inspect
currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe())))
parentdir = os.path.dirname(os.path.dirname(currentdir))
parentdir = os.path.dirname(os.path.dirname(os.path.dirname(currentdir)))
print("parentdir=",parentdir)
os.sys.path.insert(0,parentdir)
import argparse

7
examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_raibert_controller_example.py
View File

@@ -5,6 +5,13 @@ from __future__ import absolute_import
from __future__ import division
from __future__ import print_function
import os
import inspect
currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe())))
parentdir = os.path.dirname(os.path.dirname(os.path.dirname(currentdir)))
os.sys.path.insert(0,parentdir)
import tensorflow as tf
from pybullet_envs.minitaur.envs import minitaur_raibert_controller
from pybullet_envs.minitaur.envs import minitaur_gym_env

10
examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_reactive_env_example.py
View File

@@ -6,11 +6,21 @@ from __future__ import print_function
import os
import time
import inspect
currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe())))
parentdir = os.path.dirname(os.path.dirname(os.path.dirname(currentdir)))
print("parentdir=",parentdir)
os.sys.path.insert(0,parentdir)
import tensorflow as tf
from pybullet_envs.minitaur.agents.scripts import utility
import pybullet_data
from pybullet_envs.minitaur.envs import simple_ppo_agent
flags = tf.app.flags
FLAGS = tf.app.flags.FLAGS
LOG_DIR = os.path.join(pybullet_data.getDataPath(), "policies/ppo/minitaur_reactive_env")

6
examples/pybullet/gym/pybullet_envs/robot_bases.py
View File

@@ -22,9 +22,9 @@ class XmlBasedRobot:
self.robot_body = None
high = np.ones([action_dim])
self.action_space = gym.spaces.Box(-high, high, dtype=np.float32)
self.action_space = gym.spaces.Box(-high, high)
high = np.inf * np.ones([obs_dim])
self.observation_space = gym.spaces.Box(-high, high, dtype=np.float32)
self.observation_space = gym.spaces.Box(-high, high)
#self.model_xml = model_xml
self.robot_name = robot_name
@@ -235,6 +235,8 @@ class BodyPart:
(x, y, z), (a, b, c, d), _, _, _, _ = self._p.getLinkState(body_id, link_id)
return np.array([x, y, z, a, b, c, d])
def get_position(self): return self.current_position()
def get_pose(self):
return self.state_fields_of_pose_of(self.bodies[self.bodyIndex], self.bodyPartIndex)

117
examples/pybullet/pybullet.c
View File

@@ -185,6 +185,34 @@ static int pybullet_internalSetVector(PyObject* objVec, float vector[3])
return 0;
}
// vector - double[2] which will be set by values from obVec
static int pybullet_internalSetVector2d(PyObject* obVec, double vector[2])
{
int i, len;
PyObject* seq;
if (obVec == NULL)
return 0;
seq = PySequence_Fast(obVec, "expected a sequence");
if (seq)
{
len = PySequence_Size(obVec);
assert(len == 2);
if (len == 2)
{
for (i = 0; i < len; i++)
{
vector[i] = pybullet_internalGetFloatFromSequence(seq, i);
}
Py_DECREF(seq);
return 1;
}
Py_DECREF(seq);
}
PyErr_Clear();
return 0;
}
// vector - double[3] which will be set by values from obVec
static int pybullet_internalSetVectord(PyObject* obVec, double vector[3])
{
@@ -6773,6 +6801,45 @@ static int extractVertices(PyObject* verticesObj, double* vertices, int maxNumVe
return numVerticesOut;
}
static int extractUVs(PyObject* uvsObj, double* uvs, int maxNumVertices)
{
int numUVOut = 0;
if (uvsObj)
{
PyObject* seqVerticesObj = PySequence_Fast(uvsObj, "expected a sequence of uvs");
if (seqVerticesObj)
{
int numVerticesSrc = PySequence_Size(seqVerticesObj);
{
int i;
if (numVerticesSrc > B3_MAX_NUM_VERTICES)
{
PyErr_SetString(SpamError, "Number of uvs exceeds the maximum.");
Py_DECREF(seqVerticesObj);
return 0;
}
for (i = 0; i < numVerticesSrc; i++)
{
PyObject* vertexObj = PySequence_GetItem(seqVerticesObj, i);
double uv[2];
if (pybullet_internalSetVector2d(vertexObj, uv))
{
if (uvs)
{
uvs[numUVOut * 2 + 0] = uv[0];
uvs[numUVOut * 2 + 1] = uv[1];
}
numUVOut++;
}
}
}
}
}
return numUVOut;
}
static int extractIndices(PyObject* indicesObj, int* indices, int maxNumIndices)
{
int numIndicesOut=0;
@@ -7183,9 +7250,14 @@ static PyObject* pybullet_createVisualShape(PyObject* self, PyObject* args, PyOb
PyObject* halfExtentsObj = 0;
static char* kwlist[] = {"shapeType", "radius", "halfExtents", "length", "fileName", "meshScale", "planeNormal", "flags", "rgbaColor", "specularColor", "visualFramePosition", "visualFrameOrientation", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|dOdsOOiOOOOi", kwlist,
&shapeType, &radius, &halfExtentsObj, &length, &fileName, &meshScaleObj, &planeNormalObj, &flags, &rgbaColorObj, &specularColorObj, &visualFramePositionObj, &visualFrameOrientationObj, &physicsClientId))
PyObject* verticesObj = 0;
PyObject* indicesObj = 0;
PyObject* normalsObj = 0;
PyObject* uvsObj = 0;
static char* kwlist[] = {"shapeType", "radius", "halfExtents", "length", "fileName", "meshScale", "planeNormal", "flags", "rgbaColor", "specularColor", "visualFramePosition", "visualFrameOrientation", "vertices", "indices", "normals", "uvs", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|dOdsOOiOOOOOOOOi", kwlist,
&shapeType, &radius, &halfExtentsObj, &length, &fileName, &meshScaleObj, &planeNormalObj, &flags, &rgbaColorObj, &specularColorObj, &visualFramePositionObj, &visualFrameOrientationObj, &verticesObj, &indicesObj, &normalsObj, &uvsObj, &physicsClientId))
{
return NULL;
}
@@ -7228,6 +7300,45 @@ static PyObject* pybullet_createVisualShape(PyObject* self, PyObject* args, PyOb
pybullet_internalSetVectord(meshScaleObj, meshScale);
shapeIndex = b3CreateVisualShapeAddMesh(commandHandle, fileName, meshScale);
}
if (shapeType == GEOM_MESH && verticesObj && indicesObj)
{
int numVertices = extractVertices(verticesObj, 0, B3_MAX_NUM_VERTICES);
int numIndices = extractIndices(indicesObj, 0, B3_MAX_NUM_INDICES);
int numNormals = extractVertices(normalsObj, 0, B3_MAX_NUM_VERTICES);
int numUVs = extractUVs(uvsObj, 0, B3_MAX_NUM_VERTICES);
double* vertices = numVertices ? malloc(numVertices * 3 * sizeof(double)) : 0;
int* indices = numIndices ? malloc(numIndices * sizeof(int)) : 0;
double* normals = numNormals ? malloc(numNormals * 3 * sizeof(double)) : 0;
double* uvs = numUVs ? malloc(numUVs * 2 * sizeof(double)) : 0;
numVertices = extractVertices(verticesObj, vertices, B3_MAX_NUM_VERTICES);
pybullet_internalSetVectord(meshScaleObj, meshScale);
if (indicesObj)
{
numIndices = extractIndices(indicesObj, indices, B3_MAX_NUM_INDICES);
}
if (numNormals)
{
extractVertices(normalsObj, normals, numNormals);
}
if (numUVs)
{
extractUVs(uvsObj, uvs, numUVs);
}
if (numIndices)
{
shapeIndex = b3CreateVisualShapeAddMesh2(sm, commandHandle, meshScale, vertices, numVertices, indices, numIndices, normals, numNormals, uvs, numUVs);
}
free(uvs);
free(normals);
free(vertices);
free(indices);
}
if (shapeType == GEOM_PLANE)
{
double planeConstant = 0;

2
setup.py
View File

@@ -454,7 +454,7 @@ if 'BT_USE_EGL' in EGL_CXX_FLAGS:
setup(
name = 'pybullet',
version='2.4.2',
version='2.4.3',
description='Official Python Interface for the Bullet Physics SDK specialized for Robotics Simulation and Reinforcement Learning',
long_description='pybullet is an easy to use Python module for physics simulation, robotics and deep reinforcement learning based on the Bullet Physics SDK. With pybullet you can load articulated bodies from URDF, SDF and other file formats. pybullet provides forward dynamics simulation, inverse dynamics computation, forward and inverse kinematics and collision detection and ray intersection queries. Aside from physics simulation, pybullet supports to rendering, with a CPU renderer and OpenGL visualization and support for virtual reality headsets.',
url='https://github.com/bulletphysics/bullet3',