From 15cda751307424c522a805461f14b31f80b61976 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Wed, 30 Nov 2016 22:24:20 -0800 Subject: [PATCH] add in settings of URDF/SDF allow 'useMaximalCoordinates' and 'useFixedBase' in pybullet.loadURDF. enable split impulse for btRigidBody, even in btMultiBodyDynamicsWorld. allow initialization of velocity and apply force for btRigidBody in pybullet/shared memory API. process contact parameters in URDF also for btRigidBody (friction, restitution etc) add pybullet.setPhysicsEngineParameter with numSolverIterations, useSplitImpulse etc. --- .../Importers/ImportURDFDemo/URDF2Bullet.cpp | 52 ++++-- .../Importers/ImportURDFDemo/URDFJointTypes.h | 3 + .../Importers/ImportURDFDemo/UrdfParser.cpp | 25 +++ examples/SharedMemory/PhysicsClientC_API.cpp | 20 +++ examples/SharedMemory/PhysicsClientC_API.h | 3 + .../PhysicsServerCommandProcessor.cpp | 122 +++++++++++--- examples/SharedMemory/SharedMemoryCommands.h | 6 +- examples/pybullet/pybullet.c | 157 ++++++++++++++++-- .../Featherstone/btMultiBodyConstraint.cpp | 14 +- .../btMultiBodyConstraintSolver.cpp | 7 +- .../Featherstone/btMultiBodyDynamicsWorld.cpp | 2 +- 11 files changed, 354 insertions(+), 57 deletions(-) diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp index d9e58ef5a..7c0f2c86a 100644 --- a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp +++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp @@ -143,6 +143,34 @@ void InitURDF2BulletCache(const URDFImporterInterface& u2b, URDF2BulletCachedDat } +void processContactParameters(const URDFLinkContactInfo& contactInfo, btCollisionObject* col) +{ + if ((contactInfo.m_flags & URDF_CONTACT_HAS_LATERAL_FRICTION) != 0) + { + col->setFriction(contactInfo.m_lateralFriction); + } + if ((contactInfo.m_flags & URDF_CONTACT_HAS_RESTITUTION) != 0) + { + col->setRestitution(contactInfo.m_restitution); + } + + if ((contactInfo.m_flags & URDF_CONTACT_HAS_ROLLING_FRICTION) != 0) + { + col->setRollingFriction(contactInfo.m_rollingFriction); + } + if ((contactInfo.m_flags & URDF_CONTACT_HAS_SPINNING_FRICTION) != 0) + { + col->setSpinningFriction(contactInfo.m_spinningFriction); + } + if ((contactInfo.m_flags & URDF_CONTACT_HAS_STIFFNESS_DAMPING) != 0) + { + col->setContactStiffnessAndDamping(contactInfo.m_contactStiffness, contactInfo.m_contactDamping); + } +} + + + + void ConvertURDF2BulletInternal( const URDFImporterInterface& u2b, MultiBodyCreationInterface& creation, URDF2BulletCachedData& cache, int urdfLinkIndex, @@ -258,11 +286,18 @@ void ConvertURDF2BulletInternal( world1->addRigidBody(body); + compoundShape->setUserIndex(graphicsIndex); + URDFLinkContactInfo contactInfo; + u2b.getLinkContactInfo(urdfLinkIndex, contactInfo); + + processContactParameters(contactInfo, body); creation.createRigidBodyGraphicsInstance(urdfLinkIndex, body, color, graphicsIndex); cache.registerRigidBody(urdfLinkIndex, body, inertialFrameInWorldSpace, mass, localInertiaDiagonal, compoundShape, localInertialFrame); + + //untested: u2b.convertLinkVisualShapes2(urdfLinkIndex,pathPrefix,localInertialFrame,body); } else { @@ -413,22 +448,7 @@ void ConvertURDF2BulletInternal( URDFLinkContactInfo contactInfo; u2b.getLinkContactInfo(urdfLinkIndex,contactInfo); - if ((contactInfo.m_flags & URDF_CONTACT_HAS_LATERAL_FRICTION)!=0) - { - col->setFriction(contactInfo.m_lateralFriction); - } - if ((contactInfo.m_flags & URDF_CONTACT_HAS_ROLLING_FRICTION)!=0) - { - col->setRollingFriction(contactInfo.m_rollingFriction); - } - if ((contactInfo.m_flags & URDF_CONTACT_HAS_SPINNING_FRICTION)!=0) - { - col->setSpinningFriction(contactInfo.m_spinningFriction); - } - if ((contactInfo.m_flags & URDF_CONTACT_HAS_STIFFNESS_DAMPING)!=0) - { - col->setContactStiffnessAndDamping(contactInfo.m_contactStiffness,contactInfo.m_contactDamping); - } + processContactParameters(contactInfo, col); if (mbLinkIndex>=0) //???? double-check +/- 1 { diff --git a/examples/Importers/ImportURDFDemo/URDFJointTypes.h b/examples/Importers/ImportURDFDemo/URDFJointTypes.h index 88912cb1a..2bcb53214 100644 --- a/examples/Importers/ImportURDFDemo/URDFJointTypes.h +++ b/examples/Importers/ImportURDFDemo/URDFJointTypes.h @@ -22,6 +22,7 @@ enum URDF_LinkContactFlags URDF_CONTACT_HAS_STIFFNESS_DAMPING=16, URDF_CONTACT_HAS_ROLLING_FRICTION=32, URDF_CONTACT_HAS_SPINNING_FRICTION=64, + URDF_CONTACT_HAS_RESTITUTION=128, }; @@ -30,6 +31,7 @@ struct URDFLinkContactInfo btScalar m_lateralFriction; btScalar m_rollingFriction; btScalar m_spinningFriction; + btScalar m_restitution; btScalar m_inertiaScaling; btScalar m_contactCfm; btScalar m_contactErp; @@ -42,6 +44,7 @@ struct URDFLinkContactInfo :m_lateralFriction(0.5), m_rollingFriction(0), m_spinningFriction(0), + m_restitution(0), m_inertiaScaling(1), m_contactCfm(0), m_contactErp(0), diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.cpp b/examples/Importers/ImportURDFDemo/UrdfParser.cpp index 817cb0ce5..bcac5eec7 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.cpp +++ b/examples/Importers/ImportURDFDemo/UrdfParser.cpp @@ -672,6 +672,31 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *confi } } } + + { + TiXmlElement *restitution_xml = ci->FirstChildElement("restitution"); + if (restitution_xml) + { + if (m_parseSDF) + { + link.m_contactInfo.m_restitution = urdfLexicalCast(restitution_xml->GetText()); + link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_RESTITUTION; + } + else + { + if (!restitution_xml->Attribute("value")) + { + logger->reportError("Link/contact: restitution element must have value attribute"); + return false; + } + + link.m_contactInfo.m_restitution = urdfLexicalCast(restitution_xml->Attribute("value")); + link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_RESTITUTION; + + } + } + } + { TiXmlElement *spinning_xml = ci->FirstChildElement("spinning_friction"); if (spinning_xml) diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 6b3e97ae3..12bf1ce90 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -317,6 +317,26 @@ int b3PhysicsParamSetInternalSimFlags(b3SharedMemoryCommandHandle commandHan return 0; } +int b3PhysicsParamSetUseSplitImpulse(b3SharedMemoryCommandHandle commandHandle, int useSplitImpulse) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS); + + command->m_physSimParamArgs.m_useSplitImpulse = useSplitImpulse; + command->m_updateFlags |= SIM_PARAM_UPDATE_USE_SPLIT_IMPULSE; + return 0; +} + +int b3PhysicsParamSetSplitImpulsePenetrationThreshold(b3SharedMemoryCommandHandle commandHandle, double splitImpulsePenetrationThreshold) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS); + + command->m_physSimParamArgs.m_splitImpulsePenetrationThreshold = splitImpulsePenetrationThreshold; + command->m_updateFlags |= SIM_PARAM_UPDATE_SPLIT_IMPULSE_PENETRATION_THRESHOLD; + return 0; +} + int b3PhysicsParamSetNumSolverIterations(b3SharedMemoryCommandHandle commandHandle, int numSolverIterations) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 4ee0e394d..e32c55b87 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -163,6 +163,9 @@ int b3PhysicsParamSetDefaultContactERP(b3SharedMemoryCommandHandle commandHandle int b3PhysicsParamSetNumSubSteps(b3SharedMemoryCommandHandle commandHandle, int numSubSteps); int b3PhysicsParamSetRealTimeSimulation(b3SharedMemoryCommandHandle commandHandle, int enableRealTimeSimulation); int b3PhysicsParamSetNumSolverIterations(b3SharedMemoryCommandHandle commandHandle, int numSolverIterations); +int b3PhysicsParamSetUseSplitImpulse(b3SharedMemoryCommandHandle commandHandle, int useSplitImpulse); +int b3PhysicsParamSetSplitImpulsePenetrationThreshold(b3SharedMemoryCommandHandle commandHandle, double splitImpulsePenetrationThreshold); + //b3PhysicsParamSetInternalSimFlags is for internal/temporary/easter-egg/experimental demo purposes //Use at own risk: magic things may or my not happen when calling this API diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 5ffc08e1a..2872da72b 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -2338,6 +2338,17 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { m_data->m_dynamicsWorld->getSolverInfo().m_numIterations = clientCmd.m_physSimParamArgs.m_numSolverIterations; } + + if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_USE_SPLIT_IMPULSE) + { + m_data->m_dynamicsWorld->getSolverInfo().m_splitImpulse = clientCmd.m_physSimParamArgs.m_useSplitImpulse; + } + if (clientCmd.m_updateFlags &SIM_PARAM_UPDATE_SPLIT_IMPULSE_PENETRATION_THRESHOLD) + { + m_data->m_dynamicsWorld->getSolverInfo().m_splitImpulsePenetrationThreshold = clientCmd.m_physSimParamArgs.m_splitImpulsePenetrationThreshold; + } + + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_NUM_SIMULATION_SUB_STEPS) { m_data->m_numSimulationSubSteps = clientCmd.m_physSimParamArgs.m_numSimulationSubSteps; @@ -2363,27 +2374,50 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm int bodyUniqueId = clientCmd.m_initPoseArgs.m_bodyUniqueId; InteralBodyData* body = m_data->getHandle(bodyUniqueId); + btVector3 baseLinVel(0, 0, 0); + btVector3 baseAngVel(0, 0, 0); + + if (clientCmd.m_updateFlags & INIT_POSE_HAS_BASE_LINEAR_VELOCITY) + { + baseLinVel.setValue(clientCmd.m_initPoseArgs.m_initialStateQdot[0], + clientCmd.m_initPoseArgs.m_initialStateQdot[1], + clientCmd.m_initPoseArgs.m_initialStateQdot[2]); + } + if (clientCmd.m_updateFlags & INIT_POSE_HAS_BASE_ANGULAR_VELOCITY) + { + baseAngVel.setValue(clientCmd.m_initPoseArgs.m_initialStateQdot[3], + clientCmd.m_initPoseArgs.m_initialStateQdot[4], + clientCmd.m_initPoseArgs.m_initialStateQdot[5]); + } + btVector3 basePos(0, 0, 0); + if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_POSITION) + { + basePos = btVector3( + clientCmd.m_initPoseArgs.m_initialStateQ[0], + clientCmd.m_initPoseArgs.m_initialStateQ[1], + clientCmd.m_initPoseArgs.m_initialStateQ[2]); + } + btQuaternion baseOrn(0, 0, 0, 1); + if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_ORIENTATION) + { + baseOrn.setValue(clientCmd.m_initPoseArgs.m_initialStateQ[3], + clientCmd.m_initPoseArgs.m_initialStateQ[4], + clientCmd.m_initPoseArgs.m_initialStateQ[5], + clientCmd.m_initPoseArgs.m_initialStateQ[6]); + } if (body && body->m_multiBody) { btMultiBody* mb = body->m_multiBody; - btVector3 baseLinVel(0, 0, 0); - btVector3 baseAngVel(0, 0, 0); + if (clientCmd.m_updateFlags & INIT_POSE_HAS_BASE_LINEAR_VELOCITY) { - baseLinVel.setValue(clientCmd.m_initPoseArgs.m_initialStateQdot[0], - clientCmd.m_initPoseArgs.m_initialStateQdot[1], - clientCmd.m_initPoseArgs.m_initialStateQdot[2]); mb->setBaseVel(baseLinVel); - } if (clientCmd.m_updateFlags & INIT_POSE_HAS_BASE_ANGULAR_VELOCITY) { - baseAngVel.setValue(clientCmd.m_initPoseArgs.m_initialStateQdot[3], - clientCmd.m_initPoseArgs.m_initialStateQdot[4], - clientCmd.m_initPoseArgs.m_initialStateQdot[5]); mb->setBaseOmega(baseAngVel); } @@ -2396,10 +2430,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm clientCmd.m_initPoseArgs.m_hasInitialStateQ[2]); mb->setBaseVel(baseLinVel); - mb->setBasePos(btVector3( - clientCmd.m_initPoseArgs.m_initialStateQ[0], - clientCmd.m_initPoseArgs.m_initialStateQ[1], - clientCmd.m_initPoseArgs.m_initialStateQ[2])); + mb->setBasePos(basePos); } if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_ORIENTATION) { @@ -2409,10 +2440,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm clientCmd.m_initPoseArgs.m_hasInitialStateQ[6]); mb->setBaseOmega(baseAngVel); - btQuaternion invOrn(clientCmd.m_initPoseArgs.m_initialStateQ[3], - clientCmd.m_initPoseArgs.m_initialStateQ[4], - clientCmd.m_initPoseArgs.m_initialStateQ[5], - clientCmd.m_initPoseArgs.m_initialStateQ[6]); + btQuaternion invOrn(baseOrn); mb->setWorldToBaseRot(invOrn.inverse()); } @@ -2441,6 +2469,31 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm mb->updateCollisionObjectWorldTransforms(scratch_q,scratch_m); + } + + if (body && body->m_rigidBody) + { + if (clientCmd.m_updateFlags & INIT_POSE_HAS_BASE_LINEAR_VELOCITY) + { + body->m_rigidBody->setLinearVelocity(baseLinVel); + } + if (clientCmd.m_updateFlags & INIT_POSE_HAS_BASE_ANGULAR_VELOCITY) + { + body->m_rigidBody->setAngularVelocity(baseAngVel); + } + + if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_POSITION) + { + body->m_rigidBody->getWorldTransform().setOrigin(basePos); + body->m_rigidBody->setLinearVelocity(baseLinVel); + } + + if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_ORIENTATION) + { + body->m_rigidBody->getWorldTransform().setRotation(baseOrn); + body->m_rigidBody->setAngularVelocity(baseAngVel); + } + } SharedMemoryStatus& serverCmd =serverStatusOut; @@ -3114,11 +3167,12 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm for (int i = 0; i < clientCmd.m_externalForceArguments.m_numForcesAndTorques; ++i) { InteralBodyData* body = m_data->getHandle(clientCmd.m_externalForceArguments.m_bodyUniqueIds[i]); + bool isLinkFrame = ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_LINK_FRAME) != 0); + if (body && body->m_multiBody) { btMultiBody* mb = body->m_multiBody; - bool isLinkFrame = ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_LINK_FRAME)!=0); - + if ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_FORCE)!=0) { btVector3 forceLocal(clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+0], @@ -3166,6 +3220,36 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } } } + + if (body && body->m_rigidBody) + { + btRigidBody* rb = body->m_rigidBody; + if ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_FORCE) != 0) + { + btVector3 forceLocal(clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 0], + clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 1], + clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 2]); + btVector3 positionLocal( + clientCmd.m_externalForceArguments.m_positions[i * 3 + 0], + clientCmd.m_externalForceArguments.m_positions[i * 3 + 1], + clientCmd.m_externalForceArguments.m_positions[i * 3 + 2]); + + btVector3 forceWorld = isLinkFrame ? forceLocal : rb->getWorldTransform().getBasis()*forceLocal; + btVector3 relPosWorld = isLinkFrame ? positionLocal : rb->getWorldTransform().getBasis()*positionLocal; + rb->applyForce(forceWorld, relPosWorld); + + } + + if ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_TORQUE) != 0) + { + btVector3 torqueLocal(clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 0], + clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 1], + clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 2]); + + btVector3 torqueWorld = isLinkFrame ? torqueLocal : rb->getWorldTransform().getBasis()*torqueLocal; + rb->applyTorque(torqueWorld); + } + } } SharedMemoryStatus& serverCmd =serverStatusOut; diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index c360a5511..fd1c4d94a 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -287,7 +287,9 @@ enum EnumSimParamUpdateFlags SIM_PARAM_UPDATE_NUM_SIMULATION_SUB_STEPS=8, SIM_PARAM_UPDATE_REAL_TIME_SIMULATION = 16, SIM_PARAM_UPDATE_DEFAULT_CONTACT_ERP=32, - SIM_PARAM_UPDATE_INTERNAL_SIMULATION_FLAGS=64 + SIM_PARAM_UPDATE_INTERNAL_SIMULATION_FLAGS=64, + SIM_PARAM_UPDATE_USE_SPLIT_IMPULSE=128, + SIM_PARAM_UPDATE_SPLIT_IMPULSE_PENETRATION_THRESHOLD = 256, }; enum EnumLoadBunnyUpdateFlags @@ -312,6 +314,8 @@ struct SendPhysicsSimulationParameters int m_numSimulationSubSteps; int m_numSolverIterations; bool m_allowRealTimeSimulation; + int m_useSplitImpulse; + double m_splitImpulsePenetrationThreshold; int m_internalSimFlags; double m_defaultContactERP; }; diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 9ed3bdd60..58dc2127e 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -419,6 +419,62 @@ static PyObject* pybullet_loadMJCF(PyObject* self, PyObject* args) return Py_None; } +static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* args, PyObject *keywds) +{ + double fixedTimeStep = -1; + int numSolverIterations = -1; + int useSplitImpulse = -1; + double splitImpulsePenetrationThreshold = -1; + + if (0 == sm) { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + static char *kwlist[] = { "fixedTimeStep", "numSolverIterations","useSplitImpulse","splitImpulsePenetrationThreshold", NULL }; + + if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diid", kwlist,&fixedTimeStep,&numSolverIterations,&useSplitImpulse,&splitImpulsePenetrationThreshold)) + { + return NULL; + } + { + b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm); + b3SharedMemoryStatusHandle statusHandle; + + if (numSolverIterations >= 0) + { + b3PhysicsParamSetNumSolverIterations(command, numSolverIterations); + } + if (fixedTimeStep >= 0) + { + b3PhysicsParamSetTimeStep(command, fixedTimeStep); + } + if (useSplitImpulse >= 0) + { + b3PhysicsParamSetUseSplitImpulse(command,useSplitImpulse); + } + if (splitImpulsePenetrationThreshold >= 0) + { + b3PhysicsParamSetSplitImpulsePenetrationThreshold(command, splitImpulsePenetrationThreshold); + } + + //ret = b3PhysicsParamSetRealTimeSimulation(command, enableRealTimeSimulation); + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + } +#if 0 + b3SharedMemoryCommandHandle b3InitPhysicsParamCommand(b3PhysicsClientHandle physClient); + int b3PhysicsParamSetGravity(b3SharedMemoryCommandHandle commandHandle, double gravx, double gravy, double gravz); + int b3PhysicsParamSetTimeStep(b3SharedMemoryCommandHandle commandHandle, double timeStep); + int b3PhysicsParamSetDefaultContactERP(b3SharedMemoryCommandHandle commandHandle, double defaultContactERP); + int b3PhysicsParamSetNumSubSteps(b3SharedMemoryCommandHandle commandHandle, int numSubSteps); + int b3PhysicsParamSetRealTimeSimulation(b3SharedMemoryCommandHandle commandHandle, int enableRealTimeSimulation); + int b3PhysicsParamSetNumSolverIterations(b3SharedMemoryCommandHandle commandHandle, int numSolverIterations); +#endif + + Py_INCREF(Py_None); + return Py_None; +} // Load a robot from a URDF file (universal robot description format) @@ -426,8 +482,14 @@ static PyObject* pybullet_loadMJCF(PyObject* self, PyObject* args) // to position (0,0,1) with orientation(0,0,0,1) // els(x,y,z) or // loadURDF(pos_x, pos_y, pos_z, orn_x, orn_y, orn_z, orn_w) -static PyObject* pybullet_loadURDF(PyObject* self, PyObject* args) { +static PyObject* pybullet_loadURDF(PyObject* self, PyObject* args, PyObject *keywds) +{ int size = PySequence_Size(args); + static char *kwlist[] = { "fileName", "basePosition", "baseOrientation", "useMaximalCoordinates","useFixedBase", NULL }; + + static char *kwlistBackwardCompatible4[] = { "startPosX", "startPosY", "startPosZ", NULL }; + static char *kwlistBackwardCompatible8[] = { "startPosX", "startPosY", "startPosZ", "startOrnX", "startOrnY","startOrnZ","startOrnW", NULL }; + int bodyIndex = -1; const char* urdfFileName = ""; @@ -439,24 +501,79 @@ static PyObject* pybullet_loadURDF(PyObject* self, PyObject* args) { double startOrnY = 0.0; double startOrnZ = 0.0; double startOrnW = 1.0; + int useMaximalCoordinates = 0; + int useFixedBase = 0; + + int backwardsCompatibilityArgs = 0; if (0 == sm) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - if (size == 1) { - if (!PyArg_ParseTuple(args, "s", &urdfFileName)) return NULL; + + if (PyArg_ParseTupleAndKeywords(args, keywds, "sddd", kwlistBackwardCompatible4, &urdfFileName, &startPosX, + &startPosY, &startPosZ)) + { + backwardsCompatibilityArgs = 1; } - if (size == 4) { - if (!PyArg_ParseTuple(args, "sddd", &urdfFileName, &startPosX, &startPosY, - &startPosZ)) - return NULL; + else + { + PyErr_Clear(); } - if (size == 8) { - if (!PyArg_ParseTuple(args, "sddddddd", &urdfFileName, &startPosX, - &startPosY, &startPosZ, &startOrnX, &startOrnY, - &startOrnZ, &startOrnW)) - return NULL; + + + if (PyArg_ParseTupleAndKeywords(args, keywds, "sddddddd", kwlistBackwardCompatible8,&urdfFileName, &startPosX, + &startPosY, &startPosZ, &startOrnX, &startOrnY,&startOrnZ, &startOrnW)) + { + backwardsCompatibilityArgs = 1; + } + else + { + PyErr_Clear(); + } + + + + if (!backwardsCompatibilityArgs) + { + PyObject* basePosObj = 0; + PyObject* baseOrnObj = 0; + double basePos[3]; + double baseOrn[4]; + + + if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|OOii", kwlist, &urdfFileName, &basePosObj, &baseOrnObj, &useMaximalCoordinates,&useFixedBase)) + { + + return NULL; + } + else + { + if (basePosObj) + { + if (!pybullet_internalSetVectord(basePosObj, basePos)) + { + PyErr_SetString(SpamError, "Cannot convert basePosition."); + return NULL; + } + startPosX = basePos[0]; + startPosY = basePos[1]; + startPosZ = basePos[2]; + + } + if (baseOrnObj) + { + if (!pybullet_internalSetVector4d(baseOrnObj, baseOrn)) + { + PyErr_SetString(SpamError, "Cannot convert baseOrientation."); + return NULL; + } + startOrnX = baseOrn[0]; + startOrnY = baseOrn[1]; + startOrnZ = baseOrn[2]; + startOrnW = baseOrn[3]; + } + } } if (strlen(urdfFileName)) { @@ -473,6 +590,15 @@ static PyObject* pybullet_loadURDF(PyObject* self, PyObject* args) { b3LoadUrdfCommandSetStartPosition(command, startPosX, startPosY, startPosZ); b3LoadUrdfCommandSetStartOrientation(command, startOrnX, startOrnY, startOrnZ, startOrnW); + if (useMaximalCoordinates) + { + b3LoadUrdfCommandSetUseMultiBody(command, 0); + } + if (useFixedBase) + { + b3LoadUrdfCommandSetUseFixedBase(command, 1); + } + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); statusType = b3GetStatusType(statusHandle); if (statusType != CMD_URDF_LOADING_COMPLETED) { @@ -3483,6 +3609,8 @@ static PyMethodDef SpamMethods[] = { "Set the amount of time to proceed at each call to stepSimulation. (unit " "is seconds, typically range is 0.01 or 0.001)"}, + + {"setDefaultContactERP", pybullet_setDefaultContactERP, METH_VARARGS, "Set the amount of contact penetration Error Recovery Paramater " "(ERP) in each time step. \ @@ -3493,10 +3621,13 @@ static PyMethodDef SpamMethods[] = { "Enable or disable real time simulation (using the real time clock," " RTC) in the physics server. Expects one integer argument, 0 or 1" }, + { "setPhysicsEngineParameter", (PyCFunction)pybullet_setPhysicsEngineParameter, METH_VARARGS | METH_KEYWORDS, + "Set some internal physics engine parameter, such as cfm or erp etc." }, + { "setInternalSimFlags", pybullet_setInternalSimFlags, METH_VARARGS, "This is for experimental purposes, use at own risk, magic may or not happen"}, - {"loadURDF", pybullet_loadURDF, METH_VARARGS, + {"loadURDF", (PyCFunction) pybullet_loadURDF, METH_VARARGS | METH_KEYWORDS, "Create a multibody by loading a URDF file."}, {"loadSDF", pybullet_loadSDF, METH_VARARGS, diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp index 01a59890c..d52852dd8 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp @@ -378,7 +378,9 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstr btScalar erp = infoGlobal.m_erp2; - if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) + + //split impulse is not implemented yet for btMultiBody* + //if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) { erp = infoGlobal.m_erp; } @@ -388,19 +390,23 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstr btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv; btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv; - if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) + //split impulse is not implemented yet for btMultiBody* + + // if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) { //combine position and velocity into rhs solverConstraint.m_rhs = penetrationImpulse+velocityImpulse; solverConstraint.m_rhsPenetration = 0.f; - } else + } + /*else { //split position and velocity into rhs and m_rhsPenetration solverConstraint.m_rhs = velocityImpulse; solverConstraint.m_rhsPenetration = penetrationImpulse; } - + */ + solverConstraint.m_cfm = 0.f; solverConstraint.m_lowerLimit = lowerLimit; solverConstraint.m_upperLimit = upperLimit; diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp index 29f8e469c..aa0755a10 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp @@ -528,19 +528,20 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol if(!isFriction) { - if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) + // if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) { //combine position and velocity into rhs solverConstraint.m_rhs = penetrationImpulse+velocityImpulse; solverConstraint.m_rhsPenetration = 0.f; - } else + } + /*else { //split position and velocity into rhs and m_rhsPenetration solverConstraint.m_rhs = velocityImpulse; solverConstraint.m_rhsPenetration = penetrationImpulse; } - + */ solverConstraint.m_lowerLimit = 0; solverConstraint.m_upperLimit = 1e10f; } diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp index d94d1d4ea..4b33cf69d 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp @@ -384,7 +384,7 @@ btMultiBodyDynamicsWorld::btMultiBodyDynamicsWorld(btDispatcher* dispatcher,btBr m_multiBodyConstraintSolver(constraintSolver) { //split impulse is not yet supported for Featherstone hierarchies - getSolverInfo().m_splitImpulse = false; +// getSolverInfo().m_splitImpulse = false; getSolverInfo().m_solverMode |=SOLVER_USE_2_FRICTION_DIRECTIONS; m_solverMultiBodyIslandCallback = new MultiBodyInplaceSolverIslandCallback(constraintSolver,dispatcher); }