diff --git a/data/quadruped/minitaur_rainbow_dash_v1.urdf b/data/quadruped/minitaur_rainbow_dash_v1.urdf new file mode 100644 index 000000000..c69a224dc --- /dev/null +++ b/data/quadruped/minitaur_rainbow_dash_v1.urdf @@ -0,0 +1,980 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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 diff --git a/setup.py b/setup.py index 8e84b312d..9123f2ecd 100644 --- a/setup.py +++ b/setup.py @@ -443,7 +443,7 @@ print("-----") setup( name = 'pybullet', - version='1.7.4', + version='1.7.5', description='Official Python Interface for the Bullet Physics SDK specialized for Robotics Simulation and Reinforcement Learning', long_description='pybullet is an easy to use Python module for physics simulation, robotics and deep reinforcement learning based on the Bullet Physics SDK. With pybullet you can load articulated bodies from URDF, SDF and other file formats. pybullet provides forward dynamics simulation, inverse dynamics computation, forward and inverse kinematics and collision detection and ray intersection queries. Aside from physics simulation, pybullet supports to rendering, with a CPU renderer and OpenGL visualization and support for virtual reality headsets.', url='https://github.com/bulletphysics/bullet3',