From 9aa5a839d5c44b692fb8150ff0456ae5b2f703a4 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Thu, 12 Jan 2017 10:30:46 -0800 Subject: [PATCH] add pybullet.changeConstraint / b3InitChangeUserConstraintCommand/ b3InitChangeUserConstraintSetPivotInB /b3InitChangeUserConstraintSetFrameInB command, to change an existing user constraint. add constraint.py example. allow pybullet.createConstraint to create user constraint without a child body ('fixed' to the world) --- examples/SharedMemory/PhysicsClientC_API.cpp | 46 +++++++++++++++ examples/SharedMemory/PhysicsClientC_API.h | 4 ++ .../PhysicsServerCommandProcessor.cpp | 57 ++++++++++++++++--- examples/SharedMemory/SharedMemoryCommands.h | 4 +- examples/pybullet/constraint.py | 24 ++++++++ examples/pybullet/pybullet.c | 50 ++++++++++++++++ .../Featherstone/btMultiBodyConstraint.h | 4 ++ .../Featherstone/btMultiBodyFixedConstraint.h | 4 +- .../Featherstone/btMultiBodyPoint2Point.h | 3 +- .../btMultiBodySliderConstraint.h | 4 +- 10 files changed, 186 insertions(+), 14 deletions(-) create mode 100644 examples/pybullet/constraint.py diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 7082f0abb..251f4ea86 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -1064,6 +1064,52 @@ b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHan return (b3SharedMemoryCommandHandle)command; } +b3SharedMemoryCommandHandle b3InitChangeUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId) +{ + PhysicsClient* cl = (PhysicsClient* ) physClient; + b3Assert(cl); + b3Assert(cl->canSubmitCommand()); + struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); + b3Assert(command); + command->m_type = CMD_USER_CONSTRAINT; + command->m_updateFlags = USER_CONSTRAINT_CHANGE_CONSTRAINT; + command->m_userConstraintArguments.m_userConstraintUniqueId = userConstraintUniqueId; + return (b3SharedMemoryCommandHandle)command; +} + +int b3InitChangeUserConstraintSetPivotInB(b3SharedMemoryCommandHandle commandHandle, double pivotInB[3]) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_USER_CONSTRAINT); + + b3Assert(command->m_updateFlags & USER_CONSTRAINT_CHANGE_CONSTRAINT); + + command->m_updateFlags |= USER_CONSTRAINT_CHANGE_PIVOT_IN_B; + + command->m_userConstraintArguments.m_childFrame[0] = pivotInB[0]; + command->m_userConstraintArguments.m_childFrame[1] = pivotInB[1]; + command->m_userConstraintArguments.m_childFrame[2] = pivotInB[2]; + return 0; +} +int b3InitChangeUserConstraintSetFrameInB(b3SharedMemoryCommandHandle commandHandle, double frameOrnInB[4]) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_USER_CONSTRAINT); + b3Assert(command->m_updateFlags & USER_CONSTRAINT_CHANGE_CONSTRAINT); + + command->m_updateFlags |= USER_CONSTRAINT_CHANGE_FRAME_ORN_IN_B; + + command->m_userConstraintArguments.m_childFrame[3] = frameOrnInB[0]; + command->m_userConstraintArguments.m_childFrame[4] = frameOrnInB[1]; + command->m_userConstraintArguments.m_childFrame[5] = frameOrnInB[2]; + command->m_userConstraintArguments.m_childFrame[6] = frameOrnInB[3]; + + return 0; +} + + b3SharedMemoryCommandHandle b3InitRemoveUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId) { PhysicsClient* cl = (PhysicsClient* ) physClient; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 2f553ea73..54daaa77c 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -74,6 +74,10 @@ int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int jointInd b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info); int b3GetStatusUserConstraintUniqueId(b3SharedMemoryStatusHandle statusHandle); +b3SharedMemoryCommandHandle b3InitChangeUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId); +int b3InitChangeUserConstraintSetPivotInB(b3SharedMemoryCommandHandle commandHandle, double jointChildPivot[3]); +int b3InitChangeUserConstraintSetFrameInB(b3SharedMemoryCommandHandle commandHandle, double jointChildFrameOrn[4]); + b3SharedMemoryCommandHandle b3InitRemoveUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId); ///Request physics debug lines for debug visualization. The flags in debugMode are the same as used in Bullet diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 1e0844b51..92acae284 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -3460,8 +3460,9 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm InteralBodyData* parentBody = m_data->getHandle(clientCmd.m_userConstraintArguments.m_parentBodyIndex); if (parentBody && parentBody->m_multiBody) { - InteralBodyData* childBody = m_data->getHandle(clientCmd.m_userConstraintArguments.m_childBodyIndex); - if (childBody) + InteralBodyData* childBody = clientCmd.m_userConstraintArguments.m_childBodyIndex>=0 ? m_data->getHandle(clientCmd.m_userConstraintArguments.m_childBodyIndex):0; + //also create a constraint with just a single multibody/rigid body without child + //if (childBody) { btVector3 pivotInParent(clientCmd.m_userConstraintArguments.m_parentFrame[0], clientCmd.m_userConstraintArguments.m_parentFrame[1], clientCmd.m_userConstraintArguments.m_parentFrame[2]); btVector3 pivotInChild(clientCmd.m_userConstraintArguments.m_childFrame[0], clientCmd.m_userConstraintArguments.m_childFrame[1], clientCmd.m_userConstraintArguments.m_childFrame[2]); @@ -3470,7 +3471,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm btVector3 jointAxis(clientCmd.m_userConstraintArguments.m_jointAxis[0], clientCmd.m_userConstraintArguments.m_jointAxis[1], clientCmd.m_userConstraintArguments.m_jointAxis[2]); if (clientCmd.m_userConstraintArguments.m_jointType == eFixedType) { - if (childBody->m_multiBody) + if (childBody && childBody->m_multiBody) { btMultiBodyFixedConstraint* multibodyFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,childBody->m_multiBody,clientCmd.m_userConstraintArguments.m_childJointIndex,pivotInParent,pivotInChild,frameInParent,frameInChild); multibodyFixed->setMaxAppliedImpulse(500.0); @@ -3485,7 +3486,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } else { - btMultiBodyFixedConstraint* rigidbodyFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,childBody->m_rigidBody,pivotInParent,pivotInChild,frameInParent,frameInChild); + btRigidBody* rb = childBody? childBody->m_rigidBody : 0; + btMultiBodyFixedConstraint* rigidbodyFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,rb,pivotInParent,pivotInChild,frameInParent,frameInChild); rigidbodyFixed->setMaxAppliedImpulse(500.0); btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld; world->addMultiBodyConstraint(rigidbodyFixed); @@ -3500,7 +3502,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } else if (clientCmd.m_userConstraintArguments.m_jointType == ePrismaticType) { - if (childBody->m_multiBody) + if (childBody && childBody->m_multiBody) { btMultiBodySliderConstraint* multibodySlider = new btMultiBodySliderConstraint(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,childBody->m_multiBody,clientCmd.m_userConstraintArguments.m_childJointIndex,pivotInParent,pivotInChild,frameInParent,frameInChild,jointAxis); multibodySlider->setMaxAppliedImpulse(500.0); @@ -3514,7 +3516,9 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } else { - btMultiBodySliderConstraint* rigidbodySlider = new btMultiBodySliderConstraint(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,childBody->m_rigidBody,pivotInParent,pivotInChild,frameInParent,frameInChild,jointAxis); + btRigidBody* rb = childBody? childBody->m_rigidBody : 0; + + btMultiBodySliderConstraint* rigidbodySlider = new btMultiBodySliderConstraint(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,rb,pivotInParent,pivotInChild,frameInParent,frameInChild,jointAxis); rigidbodySlider->setMaxAppliedImpulse(500.0); btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld; world->addMultiBodyConstraint(rigidbodySlider); @@ -3528,7 +3532,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } else if (clientCmd.m_userConstraintArguments.m_jointType == ePoint2PointType) { - if (childBody->m_multiBody) + if (childBody && childBody->m_multiBody) { btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,childBody->m_multiBody,clientCmd.m_userConstraintArguments.m_childJointIndex,pivotInParent,pivotInChild); p2p->setMaxAppliedImpulse(500); @@ -3542,7 +3546,9 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } else { - btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,childBody->m_rigidBody,pivotInParent,pivotInChild); + btRigidBody* rb = childBody? childBody->m_rigidBody : 0; + + btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,rb,pivotInParent,pivotInChild); p2p->setMaxAppliedImpulse(500); btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld; world->addMultiBodyConstraint(p2p); @@ -3564,6 +3570,41 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } } + if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_CONSTRAINT) + { + int userConstraintUidRemove = clientCmd.m_userConstraintArguments.m_userConstraintUniqueId; + InteralUserConstraintData* userConstraintPtr = m_data->m_userConstraints.find(userConstraintUidRemove); + if (userConstraintPtr) + { + if (userConstraintPtr->m_mbConstraint) + { + if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_PIVOT_IN_B) + { + btVector3 pivotInB(clientCmd.m_userConstraintArguments.m_childFrame[0], + clientCmd.m_userConstraintArguments.m_childFrame[1], + clientCmd.m_userConstraintArguments.m_childFrame[2]); + + userConstraintPtr->m_mbConstraint->setPivotInB(pivotInB); + } + if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_FRAME_ORN_IN_B) + { + btQuaternion childFrameOrn(clientCmd.m_userConstraintArguments.m_childFrame[3], + clientCmd.m_userConstraintArguments.m_childFrame[4], + clientCmd.m_userConstraintArguments.m_childFrame[5], + clientCmd.m_userConstraintArguments.m_childFrame[6]); + + btMatrix3x3 childFrameBasis(childFrameOrn); + userConstraintPtr->m_mbConstraint->setFrameInB(childFrameBasis); + } + } + if (userConstraintPtr->m_rbConstraint) + { + + } + serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = -1; + serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED; + } + } if (clientCmd.m_updateFlags & USER_CONSTRAINT_REMOVE_CONSTRAINT) { int userConstraintUidRemove = clientCmd.m_userConstraintArguments.m_userConstraintUniqueId; diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index cd22178f3..061c9f7ba 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -535,7 +535,9 @@ enum EnumUserConstraintFlags { USER_CONSTRAINT_ADD_CONSTRAINT=1, USER_CONSTRAINT_REMOVE_CONSTRAINT=2, - USER_CONSTRAINT_CHANGE_CONSTRAINT=4 + USER_CONSTRAINT_CHANGE_CONSTRAINT=4, + USER_CONSTRAINT_CHANGE_PIVOT_IN_B=8, + USER_CONSTRAINT_CHANGE_FRAME_ORN_IN_B=16, }; struct UserConstraintArgs diff --git a/examples/pybullet/constraint.py b/examples/pybullet/constraint.py new file mode 100644 index 000000000..53f1a5505 --- /dev/null +++ b/examples/pybullet/constraint.py @@ -0,0 +1,24 @@ +import pybullet as p +import time +import math + +p.connect(p.GUI) + +p.loadURDF("plane.urdf") +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]) + +a=-math.pi +while 1: + a=a+0.01 + if (a>math.pi): + a=-math.pi + time.sleep(.01) + p.setGravity(0,0,-10) + pivot=[a,0,1] + orn = p.getQuaternionFromEuler([a,0,0]) + p.changeConstraint(cid,pivot,jointChildFrameOrientation=orn) + +p.removeConstraint(cid) \ No newline at end of file diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index def84f666..d9f6156af 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -3028,6 +3028,51 @@ static PyObject* pybullet_getClosestPointData(PyObject* self, PyObject* args, Py return Py_None; } + +static PyObject* pybullet_changeUserConstraint(PyObject* self, PyObject* args, PyObject *keywds) +{ + static char *kwlist[] = { "userConstraintUniqueId","jointChildPivot", "jointChildFrameOrientation", "physicsClientId", NULL}; + int userConstraintUniqueId=-1; + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + int physicsClientId = 0; + b3PhysicsClientHandle sm = 0; + PyObject* jointChildPivotObj=0; + PyObject* jointChildFrameOrnObj=0; + double jointChildPivot[3]; + double jointChildFrameOrn[4]; + + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|OOi", kwlist,&userConstraintUniqueId,&jointChildPivotObj, &jointChildFrameOrnObj,&physicsClientId)) + { + return NULL; + } + + sm = getPhysicsClient(physicsClientId); + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + commandHandle = b3InitChangeUserConstraintCommand(sm,userConstraintUniqueId); + + if (pybullet_internalSetVectord(jointChildPivotObj,jointChildPivot)) + { + b3InitChangeUserConstraintSetPivotInB(commandHandle, jointChildPivot); + } + if (pybullet_internalSetVector4d(jointChildFrameOrnObj,jointChildFrameOrn)) + { + b3InitChangeUserConstraintSetFrameInB(commandHandle, jointChildFrameOrn); + } + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusType = b3GetStatusType(statusHandle); + Py_INCREF(Py_None); + return Py_None; +}; + + static PyObject* pybullet_removeUserConstraint(PyObject* self, PyObject* args, PyObject *keywds) { static char *kwlist[] = { "userConstraintUniqueId","physicsClientId", NULL}; @@ -4372,6 +4417,11 @@ static PyMethodDef SpamMethods[] = { "Create a constraint between two bodies. Returns a (int) unique id, if successfull." }, + {"changeConstraint", (PyCFunction)pybullet_changeUserConstraint, METH_VARARGS | METH_KEYWORDS, + "Change some parameters of an existing constraint, such as the child pivot or child frame orientation, using its unique id." + }, + + {"removeConstraint", (PyCFunction)pybullet_removeUserConstraint, METH_VARARGS | METH_KEYWORDS, "Remove a constraint using its unique id." }, diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h index 74c6f5a81..c219b4376 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h @@ -90,6 +90,10 @@ public: void updateJacobianSizes(); void allocateJacobiansMultiDof(); + //many constraints have setFrameInB/setPivotInB. Will use 'getConstraintType' later. + virtual void setFrameInB(const btMatrix3x3& frameInB) {} + virtual void setPivotInB(const btVector3& pivotInB){} + virtual void finalizeMultiDof()=0; virtual int getIslandIdA() const =0; diff --git a/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h b/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h index 26e28a74e..036025136 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h @@ -62,7 +62,7 @@ public: return m_pivotInB; } - void setPivotInB(const btVector3& pivotInB) + virtual void setPivotInB(const btVector3& pivotInB) { m_pivotInB = pivotInB; } @@ -82,7 +82,7 @@ public: return m_frameInB; } - void setFrameInB(const btMatrix3x3& frameInB) + virtual void setFrameInB(const btMatrix3x3& frameInB) { m_frameInB = frameInB; } diff --git a/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h b/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h index b2e219ac1..ea3d85abf 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h @@ -53,11 +53,12 @@ public: return m_pivotInB; } - void setPivotInB(const btVector3& pivotInB) + virtual void setPivotInB(const btVector3& pivotInB) { m_pivotInB = pivotInB; } + virtual void debugDraw(class btIDebugDraw* drawer); }; diff --git a/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.h b/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.h index 571dcd53b..0a6cf3df1 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.h +++ b/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.h @@ -63,7 +63,7 @@ public: return m_pivotInB; } - void setPivotInB(const btVector3& pivotInB) + virtual void setPivotInB(const btVector3& pivotInB) { m_pivotInB = pivotInB; } @@ -83,7 +83,7 @@ public: return m_frameInB; } - void setFrameInB(const btMatrix3x3& frameInB) + virtual void setFrameInB(const btMatrix3x3& frameInB) { m_frameInB = frameInB; }