From eb35cba74060d0fefa7abe4e43214de2de6b40df Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Wed, 20 Dec 2017 14:54:32 -0800 Subject: [PATCH] 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 --- .../ImportURDFDemo/BulletUrdfImporter.cpp | 17 +++--- examples/SharedMemory/PhysicsClientC_API.cpp | 6 +- .../PhysicsServerCommandProcessor.cpp | 7 +++ examples/SharedMemory/SharedMemoryPublic.h | 6 +- examples/pybullet/examples/quadruped.py | 47 ++++++++++++++- .../pybullet/examples/reset_dynamic_info.py | 57 ++++++++++++++++--- examples/pybullet/pybullet.c | 30 +++++++++- 7 files changed, 150 insertions(+), 20 deletions(-) diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp index ecf175ca8..25cb52a2e 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp +++ b/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)); - convexHull->setMargin(gUrdfDefaultCollisionMargin); - convexHull->initializePolyhedralFeatures(); - convexHull->optimizeConvexHull(); + btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3)); + cylZShape->setMargin(gUrdfDefaultCollisionMargin); + cylZShape->recalcLocalAabb(); + 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; } diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index e75213c94..203b98669 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/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; } diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index c943a360c..df32a4928 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/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(); diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index a7e128e62..75b7c7b3d 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/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; }; diff --git a/examples/pybullet/examples/quadruped.py b/examples/pybullet/examples/quadruped.py index 7e3ee1262..57e975f26 100644 --- a/examples/pybullet/examples/quadruped.py +++ b/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] diff --git a/examples/pybullet/examples/reset_dynamic_info.py b/examples/pybullet/examples/reset_dynamic_info.py index 78d476a27..eb87358af 100644 --- a/examples/pybullet/examples/reset_dynamic_info.py +++ b/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) - mass2,frictionCoeff2=p.getDynamicsInfo(bodyUniqueId=cubeId,linkIndex=-1) - print mass1,frictionCoeff1 - print mass2,frictionCoeff2 - time.sleep(.01) + mass1,frictionCoeff1 =p.getDynamicsInfo(bodyUniqueId=planeId,linkIndex=-1) + mass2,frictionCoeff2 =p.getDynamicsInfo(bodyUniqueId=cubeId,linkIndex=-1) + + + print (mass1,frictionCoeff1) + print (mass2,frictionCoeff2) + time.sleep(1./240.) p.stepSimulation() diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index b96911674..c78a3c198 100644 --- a/examples/pybullet/pybullet.c +++ b/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}; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|i", kwlist, &bodyUniqueId, &linkIndex, &physicsClientId)) + static char* kwlist[] = {"bodyUniqueId", "linkIndex", "flags", "physicsClientId", NULL}; + 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; } } @@ -8168,6 +8190,8 @@ initpybullet(void) PyModule_AddIntConstant(m, "JOINT_FIXED", eFixedType); // user read 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