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

Preliminary version of pybullet.createMultiBody including links connected to parent by a joint.

Browse Source 
See createMultiBodyLinks.py example.
This commit is contained in:
Erwin Coumans
2017-06-19 13:15:05 -07:00
parent f3c11b6f31
commit 7441515c0e
6 changed files with 181 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files

57
examples/pybullet/examples/createMultiBodyLinks.py Normal file
View File

@@ -0,0 +1,57 @@
import pybullet as p
import time
p.connect(p.GUI)
p.createCollisionShape(p.GEOM_PLANE)
p.createMultiBody(0,0)
sphereRadius = 0.05
colSphereId = p.createCollisionShape(p.GEOM_SPHERE,radius=sphereRadius)
colBoxId = p.createCollisionShape(p.GEOM_BOX,halfExtents=[sphereRadius,sphereRadius,sphereRadius])
mass = 1
visualShapeId = -1
#"linkMasses","linkCollisionShapeIndices", "linkVisualShapeIndices",
#"linkPositions", "linkOrientations","linkParentIndices", "linkJointTypes","linkJointAxis",
linkMasses=[1]
linkCollisionShapeIndices=[colBoxId]
linkVisualShapeIndices=[-1]
linkPositions=[[0,0,0.11]]
linkOrientations=[[0,0,0,1]]
linkInertialFramePositions=[[0,0,0]]
linkInertialFrameOrientations=[[0,0,0,1]]
indices=[0]
jointTypes=[p.JOINT_REVOLUTE]
axis=[[0,0,1]]
for i in range (3):
for j in range (3):
for k in range (3):
basePosition = [1+i*5*sphereRadius,1+j*5*sphereRadius,1+k*5*sphereRadius+1]
baseOrientation = [0,0,0,1]
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)
p.changeDynamics(sphereUid,-1,spinningFriction=0.001, rollingFriction=0.001,linearDamping=0.0)
for joint in range (p.getNumJoints(sphereUid)):
p.setJointMotorControl2(sphereUid,joint,p.VELOCITY_CONTROL,targetVelocity=1,force=10)
p.setGravity(0,0,-10)
#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)

33
examples/pybullet/pybullet.c
View File

@@ -5093,17 +5093,19 @@ static PyObject* pybullet_createMultiBody(PyObject* self, PyObject* args, PyObje
PyObject* linkParentIndicesObj=0;
PyObject* linkJointTypesObj=0;
PyObject* linkJointAxisObj=0;
PyObject* linkInertialFramePositionObj=0;
PyObject* linkInertialFrameOrientationObj=0;
static char* kwlist[] = {"baseMass","baseCollisionShapeIndex","baseVisualShapeIndex","basePosition", "baseOrientation",
"linkMasses","linkCollisionShapeIndices", "linkVisualShapeIndices",
"linkPositions", "linkOrientations","linkParentIndices", "linkJointTypes","linkJointAxis",
"linkPositions", "linkOrientations","linkInertialFramePosition","linkInertialFrameOrientation", "linkParentIndices", "linkJointTypes","linkJointAxis",
"useMaximalCoordinates","physicsClientId",
NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diiOOOOOOOOOOii", kwlist, &baseMass,&baseCollisionShapeIndex,&baseVisualShapeIndex,
if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diiOOOOOOOOOOOOii", kwlist, &baseMass,&baseCollisionShapeIndex,&baseVisualShapeIndex,
&basePosObj, &baseOrnObj,
&linkMassesObj, &linkCollisionShapeIndicesObj, &linkVisualShapeIndicesObj, &linkPositionsObj, &linkOrientationsObj,
&linkInertialFramePositionObj, &linkInertialFrameOrientationObj,
&linkParentIndicesObj, &linkJointTypesObj,&linkJointAxisObj,
&useMaximalCoordinates, &physicsClientId))
{
@@ -5127,7 +5129,8 @@ static PyObject* pybullet_createMultiBody(PyObject* self, PyObject* args, PyObje
int numLinkParentIndices = linkParentIndicesObj?PySequence_Size(linkParentIndicesObj):0;
int numLinkJointTypes = linkJointTypesObj?PySequence_Size(linkJointTypesObj):0;
int numLinkJoinAxis = linkJointAxisObj? PySequence_Size(linkJointAxisObj):0;
int numLinkInertialFramePositions = linkInertialFramePositionObj? PySequence_Size(linkInertialFramePositionObj) : 0;
int numLinkInertialFrameOrientations = linkInertialFrameOrientationObj? PySequence_Size(linkInertialFrameOrientationObj) : 0;
PyObject* seqLinkMasses = linkMassesObj?PySequence_Fast(linkMassesObj, "expected a sequence"):0;
PyObject* seqLinkCollisionShapes = linkCollisionShapeIndicesObj?PySequence_Fast(linkCollisionShapeIndicesObj, "expected a sequence"):0;
@@ -5137,6 +5140,8 @@ static PyObject* pybullet_createMultiBody(PyObject* self, PyObject* args, PyObje
PyObject* seqLinkParentIndices = linkParentIndicesObj?PySequence_Fast(linkParentIndicesObj, "expected a sequence"):0;
PyObject* seqLinkJointTypes = linkJointTypesObj?PySequence_Fast(linkJointTypesObj, "expected a sequence"):0;
PyObject* seqLinkJoinAxis = linkJointAxisObj?PySequence_Fast(linkJointAxisObj, "expected a sequence"):0;
PyObject* seqLinkInertialFramePositions = linkInertialFramePositionObj?PySequence_Fast(linkInertialFramePositionObj, "expected a sequence"):0;
PyObject* seqLinkInertialFrameOrientations = linkInertialFrameOrientationObj?PySequence_Fast(linkInertialFrameOrientationObj, "expected a sequence"):0;
if ((numLinkMasses==numLinkCollisionShapes) &&
(numLinkMasses==numLinkVisualShapes) &&
@@ -5144,7 +5149,9 @@ static PyObject* pybullet_createMultiBody(PyObject* self, PyObject* args, PyObje
(numLinkMasses==numLinkOrientations) &&
(numLinkMasses==numLinkParentIndices) &&
(numLinkMasses==numLinkJointTypes) &&
(numLinkMasses==numLinkJoinAxis))
(numLinkMasses==numLinkJoinAxis) &&
(numLinkMasses==numLinkInertialFramePositions) &&
(numLinkMasses==numLinkInertialFrameOrientations))
{
b3SharedMemoryStatusHandle statusHandle;
int statusType;
@@ -5165,7 +5172,11 @@ static PyObject* pybullet_createMultiBody(PyObject* self, PyObject* args, PyObje
double linkPosition[3];
double linkOrientation[4];
double linkJointAxis[3];
double linkInertialFramePosition[3];
double linkInertialFrameOrientation[4];
pybullet_internalGetVector3FromSequence(seqLinkInertialFramePositions,i,linkInertialFramePosition);
pybullet_internalGetVector4FromSequence(linkInertialFrameOrientationObj,i,linkInertialFrameOrientation);
pybullet_internalGetVector3FromSequence(seqLinkPositions,i,linkPosition);
pybullet_internalGetVector4FromSequence(seqLinkOrientations,i,linkOrientation);
pybullet_internalGetVector3FromSequence(seqLinkJoinAxis,i,linkJointAxis);
@@ -5178,6 +5189,8 @@ static PyObject* pybullet_createMultiBody(PyObject* self, PyObject* args, PyObje
linkVisualShapeIndex,
linkPosition,
linkOrientation,
linkInertialFramePosition,
linkInertialFrameOrientation,
linkParentIndex,
linkJointType,
linkJointAxis
@@ -5201,6 +5214,10 @@ static PyObject* pybullet_createMultiBody(PyObject* self, PyObject* args, PyObje
Py_DECREF(seqLinkJointTypes);
if (seqLinkJoinAxis)
Py_DECREF(seqLinkJoinAxis);
if (seqLinkInertialFramePositions)
Py_DECREF(seqLinkInertialFramePositions);
if (seqLinkInertialFrameOrientations)
Py_DECREF(seqLinkInertialFrameOrientations);
if (useMaximalCoordinates>0)
{
@@ -5233,6 +5250,10 @@ static PyObject* pybullet_createMultiBody(PyObject* self, PyObject* args, PyObje
Py_DECREF(seqLinkJointTypes);
if (seqLinkJoinAxis)
Py_DECREF(seqLinkJoinAxis);
if (seqLinkInertialFramePositions)
Py_DECREF(seqLinkInertialFramePositions);
if (seqLinkInertialFrameOrientations)
Py_DECREF(seqLinkInertialFrameOrientations);
PyErr_SetString(SpamError, "All link arrays need to be same size.");
return NULL;