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

also add baseInertialFramePositionObj and baseInertialFrameOrientationObj to pybullet.createMultiBody

Browse Source 
updated createMultiBodyLinks.py example.
This commit is contained in:
Erwin Coumans
2017-06-19 17:13:20 -07:00
parent 7441515c0e
commit 61f27a5c72
4 changed files with 31 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files

24
examples/SharedMemory/PhysicsClientC_API.cpp
View File

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

2
examples/SharedMemory/PhysicsClientC_API.h
View File

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

12
examples/pybullet/examples/createMultiBodyLinks.py
View File

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

20
examples/pybullet/pybullet.c
View File

@@ -5084,6 +5084,8 @@ 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;
@@ -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<numLinkMasses;i++)
{