expose local inertia diagonal in C-API, PyBullet, through the 'getDynamicsInfo'

render the inertia boxes in examples/pybullet/examples/quadruped.py and examples/pybullet/examples/reset_dynamic_info.py
fix an issue where the original margin (0.04) was used to compute the inertia, instead of latest margin

examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp

@@ -613,18 +613,19 @@ btCollisionShape* BulletURDFImporter::convertURDFToCollisionShape(const UrdfColl
                 vertices.push_back(vert);
 
             }
-            btConvexHullShape* convexHull = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
+            btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
-            convexHull->setMargin(gUrdfDefaultCollisionMargin);
+            cylZShape->setMargin(gUrdfDefaultCollisionMargin);
-			convexHull->initializePolyhedralFeatures();
+			cylZShape->recalcLocalAabb();
-			convexHull->optimizeConvexHull();
+			cylZShape->initializePolyhedralFeatures();
+			cylZShape->optimizeConvexHull();
 			
-			//btConvexShape* cylZShape = new btConeShapeZ(cyl->radius,cyl->length);//(vexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
+			//btConvexShape* cylZShape = new btConeShapeZ(cylRadius,cylLength);//(vexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
             
-            //btVector3 halfExtents(cyl->radius,cyl->radius,cyl->length/2.);
+            //btVector3 halfExtents(cylRadius,cylRadius,cylLength);
             //btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents);
             
 
-            shape = convexHull;
+            shape = cylZShape;
             break;
         }
         case URDF_GEOM_BOX:
@@ -798,6 +799,7 @@ upAxisMat.setIdentity();
 			convexHull->optimizeConvexHull();
 			//convexHull->initializePolyhedralFeatures();
 			convexHull->setMargin(gUrdfDefaultCollisionMargin);
+			convexHull->recalcLocalAabb();
 			//convexHull->setLocalScaling(collision->m_geometry.m_meshScale);
 			shape = convexHull;
 		}
@@ -845,6 +847,7 @@ void BulletURDFImporter::convertURDFToVisualShapeInternal(const UrdfVisual* visu
 
 			btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
 			cylZShape->setMargin(gUrdfDefaultCollisionMargin);
+			cylZShape->recalcLocalAabb();
 			convexColShape = cylZShape;
 			break;
 		}

examples/SharedMemory/PhysicsClientC_API.cpp

@@ -2052,11 +2052,15 @@ B3_SHARED_API int b3GetDynamicsInfo(b3SharedMemoryStatusHandle statusHandle, str
 {
 	const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
 	const b3DynamicsInfo &dynamicsInfo = status->m_dynamicsInfo;
-	btAssert(status->m_type == CMD_GET_DYNAMICS_INFO);
+	btAssert(status->m_type == CMD_GET_DYNAMICS_INFO_COMPLETED);
 	if (status->m_type != CMD_GET_DYNAMICS_INFO_COMPLETED)
 		return false;
 
 	info->m_mass = dynamicsInfo.m_mass;
+	info->m_localInertialDiagonal[0] = dynamicsInfo.m_localInertialDiagonal[0];
+	info->m_localInertialDiagonal[1] = dynamicsInfo.m_localInertialDiagonal[1];
+	info->m_localInertialDiagonal[2] = dynamicsInfo.m_localInertialDiagonal[2];
+
 	info->m_lateralFrictionCoeff = dynamicsInfo.m_lateralFrictionCoeff;
 	return true;
 }

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -6148,11 +6148,18 @@ bool PhysicsServerCommandProcessor::processGetDynamicsInfoCommand(const struct S
 		if (linkIndex == -1)
 		{
 			serverCmd.m_dynamicsInfo.m_mass = mb->getBaseMass();
+			serverCmd.m_dynamicsInfo.m_localInertialDiagonal[0] = mb->getBaseInertia()[0];
+			serverCmd.m_dynamicsInfo.m_localInertialDiagonal[1] = mb->getBaseInertia()[1];
+			serverCmd.m_dynamicsInfo.m_localInertialDiagonal[2] = mb->getBaseInertia()[2];
 			serverCmd.m_dynamicsInfo.m_lateralFrictionCoeff = mb->getBaseCollider()->getFriction();
 		}
 		else
 		{
 			serverCmd.m_dynamicsInfo.m_mass = mb->getLinkMass(linkIndex);
+			serverCmd.m_dynamicsInfo.m_localInertialDiagonal[0] = mb->getLinkInertia(linkIndex)[0];
+			serverCmd.m_dynamicsInfo.m_localInertialDiagonal[1] = mb->getLinkInertia(linkIndex)[1];
+			serverCmd.m_dynamicsInfo.m_localInertialDiagonal[2] = mb->getLinkInertia(linkIndex)[2];
+
 			if (mb->getLinkCollider(linkIndex))
 			{
 				serverCmd.m_dynamicsInfo.m_lateralFrictionCoeff = mb->getLinkCollider(linkIndex)->getFriction();

examples/SharedMemory/SharedMemoryPublic.h

@@ -210,6 +210,10 @@ enum JointType {
 	eGearType=6
 };
 
+enum b3RequestDynamicsInfoFlags
+{
+	eDYNAMICS_INFO_REPORT_INERTIA=1,
+};
 
 enum b3JointInfoFlags
 {
@@ -266,7 +270,7 @@ struct b3BodyInfo
 struct b3DynamicsInfo
 {
 	double m_mass;
-	double m_localInertialPosition[3];
+	double m_localInertialDiagonal[3];
 	double m_lateralFrictionCoeff;
 };
 

examples/pybullet/examples/quadruped.py

@@ -2,6 +2,44 @@ import pybullet as p
 import time
 import math
 
+
+def drawInertiaBox(parentUid, parentLinkIndex, color):
+	mass,frictionCoeff, inertia =p.getDynamicsInfo(bodyUniqueId=parentUid,linkIndex=parentLinkIndex, flags = p.DYNAMICS_INFO_REPORT_INERTIA)
+	if (mass>0):
+		Ixx = inertia[0]
+		Iyy = inertia[1]
+		Izz = inertia[2]
+		boxScaleX = 0.5*math.sqrt(6*(Izz + Iyy - Ixx) / mass);
+		boxScaleY = 0.5*math.sqrt(6*(Izz + Ixx - Iyy) / mass);
+		boxScaleZ = 0.5*math.sqrt(6*(Ixx + Iyy - Izz) / mass);
+  
+		halfExtents = [boxScaleX,boxScaleY,boxScaleZ]
+		pts = [[halfExtents[0],halfExtents[1],halfExtents[2]],
+				 [-halfExtents[0],halfExtents[1],halfExtents[2]],
+				 [halfExtents[0],-halfExtents[1],halfExtents[2]],
+				 [-halfExtents[0],-halfExtents[1],halfExtents[2]],
+				 [halfExtents[0],halfExtents[1],-halfExtents[2]],
+				 [-halfExtents[0],halfExtents[1],-halfExtents[2]],
+				 [halfExtents[0],-halfExtents[1],-halfExtents[2]],
+				 [-halfExtents[0],-halfExtents[1],-halfExtents[2]]]
+	
+		
+		p.addUserDebugLine(pts[0],pts[1],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
+		p.addUserDebugLine(pts[1],pts[3],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
+		p.addUserDebugLine(pts[3],pts[2],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
+		p.addUserDebugLine(pts[2],pts[0],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
+	
+		p.addUserDebugLine(pts[0],pts[4],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
+		p.addUserDebugLine(pts[1],pts[5],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
+		p.addUserDebugLine(pts[2],pts[6],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
+		p.addUserDebugLine(pts[3],pts[7],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
+	
+		p.addUserDebugLine(pts[4+0],pts[4+1],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
+		p.addUserDebugLine(pts[4+1],pts[4+3],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
+		p.addUserDebugLine(pts[4+3],pts[4+2],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
+		p.addUserDebugLine(pts[4+2],pts[4+0],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
+
+
 toeConstraint = True
 useMaximalCoordinates = False
 useRealTime = 1
@@ -44,7 +82,6 @@ p.setRealTimeSimulation(0)
 quadruped = p.loadURDF("quadruped/minitaur_v1.urdf",[1,-1,.3],orn,useFixedBase=False, useMaximalCoordinates=useMaximalCoordinates)
 nJoints = p.getNumJoints(quadruped)
 
-
 jointNameToId = {}
 for i in range(nJoints):
 	jointInfo = p.getJointInfo(quadruped, i)
@@ -76,6 +113,14 @@ motor_back_leftL_joint = jointNameToId['motor_back_leftL_joint']
 motor_back_leftL_link = jointNameToId['motor_back_leftL_link']
 knee_back_leftL_link = jointNameToId['knee_back_leftL_link']
 
+
+drawInertiaBox(quadruped,-1, [1,0,0])
+#drawInertiaBox(quadruped,motor_front_rightR_joint, [1,0,0])
+
+for i in range (nJoints):
+	drawInertiaBox(quadruped,i, [0,1,0])
+
+
 #fixtorso = p.createConstraint(-1,-1,quadruped,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],[0,0,0])
 
 motordir=[-1,-1,-1,-1,1,1,1,1]

examples/pybullet/examples/reset_dynamic_info.py

@@ -4,20 +4,63 @@ import math
 
 p.connect(p.GUI)
 planeId = p.loadURDF(fileName="plane.urdf",baseOrientation=[0.25882,0,0,0.96593])
-p.loadURDF(fileName="cube.urdf",baseOrientation=[0.25882,0,0,0.96593],basePosition=[0,0,2])
+p.loadURDF(fileName="cube.urdf",basePosition=[0,0,2])
 cubeId = p.loadURDF(fileName="cube.urdf",baseOrientation=[0,0,0,1],basePosition=[0,0,4])
 #p.changeDynamics(bodyUniqueId=2,linkIndex=-1,mass=0.1)
-p.changeDynamics(bodyUniqueId=2,linkIndex=-1,mass=100.0)
+p.changeDynamics(bodyUniqueId=2,linkIndex=-1,mass=1.0)
 p.setGravity(0,0,-10)
 p.setRealTimeSimulation(0)
+
+def drawInertiaBox(parentUid, parentLinkIndex):
+	mass,frictionCoeff, inertia =p.getDynamicsInfo(bodyUniqueId=parentUid,linkIndex=parentLinkIndex, flags = p.DYNAMICS_INFO_REPORT_INERTIA)
+	Ixx = inertia[0]
+	Iyy = inertia[1]
+	Izz = inertia[2]
+	boxScaleX = 0.5*math.sqrt(6*(Izz + Iyy - Ixx) / mass);
+	boxScaleY = 0.5*math.sqrt(6*(Izz + Ixx - Iyy) / mass);
+	boxScaleZ = 0.5*math.sqrt(6*(Ixx + Iyy - Izz) / mass);
+  
+	halfExtents = [boxScaleX,boxScaleY,boxScaleZ]
+	pts = [[halfExtents[0],halfExtents[1],halfExtents[2]],
+				 [-halfExtents[0],halfExtents[1],halfExtents[2]],
+				 [halfExtents[0],-halfExtents[1],halfExtents[2]],
+				 [-halfExtents[0],-halfExtents[1],halfExtents[2]],
+				 [halfExtents[0],halfExtents[1],-halfExtents[2]],
+				 [-halfExtents[0],halfExtents[1],-halfExtents[2]],
+				 [halfExtents[0],-halfExtents[1],-halfExtents[2]],
+				 [-halfExtents[0],-halfExtents[1],-halfExtents[2]]]
+	
+	color=[1,0,0]
+	p.addUserDebugLine(pts[0],pts[1],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
+	p.addUserDebugLine(pts[1],pts[3],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
+	p.addUserDebugLine(pts[3],pts[2],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
+	p.addUserDebugLine(pts[2],pts[0],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
+	
+	p.addUserDebugLine(pts[0],pts[4],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
+	p.addUserDebugLine(pts[1],pts[5],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
+	p.addUserDebugLine(pts[2],pts[6],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
+	p.addUserDebugLine(pts[3],pts[7],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
+	
+	p.addUserDebugLine(pts[4+0],pts[4+1],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
+	p.addUserDebugLine(pts[4+1],pts[4+3],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
+	p.addUserDebugLine(pts[4+3],pts[4+2],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
+	p.addUserDebugLine(pts[4+2],pts[4+0],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
+	
+	
+	
+  
+drawInertiaBox(cubeId,-1)
+
 t=0
 while 1:
 	t=t+1
 	if t > 400:
 		p.changeDynamics(bodyUniqueId=0,linkIndex=-1,lateralFriction=0.01)
-	mass1,frictionCoeff1=p.getDynamicsInfo(bodyUniqueId=planeId,linkIndex=-1)
+	mass1,frictionCoeff1 =p.getDynamicsInfo(bodyUniqueId=planeId,linkIndex=-1)
-	mass2,frictionCoeff2=p.getDynamicsInfo(bodyUniqueId=cubeId,linkIndex=-1)
+	mass2,frictionCoeff2 =p.getDynamicsInfo(bodyUniqueId=cubeId,linkIndex=-1)
-	print mass1,frictionCoeff1
+	
-	print mass2,frictionCoeff2
+			
-	time.sleep(.01)
+	print (mass1,frictionCoeff1)
+	print (mass2,frictionCoeff2)
+	time.sleep(1./240.)
 	p.stepSimulation()

examples/pybullet/pybullet.c

@@ -808,11 +808,13 @@ static PyObject* pybullet_getDynamicsInfo(PyObject* self, PyObject* args, PyObje
 	{
 		int bodyUniqueId = -1;
 		int linkIndex = -2;
+		int flags = 0;
+
 		b3PhysicsClientHandle sm = 0;
 		
 		int physicsClientId = 0;
-		static char* kwlist[] = {"bodyUniqueId", "linkIndex", "physicsClientId", NULL};
+		static char* kwlist[] = {"bodyUniqueId", "linkIndex", "flags", "physicsClientId", NULL};
-		if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|i", kwlist, &bodyUniqueId, &linkIndex, &physicsClientId))
+		if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|ii", kwlist, &bodyUniqueId, &linkIndex, &flags, &physicsClientId))
 		{
 			return NULL;
 		}
@@ -827,6 +829,12 @@ static PyObject* pybullet_getDynamicsInfo(PyObject* self, PyObject* args, PyObje
 			b3SharedMemoryCommandHandle cmd_handle;
 			b3SharedMemoryStatusHandle status_handle;
 			struct b3DynamicsInfo info;
+			int numFields = 2;
+			if (flags & eDYNAMICS_INFO_REPORT_INERTIA)
+			{
+				numFields++;
+			}
+
 			if (bodyUniqueId < 0)
 			{
 				PyErr_SetString(SpamError, "getDynamicsInfo failed; invalid bodyUniqueId");
@@ -846,11 +854,25 @@ static PyObject* pybullet_getDynamicsInfo(PyObject* self, PyObject* args, PyObje
 				return NULL;
 			}
 			
+			
+			
+
 			if (b3GetDynamicsInfo(status_handle, &info))
 			{
-				PyObject* pyDynamicsInfo = PyTuple_New(2);
+			
+				PyObject* pyDynamicsInfo = PyTuple_New(numFields);
 				PyTuple_SetItem(pyDynamicsInfo, 0, PyFloat_FromDouble(info.m_mass));
 				PyTuple_SetItem(pyDynamicsInfo, 1, PyFloat_FromDouble(info.m_lateralFrictionCoeff));
+				
+				if (flags & eDYNAMICS_INFO_REPORT_INERTIA)
+				{
+					PyObject* pyInertiaDiag = PyTuple_New(3);
+					PyTuple_SetItem(pyInertiaDiag,0,PyFloat_FromDouble(info.m_localInertialDiagonal[0]));
+					PyTuple_SetItem(pyInertiaDiag,1,PyFloat_FromDouble(info.m_localInertialDiagonal[1]));
+					PyTuple_SetItem(pyInertiaDiag,2,PyFloat_FromDouble(info.m_localInertialDiagonal[2]));
+					PyTuple_SetItem(pyDynamicsInfo, 2, pyInertiaDiag);
+				}
+				
 				return pyDynamicsInfo;
 			}
 		}
@@ -8169,6 +8191,8 @@ initpybullet(void)
 	PyModule_AddIntConstant(m, "JOINT_POINT2POINT", ePoint2PointType);  // user read
 	PyModule_AddIntConstant(m, "JOINT_GEAR", eGearType);  // user read
 	
+	PyModule_AddIntConstant(m, "DYNAMICS_INFO_REPORT_INERTIA", eDYNAMICS_INFO_REPORT_INERTIA);  // report local inertia in 'getDynamicsInfo'
+
 	PyModule_AddIntConstant(m, "SENSOR_FORCE_TORQUE", eSensorForceTorqueType);  // user read
 
 	PyModule_AddIntConstant(m, "TORQUE_CONTROL", CONTROL_MODE_TORQUE);