diff --git a/examples/SharedMemory/PhysicsClient.h b/examples/SharedMemory/PhysicsClient.h index 797aa86ea..2ce391c60 100644 --- a/examples/SharedMemory/PhysicsClient.h +++ b/examples/SharedMemory/PhysicsClient.h @@ -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; diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 4d819a6b5..abf228607 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -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) { diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 339ed2a2d..ae534017d 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -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 diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index fdd48a1b3..dcac6541b 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -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() { diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.h b/examples/SharedMemory/PhysicsClientSharedMemory.h index 10fc7363d..163ea55fb 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.h +++ b/examples/SharedMemory/PhysicsClientSharedMemory.h @@ -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); diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp index 92f53e648..ef1d70aba 100644 --- a/examples/SharedMemory/PhysicsDirect.cpp +++ b/examples/SharedMemory/PhysicsDirect.cpp @@ -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 { diff --git a/examples/SharedMemory/PhysicsDirect.h b/examples/SharedMemory/PhysicsDirect.h index 8a9905de4..18e08477d 100644 --- a/examples/SharedMemory/PhysicsDirect.h +++ b/examples/SharedMemory/PhysicsDirect.h @@ -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); diff --git a/examples/SharedMemory/PhysicsLoopBack.cpp b/examples/SharedMemory/PhysicsLoopBack.cpp index 69236d71f..bf33bea2e 100644 --- a/examples/SharedMemory/PhysicsLoopBack.cpp +++ b/examples/SharedMemory/PhysicsLoopBack.cpp @@ -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) { diff --git a/examples/SharedMemory/PhysicsLoopBack.h b/examples/SharedMemory/PhysicsLoopBack.h index 790571170..cd05027f1 100644 --- a/examples/SharedMemory/PhysicsLoopBack.h +++ b/examples/SharedMemory/PhysicsLoopBack.h @@ -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); diff --git a/examples/pybullet/examples/constraint.py b/examples/pybullet/examples/constraint.py index 040708664..51ac0750a 100644 --- a/examples/pybullet/examples/constraint.py +++ b/examples/pybullet/examples/constraint.py @@ -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) \ No newline at end of file +p.removeConstraint(cid) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 5211c8a85..2154d7414 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -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,