Add API to get user constraint id.

This commit is contained in:
yunfeibai 2017-05-03 18:25:25 -07:00
parent 4da2c076a7
commit c7e9a31898
11 changed files with 73 additions and 3 deletions

View File

@ -37,6 +37,8 @@ public:
virtual int getNumUserConstraints() const = 0;
virtual int getUserConstraintInfo(int constraintUniqueId, struct b3UserConstraint& info) const = 0;
virtual int getUserConstraintId(int serialIndex) const = 0;
virtual void setSharedMemoryKey(int key) = 0;

View File

@ -1179,6 +1179,13 @@ int b3GetUserConstraintInfo(b3PhysicsClientHandle physClient, int constraintUniq
return 0;
}
/// return the user constraint id, given the index in range [0 , b3GetNumUserConstraints() )
int b3GetUserConstraintId(b3PhysicsClientHandle physClient, int serialIndex)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
return cl->getUserConstraintId(serialIndex);
}
/// return the body unique id, given the index in range [0 , b3GetNumBodies() )
int b3GetBodyUniqueId(b3PhysicsClientHandle physClient, int serialIndex)
{

View File

@ -96,6 +96,8 @@ b3SharedMemoryCommandHandle b3InitRemoveUserConstraintCommand(b3PhysicsClientHa
int b3GetNumUserConstraints(b3PhysicsClientHandle physClient);
int b3GetUserConstraintInfo(b3PhysicsClientHandle physClient, int constraintUniqueId, struct b3UserConstraint* info);
/// return the user constraint id, given the index in range [0 , b3GetNumUserConstraints() )
int b3GetUserConstraintId(b3PhysicsClientHandle physClient, int serialIndex);
///Request physics debug lines for debug visualization. The flags in debugMode are the same as used in Bullet
///See btIDebugDraw::DebugDrawModes in Bullet/src/LinearMath/btIDebugDraw.h

View File

@ -163,6 +163,15 @@ int PhysicsClientSharedMemory::getUserConstraintInfo(int constraintUniqueId, str
return 0;
}
int PhysicsClientSharedMemory::getUserConstraintId(int serialIndex) const
{
if ((serialIndex >= 0) && (serialIndex < getNumUserConstraints()))
{
return m_data->m_userConstraintInfoMap.getKeyAtIndex(serialIndex).getUid1();
}
return -1;
}
PhysicsClientSharedMemory::PhysicsClientSharedMemory()
{

View File

@ -47,6 +47,8 @@ public:
virtual int getNumUserConstraints() const;
virtual int getUserConstraintInfo(int constraintUniqueId, struct b3UserConstraint& info) const;
virtual int getUserConstraintId(int serialIndex) const;
virtual void setSharedMemoryKey(int key);

View File

@ -925,7 +925,14 @@ int PhysicsDirect::getUserConstraintInfo(int constraintUniqueId, struct b3UserCo
return 0;
}
int PhysicsDirect::getUserConstraintId(int serialIndex) const
{
if ((serialIndex >= 0) && (serialIndex < getNumUserConstraints()))
{
return m_data->m_userConstraintInfoMap.getKeyAtIndex(serialIndex).getUid1();
}
return -1;
}
int PhysicsDirect::getBodyUniqueId(int serialIndex) const
{

View File

@ -66,6 +66,8 @@ public:
virtual int getNumUserConstraints() const;
virtual int getUserConstraintInfo(int constraintUniqueId, struct b3UserConstraint& info) const;
virtual int getUserConstraintId(int serialIndex) const;
///todo: move this out of the
virtual void setSharedMemoryKey(int key);

View File

@ -108,6 +108,11 @@ int PhysicsLoopBack::getUserConstraintInfo(int constraintUniqueId, struct b3User
return m_data->m_physicsClient->getUserConstraintInfo( constraintUniqueId, info);
}
int PhysicsLoopBack::getUserConstraintId(int serialIndex) const
{
return m_data->m_physicsClient->getUserConstraintId(serialIndex);
}
///todo: move this out of the interface
void PhysicsLoopBack::setSharedMemoryKey(int key)
{

View File

@ -51,6 +51,8 @@ public:
virtual int getNumUserConstraints() const;
virtual int getUserConstraintInfo(int constraintUniqueId, struct b3UserConstraint&info) const;
virtual int getUserConstraintId(int serialIndex) const;
///todo: move this out of the
virtual void setSharedMemoryKey(int key);

View File

@ -9,6 +9,8 @@ cubeId = p.loadURDF("cube_small.urdf",0,0,1)
p.setGravity(0,0,-10)
p.setRealTimeSimulation(1)
cid = p.createConstraint(cubeId,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],[0,0,1])
print cid
print p.getConstraintUniqueId(0)
prev=[0,0,1]
a=-math.pi
while 1:
@ -21,4 +23,4 @@ while 1:
orn = p.getQuaternionFromEuler([a,0,0])
p.changeConstraint(cid,pivot,jointChildFrameOrientation=orn, maxForce=50)
p.removeConstraint(cid)
p.removeConstraint(cid)

View File

@ -1846,6 +1846,36 @@ static PyObject* pybullet_getConstraintInfo(PyObject* self, PyObject* args, PyOb
return NULL;
}
static PyObject* pybullet_getConstraintUniqueId(PyObject* self, PyObject* args, PyObject* keywds)
{
int physicsClientId = 0;
int serialIndex = -1;
b3PhysicsClientHandle sm = 0;
static char* kwlist[] = {"serialIndex", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist, &serialIndex, &physicsClientId))
{
return NULL;
}
sm = getPhysicsClient(physicsClientId);
if (sm == 0)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
{
int userConstraintId = -1;
userConstraintId = b3GetUserConstraintId(sm, serialIndex);
#if PY_MAJOR_VERSION >= 3
return PyLong_FromLong(userConstraintId);
#else
return PyInt_FromLong(userConstraintId);
#endif
}
}
static PyObject* pybullet_getNumConstraints(PyObject* self, PyObject* args, PyObject* keywds)
{
int numConstraints = 0;
@ -5430,7 +5460,7 @@ static PyMethodDef SpamMethods[] = {
{"getConstraintInfo", (PyCFunction)pybullet_getConstraintInfo, METH_VARARGS | METH_KEYWORDS,
"Get the user-created constraint info, given a constraint unique id."},
{"getConstraintUniqueId", (PyCFunction)pybullet_getBodyUniqueId, METH_VARARGS | METH_KEYWORDS,
{"getConstraintUniqueId", (PyCFunction)pybullet_getConstraintUniqueId, METH_VARARGS | METH_KEYWORDS,
"Get the unique id of the constraint, given a integer index in range [0.. number of constraints)."},
{"getBasePositionAndOrientation", (PyCFunction)pybullet_getBasePositionAndOrientation,