use "world" to make door static (instead of using mass = 0)

minor improvements to pybullet

data/door.urdf

@@ -1,10 +1,17 @@
 <?xml version="0.0" ?>
 <robot name="urdf_door">
+ <link name="world"/>
+  <joint name="fixed" type="fixed">
+    <parent link="world"/>
+    <child link="baseLink"/>
+    <origin xyz="0 0 0"/>
+  </joint>
+
   <link name="baseLink">
     <inertial>
       <origin rpy="0 0 0" xyz="0 0 0.35"/>
-       <mass value="0.0"/>
+       <mass value="1.0"/>
-       <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
+       <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
     </inertial>
     <visual>
       <origin rpy="0 0 0" xyz="0.05 0 0.5"/>

examples/pybullet/pybullet.c

@@ -8,15 +8,21 @@
 #endif
 
 static PyObject *SpamError;
-static  b3PhysicsClientHandle sm;
+static  b3PhysicsClientHandle sm=0;
+
 
 static PyObject *
-spam_step(PyObject *self, PyObject *args)
+pybullet_stepSimulation(PyObject *self, PyObject *args)
 {
+    if (0==sm)
+    {
+        PyErr_SetString(SpamError, "Not connected to physics server.");
+        return NULL;
+    }
 
         {
-                        b3SharedMemoryStatusHandle statusHandle;
+            b3SharedMemoryStatusHandle statusHandle;
-                        int statusType;
+            int statusType;
 
             if (b3CanSubmitCommand(sm))
             {
@@ -29,15 +35,53 @@ return PyLong_FromLong(1);
 }
 
 static PyObject *
-spam_loadURDF(PyObject* self, PyObject* args)
+pybullet_connectPhysicsServer(PyObject *self, PyObject *args)
 {
+    if (0!=sm)
+    {
+        PyErr_SetString(SpamError, "Already connected to physics server, disconnect first.");
+        return NULL;
+    }
+    
+    {
+        sm = b3ConnectSharedMemory(SHARED_MEMORY_KEY);
+    }
+    
+    return PyLong_FromLong(1);
+}
+
+static PyObject *
+pybullet_disconnectPhysicsServer(PyObject *self, PyObject *args)
+{
+    if (0==sm)
+    {
+        PyErr_SetString(SpamError, "Not connected to physics server.");
+        return NULL;
+    }
+    {
+        b3DisconnectSharedMemory(sm);
+        sm = 0;
+    }
+    
+    return PyLong_FromLong(1);
+}
+
+
+static PyObject *
+pybullet_loadURDF(PyObject* self, PyObject* args)
+{
+    if (0==sm)
+    {
+        PyErr_SetString(SpamError, "Not connected to physics server.");
+        return NULL;
+    }
 	int size= PySequence_Size(args);
 	
 	int bodyIndex = -1;
 	const char* urdfFileName=0;
 	float startPosX =0;
-        float startPosY =0;
+    float startPosY =0;
-        float startPosZ = 1;
+    float startPosZ = 1;
 	float startOrnX = 0;
 	float startOrnY = 0;
 	float startOrnZ = 0;
@@ -61,29 +105,56 @@ spam_loadURDF(PyObject* self, PyObject* args)
 		&startOrnX,&startOrnY,&startOrnZ, &startOwnW))
                 return NULL;
 	}
-	printf("urdf filename = %s\n", urdfFileName);
+    {
-	b3SharedMemoryStatusHandle statusHandle;
+        printf("urdf filename = %s\n", urdfFileName);
-            int statusType;
+        b3SharedMemoryStatusHandle statusHandle;
-             b3SharedMemoryCommandHandle command = b3LoadUrdfCommandInit(sm, urdfFileName);
+        int statusType;
+        b3SharedMemoryCommandHandle command = b3LoadUrdfCommandInit(sm, urdfFileName);
 
-            //setting the initial position, orientation and other arguments are optional
+        //setting the initial position, orientation and other arguments are optional
-            int ret = b3LoadUrdfCommandSetStartPosition(command, startPosX,startPosY,startPosZ);
+        b3LoadUrdfCommandSetStartPosition(command, startPosX,startPosY,startPosZ);
-            statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
+        statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
-                        statusType = b3GetStatusType(statusHandle);
+        statusType = b3GetStatusType(statusHandle);
-                        if (statusType!=CMD_URDF_LOADING_COMPLETED)
+        if (statusType!=CMD_URDF_LOADING_COMPLETED)
-			{
+		{
-				PyErr_SetString(SpamError, "Cannot load URDF file.");
+			PyErr_SetString(SpamError, "Cannot load URDF file.");
-				return NULL;				
+			return NULL;
-			}
+		}
-                        bodyIndex = b3GetStatusBodyIndex(statusHandle);
+        bodyIndex = b3GetStatusBodyIndex(statusHandle);
+    }
 	return PyLong_FromLong(bodyIndex);
 }
+static PyObject *
+pybullet_resetSimulation(PyObject* self, PyObject* args)
+{
+    if (0==sm)
+    {
+        PyErr_SetString(SpamError, "Not connected to physics server.");
+        return NULL;
+    }
+
+    {
+        b3SharedMemoryStatusHandle statusHandle;
+        statusHandle = b3SubmitClientCommandAndWaitStatus(sm, b3InitResetSimulationCommand(sm));
+//        ASSERT_EQ(b3GetStatusType(statusHandle), CMD_RESET_SIMULATION_COMPLETED);
+    }
+}
 
 static PyMethodDef SpamMethods[] = {
-    {"step",  spam_step, METH_VARARGS,
+    {"loadURDF",  pybullet_loadURDF, METH_VARARGS,
-     "Step the simulation forward."},
+        "Create a multibody by loading a URDF file."},
-{"loadURDF",  spam_loadURDF, METH_VARARGS,
+    
-     "Create a multibody by loading a URDF file."},
+    {"connect",  pybullet_connectPhysicsServer, METH_VARARGS,
+        "Connect to an existing physics server (using shared memory by default)."},
+    {"disconnect",  pybullet_disconnectPhysicsServer, METH_VARARGS,
+        "Disconnect from the physics server."},
+
+    {"resetSimulation",  pybullet_resetSimulation, METH_VARARGS,
+        "Reset the simulation: remove all objects and start from an empty world."},
+
+    {"stepSimulation",  pybullet_stepSimulation, METH_VARARGS,
+        "Step the simulation using forward dynamics."},
+    
     {NULL, NULL, 0, NULL}        /* Sentinel */
 };
 
@@ -92,16 +163,7 @@ PyMODINIT_FUNC
 initpybullet(void)
 {
 
-	b3PhysicsClientHandle h;
-
     PyObject *m;
-sm = b3ConnectSharedMemory(SHARED_MEMORY_KEY);
-
-//#ifdef __APPLE__
-//sm = b3CreateInProcessPhysicsServerAndConnectMainThread(0,0);
-//#else
-//sm = b3CreateInProcessPhysicsServerAndConnect(0,0);
-//#endif
     m = Py_InitModule("pybullet", SpamMethods);
     if (m == NULL)
         return;