Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

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

Browse Source 
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
This commit is contained in:
erwincoumans
2017-12-20 14:54:32 -08:00
parent ecba966890
commit eb35cba740
7 changed files with 150 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files

17
examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
View File

@@ -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;
}

6
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -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;
}

7
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -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();

6
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -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;
};

47
examples/pybullet/examples/quadruped.py
View File

@@ -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]

57
examples/pybullet/examples/reset_dynamic_info.py
View File

@@ -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()

30
examples/pybullet/pybullet.c
View File

@@ -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