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

Allow to compile pybullet using btDiscreteDynamicsWorld (no multibodies and no deformables), this allows to create Jacobian and Mass matrix (and A=J*M-1*J_transpose) with MLCP solvers

Browse Source 
Add examples/pybullet/gym/pybullet_utils/readwriteurdf.py, this allows to read a URDF and write the URDF with more reasonable inertia tensors (based on mass and collision volumes)
This commit is contained in:
erwin coumans 2021-03-15 22:44:55 -07:00
parent 0b8f95598d
commit 5cf8ee3360
10 changed files with 177 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
examples/DeformableDemo/DeformableSelfCollision.cpp
View File

@ -55,7 +55,7 @@ public:
m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep); m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep);
} }
void addCloth(btVector3 origin); void addCloth(const btVector3& origin);
virtual void renderScene() virtual void renderScene()
{ {
@ -126,7 +126,7 @@ void DeformableSelfCollision::initPhysics()
getDeformableDynamicsWorld()->setLineSearch(false); getDeformableDynamicsWorld()->setLineSearch(false);
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
} }
void DeformableSelfCollision::addCloth(btVector3 origin) void DeformableSelfCollision::addCloth(const btVector3& origin)
// create a piece of cloth // create a piece of cloth
{ {
const btScalar s = 0.6; const btScalar s = 0.6;

10
examples/ExampleBrowser/GwenGUISupport/GwenParameterInterface.cpp
View File

@ -1,7 +1,11 @@
#include "GwenParameterInterface.h" #include "GwenParameterInterface.h"
#include "gwenInternalData.h" #include "gwenInternalData.h"
#include <cstring> #include <cstring>
#ifdef _WIN32
#define safe_printf _snprintf
#else
#define safe_printf snprintf
#endif
struct MyButtonEventHandler : public Gwen::Event::Handler struct MyButtonEventHandler : public Gwen::Event::Handler
{ {
Gwen::Controls::Button* m_buttonControl; Gwen::Controls::Button* m_buttonControl;
@ -85,7 +89,7 @@ struct MySliderEventHandler : public Gwen::Event::Handler
if (m_showValue) if (m_showValue)
{ {
char txt[1024]; char txt[1024];
snprintf(txt, sizeof(txt), "%s : %.3f", m_variableName, val); safe_printf(txt, sizeof(txt), "%s : %.3f", m_variableName, val);
m_label->SetText(txt); m_label->SetText(txt);
} }
} }
@ -231,7 +235,7 @@ void GwenParameterInterface::registerSliderFloatParameter(SliderParams& params)
} }
pSlider->SetValue(*params.m_paramValuePointer); //dimensions[i] ); pSlider->SetValue(*params.m_paramValuePointer); //dimensions[i] );
char labelName[1024]; char labelName[1024];
snprintf(labelName, sizeof(labelName), "%s", params.m_name); //axisNames[0]); safe_printf(labelName, sizeof(labelName), "%s", params.m_name); //axisNames[0]);
MySliderEventHandler<btScalar>* handler = new MySliderEventHandler<btScalar>(labelName, label, pSlider, params.m_paramValuePointer, params.m_callback, params.m_userPointer); MySliderEventHandler<btScalar>* handler = new MySliderEventHandler<btScalar>(labelName, label, pSlider, params.m_paramValuePointer, params.m_callback, params.m_userPointer);
handler->m_showValue = params.m_showValues; handler->m_showValue = params.m_showValues;
m_paramInternalData->m_sliderEventHandlers.push_back(handler); m_paramInternalData->m_sliderEventHandlers.push_back(handler);

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

@ -199,7 +199,15 @@ btScalar tmpUrdfScaling = 2;
btTransform ConvertURDF2BulletInternal( btTransform ConvertURDF2BulletInternal(
const URDFImporterInterface& u2b, MultiBodyCreationInterface& creation, const URDFImporterInterface& u2b, MultiBodyCreationInterface& creation,
URDF2BulletCachedData& cache, int urdfLinkIndex, URDF2BulletCachedData& cache, int urdfLinkIndex,
const btTransform& parentTransformInWorldSpace, btMultiBodyDynamicsWorld* world1, const btTransform& parentTransformInWorldSpace,
#ifdef USE_DISCRETE_DYNAMICS_WORLD
btDiscreteDynamicsWorld* world1,
#else
btMultiBodyDynamicsWorld* world1,
#endif
bool createMultiBody, const char* pathPrefix, bool createMultiBody, const char* pathPrefix,
int flags, UrdfVisualShapeCache* cachedLinkGraphicsShapesIn, UrdfVisualShapeCache* cachedLinkGraphicsShapesOut, bool recursive) int flags, UrdfVisualShapeCache* cachedLinkGraphicsShapesIn, UrdfVisualShapeCache* cachedLinkGraphicsShapesOut, bool recursive)
{ {
@ -505,6 +513,7 @@ btTransform ConvertURDF2BulletInternal(
creation.addLinkMapping(urdfLinkIndex, mbLinkIndex); creation.addLinkMapping(urdfLinkIndex, mbLinkIndex);
if (createMultiBody) if (createMultiBody)
{ {
#ifndef USE_DISCRETE_DYNAMICS_WORLD
cache.m_bulletMultiBody->setupRevolute(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex, cache.m_bulletMultiBody->setupRevolute(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex,
parentRotToThis, quatRotate(offsetInB.getRotation(), parentRotToThis, quatRotate(offsetInB.getRotation(),
jointAxisInJointSpace), jointAxisInJointSpace),
@ -519,8 +528,10 @@ btTransform ConvertURDF2BulletInternal(
btMultiBodyConstraint* con = new btMultiBodyJointLimitConstraint(cache.m_bulletMultiBody, mbLinkIndex, jointLowerLimit, jointUpperLimit); btMultiBodyConstraint* con = new btMultiBodyJointLimitConstraint(cache.m_bulletMultiBody, mbLinkIndex, jointLowerLimit, jointUpperLimit);
world1->addMultiBodyConstraint(con); world1->addMultiBodyConstraint(con);
} }
#endif
} }
else else
{ {
btGeneric6DofSpring2Constraint* dof6 = 0; btGeneric6DofSpring2Constraint* dof6 = 0;
if (jointType == URDFRevoluteJoint && jointLowerLimit <= jointUpperLimit) if (jointType == URDFRevoluteJoint && jointLowerLimit <= jointUpperLimit)
@ -560,6 +571,7 @@ btTransform ConvertURDF2BulletInternal(
if (createMultiBody) if (createMultiBody)
{ {
#ifndef USE_DISCRETE_DYNAMICS_WORLD
cache.m_bulletMultiBody->setupPrismatic(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex, cache.m_bulletMultiBody->setupPrismatic(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex,
parentRotToThis, quatRotate(offsetInB.getRotation(), jointAxisInJointSpace), offsetInA.getOrigin(), //parent2joint.getOrigin(), parentRotToThis, quatRotate(offsetInB.getRotation(), jointAxisInJointSpace), offsetInA.getOrigin(), //parent2joint.getOrigin(),
-offsetInB.getOrigin(), -offsetInB.getOrigin(),
@ -574,6 +586,7 @@ btTransform ConvertURDF2BulletInternal(
world1->addMultiBodyConstraint(con); world1->addMultiBodyConstraint(con);
} }
//printf("joint lower limit=%d, upper limit = %f\n", jointLowerLimit, jointUpperLimit); //printf("joint lower limit=%d, upper limit = %f\n", jointLowerLimit, jointUpperLimit);
#endif
} }
else else
{ {
@ -775,10 +788,18 @@ bool MyIntCompareFunc(childParentIndex a, childParentIndex b)
} }
void ConvertURDF2Bullet( void ConvertURDF2Bullet(
const URDFImporterInterface& u2b, MultiBodyCreationInterface& creation, const URDFImporterInterface& u2b, MultiBodyCreationInterface& creation,
const btTransform& rootTransformInWorldSpace, const btTransform& rootTransformInWorldSpace,
#ifdef USE_DISCRETE_DYNAMICS_WORLD
btDiscreteDynamicsWorld* world1,
#else
btMultiBodyDynamicsWorld* world1, btMultiBodyDynamicsWorld* world1,
#endif
bool createMultiBody, const char* pathPrefix, int flags, UrdfVisualShapeCache* cachedLinkGraphicsShapes) bool createMultiBody, const char* pathPrefix, int flags, UrdfVisualShapeCache* cachedLinkGraphicsShapes)
{ {
URDF2BulletCachedData cache; URDF2BulletCachedData cache;
@ -829,7 +850,7 @@ void ConvertURDF2Bullet(
{ {
*cachedLinkGraphicsShapes = cachedLinkGraphicsShapesOut; *cachedLinkGraphicsShapes = cachedLinkGraphicsShapesOut;
} }
#ifndef USE_DISCRETE_DYNAMICS_WORLD
if (world1 && cache.m_bulletMultiBody) if (world1 && cache.m_bulletMultiBody)
{ {
B3_PROFILE("Post process"); B3_PROFILE("Post process");
@ -855,4 +876,5 @@ void ConvertURDF2Bullet(
world1->addMultiBody(mb); world1->addMultiBody(mb);
} }
#endif
} }

14
examples/Importers/ImportURDFDemo/URDF2Bullet.h
View File

@ -8,6 +8,7 @@
class btVector3; class btVector3;
class btTransform; class btTransform;
class btMultiBodyDynamicsWorld; class btMultiBodyDynamicsWorld;
class btDiscreteDynamicsWorld;
class btTransform; class btTransform;
class URDFImporterInterface; class URDFImporterInterface;
@ -20,7 +21,18 @@ struct UrdfVisualShapeCache
btAlignedObjectArray<UrdfMaterialColor> m_cachedUrdfLinkColors; btAlignedObjectArray<UrdfMaterialColor> m_cachedUrdfLinkColors;
btAlignedObjectArray<int> m_cachedUrdfLinkVisualShapeIndices; btAlignedObjectArray<int> m_cachedUrdfLinkVisualShapeIndices;
}; };
//#define USE_DISCRETE_DYNAMICS_WORLD
#ifdef USE_DISCRETE_DYNAMICS_WORLD
void ConvertURDF2Bullet(const URDFImporterInterface& u2b,
MultiBodyCreationInterface& creationCallback,
const btTransform& rootTransformInWorldSpace,
btDiscreteDynamicsWorld* world,
bool createMultiBody,
const char* pathPrefix,
int flags = 0,
UrdfVisualShapeCache* cachedLinkGraphicsShapes = 0);
#else
void ConvertURDF2Bullet(const URDFImporterInterface& u2b, void ConvertURDF2Bullet(const URDFImporterInterface& u2b,
MultiBodyCreationInterface& creationCallback, MultiBodyCreationInterface& creationCallback,
const btTransform& rootTransformInWorldSpace, const btTransform& rootTransformInWorldSpace,
@ -29,5 +41,5 @@ void ConvertURDF2Bullet(const URDFImporterInterface& u2b,
const char* pathPrefix, const char* pathPrefix,
int flags = 0, int flags = 0,
UrdfVisualShapeCache* cachedLinkGraphicsShapes = 0); UrdfVisualShapeCache* cachedLinkGraphicsShapes = 0);
#endif
#endif //_URDF2BULLET_H #endif //_URDF2BULLET_H

8
examples/SharedMemory/PhysicsClientExample.cpp
View File

@ -13,6 +13,12 @@
#include "PhysicsDirectC_API.h" #include "PhysicsDirectC_API.h"
#include "PhysicsClientC_API.h" #include "PhysicsClientC_API.h"
#include "PhysicsServerSharedMemory.h" #include "PhysicsServerSharedMemory.h"
#include <stdio.h>
#ifdef _WIN32
#define safe_printf _snprintf
#else
#define safe_printf snprintf
#endif
struct MyMotorInfo2 struct MyMotorInfo2
{ {
btScalar m_velTarget; btScalar m_velTarget;
@ -655,7 +661,7 @@ void PhysicsClientExample::createButtons()
if (m_numMotors < MAX_NUM_MOTORS) if (m_numMotors < MAX_NUM_MOTORS)
{ {
char motorName[1026]; char motorName[1026];
snprintf(motorName, sizeof(motorName), "%s q", info.m_jointName); safe_printf(motorName, sizeof(motorName), "%s q", info.m_jointName);
// MyMotorInfo2* motorInfo = &m_motorTargetVelocities[m_numMotors]; // MyMotorInfo2* motorInfo = &m_motorTargetVelocities[m_numMotors];
MyMotorInfo2* motorInfo = &m_motorTargetPositions[m_numMotors]; MyMotorInfo2* motorInfo = &m_motorTargetPositions[m_numMotors];
motorInfo->m_velTarget = 0.f; motorInfo->m_velTarget = 0.f;

102
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@ -12,10 +12,15 @@
#include "../Importers/ImportMeshUtility/b3ImportMeshUtility.h" #include "../Importers/ImportMeshUtility/b3ImportMeshUtility.h"
#include "BulletDynamics/MLCPSolvers/btDantzigSolver.h" #include "BulletDynamics/MLCPSolvers/btDantzigSolver.h"
#include "BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h" #include "BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h"
#include "BulletDynamics/MLCPSolvers/btMLCPSolver.h"
#include "BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h" #include "BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h"
#include "BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.h" #include "BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.h"
#include "BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h" #include "BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h"
//#define USE_DISCRETE_DYNAMICS_WORLD
//#define SKIP_DEFORMABLE_BODY
//#define SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
#include "../Utils/b3BulletDefaultFileIO.h" #include "../Utils/b3BulletDefaultFileIO.h"
#include "BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h" #include "BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h"
@ -1626,7 +1631,11 @@ struct PhysicsServerCommandProcessorInternalData
b3AlignedObjectArray<SaveWorldObjectData> m_saveWorldBodyData; b3AlignedObjectArray<SaveWorldObjectData> m_saveWorldBodyData;
#ifdef USE_DISCRETE_DYNAMICS_WORLD
btAlignedObjectArray<btWorldImporter*> m_worldImporters;
#else
btAlignedObjectArray<btMultiBodyWorldImporter*> m_worldImporters; btAlignedObjectArray<btMultiBodyWorldImporter*> m_worldImporters;
#endif
btAlignedObjectArray<std::string*> m_strings; btAlignedObjectArray<std::string*> m_strings;
@ -1642,7 +1651,11 @@ struct PhysicsServerCommandProcessorInternalData
btBroadphaseInterface* m_broadphase; btBroadphaseInterface* m_broadphase;
btCollisionDispatcher* m_dispatcher; btCollisionDispatcher* m_dispatcher;
#ifdef USE_DISCRETE_DYNAMICS_WORLD
btSequentialImpulseConstraintSolver* m_solver;
#else
btMultiBodyConstraintSolver* m_solver; btMultiBodyConstraintSolver* m_solver;
#endif
btDefaultCollisionConfiguration* m_collisionConfiguration; btDefaultCollisionConfiguration* m_collisionConfiguration;
@ -1654,7 +1667,12 @@ struct PhysicsServerCommandProcessorInternalData
btAlignedObjectArray<btDeformableLagrangianForce*> m_lf; btAlignedObjectArray<btDeformableLagrangianForce*> m_lf;
#endif #endif
#ifdef USE_DISCRETE_DYNAMICS_WORLD
btDiscreteDynamicsWorld* m_dynamicsWorld;
#else
btMultiBodyDynamicsWorld* m_dynamicsWorld; btMultiBodyDynamicsWorld* m_dynamicsWorld;
#endif
int m_constraintSolverType; int m_constraintSolverType;
SharedMemoryDebugDrawer* m_remoteDebugDrawer; SharedMemoryDebugDrawer* m_remoteDebugDrawer;
@ -2714,18 +2732,30 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld(int flags)
if ((0 == m_data->m_dynamicsWorld) && (0 == (flags & RESET_USE_DISCRETE_DYNAMICS_WORLD))) if ((0 == m_data->m_dynamicsWorld) && (0 == (flags & RESET_USE_DISCRETE_DYNAMICS_WORLD)))
{ {
m_data->m_solver = new btMultiBodyConstraintSolver;
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
m_data->m_dynamicsWorld = new btSoftMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration); m_data->m_dynamicsWorld = new btSoftMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration);
#else #else
#ifdef USE_DISCRETE_DYNAMICS_WORLD
m_data->m_solver = new btSequentialImpulseConstraintSolver;
m_data->m_dynamicsWorld = new btDiscreteDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration);
#else
m_data->m_solver = new btMultiBodyConstraintSolver;
m_data->m_dynamicsWorld = new btMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration); m_data->m_dynamicsWorld = new btMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration);
#endif #endif
#endif
} }
if (0 == m_data->m_dynamicsWorld) if (0 == m_data->m_dynamicsWorld)
{ {
#ifdef USE_DISCRETE_DYNAMICS_WORLD
m_data->m_solver = new btSequentialImpulseConstraintSolver;
m_data->m_dynamicsWorld = new btDiscreteDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration);
#else
m_data->m_solver = new btMultiBodyConstraintSolver; m_data->m_solver = new btMultiBodyConstraintSolver;
m_data->m_dynamicsWorld = new btMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration); m_data->m_dynamicsWorld = new btMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration);
#endif
} }
//Workaround: in a VR application, where we avoid synchronizing between GFX/Physics threads, we don't want to resize this array, so pre-allocate it //Workaround: in a VR application, where we avoid synchronizing between GFX/Physics threads, we don't want to resize this array, so pre-allocate it
@ -2860,13 +2890,14 @@ void PhysicsServerCommandProcessor::deleteDynamicsWorld()
constraints.push_back(constraint); constraints.push_back(constraint);
m_data->m_dynamicsWorld->removeConstraint(constraint); m_data->m_dynamicsWorld->removeConstraint(constraint);
} }
#ifndef USE_DISCRETE_DYNAMICS_WORLD
for (i = m_data->m_dynamicsWorld->getNumMultiBodyConstraints() - 1; i >= 0; i--) for (i = m_data->m_dynamicsWorld->getNumMultiBodyConstraints() - 1; i >= 0; i--)
{ {
btMultiBodyConstraint* mbconstraint = m_data->m_dynamicsWorld->getMultiBodyConstraint(i); btMultiBodyConstraint* mbconstraint = m_data->m_dynamicsWorld->getMultiBodyConstraint(i);
mbconstraints.push_back(mbconstraint); mbconstraints.push_back(mbconstraint);
m_data->m_dynamicsWorld->removeMultiBodyConstraint(mbconstraint); m_data->m_dynamicsWorld->removeMultiBodyConstraint(mbconstraint);
} }
#endif
for (i = m_data->m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--) for (i = m_data->m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--)
{ {
btCollisionObject* obj = m_data->m_dynamicsWorld->getCollisionObjectArray()[i]; btCollisionObject* obj = m_data->m_dynamicsWorld->getCollisionObjectArray()[i];
@ -2878,12 +2909,14 @@ void PhysicsServerCommandProcessor::deleteDynamicsWorld()
m_data->m_dynamicsWorld->removeCollisionObject(obj); m_data->m_dynamicsWorld->removeCollisionObject(obj);
delete obj; delete obj;
} }
#ifndef USE_DISCRETE_DYNAMICS_WORLD
for (i = m_data->m_dynamicsWorld->getNumMultibodies() - 1; i >= 0; i--) for (i = m_data->m_dynamicsWorld->getNumMultibodies() - 1; i >= 0; i--)
{ {
btMultiBody* mb = m_data->m_dynamicsWorld->getMultiBody(i); btMultiBody* mb = m_data->m_dynamicsWorld->getMultiBody(i);
m_data->m_dynamicsWorld->removeMultiBody(mb); m_data->m_dynamicsWorld->removeMultiBody(mb);
delete mb; delete mb;
} }
#endif
#ifndef SKIP_DEFORMABLE_BODY #ifndef SKIP_DEFORMABLE_BODY
for (int j = 0; j < m_data->m_lf.size(); j++) for (int j = 0; j < m_data->m_lf.size(); j++)
{ {
@ -3052,14 +3085,18 @@ void PhysicsServerCommandProcessor::createJointMotors(btMultiBody* mb)
//motor->setRhsClamp(gRhsClamp); //motor->setRhsClamp(gRhsClamp);
//motor->setMaxAppliedImpulse(0); //motor->setMaxAppliedImpulse(0);
mb->getLink(mbLinkIndex).m_userPtr = motor; mb->getLink(mbLinkIndex).m_userPtr = motor;
#ifndef USE_DISCRETE_DYNAMICS_WORLD
m_data->m_dynamicsWorld->addMultiBodyConstraint(motor); m_data->m_dynamicsWorld->addMultiBodyConstraint(motor);
#endif
motor->finalizeMultiDof(); motor->finalizeMultiDof();
} }
if (mb->getLink(mbLinkIndex).m_jointType == btMultibodyLink::eSpherical) if (mb->getLink(mbLinkIndex).m_jointType == btMultibodyLink::eSpherical)
{ {
btMultiBodySphericalJointMotor* motor = new btMultiBodySphericalJointMotor(mb, mbLinkIndex, 1000 * maxMotorImpulse); btMultiBodySphericalJointMotor* motor = new btMultiBodySphericalJointMotor(mb, mbLinkIndex, 1000 * maxMotorImpulse);
mb->getLink(mbLinkIndex).m_userPtr = motor; mb->getLink(mbLinkIndex).m_userPtr = motor;
#ifndef USE_DISCRETE_DYNAMICS_WORLD
m_data->m_dynamicsWorld->addMultiBodyConstraint(motor); m_data->m_dynamicsWorld->addMultiBodyConstraint(motor);
#endif
motor->finalizeMultiDof(); motor->finalizeMultiDof();
} }
} }
@ -3166,11 +3203,15 @@ bool PhysicsServerCommandProcessor::processImportedObjects(const char* fileName,
} }
} }
UrdfVisualShapeCache* cachedVisualShapesPtr = m_data->m_cachedVUrdfisualShapes[fileName]; UrdfVisualShapeCache* cachedVisualShapesPtr = m_data->m_cachedVUrdfisualShapes[fileName];
ConvertURDF2Bullet(u2b, creation, rootTrans, m_data->m_dynamicsWorld, useMultiBody, u2b.getPathPrefix(), flags, cachedVisualShapesPtr); ConvertURDF2Bullet(u2b, creation, rootTrans, m_data->m_dynamicsWorld, useMultiBody, u2b.getPathPrefix(), flags, cachedVisualShapesPtr);
} }
else else
{ {
ConvertURDF2Bullet(u2b, creation, rootTrans, m_data->m_dynamicsWorld, useMultiBody, u2b.getPathPrefix(), flags); ConvertURDF2Bullet(u2b, creation, rootTrans, m_data->m_dynamicsWorld, useMultiBody, u2b.getPathPrefix(), flags);
} }
mb = creation.getBulletMultiBody(); mb = creation.getBulletMultiBody();
@ -3788,9 +3829,10 @@ bool PhysicsServerCommandProcessor::processStateLoggingCommand(const struct Shar
} }
} }
} }
#ifndef USE_DISCRETE_DYNAMICS_WORLD
if (clientCmd.m_stateLoggingArguments.m_logType == STATE_LOGGING_GENERIC_ROBOT) if (clientCmd.m_stateLoggingArguments.m_logType == STATE_LOGGING_GENERIC_ROBOT)
{ {
std::string fileName = clientCmd.m_stateLoggingArguments.m_fileName; std::string fileName = clientCmd.m_stateLoggingArguments.m_fileName;
int loggerUid = m_data->m_stateLoggersUniqueId++; int loggerUid = m_data->m_stateLoggersUniqueId++;
@ -3848,6 +3890,7 @@ bool PhysicsServerCommandProcessor::processStateLoggingCommand(const struct Shar
serverStatusOut.m_type = CMD_STATE_LOGGING_START_COMPLETED; serverStatusOut.m_type = CMD_STATE_LOGGING_START_COMPLETED;
serverStatusOut.m_stateLoggingResultArgs.m_loggingUniqueId = loggerUid; serverStatusOut.m_stateLoggingResultArgs.m_loggingUniqueId = loggerUid;
} }
#endif
if (clientCmd.m_stateLoggingArguments.m_logType == STATE_LOGGING_VR_CONTROLLERS) if (clientCmd.m_stateLoggingArguments.m_logType == STATE_LOGGING_VR_CONTROLLERS)
{ {
std::string fileName = clientCmd.m_stateLoggingArguments.m_fileName; std::string fileName = clientCmd.m_stateLoggingArguments.m_fileName;
@ -4733,7 +4776,11 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
bool hasStatus = true; bool hasStatus = true;
serverStatusOut.m_type = CMD_CREATE_COLLISION_SHAPE_FAILED; serverStatusOut.m_type = CMD_CREATE_COLLISION_SHAPE_FAILED;
#ifdef USE_DISCRETE_DYNAMICS_WORLD
btWorldImporter* worldImporter = new btWorldImporter(m_data->m_dynamicsWorld);
#else
btMultiBodyWorldImporter* worldImporter = new btMultiBodyWorldImporter(m_data->m_dynamicsWorld); btMultiBodyWorldImporter* worldImporter = new btMultiBodyWorldImporter(m_data->m_dynamicsWorld);
#endif
btCollisionShape* shape = 0; btCollisionShape* shape = 0;
b3AlignedObjectArray<UrdfCollision> urdfCollisionObjects; b3AlignedObjectArray<UrdfCollision> urdfCollisionObjects;
@ -9444,6 +9491,7 @@ bool PhysicsServerCommandProcessor::processForwardDynamicsCommand(const struct S
b3Printf("Step simulation request"); b3Printf("Step simulation request");
b3Printf("CMD_STEP_FORWARD_SIMULATION clientCmd = %d\n", clientCmd.m_sequenceNumber); b3Printf("CMD_STEP_FORWARD_SIMULATION clientCmd = %d\n", clientCmd.m_sequenceNumber);
} }
#ifndef USE_DISCRETE_DYNAMICS_WORLD
///todo(erwincoumans) move this damping inside Bullet ///todo(erwincoumans) move this damping inside Bullet
for (int i = 0; i < m_data->m_dynamicsWorld->getNumMultibodies(); i++) for (int i = 0; i < m_data->m_dynamicsWorld->getNumMultibodies(); i++)
{ {
@ -9458,7 +9506,7 @@ bool PhysicsServerCommandProcessor::processForwardDynamicsCommand(const struct S
} }
} }
} }
#endif
btScalar deltaTimeScaled = m_data->m_physicsDeltaTime * simTimeScalingFactor; btScalar deltaTimeScaled = m_data->m_physicsDeltaTime * simTimeScalingFactor;
int numSteps = 0; int numSteps = 0;
@ -9483,8 +9531,9 @@ bool PhysicsServerCommandProcessor::processForwardDynamicsCommand(const struct S
serverCmd.m_forwardDynamicsAnalyticsArgs.m_numSteps = numSteps; serverCmd.m_forwardDynamicsAnalyticsArgs.m_numSteps = numSteps;
btAlignedObjectArray<btSolverAnalyticsData> islandAnalyticsData; btAlignedObjectArray<btSolverAnalyticsData> islandAnalyticsData;
#ifndef USE_DISCRETE_DYNAMICS_WORLD
m_data->m_dynamicsWorld->getAnalyticsData(islandAnalyticsData); m_data->m_dynamicsWorld->getAnalyticsData(islandAnalyticsData);
#endif
serverCmd.m_forwardDynamicsAnalyticsArgs.m_numIslands = islandAnalyticsData.size(); serverCmd.m_forwardDynamicsAnalyticsArgs.m_numIslands = islandAnalyticsData.size();
int numIslands = btMin(islandAnalyticsData.size(), MAX_ISLANDS_ANALYTICS); int numIslands = btMin(islandAnalyticsData.size(), MAX_ISLANDS_ANALYTICS);
@ -9555,7 +9604,7 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc
if (bodyUniqueId >= 0) if (bodyUniqueId >= 0)
{ {
InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
#ifndef USE_DISCRETE_DYNAMICS_WORLD
if (body && body->m_multiBody) if (body && body->m_multiBody)
{ {
btMultiBody* mb = body->m_multiBody; btMultiBody* mb = body->m_multiBody;
@ -9742,6 +9791,7 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc
} }
} }
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_JOINT_LIMITS) if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_JOINT_LIMITS)
{ {
//find a joint limit //find a joint limit
@ -9875,6 +9925,7 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc
} }
} }
else else
#endif
{ {
btRigidBody* rb = 0; btRigidBody* rb = 0;
if (body && body->m_rigidBody) if (body && body->m_rigidBody)
@ -10401,7 +10452,11 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st
btConstraintSolver* oldSolver = m_data->m_dynamicsWorld->getConstraintSolver(); btConstraintSolver* oldSolver = m_data->m_dynamicsWorld->getConstraintSolver();
#ifdef USE_DISCRETE_DYNAMICS_WORLD
btSequentialImpulseConstraintSolver* newSolver = 0;
#else
btMultiBodyConstraintSolver* newSolver = 0; btMultiBodyConstraintSolver* newSolver = 0;
#endif
switch (clientCmd.m_physSimParamArgs.m_constraintSolverType) switch (clientCmd.m_physSimParamArgs.m_constraintSolverType)
{ {
@ -10414,7 +10469,12 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st
case eConstraintSolverLCP_PGS: case eConstraintSolverLCP_PGS:
{ {
btSolveProjectedGaussSeidel* mlcp = new btSolveProjectedGaussSeidel(); btSolveProjectedGaussSeidel* mlcp = new btSolveProjectedGaussSeidel();
#ifdef USE_DISCRETE_DYNAMICS_WORLD
newSolver = new btMLCPSolver(mlcp);
#else
newSolver = new btMultiBodyMLCPConstraintSolver(mlcp); newSolver = new btMultiBodyMLCPConstraintSolver(mlcp);
#endif
b3Printf("PyBullet: Constraint Solver: MLCP + PGS\n"); b3Printf("PyBullet: Constraint Solver: MLCP + PGS\n");
break; break;
} }
@ -10438,7 +10498,11 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st
{ {
delete oldSolver; delete oldSolver;
#ifdef USE_DISCRETE_DYNAMICS_WORLD
m_data->m_dynamicsWorld->setConstraintSolver(newSolver);
#else
m_data->m_dynamicsWorld->setMultiBodyConstraintSolver(newSolver); m_data->m_dynamicsWorld->setMultiBodyConstraintSolver(newSolver);
#endif
m_data->m_solver = newSolver; m_data->m_solver = newSolver;
printf("switched solver\n"); printf("switched solver\n");
} }
@ -10840,7 +10904,11 @@ bool PhysicsServerCommandProcessor::processCreateRigidBodyCommand(const struct S
shapeType = clientCmd.m_createBoxShapeArguments.m_collisionShapeType; shapeType = clientCmd.m_createBoxShapeArguments.m_collisionShapeType;
} }
#ifdef USE_DISCRETE_DYNAMICS_WORLD
btWorldImporter* worldImporter = new btWorldImporter(m_data->m_dynamicsWorld);
#else
btMultiBodyWorldImporter* worldImporter = new btMultiBodyWorldImporter(m_data->m_dynamicsWorld); btMultiBodyWorldImporter* worldImporter = new btMultiBodyWorldImporter(m_data->m_dynamicsWorld);
#endif
m_data->m_worldImporters.push_back(worldImporter); m_data->m_worldImporters.push_back(worldImporter);
btCollisionShape* shape = 0; btCollisionShape* shape = 0;
@ -11585,6 +11653,7 @@ bool PhysicsServerCommandProcessor::processRemoveBodyCommand(const struct Shared
InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(bodyUniqueId); InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(bodyUniqueId);
if (bodyHandle) if (bodyHandle)
{ {
#ifndef USE_DISCRETE_DYNAMICS_WORLD
if (bodyHandle->m_multiBody) if (bodyHandle->m_multiBody)
{ {
serverCmd.m_removeObjectArgs.m_bodyUniqueIds[serverCmd.m_removeObjectArgs.m_numBodies++] = bodyUniqueId; serverCmd.m_removeObjectArgs.m_bodyUniqueIds[serverCmd.m_removeObjectArgs.m_numBodies++] = bodyUniqueId;
@ -11654,6 +11723,7 @@ bool PhysicsServerCommandProcessor::processRemoveBodyCommand(const struct Shared
bodyHandle->m_multiBody = 0; bodyHandle->m_multiBody = 0;
serverCmd.m_type = CMD_REMOVE_BODY_COMPLETED; serverCmd.m_type = CMD_REMOVE_BODY_COMPLETED;
} }
#endif
if (bodyHandle->m_rigidBody) if (bodyHandle->m_rigidBody)
{ {
if (m_data->m_pluginManager.getRenderInterface()) if (m_data->m_pluginManager.getRenderInterface())
@ -11731,7 +11801,11 @@ bool PhysicsServerCommandProcessor::processRemoveBodyCommand(const struct Shared
for (int i = 0; i < m_data->m_worldImporters.size(); i++) for (int i = 0; i < m_data->m_worldImporters.size(); i++)
{ {
#ifdef USE_DISCRETE_DYNAMICS_WORLD
btWorldImporter* importer = m_data->m_worldImporters[i];
#else
btMultiBodyWorldImporter* importer = m_data->m_worldImporters[i]; btMultiBodyWorldImporter* importer = m_data->m_worldImporters[i];
#endif
for (int c = 0; c < importer->getNumCollisionShapes(); c++) for (int c = 0; c < importer->getNumCollisionShapes(); c++)
{ {
if (importer->getCollisionShapeByIndex(c) == handle->m_collisionShape) if (importer->getCollisionShapeByIndex(c) == handle->m_collisionShape)
@ -11747,7 +11821,11 @@ bool PhysicsServerCommandProcessor::processRemoveBodyCommand(const struct Shared
} }
if (foundIndex >= 0) if (foundIndex >= 0)
{ {
#ifdef USE_DISCRETE_DYNAMICS_WORLD
btWorldImporter* importer = m_data->m_worldImporters[foundIndex];
#else
btMultiBodyWorldImporter* importer = m_data->m_worldImporters[foundIndex]; btMultiBodyWorldImporter* importer = m_data->m_worldImporters[foundIndex];
#endif
m_data->m_worldImporters.removeAtIndex(foundIndex); m_data->m_worldImporters.removeAtIndex(foundIndex);
importer->deleteAllData(); importer->deleteAllData();
delete importer; delete importer;
@ -11901,6 +11979,7 @@ bool PhysicsServerCommandProcessor::processCreateUserConstraintCommand(const str
{ {
btScalar defaultMaxForce = 500.0; btScalar defaultMaxForce = 500.0;
InternalBodyData* parentBody = m_data->m_bodyHandles.getHandle(clientCmd.m_userConstraintArguments.m_parentBodyIndex); InternalBodyData* parentBody = m_data->m_bodyHandles.getHandle(clientCmd.m_userConstraintArguments.m_parentBodyIndex);
#ifndef USE_DISCRETE_DYNAMICS_WORLD
if (parentBody && parentBody->m_multiBody) if (parentBody && parentBody->m_multiBody)
{ {
if ((clientCmd.m_userConstraintArguments.m_parentJointIndex >= -1) && clientCmd.m_userConstraintArguments.m_parentJointIndex < parentBody->m_multiBody->getNumLinks()) if ((clientCmd.m_userConstraintArguments.m_parentJointIndex >= -1) && clientCmd.m_userConstraintArguments.m_parentJointIndex < parentBody->m_multiBody->getNumLinks())
@ -12054,6 +12133,7 @@ bool PhysicsServerCommandProcessor::processCreateUserConstraintCommand(const str
} }
} }
else else
#endif
{ {
InternalBodyData* childBody = clientCmd.m_userConstraintArguments.m_childBodyIndex >= 0 ? m_data->m_bodyHandles.getHandle(clientCmd.m_userConstraintArguments.m_childBodyIndex) : 0; InternalBodyData* childBody = clientCmd.m_userConstraintArguments.m_childBodyIndex >= 0 ? m_data->m_bodyHandles.getHandle(clientCmd.m_userConstraintArguments.m_childBodyIndex) : 0;
@ -12301,12 +12381,14 @@ bool PhysicsServerCommandProcessor::processCreateUserConstraintCommand(const str
InteralUserConstraintData* userConstraintPtr = m_data->m_userConstraints.find(userConstraintUidRemove); InteralUserConstraintData* userConstraintPtr = m_data->m_userConstraints.find(userConstraintUidRemove);
if (userConstraintPtr) if (userConstraintPtr)
{ {
#ifndef USE_DISCRETE_DYNAMICS_WORLD
if (userConstraintPtr->m_mbConstraint) if (userConstraintPtr->m_mbConstraint)
{ {
m_data->m_dynamicsWorld->removeMultiBodyConstraint(userConstraintPtr->m_mbConstraint); m_data->m_dynamicsWorld->removeMultiBodyConstraint(userConstraintPtr->m_mbConstraint);
delete userConstraintPtr->m_mbConstraint; delete userConstraintPtr->m_mbConstraint;
m_data->m_userConstraints.remove(userConstraintUidRemove); m_data->m_userConstraints.remove(userConstraintUidRemove);
} }
#endif//USE_DISCRETE_DYNAMICS_WORLD
if (userConstraintPtr->m_rbConstraint) if (userConstraintPtr->m_rbConstraint)
{ {
m_data->m_dynamicsWorld->removeConstraint(userConstraintPtr->m_rbConstraint); m_data->m_dynamicsWorld->removeConstraint(userConstraintPtr->m_rbConstraint);
@ -13654,12 +13736,14 @@ bool PhysicsServerCommandProcessor::processRemoveStateCommand(const struct Share
bool PhysicsServerCommandProcessor::processRestoreStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) bool PhysicsServerCommandProcessor::processRestoreStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{ {
BT_PROFILE("CMD_RESTORE_STATE"); BT_PROFILE("CMD_RESTORE_STATE");
bool hasStatus = true; bool hasStatus = true;
SharedMemoryStatus& serverCmd = serverStatusOut; SharedMemoryStatus& serverCmd = serverStatusOut;
serverCmd.m_type = CMD_RESTORE_STATE_FAILED; serverCmd.m_type = CMD_RESTORE_STATE_FAILED;
#ifndef USE_DISCRETE_DYNAMICS_WORLD
btMultiBodyWorldImporter* importer = new btMultiBodyWorldImporter(m_data->m_dynamicsWorld); btMultiBodyWorldImporter* importer = new btMultiBodyWorldImporter(m_data->m_dynamicsWorld);
importer->setImporterFlags(eRESTORE_EXISTING_OBJECTS); importer->setImporterFlags(eRESTORE_EXISTING_OBJECTS);
bool ok = false; bool ok = false;
@ -13727,7 +13811,7 @@ bool PhysicsServerCommandProcessor::processRestoreStateCommand(const struct Shar
{ {
serverCmd.m_type = CMD_RESTORE_STATE_COMPLETED; serverCmd.m_type = CMD_RESTORE_STATE_COMPLETED;
} }
#endif
return hasStatus; return hasStatus;
} }
@ -13739,6 +13823,7 @@ bool PhysicsServerCommandProcessor::processLoadBulletCommand(const struct Shared
SharedMemoryStatus& serverCmd = serverStatusOut; SharedMemoryStatus& serverCmd = serverStatusOut;
serverCmd.m_type = CMD_BULLET_LOADING_FAILED; serverCmd.m_type = CMD_BULLET_LOADING_FAILED;
#ifndef USE_DISCRETE_DYNAMICS_WORLD
//btBulletWorldImporter* importer = new btBulletWorldImporter(m_data->m_dynamicsWorld); //btBulletWorldImporter* importer = new btBulletWorldImporter(m_data->m_dynamicsWorld);
btMultiBodyWorldImporter* importer = new btMultiBodyWorldImporter(m_data->m_dynamicsWorld); btMultiBodyWorldImporter* importer = new btMultiBodyWorldImporter(m_data->m_dynamicsWorld);
@ -13817,6 +13902,7 @@ bool PhysicsServerCommandProcessor::processLoadBulletCommand(const struct Shared
m_data->m_guiHelper->autogenerateGraphicsObjects(m_data->m_dynamicsWorld); m_data->m_guiHelper->autogenerateGraphicsObjects(m_data->m_dynamicsWorld);
} }
} }
#endif
return hasStatus; return hasStatus;
} }

25
examples/pybullet/gym/pybullet_utils/readwriteurdf.py Normal file
View File

@ -0,0 +1,25 @@
from pybullet_utils import bullet_client as bc
from pybullet_utils import urdfEditor as ed
import pybullet
import pybullet_data
import time
import argparse
parser = argparse.ArgumentParser(formatter_class=argparse.ArgumentDefaultsHelpFormatter)
parser.add_argument('--urdf_in', help='urdf file name input', default='test.urdf')
parser.add_argument('--urdf_out', help='urdf file name output', default='out.urdf')
parser.add_argument('--urdf_flags', help='urdf flags', type=int, default=0)
args = parser.parse_args()
p0 = bc.BulletClient(connection_mode=pybullet.DIRECT)
p0.setAdditionalSearchPath(pybullet_data.getDataPath())
body_id = p0.loadURDF(args.urdf_in, flags = args.urdf_flags)
ed0 = ed.UrdfEditor()
ed0.initializeFromBulletBody(body_id, p0._client)
ed0.saveUrdf(args.urdf_out)

1
examples/pybullet/gym/pybullet_utils/urdfEditor.py
View File

@ -152,6 +152,7 @@ class UrdfEditor(object):
baseLinkIndex = -1 baseLinkIndex = -1
self.convertLinkFromMultiBody(bodyUid, baseLinkIndex, baseLink, physicsClientId) self.convertLinkFromMultiBody(bodyUid, baseLinkIndex, baseLink, physicsClientId)
baseLink.link_name = p.getBodyInfo(bodyUid, physicsClientId=physicsClientId)[0].decode("utf-8") baseLink.link_name = p.getBodyInfo(bodyUid, physicsClientId=physicsClientId)[0].decode("utf-8")
self.robotName = p.getBodyInfo(bodyUid, physicsClientId=physicsClientId)[1].decode("utf-8")
self.linkNameToIndex[baseLink.link_name] = len(self.urdfLinks) self.linkNameToIndex[baseLink.link_name] = len(self.urdfLinks)
self.urdfLinks.append(baseLink) self.urdfLinks.append(baseLink)

4
src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp
View File

@ -532,7 +532,7 @@ void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal)
J_transpose = J.transpose(); J_transpose = J.transpose();
btMatrixXu& tmp = m_scratchTmp; btMatrixXu& tmp = m_scratchTmp;
//Minv.printMatrix("Minv=");
{ {
{ {
BT_PROFILE("J*Minv"); BT_PROFILE("J*Minv");
@ -543,7 +543,7 @@ void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal)
m_A = tmp * J_transpose; m_A = tmp * J_transpose;
} }
} }
//J.printMatrix("J");
if (1) if (1)
{ {
// add cfm to the diagonal of m_A // add cfm to the diagonal of m_A

2
src/BulletSoftBody/btDeformableMousePickingForce.h
View File

@ -29,7 +29,7 @@ class btDeformableMousePickingForce : public btDeformableLagrangianForce
public: public:
typedef btAlignedObjectArray<btVector3> TVStack; typedef btAlignedObjectArray<btVector3> TVStack;
btDeformableMousePickingForce(btScalar k, btScalar d, const btSoftBody::Face& face, btVector3 mouse_pos, btScalar maxForce = 0.3) : m_elasticStiffness(k), m_dampingStiffness(d), m_face(face), m_mouse_pos(mouse_pos), m_maxForce(maxForce) btDeformableMousePickingForce(btScalar k, btScalar d, const btSoftBody::Face& face, const btVector3& mouse_pos, btScalar maxForce = 0.3) : m_elasticStiffness(k), m_dampingStiffness(d), m_face(face), m_mouse_pos(mouse_pos), m_maxForce(maxForce)
{ {
} }