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

Add rudimentary 'saveWorld' command in shared memory API and pybullet, see examples/pybullet/saveWorld.py

Browse Source 
Use trilinear filtering instead of bilinear
This commit is contained in:
erwincoumans
2016-10-12 23:03:36 -07:00
parent 7cb8756c28
commit 33d9603e16
12 changed files with 277 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
examples/OpenGLWindow/GLInstancingRenderer.cpp
View File

@@ -1465,7 +1465,9 @@ void GLInstancingRenderer::renderSceneInternal(int renderMode)
#endif//OLD_SHADOWMAP_INIT
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR_MIPMAP_LINEAR);
float l_ClampColor[] = {1.0, 1.0, 1.0, 1.0};
glTexParameterfv(GL_TEXTURE_2D, GL_TEXTURE_BORDER_COLOR, l_ClampColor);

2
examples/OpenGLWindow/GLPrimitiveRenderer.cpp
View File

@@ -223,7 +223,7 @@ void GLPrimitiveRenderer::drawTexturedRect3D(const PrimVertex& v0,const PrimVert
bool useFiltering = false;
if (useFiltering)
{
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR_MIPMAP_LINEAR);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
} else
{

4
examples/OpenGLWindow/SimpleOpenGL3App.cpp
View File

@@ -107,7 +107,7 @@ static GLuint BindFont(const CTexFont *_Font)
glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER,GL_NEAREST);
glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER,GL_NEAREST);
glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER,GL_LINEAR_MIPMAP_LINEAR);
glBindTexture(GL_TEXTURE_2D, 0);
return TexID;
@@ -830,7 +830,7 @@ void SimpleOpenGL3App::dumpNextFrameToPng(const char* filename)
, 0,GL_RGBA, GL_FLOAT, 0);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR_MIPMAP_LINEAR);
//glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
//glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);

4
examples/OpenGLWindow/opengl_fontstashcallbacks.cpp
View File

@@ -126,7 +126,7 @@ void InternalOpenGL2RenderCallbacks::updateTexture(sth_texture* texture, sth_gly
texture->m_texels = (unsigned char*)malloc(textureWidth*textureHeight);
memset(texture->m_texels,0,textureWidth*textureHeight);
glTexImage2D(GL_TEXTURE_2D, 0, GL_RED, textureWidth, textureHeight, 0, GL_RED, GL_UNSIGNED_BYTE, texture->m_texels);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR_MIPMAP_LINEAR);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
assert(glGetError()==GL_NO_ERROR);
@@ -187,7 +187,7 @@ void InternalOpenGL2RenderCallbacks::render(sth_texture* texture)
bool useFiltering = false;
if (useFiltering)
{
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR_MIPMAP_LINEAR);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
} else
{

22
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -30,7 +30,27 @@ b3SharedMemoryCommandHandle b3LoadSdfCommandInit(b3PhysicsClientHandle physClien
return (b3SharedMemoryCommandHandle) command;
}
b3SharedMemoryCommandHandle b3SaveWorldCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_SAVE_WORLD;
int len = strlen(sdfFileName);
if (len<MAX_SDF_FILENAME_LENGTH)
{
strcpy(command->m_sdfArguments.m_sdfFileName,sdfFileName);
} else
{
command->m_sdfArguments.m_sdfFileName[0] = 0;
}
command->m_updateFlags = SDF_ARGS_FILE_NAME;
return (b3SharedMemoryCommandHandle) command;
}
b3SharedMemoryCommandHandle b3LoadUrdfCommandInit(b3PhysicsClientHandle physClient, const char* urdfFileName)
{

3
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -146,6 +146,9 @@ int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle status
b3SharedMemoryCommandHandle b3LoadSdfCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName);
int b3LoadSdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody);
b3SharedMemoryCommandHandle b3SaveWorldCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName);
///The b3JointControlCommandInit method is obsolete, use b3JointControlCommandInit2 instead
b3SharedMemoryCommandHandle b3JointControlCommandInit(b3PhysicsClientHandle physClient, int controlMode);

7
examples/SharedMemory/PhysicsClientExample.cpp
View File

@@ -451,6 +451,12 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
break;
}
case CMD_SAVE_WORLD:
{
b3SharedMemoryCommandHandle commandHandle = b3SaveWorldCommandInit(m_physicsClientHandle, "saveWorld.py");
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
break;
}
default:
{
b3Error("Unknown buttonId");
@@ -525,6 +531,7 @@ void PhysicsClientExample::createButtons()
createButton("Load URDF",CMD_LOAD_URDF, isTrigger);
createButton("Load SDF",CMD_LOAD_SDF, isTrigger);
createButton("Save World",CMD_SAVE_WORLD, isTrigger);
createButton("Get Camera Image",CMD_REQUEST_CAMERA_IMAGE_DATA,isTrigger);
createButton("Step Sim",CMD_STEP_FORWARD_SIMULATION, isTrigger);
createButton("Send Bullet Stream",CMD_SEND_BULLET_DATA_STREAM, isTrigger);

9
examples/SharedMemory/PhysicsClientSharedMemory.cpp
View File

@@ -615,7 +615,14 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
b3Warning("Contact Point Information Request failed");
break;
}
case CMD_SAVE_WORLD_COMPLETED:
break;
case CMD_SAVE_WORLD_FAILED:
{
b3Warning("Saving world failed");
break;
}
default: {
b3Error("Unknown server status\n");
btAssert(0);

172
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -301,6 +301,12 @@ struct CommandLogPlayback
}
};
struct SaveWorldObjectData
{
b3AlignedObjectArray<int> m_bodyUniqueIds;
std::string m_fileName;
};
struct PhysicsServerCommandProcessorInternalData
{
///handle management
@@ -410,7 +416,7 @@ struct PhysicsServerCommandProcessorInternalData
btAlignedObjectArray<btMultiBodyJointFeedback*> m_multiBodyJointFeedbacks;
btHashMap<btHashPtr, btInverseDynamics::MultiBodyTree*> m_inverseDynamicsBodies;
btHashMap<btHashPtr, IKTrajectoryHelper*> m_inverseKinematicsHelpers;
b3AlignedObjectArray<SaveWorldObjectData> m_saveWorldBodyData;
btAlignedObjectArray<btBulletWorldImporter*> m_worldImporters;
@@ -811,6 +817,9 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe
{
b3Printf("loaded %s OK!", fileName);
}
SaveWorldObjectData sd;
sd.m_fileName = fileName;
for (int m =0; m<u2b.getNumModels();m++)
{
@@ -824,6 +833,7 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe
InternalBodyHandle* bodyHandle = m_data->getHandle(bodyUniqueId);
sd.m_bodyUniqueIds.push_back(bodyUniqueId);
u2b.setBodyUniqueId(bodyUniqueId);
{
@@ -854,6 +864,7 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe
if (mb)
mb->setUserIndex2(bodyUniqueId);
if (mb)
{
bodyHandle->m_multiBody = mb;
@@ -898,6 +909,9 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe
}
}
m_data->m_saveWorldBodyData.push_back(sd);
}
return loadOk;
}
@@ -930,6 +944,14 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto
if (bodyUniqueIdPtr)
*bodyUniqueIdPtr= bodyUniqueId;
//quick prototype of 'save world' for crude world editing
{
SaveWorldObjectData sd;
sd.m_fileName = fileName;
sd.m_bodyUniqueIds.push_back(bodyUniqueId);
m_data->m_saveWorldBodyData.push_back(sd);
}
u2b.setBodyUniqueId(bodyUniqueId);
InternalBodyHandle* bodyHandle = m_data->getHandle(bodyUniqueId);
@@ -1336,6 +1358,154 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
hasStatus = true;
break;
}
case CMD_SAVE_WORLD:
{
///this is a very rudimentary way to save the state of the world, for scene authoring
///many todo's, for example save the state of motor controllers etc.
if (clientCmd.m_sdfArguments.m_sdfFileName)
{
//saveWorld(clientCmd.m_sdfArguments.m_sdfFileName);
FILE* f = fopen(clientCmd.m_sdfArguments.m_sdfFileName,"w");
if (f)
{
char line[1024];
{
sprintf(line,"import pybullet as p\n");
int len = strlen(line);
fwrite(line,len,1,f);
}
{
sprintf(line,"p.connect(p.SHARED_MEMORY)\n");
int len = strlen(line);
fwrite(line,len,1,f);
}
//for each objects ...
for (int i=0;i<m_data->m_saveWorldBodyData.size();i++)
{
SaveWorldObjectData& sd = m_data->m_saveWorldBodyData[i];
for (int i=0;i<sd.m_bodyUniqueIds.size();i++)
{
{
int bodyUniqueId = sd.m_bodyUniqueIds[i];
InteralBodyData* body = m_data->getHandle(bodyUniqueId);
if (body)
{
if (body->m_multiBody)
{
btMultiBody* mb = body->m_multiBody;
btTransform comTr = mb->getBaseWorldTransform();
btTransform tr = comTr * body->m_rootLocalInertialFrame.inverse();
if (strstr(sd.m_fileName.c_str(),".urdf"))
{
sprintf(line,"objects = [p.loadURDF(\"%s\", %f,%f,%f,%f,%f,%f,%f)]\n",sd.m_fileName.c_str(),
tr.getOrigin()[0],tr.getOrigin()[1],tr.getOrigin()[2],
tr.getRotation()[0],tr.getRotation()[1],tr.getRotation()[2],tr.getRotation()[3]);
int len = strlen(line);
fwrite(line,len,1,f);
}
if (strstr(sd.m_fileName.c_str(),".sdf") && i==0)
{
sprintf(line,"objects = p.loadSDF(\"%s\")\n",sd.m_fileName.c_str());
int len = strlen(line);
fwrite(line,len,1,f);
}
if (strstr(sd.m_fileName.c_str(),".sdf") || ((strstr(sd.m_fileName.c_str(),".urdf")) && mb->getNumLinks()) )
{
sprintf(line,"ob = objects[%d]\n",i);
int len = strlen(line);
fwrite(line,len,1,f);
}
if (strstr(sd.m_fileName.c_str(),".sdf"))
{
sprintf(line,"p.resetBasePositionAndOrientation(ob,[%f,%f,%f],[%f,%f,%f,%f])\n",
comTr.getOrigin()[0],comTr.getOrigin()[1],comTr.getOrigin()[2],
comTr.getRotation()[0],comTr.getRotation()[1],comTr.getRotation()[2],comTr.getRotation()[3]);
int len = strlen(line);
fwrite(line,len,1,f);
}
if (mb->getNumLinks())
{
{
sprintf(line,"jointPositions=[");
int len = strlen(line);
fwrite(line,len,1,f);
}
for (int i=0;i<mb->getNumLinks();i++)
{
btScalar jointPos = mb->getJointPosMultiDof(i)[0];
if (i<mb->getNumLinks()-1)
{
sprintf(line," %f,",jointPos);
int len = strlen(line);
fwrite(line,len,1,f);
} else
{
sprintf(line," %f ",jointPos);
int len = strlen(line);
fwrite(line,len,1,f);
}
}
{
sprintf(line,"]\nfor jointIndex in range (p.getNumJoints(ob)):\n\tp.resetJointState(ob,jointIndex,jointPositions[jointIndex])\n\n");
int len = strlen(line);
fwrite(line,len,1,f);
}
}
} else
{
//todo: btRigidBody/btSoftBody etc case
}
}
}
}
//for URDF, load at origin, then reposition...
struct SaveWorldObjectData
{
b3AlignedObjectArray<int> m_bodyUniqueIds;
std::string m_fileName;
};
}
{
btVector3 grav=this->m_data->m_dynamicsWorld->getGravity();
sprintf(line,"p.setGravity(%f,%f,%f)\n",grav[0],grav[1],grav[2]);
int len = strlen(line);
fwrite(line,len,1,f);
}
{
sprintf(line,"p.stepSimulation()\np.disconnect()\n");
int len = strlen(line);
fwrite(line,len,1,f);
}
fclose(f);
}
serverStatusOut.m_type = CMD_SAVE_WORLD_COMPLETED;
hasStatus = true;
break;
}
serverStatusOut.m_type = CMD_SAVE_WORLD_FAILED;
hasStatus = true;
break;
}
case CMD_LOAD_SDF:
{
const SdfArgs& sdfArgs = clientCmd.m_sdfArguments;

5
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -32,6 +32,7 @@ enum EnumSharedMemoryClientCommand
CMD_CALCULATE_JACOBIAN,
CMD_CREATE_JOINT,
CMD_REQUEST_CONTACT_POINT_INFORMATION,
CMD_SAVE_WORLD,
//don't go beyond this command!
CMD_MAX_CLIENT_COMMANDS,
@@ -74,6 +75,10 @@ enum EnumSharedMemoryServerStatus
CMD_CONTACT_POINT_INFORMATION_FAILED,
CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED,
CMD_CALCULATE_INVERSE_KINEMATICS_FAILED,
CMD_SAVE_WORLD_COMPLETED,
CMD_SAVE_WORLD_FAILED,
//don't go beyond 'CMD_MAX_SERVER_COMMANDS!
CMD_MAX_SERVER_COMMANDS
};

46
examples/pybullet/pybullet.c
View File

@@ -112,6 +112,48 @@ static PyObject* pybullet_disconnectPhysicsServer(PyObject* self,
return Py_None;
}
static PyObject* pybullet_saveWorld(PyObject* self, PyObject* args) {
int size = PySequence_Size(args);
const char* worldFileName = "";
if (0 == sm) {
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
if (size == 1) {
if (!PyArg_ParseTuple(args, "s", &worldFileName))
{
return NULL;
}
else
{
b3SharedMemoryCommandHandle command;
b3SharedMemoryStatusHandle statusHandle;
int statusType;
command = b3SaveWorldCommandInit(sm, worldFileName);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
statusType = b3GetStatusType(statusHandle);
if (statusType != CMD_SAVE_WORLD_COMPLETED) {
PyErr_SetString(SpamError, "saveWorld command execution failed.");
return NULL;
}
Py_INCREF(Py_None);
return Py_None;
}
}
PyErr_SetString(SpamError, "Cannot execute saveWorld command.");
return NULL;
return NULL;
}
// Load a URDF file indicating the links and joints of an object
// function can be called without arguments and will default
// to position (0,0,1) with orientation(0,0,0,1)
@@ -2082,6 +2124,10 @@ static PyMethodDef SpamMethods[] = {
{"loadSDF", pybullet_loadSDF, METH_VARARGS,
"Load multibodies from an SDF file."},
{"saveWorld", pybullet_saveWorld, METH_VARARGS,
"Save an approximate Python file to reproduce the current state of the world: saveWorld"
"(filename). (very preliminary and approximately)"},
{"getNumBodies", pybullet_getNumBodies, METH_VARARGS,
"Get the number of bodies in the simulation."},

8
examples/pybullet/saveWorld.py Normal file
View File

@@ -0,0 +1,8 @@
import pybullet as p
import time
p.connect(p.SHARED_MEMORY)
timestr = time.strftime("%Y%m%d-%H%M%S")
filename = "saveWorld" + timestr + ".py"
p.saveWorld(filename)
p.disconnect()