AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-14 13:50:04 +00:00
Code Issues Releases Wiki Activity

Merge remote-tracking branch 'upstream/master'

Browse Source
This commit is contained in:
yunfeibai 2016-12-01 16:48:22 -08:00
commit bf83c77dab
11 changed files with 354 additions and 57 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

52
examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
View File

@ -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
{

3
examples/Importers/ImportURDFDemo/URDFJointTypes.h
View File

@ -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),

25
examples/Importers/ImportURDFDemo/UrdfParser.cpp
View File

@ -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<double>(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<double>(restitution_xml->Attribute("value"));
link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_RESTITUTION;
}
}
}
{
TiXmlElement *spinning_xml = ci->FirstChildElement("spinning_friction");
if (spinning_xml)

20
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@ -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;

3
examples/SharedMemory/PhysicsClientC_API.h
View File

@ -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

122
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@ -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;

6
examples/SharedMemory/SharedMemoryCommands.h
View File

@ -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;
};

157
examples/pybullet/pybullet.c
View File

@ -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[] = { "fileName", "startPosX", "startPosY", "startPosZ", NULL };
static char *kwlistBackwardCompatible8[] = { "fileName", "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,

14
src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp
View File

@ -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;

7
src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp
View File

@ -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;
}

2
src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
View File

@ -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);
}