diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index b4f797e44..be798c68f 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -960,7 +960,9 @@ int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int jointInd return cl->getJointInfo(bodyIndex, jointIndex, *info); } -b3SharedMemoryCommandHandle b3CreateJoint(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info) + + +b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info) { PhysicsClient* cl = (PhysicsClient* ) physClient; b3Assert(cl); @@ -968,22 +970,52 @@ b3SharedMemoryCommandHandle b3CreateJoint(b3PhysicsClientHandle physClient, int struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); b3Assert(command); - command->m_type = CMD_CREATE_JOINT; - command->m_createJointArguments.m_parentBodyIndex = parentBodyIndex; - command->m_createJointArguments.m_parentJointIndex = parentJointIndex; - command->m_createJointArguments.m_childBodyIndex = childBodyIndex; - command->m_createJointArguments.m_childJointIndex = childJointIndex; + command->m_type = CMD_USER_CONSTRAINT; + command->m_updateFlags = USER_CONSTRAINT_ADD_CONSTRAINT; + + command->m_userConstraintArguments.m_parentBodyIndex = parentBodyIndex; + command->m_userConstraintArguments.m_parentJointIndex = parentJointIndex; + command->m_userConstraintArguments.m_childBodyIndex = childBodyIndex; + command->m_userConstraintArguments.m_childJointIndex = childJointIndex; for (int i = 0; i < 7; ++i) { - command->m_createJointArguments.m_parentFrame[i] = info->m_parentFrame[i]; - command->m_createJointArguments.m_childFrame[i] = info->m_childFrame[i]; + command->m_userConstraintArguments.m_parentFrame[i] = info->m_parentFrame[i]; + command->m_userConstraintArguments.m_childFrame[i] = info->m_childFrame[i]; } for (int i = 0; i < 3; ++i) { - command->m_createJointArguments.m_jointAxis[i] = info->m_jointAxis[i]; + command->m_userConstraintArguments.m_jointAxis[i] = info->m_jointAxis[i]; } - command->m_createJointArguments.m_jointType = info->m_jointType; + command->m_userConstraintArguments.m_jointType = info->m_jointType; return (b3SharedMemoryCommandHandle)command; } +b3SharedMemoryCommandHandle b3InitRemoveUserConstraintCommand(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_REMOVE_CONSTRAINT; + command->m_userConstraintArguments.m_userConstraintUniqueId = userConstraintUniqueId; + return (b3SharedMemoryCommandHandle)command; +} +int b3GetStatusUserConstraintUniqueId(b3SharedMemoryStatusHandle statusHandle) +{ + const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle; + b3Assert(status); + b3Assert(status->m_type == CMD_USER_CONSTRAINT_COMPLETED); + if (status && status->m_type == CMD_USER_CONSTRAINT_COMPLETED) + { + return status->m_userConstraintResultArgs.m_userConstraintUniqueId; + } + + return -1; + +} + + b3SharedMemoryCommandHandle b3PickBody(b3PhysicsClientHandle physClient, double rayFromWorldX, double rayFromWorldY, double rayFromWorldZ, double rayToWorldX, double rayToWorldY, double rayToWorldZ) diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index e82b6824b..18eba1c0a 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -72,7 +72,9 @@ int b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyIndex); ///given a body and joint index, return the joint information. See b3JointInfo in SharedMemoryPublic.h int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int jointIndex, struct b3JointInfo* info); -b3SharedMemoryCommandHandle b3CreateJoint(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info); +b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info); +int b3GetStatusUserConstraintUniqueId(b3SharedMemoryStatusHandle statusHandle); +b3SharedMemoryCommandHandle b3InitRemoveUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId); ///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 e2b072b44..9a323933a 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -731,7 +731,15 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { b3Warning("User debug draw failed"); break; } - + case CMD_USER_CONSTRAINT_COMPLETED: + { + break; + } + case CMD_USER_CONSTRAINT_FAILED: + { + b3Warning("createConstraint failed"); + break; + } default: { b3Error("Unknown server status %d\n", serverCmd.m_type); btAssert(0); diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp index 011204e80..cb7a16abd 100644 --- a/examples/SharedMemory/PhysicsDirect.cpp +++ b/examples/SharedMemory/PhysicsDirect.cpp @@ -671,10 +671,18 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd { break; } - + case CMD_USER_CONSTRAINT_COMPLETED: + { + break; + } + case CMD_USER_CONSTRAINT_FAILED: + { + b3Warning("createConstraint failed"); + break; + } default: { - b3Warning("Unknown server status type"); + //b3Warning("Unknown server status type"); } }; diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index b51d5c736..0a2cc253e 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -123,6 +123,17 @@ struct InteralBodyData } }; +struct InteralUserConstraintData +{ + btTypedConstraint* m_rbConstraint; + btMultiBodyConstraint* m_mbConstraint; + InteralUserConstraintData() + :m_rbConstraint(0), + m_mbConstraint(0) + { + } +}; + ///todo: templatize this struct InternalBodyHandle : public InteralBodyData { @@ -468,6 +479,10 @@ struct PhysicsServerCommandProcessorInternalData btAlignedObjectArray m_multiBodyJointFeedbacks; btHashMap m_inverseDynamicsBodies; btHashMap m_inverseKinematicsHelpers; + + int m_userConstraintUIDGenerator; + btHashMap m_userConstraints; + b3AlignedObjectArray m_saveWorldBodyData; @@ -532,6 +547,7 @@ struct PhysicsServerCommandProcessorInternalData m_logPlayback(0), m_physicsDeltaTime(1./240.), m_numSimulationSubSteps(0), + m_userConstraintUIDGenerator(1), m_dynamicsWorld(0), m_remoteDebugDrawer(0), m_guiHelper(0), @@ -3097,72 +3113,144 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm hasStatus = true; break; } - case CMD_CREATE_JOINT: + case CMD_USER_CONSTRAINT: { - InteralBodyData* parentBody = m_data->getHandle(clientCmd.m_createJointArguments.m_parentBodyIndex); - if (parentBody && parentBody->m_multiBody) - { - InteralBodyData* childBody = m_data->getHandle(clientCmd.m_createJointArguments.m_childBodyIndex); - if (childBody) - { - btVector3 pivotInParent(clientCmd.m_createJointArguments.m_parentFrame[0], clientCmd.m_createJointArguments.m_parentFrame[1], clientCmd.m_createJointArguments.m_parentFrame[2]); - btVector3 pivotInChild(clientCmd.m_createJointArguments.m_childFrame[0], clientCmd.m_createJointArguments.m_childFrame[1], clientCmd.m_createJointArguments.m_childFrame[2]); - btMatrix3x3 frameInParent(btQuaternion(clientCmd.m_createJointArguments.m_parentFrame[3], clientCmd.m_createJointArguments.m_parentFrame[4], clientCmd.m_createJointArguments.m_parentFrame[5], clientCmd.m_createJointArguments.m_parentFrame[6])); - btMatrix3x3 frameInChild(btQuaternion(clientCmd.m_createJointArguments.m_childFrame[3], clientCmd.m_createJointArguments.m_childFrame[4], clientCmd.m_createJointArguments.m_childFrame[5], clientCmd.m_createJointArguments.m_childFrame[6])); - btVector3 jointAxis(clientCmd.m_createJointArguments.m_jointAxis[0], clientCmd.m_createJointArguments.m_jointAxis[1], clientCmd.m_createJointArguments.m_jointAxis[2]); - if (clientCmd.m_createJointArguments.m_jointType == eFixedType) - { - if (childBody->m_multiBody) - { - btMultiBodyFixedConstraint* multibodyFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody,clientCmd.m_createJointArguments.m_parentJointIndex,childBody->m_multiBody,clientCmd.m_createJointArguments.m_childJointIndex,pivotInParent,pivotInChild,frameInParent,frameInChild); - multibodyFixed->setMaxAppliedImpulse(500.0); - m_data->m_dynamicsWorld->addMultiBodyConstraint(multibodyFixed); - } - else - { - btMultiBodyFixedConstraint* rigidbodyFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody,clientCmd.m_createJointArguments.m_parentJointIndex,childBody->m_rigidBody,pivotInParent,pivotInChild,frameInParent,frameInChild); - rigidbodyFixed->setMaxAppliedImpulse(500.0); - btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld; - world->addMultiBodyConstraint(rigidbodyFixed); - } - } - else if (clientCmd.m_createJointArguments.m_jointType == ePrismaticType) - { - if (childBody->m_multiBody) - { - btMultiBodySliderConstraint* multibodySlider = new btMultiBodySliderConstraint(parentBody->m_multiBody,clientCmd.m_createJointArguments.m_parentJointIndex,childBody->m_multiBody,clientCmd.m_createJointArguments.m_childJointIndex,pivotInParent,pivotInChild,frameInParent,frameInChild,jointAxis); - multibodySlider->setMaxAppliedImpulse(500.0); - m_data->m_dynamicsWorld->addMultiBodyConstraint(multibodySlider); - } - else - { - btMultiBodySliderConstraint* rigidbodySlider = new btMultiBodySliderConstraint(parentBody->m_multiBody,clientCmd.m_createJointArguments.m_parentJointIndex,childBody->m_rigidBody,pivotInParent,pivotInChild,frameInParent,frameInChild,jointAxis); - rigidbodySlider->setMaxAppliedImpulse(500.0); - btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld; - world->addMultiBodyConstraint(rigidbodySlider); - } - } else if (clientCmd.m_createJointArguments.m_jointType == ePoint2PointType) - { - if (childBody->m_multiBody) - { - btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(parentBody->m_multiBody,clientCmd.m_createJointArguments.m_parentJointIndex,childBody->m_multiBody,clientCmd.m_createJointArguments.m_childJointIndex,pivotInParent,pivotInChild); - p2p->setMaxAppliedImpulse(500); - m_data->m_dynamicsWorld->addMultiBodyConstraint(p2p); - } - else - { - btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(parentBody->m_multiBody,clientCmd.m_createJointArguments.m_parentJointIndex,childBody->m_rigidBody,pivotInParent,pivotInChild); - p2p->setMaxAppliedImpulse(500); - btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld; - world->addMultiBodyConstraint(p2p); - } - } - - } - } SharedMemoryStatus& serverCmd =serverStatusOut; - serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED; + serverCmd.m_type = CMD_USER_CONSTRAINT_FAILED; hasStatus = true; + + if (clientCmd.m_updateFlags & USER_CONSTRAINT_ADD_CONSTRAINT) + { + 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) + { + 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]); + btMatrix3x3 frameInParent(btQuaternion(clientCmd.m_userConstraintArguments.m_parentFrame[3], clientCmd.m_userConstraintArguments.m_parentFrame[4], clientCmd.m_userConstraintArguments.m_parentFrame[5], clientCmd.m_userConstraintArguments.m_parentFrame[6])); + btMatrix3x3 frameInChild(btQuaternion(clientCmd.m_userConstraintArguments.m_childFrame[3], clientCmd.m_userConstraintArguments.m_childFrame[4], clientCmd.m_userConstraintArguments.m_childFrame[5], clientCmd.m_userConstraintArguments.m_childFrame[6])); + 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) + { + 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); + m_data->m_dynamicsWorld->addMultiBodyConstraint(multibodyFixed); + InteralUserConstraintData userConstraintData; + userConstraintData.m_mbConstraint = multibodyFixed; + int uid = m_data->m_userConstraintUIDGenerator++; + m_data->m_userConstraints.insert(uid,userConstraintData); + serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid; + serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED; + + } + else + { + btMultiBodyFixedConstraint* rigidbodyFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,childBody->m_rigidBody,pivotInParent,pivotInChild,frameInParent,frameInChild); + rigidbodyFixed->setMaxAppliedImpulse(500.0); + btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld; + world->addMultiBodyConstraint(rigidbodyFixed); + InteralUserConstraintData userConstraintData; + userConstraintData.m_mbConstraint = rigidbodyFixed; + int uid = m_data->m_userConstraintUIDGenerator++; + m_data->m_userConstraints.insert(uid,userConstraintData); + serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid; + serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED; + } + + } + else if (clientCmd.m_userConstraintArguments.m_jointType == ePrismaticType) + { + if (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); + m_data->m_dynamicsWorld->addMultiBodyConstraint(multibodySlider); + InteralUserConstraintData userConstraintData; + userConstraintData.m_mbConstraint = multibodySlider; + int uid = m_data->m_userConstraintUIDGenerator++; + m_data->m_userConstraints.insert(uid,userConstraintData); + serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid; + serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED; + } + else + { + btMultiBodySliderConstraint* rigidbodySlider = new btMultiBodySliderConstraint(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,childBody->m_rigidBody,pivotInParent,pivotInChild,frameInParent,frameInChild,jointAxis); + rigidbodySlider->setMaxAppliedImpulse(500.0); + btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld; + world->addMultiBodyConstraint(rigidbodySlider); + InteralUserConstraintData userConstraintData; + userConstraintData.m_mbConstraint = rigidbodySlider; + int uid = m_data->m_userConstraintUIDGenerator++; + m_data->m_userConstraints.insert(uid,userConstraintData); + serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid; + serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED; + } + + } else if (clientCmd.m_userConstraintArguments.m_jointType == ePoint2PointType) + { + if (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); + m_data->m_dynamicsWorld->addMultiBodyConstraint(p2p); + InteralUserConstraintData userConstraintData; + userConstraintData.m_mbConstraint = p2p; + int uid = m_data->m_userConstraintUIDGenerator++; + m_data->m_userConstraints.insert(uid,userConstraintData); + serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid; + serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED; + } + else + { + btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,childBody->m_rigidBody,pivotInParent,pivotInChild); + p2p->setMaxAppliedImpulse(500); + btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld; + world->addMultiBodyConstraint(p2p); + InteralUserConstraintData userConstraintData; + userConstraintData.m_mbConstraint = p2p; + int uid = m_data->m_userConstraintUIDGenerator++; + m_data->m_userConstraints.insert(uid,userConstraintData); + serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid; + serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED; + } + + } else + { + b3Warning("unknown constraint type"); + } + + + } + } + } + + if (clientCmd.m_updateFlags & USER_CONSTRAINT_REMOVE_CONSTRAINT) + { + int userConstraintUidRemove = clientCmd.m_userConstraintArguments.m_userConstraintUniqueId; + InteralUserConstraintData* userConstraintPtr = m_data->m_userConstraints.find(userConstraintUidRemove); + if (userConstraintPtr) + { + if (userConstraintPtr->m_mbConstraint) + { + m_data->m_dynamicsWorld->removeMultiBodyConstraint(userConstraintPtr->m_mbConstraint); + delete userConstraintPtr->m_mbConstraint; + m_data->m_userConstraints.remove(userConstraintUidRemove); + } + if (userConstraintPtr->m_rbConstraint) + { + + } + serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = -1; + serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED; + } + + + } + break; } case CMD_CALCULATE_INVERSE_KINEMATICS: diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 69b7916a1..715bdd11d 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -488,7 +488,14 @@ struct CalculateInverseKinematicsResultArgs double m_jointPositions[MAX_DEGREE_OF_FREEDOM]; }; -struct CreateJointArgs +enum EnumUserConstraintFlags +{ + USER_CONSTRAINT_ADD_CONSTRAINT=1, + USER_CONSTRAINT_REMOVE_CONSTRAINT=2, + USER_CONSTRAINT_CHANGE_CONSTRAINT=4 +}; + +struct UserConstraintArgs { int m_parentBodyIndex; int m_parentJointIndex; @@ -498,9 +505,13 @@ struct CreateJointArgs double m_childFrame[7]; double m_jointAxis[3]; int m_jointType; + int m_userConstraintUniqueId; }; - +struct UserConstraintResultArgs +{ + int m_userConstraintUniqueId; +}; enum EnumUserDebugDrawFlags { @@ -563,7 +574,7 @@ struct SharedMemoryCommand struct ExternalForceArgs m_externalForceArguments; struct CalculateInverseDynamicsArgs m_calculateInverseDynamicsArguments; struct CalculateJacobianArgs m_calculateJacobianArguments; - struct CreateJointArgs m_createJointArguments; + struct UserConstraintArgs m_userConstraintArguments; struct RequestContactDataArgs m_requestContactPointArguments; struct RequestOverlappingObjectsArgs m_requestOverlappingObjectsArgs; struct RequestVisualShapeDataArgs m_requestVisualShapeDataArguments; @@ -620,6 +631,7 @@ struct SharedMemoryStatus struct CalculateInverseKinematicsResultArgs m_inverseKinematicsResultArgs; struct SendVisualShapeDataArgs m_sendVisualShapeArgs; struct UserDebugDrawResultArgs m_userDebugDrawArgs; + struct UserConstraintResultArgs m_userConstraintResultArgs; }; }; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 1c2e5835a..d6a8e246a 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -33,7 +33,7 @@ enum EnumSharedMemoryClientCommand CMD_CALCULATE_INVERSE_DYNAMICS, CMD_CALCULATE_INVERSE_KINEMATICS, CMD_CALCULATE_JACOBIAN, - CMD_CREATE_JOINT, + CMD_USER_CONSTRAINT, CMD_REQUEST_CONTACT_POINT_INFORMATION, CMD_REQUEST_AABB_OVERLAP, CMD_SAVE_WORLD, @@ -104,6 +104,8 @@ enum EnumSharedMemoryServerStatus CMD_LOAD_TEXTURE_FAILED, CMD_USER_DEBUG_DRAW_COMPLETED, CMD_USER_DEBUG_DRAW_FAILED, + CMD_USER_CONSTRAINT_COMPLETED, + CMD_USER_CONSTRAINT_FAILED, //don't go beyond 'CMD_MAX_SERVER_COMMANDS! CMD_MAX_SERVER_COMMANDS }; diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 79153c09a..47aaee9d9 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -263,7 +263,8 @@ static PyObject* pybullet_saveBullet(PyObject* self, PyObject* args) } command = b3SaveBulletCommandInit(sm, bulletFileName); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); - if (statusHandle != CMD_BULLET_SAVING_COMPLETED) + statusType = b3GetStatusType(statusHandle); + if (statusType != CMD_BULLET_SAVING_COMPLETED) { PyErr_SetString(SpamError, "Couldn't save .bullet file."); return NULL; @@ -291,7 +292,8 @@ static PyObject* pybullet_loadMJCF(PyObject* self, PyObject* args) } command = b3LoadMJCFCommandInit(sm, mjcfjFileName); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); - if (statusHandle != CMD_MJCF_LOADING_COMPLETED) + statusType = b3GetStatusType(statusHandle); + if (statusType != CMD_MJCF_LOADING_COMPLETED) { PyErr_SetString(SpamError, "Couldn't load .mjcf file."); return NULL; @@ -1433,7 +1435,7 @@ static int pybullet_internalSetVectord(PyObject* obVec, double vector[3]) { } // vector - double[3] which will be set by values from obVec -static int pybullet_internalSetVector4(PyObject* obVec, double vector[4]) { +static int pybullet_internalSetVector4d(PyObject* obVec, double vector[4]) { int i, len; PyObject* seq; if (obVec==NULL) @@ -1459,7 +1461,6 @@ static PyObject* pybullet_addUserDebugText(PyObject* self, PyObject* args, PyObj int size = PySequence_Size(args); b3SharedMemoryCommandHandle commandHandle; - struct b3ContactInformation contactPointData; b3SharedMemoryStatusHandle statusHandle; int statusType; int res = 0; @@ -1526,7 +1527,6 @@ static PyObject* pybullet_addUserDebugLine(PyObject* self, PyObject* args, PyObj int size = PySequence_Size(args); b3SharedMemoryCommandHandle commandHandle; - struct b3ContactInformation contactPointData; b3SharedMemoryStatusHandle statusHandle; int statusType; int res = 0; @@ -1594,7 +1594,6 @@ static PyObject* pybullet_addUserDebugLine(PyObject* self, PyObject* args, PyObj static PyObject* pybullet_removeUserDebugItem(PyObject* self, PyObject* args, PyObject *keywds) { b3SharedMemoryCommandHandle commandHandle; - struct b3ContactInformation contactPointData; b3SharedMemoryStatusHandle statusHandle; int statusType; int itemUniqueId; @@ -1621,7 +1620,6 @@ static PyObject* pybullet_removeUserDebugItem(PyObject* self, PyObject* args, Py static PyObject* pybullet_removeAllUserDebugItems(PyObject* self, PyObject* args, PyObject *keywds) { b3SharedMemoryCommandHandle commandHandle; - struct b3ContactInformation contactPointData; b3SharedMemoryStatusHandle statusHandle; int statusType; @@ -2016,6 +2014,144 @@ static PyObject* pybullet_getClosestPointData(PyObject* self, PyObject* args, Py return Py_None; } +static PyObject* pybullet_removeUserConstraint(PyObject* self, PyObject* args, PyObject *keywds) +{ + static char *kwlist[] = { "userConstraintUniqueId",NULL}; + int userConstraintUniqueId=-1; + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + + + if (0 == sm) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i", kwlist,&userConstraintUniqueId)) + { + return NULL; + } + + commandHandle = b3InitRemoveUserConstraintCommand(sm,userConstraintUniqueId); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusType = b3GetStatusType(statusHandle); + Py_INCREF(Py_None); + return Py_None; +}; + + +/* +static PyObject* pybullet_updateUserConstraint(PyObject* self, PyObject* args, PyObject *keywds) +{ + return NULL; +} +*/ + +static PyObject* pybullet_createUserConstraint(PyObject* self, PyObject* args, PyObject *keywds) +{ + int size = PySequence_Size(args); + int bodyUniqueIdA = -1; + int bodyUniqueIdB = -1; + + b3SharedMemoryCommandHandle commandHandle; + int parentBodyUniqueId=-1; + int parentLinkIndex=-1; + int childBodyUniqueId=-1; + int childLinkIndex=-1; + int jointType=ePoint2PointType; + PyObject* jointAxisObj=0; + double jointAxis[3]={0,0,0}; + PyObject* parentFramePositionObj = 0; + double parentFramePosition[3]={0,0,0}; + PyObject* childFramePositionObj = 0; + double childFramePosition[3]={0,0,0}; + PyObject* parentFrameOrientationObj = 0; + double parentFrameOrientation[4]={0,0,0,1}; + PyObject* childFrameOrientationObj = 0; + double childFrameOrientation[4]={0,0,0,1}; + + struct b3JointInfo jointInfo; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + PyObject* pyResultList = 0; + + + static char *kwlist[] = { "parentBodyUniqueId", "parentLinkIndex", + "childBodyUniqueId", "childLinkIndex", + "jointType", + "jointAxis", + "parentFramePosition", + "childFramePosition", + "parentFrameOrientation", + "childFrameOrientation", + NULL }; + + if (0 == sm) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + if (!PyArg_ParseTupleAndKeywords(args, keywds, "iiiiiOOO|OO", kwlist,&parentBodyUniqueId,&parentLinkIndex, + &childBodyUniqueId,&childLinkIndex, + &jointType,&jointAxisObj, + &parentFramePositionObj, + &childFramePositionObj, + &parentFrameOrientationObj, + &childFrameOrientationObj + )) + { + return NULL; + } + + pybullet_internalSetVectord(jointAxisObj,jointAxis); + pybullet_internalSetVectord(parentFramePositionObj,parentFramePosition); + pybullet_internalSetVectord(childFramePositionObj,childFramePosition); + pybullet_internalSetVector4d(parentFrameOrientationObj,parentFrameOrientation); + pybullet_internalSetVector4d(childFrameOrientationObj,childFrameOrientation); + + jointInfo.m_jointType = jointType; + jointInfo.m_parentFrame[0] = parentFramePosition[0]; + jointInfo.m_parentFrame[1] = parentFramePosition[1]; + jointInfo.m_parentFrame[2] = parentFramePosition[2]; + jointInfo.m_parentFrame[3] = parentFrameOrientation[0]; + jointInfo.m_parentFrame[4] = parentFrameOrientation[1]; + jointInfo.m_parentFrame[5] = parentFrameOrientation[2]; + jointInfo.m_parentFrame[6] = parentFrameOrientation[3]; + + jointInfo.m_childFrame[0] = childFramePosition[0]; + jointInfo.m_childFrame[1] = childFramePosition[1]; + jointInfo.m_childFrame[2] = childFramePosition[2]; + jointInfo.m_childFrame[3] = childFrameOrientation[0]; + jointInfo.m_childFrame[4] = childFrameOrientation[1]; + jointInfo.m_childFrame[5] = childFrameOrientation[2]; + jointInfo.m_childFrame[6] = childFrameOrientation[3]; + + jointInfo.m_jointAxis[0] = jointAxis[0]; + jointInfo.m_jointAxis[1] = jointAxis[1]; + jointInfo.m_jointAxis[2] = jointAxis[2]; + + commandHandle = b3InitCreateUserConstraintCommand(sm, parentBodyUniqueId, parentLinkIndex, childBodyUniqueId, childLinkIndex, &jointInfo); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusType = b3GetStatusType(statusHandle); + if (statusType==CMD_USER_CONSTRAINT_COMPLETED) + { + int userConstraintUid = b3GetStatusUserConstraintUniqueId(statusHandle); + PyObject* ob = PyLong_FromLong(userConstraintUid); + return ob; + } else + { + PyErr_SetString(SpamError, "createConstraint failed."); + return NULL; + } + + Py_INCREF(Py_None); + return Py_None; + +} static PyObject* pybullet_getContactPointData(PyObject* self, PyObject* args, PyObject *keywds) { int size = PySequence_Size(args); @@ -2594,7 +2730,7 @@ static PyObject* pybullet_calculateInverseKinematicsKuka(PyObject* self, double pos[3]; double ori[4]={0,1.0,0,0}; - if (pybullet_internalSetVectord(targetPosObj,pos) && pybullet_internalSetVector4(targetOrnObj,ori)) + if (pybullet_internalSetVectord(targetPosObj,pos) && pybullet_internalSetVector4d(targetOrnObj,ori)) { b3SharedMemoryStatusHandle statusHandle; int numPos=0; @@ -2820,6 +2956,14 @@ static PyMethodDef SpamMethods[] = { { "loadMJCF", pybullet_loadMJCF, METH_VARARGS, "Load multibodies from an MJCF file." }, + {"createConstraint", (PyCFunction)pybullet_createUserConstraint, METH_VARARGS | METH_KEYWORDS, + "Create a constraint between two bodies. Returns a (int) unique id, if successfull." + }, + + {"removeConstraint", (PyCFunction)pybullet_removeUserConstraint, METH_VARARGS | METH_KEYWORDS, + "Remove a constraint using its unique id." + }, + {"saveWorld", pybullet_saveWorld, METH_VARARGS, "Save a approximate Python file to reproduce the current state of the world: saveWorld" "(filename). (very preliminary and approximately)"}, diff --git a/examples/pybullet/quadruped.py b/examples/pybullet/quadruped.py new file mode 100644 index 000000000..a1062f3eb --- /dev/null +++ b/examples/pybullet/quadruped.py @@ -0,0 +1,35 @@ +import pybullet as p +p.connect(p.GUI) +p.loadURDF("plane.urdf") +p.loadURDF("quadruped/quadruped.urdf",0,0,0.2) +#p.getNumJoints(1) + +#right front leg +p.resetJointState(1,0,1.57) +p.resetJointState(1,2,-2.2) +p.resetJointState(1,3,-1.57) +p.resetJointState(1,5,2.2) +p.createConstraint(1,2,1,5,3,[0,0,0],[0,0.01,0.2],[0,-0.015,0.2]) + +#left front leg +p.resetJointState(1,6,1.57) +p.resetJointState(1,8,-2.2) +p.resetJointState(1,9,-1.57) +p.resetJointState(1,11,2.2) +p.createConstraint(1,8,1,11,3,[0,0,0],[0,-0.01,0.2],[0,0.015,0.2]) + +#right back leg +p.resetJointState(1,12,1.57) +p.resetJointState(1,14,-2.2) +p.resetJointState(1,15,-1.57) +p.resetJointState(1,17,2.2) +p.createConstraint(1,14,1,17,3,[0,0,0],[0,0.01,0.2],[0,-0.015,0.2]) + +#left back leg +p.resetJointState(1,18,1.57) +p.resetJointState(1,20,-2.2) +p.resetJointState(1,21,-1.57) +p.resetJointState(1,23,2.2) +p.createConstraint(1,20,1,23,3,[0,0,0],[0,-0.01,0.2],[0,0.015,0.2]) + + \ No newline at end of file