Skip to content

Commit 4ebc327

Browse files
erwincoumanserwincoumans
authored
Merge pull request #834 from erwincoumans/master
Add rudimentary 'saveWorld' command in shared memory API and pybullet…
2 parents 729ae8d + 1a62f21 commit 4ebc327

12 files changed

+281
-10
lines changed

‎examples/OpenGLWindow/GLInstancingRenderer.cpp

+3-1
Original file line numberDiff line numberDiff line change
@@ -1465,7 +1465,9 @@ void GLInstancingRenderer::renderSceneInternal(int renderMode)
14651465
#endif//OLD_SHADOWMAP_INIT
14661466

14671467
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
1468-
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
1468+
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR_MIPMAP_LINEAR);
1469+
1470+
14691471

14701472
float l_ClampColor[] = {1.0, 1.0, 1.0, 1.0};
14711473
glTexParameterfv(GL_TEXTURE_2D, GL_TEXTURE_BORDER_COLOR, l_ClampColor);

‎examples/OpenGLWindow/GLPrimitiveRenderer.cpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -223,7 +223,7 @@ void GLPrimitiveRenderer::drawTexturedRect3D(const PrimVertex& v0,const PrimVert
223223
bool useFiltering = false;
224224
if (useFiltering)
225225
{
226-
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
226+
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR_MIPMAP_LINEAR);
227227
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
228228
} else
229229
{

‎examples/OpenGLWindow/SimpleOpenGL3App.cpp

+2-2
Original file line numberDiff line numberDiff line change
@@ -107,7 +107,7 @@ static GLuint BindFont(const CTexFont *_Font)
107107
glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
108108
glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
109109
glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER,GL_NEAREST);
110-
glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER,GL_NEAREST);
110+
glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER,GL_LINEAR_MIPMAP_LINEAR);
111111
glBindTexture(GL_TEXTURE_2D, 0);
112112

113113
return TexID;
@@ -830,7 +830,7 @@ void SimpleOpenGL3App::dumpNextFrameToPng(const char* filename)
830830
, 0,GL_RGBA, GL_FLOAT, 0);
831831

832832
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
833-
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
833+
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR_MIPMAP_LINEAR);
834834
//glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
835835
//glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
836836

‎examples/OpenGLWindow/opengl_fontstashcallbacks.cpp

+2-2
Original file line numberDiff line numberDiff line change
@@ -126,7 +126,7 @@ void InternalOpenGL2RenderCallbacks::updateTexture(sth_texture* texture, sth_gly
126126
texture->m_texels = (unsigned char*)malloc(textureWidth*textureHeight);
127127
memset(texture->m_texels,0,textureWidth*textureHeight);
128128
glTexImage2D(GL_TEXTURE_2D, 0, GL_RED, textureWidth, textureHeight, 0, GL_RED, GL_UNSIGNED_BYTE, texture->m_texels);
129-
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
129+
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR_MIPMAP_LINEAR);
130130
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
131131
assert(glGetError()==GL_NO_ERROR);
132132

@@ -187,7 +187,7 @@ void InternalOpenGL2RenderCallbacks::render(sth_texture* texture)
187187
bool useFiltering = false;
188188
if (useFiltering)
189189
{
190-
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
190+
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR_MIPMAP_LINEAR);
191191
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
192192
} else
193193
{

‎examples/SharedMemory/PhysicsClientC_API.cpp

+21-1
Original file line numberDiff line numberDiff line change
@@ -30,7 +30,27 @@ b3SharedMemoryCommandHandle b3LoadSdfCommandInit(b3PhysicsClientHandle physClien
3030
return (b3SharedMemoryCommandHandle) command;
3131
}
3232

33-
33+
b3SharedMemoryCommandHandle b3SaveWorldCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName)
34+
{
35+
PhysicsClient* cl = (PhysicsClient* ) physClient;
36+
b3Assert(cl);
37+
b3Assert(cl->canSubmitCommand());
38+
39+
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
40+
b3Assert(command);
41+
command->m_type = CMD_SAVE_WORLD;
42+
int len = strlen(sdfFileName);
43+
if (len<MAX_SDF_FILENAME_LENGTH)
44+
{
45+
strcpy(command->m_sdfArguments.m_sdfFileName,sdfFileName);
46+
} else
47+
{
48+
command->m_sdfArguments.m_sdfFileName[0] = 0;
49+
}
50+
command->m_updateFlags = SDF_ARGS_FILE_NAME;
51+
52+
return (b3SharedMemoryCommandHandle) command;
53+
}
3454

3555
b3SharedMemoryCommandHandle b3LoadUrdfCommandInit(b3PhysicsClientHandle physClient, const char* urdfFileName)
3656
{

‎examples/SharedMemory/PhysicsClientC_API.h

+3
Original file line numberDiff line numberDiff line change
@@ -146,6 +146,9 @@ int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle status
146146
b3SharedMemoryCommandHandle b3LoadSdfCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName);
147147
int b3LoadSdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody);
148148

149+
b3SharedMemoryCommandHandle b3SaveWorldCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName);
150+
151+
149152
///The b3JointControlCommandInit method is obsolete, use b3JointControlCommandInit2 instead
150153
b3SharedMemoryCommandHandle b3JointControlCommandInit(b3PhysicsClientHandle physClient, int controlMode);
151154

‎examples/SharedMemory/PhysicsClientExample.cpp

+7
Original file line numberDiff line numberDiff line change
@@ -451,6 +451,12 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
451451
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
452452
break;
453453
}
454+
case CMD_SAVE_WORLD:
455+
{
456+
b3SharedMemoryCommandHandle commandHandle = b3SaveWorldCommandInit(m_physicsClientHandle, "saveWorld.py");
457+
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
458+
break;
459+
}
454460
default:
455461
{
456462
b3Error("Unknown buttonId");
@@ -525,6 +531,7 @@ void PhysicsClientExample::createButtons()
525531

526532
createButton("Load URDF",CMD_LOAD_URDF, isTrigger);
527533
createButton("Load SDF",CMD_LOAD_SDF, isTrigger);
534+
createButton("Save World",CMD_SAVE_WORLD, isTrigger);
528535
createButton("Get Camera Image",CMD_REQUEST_CAMERA_IMAGE_DATA,isTrigger);
529536
createButton("Step Sim",CMD_STEP_FORWARD_SIMULATION, isTrigger);
530537
createButton("Send Bullet Stream",CMD_SEND_BULLET_DATA_STREAM, isTrigger);

‎examples/SharedMemory/PhysicsClientSharedMemory.cpp

+12-2
Original file line numberDiff line numberDiff line change
@@ -615,15 +615,25 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
615615
b3Warning("Contact Point Information Request failed");
616616
break;
617617
}
618-
case CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED:
619-
{
618+
619+
case CMD_SAVE_WORLD_COMPLETED:
620+
break;
621+
622+
case CMD_SAVE_WORLD_FAILED:
623+
{
624+
b3Warning("Saving world failed");
625+
break;
626+
}
627+
case CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED:
628+
{
620629
break;
621630
}
622631
case CMD_CALCULATE_INVERSE_KINEMATICS_FAILED:
623632
{
624633
b3Warning("Calculate Inverse Kinematics Request failed");
625634
break;
626635
}
636+
627637
default: {
628638
b3Error("Unknown server status %d\n", serverCmd.m_type);
629639
btAssert(0);

‎examples/SharedMemory/PhysicsServerCommandProcessor.cpp

+171-1
Original file line numberDiff line numberDiff line change
@@ -301,6 +301,12 @@ struct CommandLogPlayback
301301
}
302302
};
303303

304+
struct SaveWorldObjectData
305+
{
306+
b3AlignedObjectArray<int> m_bodyUniqueIds;
307+
std::string m_fileName;
308+
};
309+
304310
struct PhysicsServerCommandProcessorInternalData
305311
{
306312
///handle management
@@ -410,7 +416,7 @@ struct PhysicsServerCommandProcessorInternalData
410416
btAlignedObjectArray<btMultiBodyJointFeedback*> m_multiBodyJointFeedbacks;
411417
btHashMap<btHashPtr, btInverseDynamics::MultiBodyTree*> m_inverseDynamicsBodies;
412418
btHashMap<btHashPtr, IKTrajectoryHelper*> m_inverseKinematicsHelpers;
413-
419+
b3AlignedObjectArray<SaveWorldObjectData> m_saveWorldBodyData;
414420

415421

416422
btAlignedObjectArray<btBulletWorldImporter*> m_worldImporters;
@@ -811,6 +817,9 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe
811817
{
812818
b3Printf("loaded %s OK!", fileName);
813819
}
820+
SaveWorldObjectData sd;
821+
sd.m_fileName = fileName;
822+
814823

815824
for (int m =0; m<u2b.getNumModels();m++)
816825
{
@@ -824,6 +833,7 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe
824833

825834
InternalBodyHandle* bodyHandle = m_data->getHandle(bodyUniqueId);
826835

836+
sd.m_bodyUniqueIds.push_back(bodyUniqueId);
827837

828838
u2b.setBodyUniqueId(bodyUniqueId);
829839
{
@@ -854,6 +864,7 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe
854864
if (mb)
855865
mb->setUserIndex2(bodyUniqueId);
856866

867+
857868
if (mb)
858869
{
859870
bodyHandle->m_multiBody = mb;
@@ -898,6 +909,9 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe
898909
}
899910

900911
}
912+
913+
m_data->m_saveWorldBodyData.push_back(sd);
914+
901915
}
902916
return loadOk;
903917
}
@@ -930,6 +944,14 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto
930944
if (bodyUniqueIdPtr)
931945
*bodyUniqueIdPtr= bodyUniqueId;
932946

947+
//quick prototype of 'save world' for crude world editing
948+
{
949+
SaveWorldObjectData sd;
950+
sd.m_fileName = fileName;
951+
sd.m_bodyUniqueIds.push_back(bodyUniqueId);
952+
m_data->m_saveWorldBodyData.push_back(sd);
953+
}
954+
933955
u2b.setBodyUniqueId(bodyUniqueId);
934956
InternalBodyHandle* bodyHandle = m_data->getHandle(bodyUniqueId);
935957

@@ -1336,6 +1358,154 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
13361358
hasStatus = true;
13371359
break;
13381360
}
1361+
case CMD_SAVE_WORLD:
1362+
{
1363+
///this is a very rudimentary way to save the state of the world, for scene authoring
1364+
///many todo's, for example save the state of motor controllers etc.
1365+
1366+
if (clientCmd.m_sdfArguments.m_sdfFileName)
1367+
{
1368+
//saveWorld(clientCmd.m_sdfArguments.m_sdfFileName);
1369+
1370+
FILE* f = fopen(clientCmd.m_sdfArguments.m_sdfFileName,"w");
1371+
if (f)
1372+
{
1373+
char line[1024];
1374+
{
1375+
sprintf(line,"import pybullet as p\n");
1376+
int len = strlen(line);
1377+
fwrite(line,len,1,f);
1378+
}
1379+
{
1380+
sprintf(line,"p.connect(p.SHARED_MEMORY)\n");
1381+
int len = strlen(line);
1382+
fwrite(line,len,1,f);
1383+
}
1384+
//for each objects ...
1385+
for (int i=0;i<m_data->m_saveWorldBodyData.size();i++)
1386+
{
1387+
SaveWorldObjectData& sd = m_data->m_saveWorldBodyData[i];
1388+
1389+
for (int i=0;i<sd.m_bodyUniqueIds.size();i++)
1390+
{
1391+
{
1392+
int bodyUniqueId = sd.m_bodyUniqueIds[i];
1393+
InteralBodyData* body = m_data->getHandle(bodyUniqueId);
1394+
if (body)
1395+
{
1396+
if (body->m_multiBody)
1397+
{
1398+
btMultiBody* mb = body->m_multiBody;
1399+
1400+
btTransform comTr = mb->getBaseWorldTransform();
1401+
btTransform tr = comTr * body->m_rootLocalInertialFrame.inverse();
1402+
1403+
if (strstr(sd.m_fileName.c_str(),".urdf"))
1404+
{
1405+
sprintf(line,"objects = [p.loadURDF(\"%s\", %f,%f,%f,%f,%f,%f,%f)]\n",sd.m_fileName.c_str(),
1406+
tr.getOrigin()[0],tr.getOrigin()[1],tr.getOrigin()[2],
1407+
tr.getRotation()[0],tr.getRotation()[1],tr.getRotation()[2],tr.getRotation()[3]);
1408+
int len = strlen(line);
1409+
fwrite(line,len,1,f);
1410+
}
1411+
1412+
if (strstr(sd.m_fileName.c_str(),".sdf") && i==0)
1413+
{
1414+
sprintf(line,"objects = p.loadSDF(\"%s\")\n",sd.m_fileName.c_str());
1415+
int len = strlen(line);
1416+
fwrite(line,len,1,f);
1417+
}
1418+
1419+
if (strstr(sd.m_fileName.c_str(),".sdf") || ((strstr(sd.m_fileName.c_str(),".urdf")) && mb->getNumLinks()) )
1420+
{
1421+
sprintf(line,"ob = objects[%d]\n",i);
1422+
int len = strlen(line);
1423+
fwrite(line,len,1,f);
1424+
}
1425+
1426+
if (strstr(sd.m_fileName.c_str(),".sdf"))
1427+
{
1428+
sprintf(line,"p.resetBasePositionAndOrientation(ob,[%f,%f,%f],[%f,%f,%f,%f])\n",
1429+
comTr.getOrigin()[0],comTr.getOrigin()[1],comTr.getOrigin()[2],
1430+
comTr.getRotation()[0],comTr.getRotation()[1],comTr.getRotation()[2],comTr.getRotation()[3]);
1431+
int len = strlen(line);
1432+
fwrite(line,len,1,f);
1433+
}
1434+
1435+
if (mb->getNumLinks())
1436+
{
1437+
{
1438+
sprintf(line,"jointPositions=[");
1439+
int len = strlen(line);
1440+
fwrite(line,len,1,f);
1441+
}
1442+
1443+
for (int i=0;i<mb->getNumLinks();i++)
1444+
{
1445+
btScalar jointPos = mb->getJointPosMultiDof(i)[0];
1446+
if (i<mb->getNumLinks()-1)
1447+
{
1448+
sprintf(line," %f,",jointPos);
1449+
int len = strlen(line);
1450+
fwrite(line,len,1,f);
1451+
} else
1452+
{
1453+
sprintf(line," %f ",jointPos);
1454+
int len = strlen(line);
1455+
fwrite(line,len,1,f);
1456+
}
1457+
}
1458+
1459+
{
1460+
sprintf(line,"]\nfor jointIndex in range (p.getNumJoints(ob)):\n\tp.resetJointState(ob,jointIndex,jointPositions[jointIndex])\n\n");
1461+
int len = strlen(line);
1462+
fwrite(line,len,1,f);
1463+
}
1464+
}
1465+
} else
1466+
{
1467+
//todo: btRigidBody/btSoftBody etc case
1468+
}
1469+
}
1470+
}
1471+
1472+
}
1473+
1474+
//for URDF, load at origin, then reposition...
1475+
1476+
1477+
struct SaveWorldObjectData
1478+
{
1479+
b3AlignedObjectArray<int> m_bodyUniqueIds;
1480+
std::string m_fileName;
1481+
};
1482+
}
1483+
1484+
{
1485+
btVector3 grav=this->m_data->m_dynamicsWorld->getGravity();
1486+
sprintf(line,"p.setGravity(%f,%f,%f)\n",grav[0],grav[1],grav[2]);
1487+
int len = strlen(line);
1488+
fwrite(line,len,1,f);
1489+
}
1490+
1491+
1492+
{
1493+
sprintf(line,"p.stepSimulation()\np.disconnect()\n");
1494+
int len = strlen(line);
1495+
fwrite(line,len,1,f);
1496+
}
1497+
fclose(f);
1498+
}
1499+
1500+
1501+
serverStatusOut.m_type = CMD_SAVE_WORLD_COMPLETED;
1502+
hasStatus = true;
1503+
break;
1504+
}
1505+
serverStatusOut.m_type = CMD_SAVE_WORLD_FAILED;
1506+
hasStatus = true;
1507+
break;
1508+
}
13391509
case CMD_LOAD_SDF:
13401510
{
13411511
const SdfArgs& sdfArgs = clientCmd.m_sdfArguments;

‎examples/SharedMemory/SharedMemoryPublic.h

+5
Original file line numberDiff line numberDiff line change
@@ -32,6 +32,7 @@ enum EnumSharedMemoryClientCommand
3232
CMD_CALCULATE_JACOBIAN,
3333
CMD_CREATE_JOINT,
3434
CMD_REQUEST_CONTACT_POINT_INFORMATION,
35+
CMD_SAVE_WORLD,
3536
//don't go beyond this command!
3637
CMD_MAX_CLIENT_COMMANDS,
3738

@@ -74,6 +75,10 @@ enum EnumSharedMemoryServerStatus
7475
CMD_CONTACT_POINT_INFORMATION_FAILED,
7576
CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED,
7677
CMD_CALCULATE_INVERSE_KINEMATICS_FAILED,
78+
CMD_SAVE_WORLD_COMPLETED,
79+
CMD_SAVE_WORLD_FAILED,
80+
81+
//don't go beyond 'CMD_MAX_SERVER_COMMANDS!
7782
CMD_MAX_SERVER_COMMANDS
7883
};
7984

0 commit comments

Comments
 (0)