diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 84f856467..de1e2e2d9 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -951,8 +951,7 @@ b3SharedMemoryCommandHandle b3CreateMultiBodyCommandInit(b3PhysicsClientHandle p return 0; } - -int b3CreateMultiBodyBase(b3SharedMemoryCommandHandle commandHandle, double mass, int collisionShapeUnique, int visualShapeUniqueId, double basePosition[3], double baseOrientation[4]) +int b3CreateMultiBodyBase(b3SharedMemoryCommandHandle commandHandle, double mass, int collisionShapeUnique, int visualShapeUniqueId, double basePosition[3], double baseOrientation[4] , double baseInertialFramePosition[3], double baseInertialFrameOrientation[4]) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -975,19 +974,18 @@ int b3CreateMultiBodyBase(b3SharedMemoryCommandHandle commandHandle, double mass command->m_createMultiBodyArgs.m_linkOrientations[baseLinkIndex*4+2]=baseOrientation[2]; command->m_createMultiBodyArgs.m_linkOrientations[baseLinkIndex*4+3]=baseOrientation[3]; - command->m_createMultiBodyArgs.m_linkInertias[baseLinkIndex*3+0] = mass; - command->m_createMultiBodyArgs.m_linkInertias[baseLinkIndex*3+1] = mass; - command->m_createMultiBodyArgs.m_linkInertias[baseLinkIndex*3+2] = mass; + command->m_createMultiBodyArgs.m_linkInertias[baseLinkIndex*3+0] = 0;//unused, is computed automatically. Will add a method to explicitly set it (with a flag), similar to loadURDF etc. + command->m_createMultiBodyArgs.m_linkInertias[baseLinkIndex*3+1] = 0; + command->m_createMultiBodyArgs.m_linkInertias[baseLinkIndex*3+2] = 0; + command->m_createMultiBodyArgs.m_linkInertialFramePositions[baseLinkIndex*3+0] = baseInertialFramePosition[0]; + command->m_createMultiBodyArgs.m_linkInertialFramePositions[baseLinkIndex*3+1] = baseInertialFramePosition[1]; + command->m_createMultiBodyArgs.m_linkInertialFramePositions[baseLinkIndex*3+2] = baseInertialFramePosition[2]; - command->m_createMultiBodyArgs.m_linkInertialFramePositions[baseLinkIndex*3+0] = 0; - command->m_createMultiBodyArgs.m_linkInertialFramePositions[baseLinkIndex*3+1] = 0; - command->m_createMultiBodyArgs.m_linkInertialFramePositions[baseLinkIndex*3+2] = 0; - - command->m_createMultiBodyArgs.m_linkInertialFrameOrientations[baseLinkIndex*4+0] = 0; - command->m_createMultiBodyArgs.m_linkInertialFrameOrientations[baseLinkIndex*4+1] = 0; - command->m_createMultiBodyArgs.m_linkInertialFrameOrientations[baseLinkIndex*4+2] = 0; - command->m_createMultiBodyArgs.m_linkInertialFrameOrientations[baseLinkIndex*4+3] = 1; + command->m_createMultiBodyArgs.m_linkInertialFrameOrientations[baseLinkIndex*4+0] = baseInertialFrameOrientation[0]; + command->m_createMultiBodyArgs.m_linkInertialFrameOrientations[baseLinkIndex*4+1] = baseInertialFrameOrientation[1]; + command->m_createMultiBodyArgs.m_linkInertialFrameOrientations[baseLinkIndex*4+2] = baseInertialFrameOrientation[2]; + command->m_createMultiBodyArgs.m_linkInertialFrameOrientations[baseLinkIndex*4+3] = baseInertialFrameOrientation[3]; command->m_createMultiBodyArgs.m_linkCollisionShapeUniqueIds[baseLinkIndex]= collisionShapeUnique; command->m_createMultiBodyArgs.m_linkVisualShapeUniqueIds[baseLinkIndex] = visualShapeUniqueId; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 9b32a0daf..efa45d495 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -342,7 +342,7 @@ b3SharedMemoryCommandHandle b3CreateVisualShapeCommandInit(b3PhysicsClientHandle int b3GetStatusVisualShapeUniqueId(b3SharedMemoryStatusHandle statusHandle); b3SharedMemoryCommandHandle b3CreateMultiBodyCommandInit(b3PhysicsClientHandle physClient); -int b3CreateMultiBodyBase(b3SharedMemoryCommandHandle commandHandle, double mass, int collisionShapeUnique, int visualShapeUniqueId, double basePosition[3], double baseOrientation[4]); +int b3CreateMultiBodyBase(b3SharedMemoryCommandHandle commandHandle, double mass, int collisionShapeUnique, int visualShapeUniqueId, double basePosition[3], double baseOrientation[4] , double baseInertialFramePosition[3], double baseInertialFrameOrientation[4]); int b3CreateMultiBodyLink(b3SharedMemoryCommandHandle commandHandle, double linkMass, double linkCollisionShapeIndex, double linkVisualShapeIndex, diff --git a/examples/pybullet/examples/createMultiBodyLinks.py b/examples/pybullet/examples/createMultiBodyLinks.py index 7cfc03272..9c776cab6 100644 --- a/examples/pybullet/examples/createMultiBodyLinks.py +++ b/examples/pybullet/examples/createMultiBodyLinks.py @@ -13,10 +13,9 @@ colBoxId = p.createCollisionShape(p.GEOM_BOX,halfExtents=[sphereRadius,sphereRad mass = 1 visualShapeId = -1 -#"linkMasses","linkCollisionShapeIndices", "linkVisualShapeIndices", -#"linkPositions", "linkOrientations","linkParentIndices", "linkJointTypes","linkJointAxis", - -linkMasses=[1] + + +link_Masses=[1] linkCollisionShapeIndices=[colBoxId] linkVisualShapeIndices=[-1] linkPositions=[[0,0,0.11]] @@ -35,7 +34,7 @@ for i in range (3): if (k&2): sphereUid = p.createMultiBody(mass,colSphereId,visualShapeId,basePosition,baseOrientation) else: - sphereUid = p.createMultiBody(mass,colBoxId,visualShapeId,basePosition,baseOrientation,linkMasses,linkCollisionShapeIndices,linkVisualShapeIndices,linkPositions,linkOrientations,linkInertialFramePositions, linkInertialFrameOrientations,linkParentIndices=indices,linkJointTypes=jointTypes,linkJointAxis=axis) + sphereUid = p.createMultiBody(mass,colBoxId,visualShapeId,basePosition,baseOrientation,linkMasses=link_Masses,linkCollisionShapeIndices=linkCollisionShapeIndices,linkVisualShapeIndices=linkVisualShapeIndices,linkPositions=linkPositions,linkOrientations=linkOrientations,linkInertialFramePositions=linkInertialFramePositions, linkInertialFrameOrientations=linkInertialFrameOrientations,linkParentIndices=indices,linkJointTypes=jointTypes,linkJointAxis=axis) p.changeDynamics(sphereUid,-1,spinningFriction=0.001, rollingFriction=0.001,linearDamping=0.0) for joint in range (p.getNumJoints(sphereUid)): @@ -43,15 +42,12 @@ for i in range (3): p.setGravity(0,0,-10) -#p.setRealTimeSimulation(1) +p.setRealTimeSimulation(1) p.getNumJoints(sphereUid) for i in range (p.getNumJoints(sphereUid)): p.getJointInfo(sphereUid,i) while (1): - events = p.getKeyboardEvents() - if (len(events)): - p.stepSimulation() time.sleep(0.01) \ No newline at end of file diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index fdbf401a0..e7b83649c 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -5084,7 +5084,9 @@ static PyObject* pybullet_createMultiBody(PyObject* self, PyObject* args, PyObje int useMaximalCoordinates = 0; PyObject* basePosObj=0; PyObject* baseOrnObj=0; - + PyObject* baseInertialFramePositionObj=0; + PyObject* baseInertialFrameOrientationObj=0; + PyObject* linkMassesObj=0; PyObject* linkCollisionShapeIndicesObj=0; PyObject* linkVisualShapeIndicesObj=0; @@ -5096,14 +5098,13 @@ static PyObject* pybullet_createMultiBody(PyObject* self, PyObject* args, PyObje PyObject* linkInertialFramePositionObj=0; PyObject* linkInertialFrameOrientationObj=0; - static char* kwlist[] = {"baseMass","baseCollisionShapeIndex","baseVisualShapeIndex","basePosition", "baseOrientation", + static char* kwlist[] = {"baseMass","baseCollisionShapeIndex","baseVisualShapeIndex","basePosition", "baseOrientation", "baseInertialFramePosition", "baseInertialFrameOrientation", "linkMasses","linkCollisionShapeIndices", "linkVisualShapeIndices", - "linkPositions", "linkOrientations","linkInertialFramePosition","linkInertialFrameOrientation", "linkParentIndices", "linkJointTypes","linkJointAxis", - "useMaximalCoordinates","physicsClientId", + "linkPositions", "linkOrientations","linkInertialFramePositions","linkInertialFrameOrientations", "linkParentIndices", "linkJointTypes","linkJointAxis", + "useMaximalCoordinates","physicsClientId", NULL}; - NULL}; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diiOOOOOOOOOOOOii", kwlist, &baseMass,&baseCollisionShapeIndex,&baseVisualShapeIndex, - &basePosObj, &baseOrnObj, + if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diiOOOOOOOOOOOOOOii", kwlist, &baseMass,&baseCollisionShapeIndex,&baseVisualShapeIndex, + &basePosObj, &baseOrnObj,&baseInertialFramePositionObj, &baseInertialFrameOrientationObj, &linkMassesObj, &linkCollisionShapeIndicesObj, &linkVisualShapeIndicesObj, &linkPositionsObj, &linkOrientationsObj, &linkInertialFramePositionObj, &linkInertialFrameOrientationObj, &linkParentIndicesObj, &linkJointTypesObj,&linkJointAxisObj, @@ -5159,10 +5160,15 @@ static PyObject* pybullet_createMultiBody(PyObject* self, PyObject* args, PyObje b3SharedMemoryCommandHandle commandHandle = b3CreateMultiBodyCommandInit(sm); double basePosition[3]={0,0,0}; double baseOrientation[4]={0,0,0,1}; + double baseInertialFramePosition[3] = {0,0,0}; + double baseInertialFrameOrientation[4]={0,0,0,1}; int baseIndex; pybullet_internalSetVectord(basePosObj,basePosition); pybullet_internalSetVector4d(baseOrnObj,baseOrientation); - baseIndex = b3CreateMultiBodyBase(commandHandle,baseMass,baseCollisionShapeIndex,baseVisualShapeIndex,basePosition,baseOrientation); + pybullet_internalSetVectord(baseInertialFramePositionObj,baseInertialFramePosition); + pybullet_internalSetVector4d(baseInertialFrameOrientationObj,baseInertialFrameOrientation); + + baseIndex = b3CreateMultiBodyBase(commandHandle,baseMass,baseCollisionShapeIndex,baseVisualShapeIndex, basePosition, baseOrientation, baseInertialFramePosition, baseInertialFrameOrientation ); for (i=0;i