add pybullet.getBasePositionAndOrientation

add missing file to pybullet CMakeLists.txt

examples/pybullet/CMakeLists.txt

@@ -48,6 +48,7 @@ SET(pybullet_SRCS
                         ../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
                         ../../examples/Importers/ImportURDFDemo/UrdfParser.cpp
                         ../../examples/Importers/ImportURDFDemo/urdfStringSplit.cpp
+			../../examples/Importers/ImportURDFDemo/DefaultVisualShapeConverter.cpp
                         ../../examples/MultiThreading/b3PosixThreadSupport.cpp
                         ../../examples/MultiThreading/b3Win32ThreadSupport.cpp
 			../../examples/MultiThreading/b3ThreadSupportInterface.cpp

examples/pybullet/pybullet.c

@@ -224,25 +224,114 @@ pybullet_setGravity(PyObject* self, PyObject* args)
 		//ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED);
 	}
 
-	if (1)
-	{
-		PyObject *pylist; 
-		PyObject *item; 
-		int i;
-		int num=3;
-		pylist = PyTuple_New(num);
-		for (i = 0; i < num; i++) 
-		{
-			item = PyFloat_FromDouble(i);
-			PyTuple_SetItem(pylist, i, item);
-		}
-		return pylist;
-	}
-
 	Py_INCREF(Py_None);
 	return Py_None;
 }
 
+
+static void pybullet_internalGetBasePositionAndOrientation(int bodyIndex, double basePosition[3],double baseOrientation[3])
+{
+    basePosition[0] = 0.;
+    basePosition[1] = 0.;
+    basePosition[2] = 0.;
+    
+    baseOrientation[0] = 0.;
+    baseOrientation[1] = 0.;
+    baseOrientation[2] = 0.;
+    baseOrientation[3] = 1.;
+
+	{
+		
+		
+		{
+	      b3SharedMemoryCommandHandle cmd_handle =
+                b3RequestActualStateCommandInit(sm, bodyIndex);
+            b3SharedMemoryStatusHandle status_handle =
+                    b3SubmitClientCommandAndWaitStatus(sm, cmd_handle);
+
+            const int status_type = b3GetStatusType(status_handle);
+            
+            const double* actualStateQ;
+            b3GetStatusActualState(status_handle, 0/* body_unique_id */,
+                               0/* num_degree_of_freedom_q */,
+                               0/* num_degree_of_freedom_u */, 0 /*root_local_inertial_frame*/,
+                               &actualStateQ , 0 /* actual_state_q_dot */,
+                               0 /* joint_reaction_forces */);
+
+            //now, position x,y,z = actualStateQ[0],actualStateQ[1],actualStateQ[2]
+            //and orientation x,y,z,w = actualStateQ[3],actualStateQ[4],actualStateQ[5],actualStateQ[6]
+            basePosition[0] = actualStateQ[0];
+            basePosition[1] = actualStateQ[1];
+            basePosition[2] = actualStateQ[2];
+            
+            baseOrientation[0] = actualStateQ[3];
+            baseOrientation[1] = actualStateQ[4];
+            baseOrientation[2] = actualStateQ[5];
+            baseOrientation[3] = actualStateQ[6];
+		}
+	}
+}
+
+static PyObject *
+pybullet_getBasePositionAndOrientation(PyObject* self, PyObject* args)
+{
+    if (0==sm)
+    {
+        PyErr_SetString(SpamError, "Not connected to physics server.");
+        return NULL;
+    }
+    
+    int bodyIndex = -1;
+    if (!PyArg_ParseTuple(args, "i", &bodyIndex ))
+    {
+        PyErr_SetString(SpamError, "Expected a body index (integer).");
+        return NULL;
+    }
+    
+    double basePosition[3];
+    double baseOrientation[4];
+    
+    pybullet_internalGetBasePositionAndOrientation(bodyIndex,basePosition,baseOrientation);
+    PyObject *pylistPos; 
+    {
+    
+        PyObject *item; 
+        int i;
+        int num=3;
+        pylistPos = PyTuple_New(num);
+        for (i = 0; i < num; i++) 
+        {
+            item = PyFloat_FromDouble(basePosition[i]);
+            PyTuple_SetItem(pylistPos, i, item);
+        }
+    
+    }
+    PyObject *pylistOrientation; 
+    {
+        
+        PyObject *item; 
+        int i;
+        int num=4;
+        pylistOrientation = PyTuple_New(num);
+        for (i = 0; i < num; i++) 
+        {
+            item = PyFloat_FromDouble(baseOrientation[i]);
+            PyTuple_SetItem(pylistOrientation, i, item);
+        }
+        
+    }
+    
+    {
+        PyObject *pylist; 
+        pylist = PyTuple_New(2);
+        PyTuple_SetItem(pylist,0,pylistPos);
+        PyTuple_SetItem(pylist,1,pylistOrientation);
+        return pylist;
+    }
+    
+}
+
+
 static PyObject *
 pybullet_getNumJoints(PyObject* self, PyObject* args)
 {
@@ -290,6 +379,9 @@ static PyMethodDef SpamMethods[] = {
 	{"setGravity",  pybullet_setGravity, METH_VARARGS,
         "Set the gravity acceleration (x,y,z)."},
 	
+	{"getBasePositionAndOrientation",  pybullet_getBasePositionAndOrientation, METH_VARARGS,
+        "Get the world position and orientation of the base of the object. (x,y,z) position vector and (x,y,z,w) quaternion orientation."},
+	
 	{"getNumsetGravity",  pybullet_setGravity, METH_VARARGS,
         "Set the gravity acceleration (x,y,z)."},
 	{

examples/pybullet/test.py

@@ -2,7 +2,7 @@ import pybullet
 import time
 
 #choose connection method: GUI, DIRECT, SHARED_MEMORY
-pybullet.connect(pybullet.SHARED_MEMORY)
+pybullet.connect(pybullet.GUI)
 
 #load URDF, given a relative or absolute file+path
 obj = pybullet.loadURDF("r2d2.urdf")
@@ -24,6 +24,8 @@ pybullet.setGravity(0,0,-9.8)
 t_end = time.time() + 5
 while time.time() < t_end:
 	pybullet.stepSimulation()
+	posAndOrn = pybullet.getBasePositionAndOrientation(obj)
+	print (posAndOrn)
 
 print ("finished")
 #remove all objects