AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-13 13:20:07 +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);
}
void addCloth(btVector3 origin);
void addCloth(const btVector3& origin);
virtual void renderScene()
{
@ -126,7 +126,7 @@ void DeformableSelfCollision::initPhysics()
getDeformableDynamicsWorld()->setLineSearch(false);
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
}
void DeformableSelfCollision::addCloth(btVector3 origin)
void DeformableSelfCollision::addCloth(const btVector3& origin)
// create a piece of cloth
{
const btScalar s = 0.6;

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

@ -1,7 +1,11 @@
#include "GwenParameterInterface.h"
#include "gwenInternalData.h"
#include <cstring>
#ifdef _WIN32
#define safe_printf _snprintf
#else
#define safe_printf snprintf
#endif
struct MyButtonEventHandler : public Gwen::Event::Handler
{
Gwen::Controls::Button* m_buttonControl;
@ -85,7 +89,7 @@ struct MySliderEventHandler : public Gwen::Event::Handler
if (m_showValue)
{
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);
}
}
@ -231,7 +235,7 @@ void GwenParameterInterface::registerSliderFloatParameter(SliderParams& params)
}
pSlider->SetValue(*params.m_paramValuePointer); //dimensions[i] );
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);
handler->m_showValue = params.m_showValues;
m_paramInternalData->m_sliderEventHandlers.push_back(handler);

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

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

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

@ -8,6 +8,7 @@
class btVector3;
class btTransform;
class btMultiBodyDynamicsWorld;
class btDiscreteDynamicsWorld;
class btTransform;
class URDFImporterInterface;
@ -20,7 +21,18 @@ struct UrdfVisualShapeCache
btAlignedObjectArray<UrdfMaterialColor> m_cachedUrdfLinkColors;
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,
MultiBodyCreationInterface& creationCallback,
const btTransform& rootTransformInWorldSpace,
@ -29,5 +41,5 @@ void ConvertURDF2Bullet(const URDFImporterInterface& u2b,
const char* pathPrefix,
int flags = 0,
UrdfVisualShapeCache* cachedLinkGraphicsShapes = 0);
#endif
#endif //_URDF2BULLET_H

8
examples/SharedMemory/PhysicsClientExample.cpp
View File

@ -13,6 +13,12 @@
#include "PhysicsDirectC_API.h"
#include "PhysicsClientC_API.h"
#include "PhysicsServerSharedMemory.h"
#include <stdio.h>
#ifdef _WIN32
#define safe_printf _snprintf
#else
#define safe_printf snprintf
#endif
struct MyMotorInfo2
{
btScalar m_velTarget;
@ -655,7 +661,7 @@ void PhysicsClientExample::createButtons()
if (m_numMotors < MAX_NUM_MOTORS)
{
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_motorTargetPositions[m_numMotors];
motorInfo->m_velTarget = 0.f;

104
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@ -12,10 +12,15 @@
#include "../Importers/ImportMeshUtility/b3ImportMeshUtility.h"
#include "BulletDynamics/MLCPSolvers/btDantzigSolver.h"
#include "BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h"
#include "BulletDynamics/MLCPSolvers/btMLCPSolver.h"
#include "BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h"
#include "BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.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 "BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h"
@ -1626,7 +1631,11 @@ struct PhysicsServerCommandProcessorInternalData
b3AlignedObjectArray<SaveWorldObjectData> m_saveWorldBodyData;
#ifdef USE_DISCRETE_DYNAMICS_WORLD
btAlignedObjectArray<btWorldImporter*> m_worldImporters;
#else
btAlignedObjectArray<btMultiBodyWorldImporter*> m_worldImporters;
#endif
btAlignedObjectArray<std::string*> m_strings;
@ -1641,8 +1650,12 @@ struct PhysicsServerCommandProcessorInternalData
btHashedOverlappingPairCache* m_pairCache;
btBroadphaseInterface* m_broadphase;
btCollisionDispatcher* m_dispatcher;
#ifdef USE_DISCRETE_DYNAMICS_WORLD
btSequentialImpulseConstraintSolver* m_solver;
#else
btMultiBodyConstraintSolver* m_solver;
#endif
btDefaultCollisionConfiguration* m_collisionConfiguration;
@ -1654,7 +1667,12 @@ struct PhysicsServerCommandProcessorInternalData
btAlignedObjectArray<btDeformableLagrangianForce*> m_lf;
#endif
#ifdef USE_DISCRETE_DYNAMICS_WORLD
btDiscreteDynamicsWorld* m_dynamicsWorld;
#else
btMultiBodyDynamicsWorld* m_dynamicsWorld;
#endif
int m_constraintSolverType;
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)))
{
m_data->m_solver = new btMultiBodyConstraintSolver;
#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);
#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);
#endif
#endif
}
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_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
@ -2860,13 +2890,14 @@ void PhysicsServerCommandProcessor::deleteDynamicsWorld()
constraints.push_back(constraint);
m_data->m_dynamicsWorld->removeConstraint(constraint);
}
#ifndef USE_DISCRETE_DYNAMICS_WORLD
for (i = m_data->m_dynamicsWorld->getNumMultiBodyConstraints() - 1; i >= 0; i--)
{
btMultiBodyConstraint* mbconstraint = m_data->m_dynamicsWorld->getMultiBodyConstraint(i);
mbconstraints.push_back(mbconstraint);
m_data->m_dynamicsWorld->removeMultiBodyConstraint(mbconstraint);
}
#endif
for (i = m_data->m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--)
{
btCollisionObject* obj = m_data->m_dynamicsWorld->getCollisionObjectArray()[i];
@ -2878,12 +2909,14 @@ void PhysicsServerCommandProcessor::deleteDynamicsWorld()
m_data->m_dynamicsWorld->removeCollisionObject(obj);
delete obj;
}
#ifndef USE_DISCRETE_DYNAMICS_WORLD
for (i = m_data->m_dynamicsWorld->getNumMultibodies() - 1; i >= 0; i--)
{
btMultiBody* mb = m_data->m_dynamicsWorld->getMultiBody(i);
m_data->m_dynamicsWorld->removeMultiBody(mb);
delete mb;
}
#endif
#ifndef SKIP_DEFORMABLE_BODY
for (int j = 0; j < m_data->m_lf.size(); j++)
{
@ -3052,14 +3085,18 @@ void PhysicsServerCommandProcessor::createJointMotors(btMultiBody* mb)
//motor->setRhsClamp(gRhsClamp);
//motor->setMaxAppliedImpulse(0);
mb->getLink(mbLinkIndex).m_userPtr = motor;
#ifndef USE_DISCRETE_DYNAMICS_WORLD
m_data->m_dynamicsWorld->addMultiBodyConstraint(motor);
#endif
motor->finalizeMultiDof();
}
if (mb->getLink(mbLinkIndex).m_jointType == btMultibodyLink::eSpherical)
{
btMultiBodySphericalJointMotor* motor = new btMultiBodySphericalJointMotor(mb, mbLinkIndex, 1000 * maxMotorImpulse);
mb->getLink(mbLinkIndex).m_userPtr = motor;
#ifndef USE_DISCRETE_DYNAMICS_WORLD
m_data->m_dynamicsWorld->addMultiBodyConstraint(motor);
#endif
motor->finalizeMultiDof();
}
}
@ -3166,11 +3203,15 @@ bool PhysicsServerCommandProcessor::processImportedObjects(const char* fileName,
}
}
UrdfVisualShapeCache* cachedVisualShapesPtr = m_data->m_cachedVUrdfisualShapes[fileName];
ConvertURDF2Bullet(u2b, creation, rootTrans, m_data->m_dynamicsWorld, useMultiBody, u2b.getPathPrefix(), flags, cachedVisualShapesPtr);
}
else
{
ConvertURDF2Bullet(u2b, creation, rootTrans, m_data->m_dynamicsWorld, useMultiBody, u2b.getPathPrefix(), flags);
}
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)
{
std::string fileName = clientCmd.m_stateLoggingArguments.m_fileName;
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_stateLoggingResultArgs.m_loggingUniqueId = loggerUid;
}
#endif
if (clientCmd.m_stateLoggingArguments.m_logType == STATE_LOGGING_VR_CONTROLLERS)
{
std::string fileName = clientCmd.m_stateLoggingArguments.m_fileName;
@ -4733,7 +4776,11 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
bool hasStatus = true;
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);
#endif
btCollisionShape* shape = 0;
b3AlignedObjectArray<UrdfCollision> urdfCollisionObjects;
@ -9444,6 +9491,7 @@ bool PhysicsServerCommandProcessor::processForwardDynamicsCommand(const struct S
b3Printf("Step simulation request");
b3Printf("CMD_STEP_FORWARD_SIMULATION clientCmd = %d\n", clientCmd.m_sequenceNumber);
}
#ifndef USE_DISCRETE_DYNAMICS_WORLD
///todo(erwincoumans) move this damping inside Bullet
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;
int numSteps = 0;
@ -9483,8 +9531,9 @@ bool PhysicsServerCommandProcessor::processForwardDynamicsCommand(const struct S
serverCmd.m_forwardDynamicsAnalyticsArgs.m_numSteps = numSteps;
btAlignedObjectArray<btSolverAnalyticsData> islandAnalyticsData;
#ifndef USE_DISCRETE_DYNAMICS_WORLD
m_data->m_dynamicsWorld->getAnalyticsData(islandAnalyticsData);
#endif
serverCmd.m_forwardDynamicsAnalyticsArgs.m_numIslands = islandAnalyticsData.size();
int numIslands = btMin(islandAnalyticsData.size(), MAX_ISLANDS_ANALYTICS);
@ -9555,7 +9604,7 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc
if (bodyUniqueId >= 0)
{
InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
#ifndef USE_DISCRETE_DYNAMICS_WORLD
if (body && body->m_multiBody)
{
btMultiBody* mb = body->m_multiBody;
@ -9741,6 +9790,7 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc
}
}
}
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_JOINT_LIMITS)
{
@ -9875,6 +9925,7 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc
}
}
else
#endif
{
btRigidBody* rb = 0;
if (body && body->m_rigidBody)
@ -10401,7 +10452,11 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st
btConstraintSolver* oldSolver = m_data->m_dynamicsWorld->getConstraintSolver();
#ifdef USE_DISCRETE_DYNAMICS_WORLD
btSequentialImpulseConstraintSolver* newSolver = 0;
#else
btMultiBodyConstraintSolver* newSolver = 0;
#endif
switch (clientCmd.m_physSimParamArgs.m_constraintSolverType)
{
@ -10414,7 +10469,12 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st
case eConstraintSolverLCP_PGS:
{
btSolveProjectedGaussSeidel* mlcp = new btSolveProjectedGaussSeidel();
#ifdef USE_DISCRETE_DYNAMICS_WORLD
newSolver = new btMLCPSolver(mlcp);
#else
newSolver = new btMultiBodyMLCPConstraintSolver(mlcp);
#endif
b3Printf("PyBullet: Constraint Solver: MLCP + PGS\n");
break;
}
@ -10438,7 +10498,11 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st
{
delete oldSolver;
#ifdef USE_DISCRETE_DYNAMICS_WORLD
m_data->m_dynamicsWorld->setConstraintSolver(newSolver);
#else
m_data->m_dynamicsWorld->setMultiBodyConstraintSolver(newSolver);
#endif
m_data->m_solver = newSolver;
printf("switched solver\n");
}
@ -10840,7 +10904,11 @@ bool PhysicsServerCommandProcessor::processCreateRigidBodyCommand(const struct S
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);
#endif
m_data->m_worldImporters.push_back(worldImporter);
btCollisionShape* shape = 0;
@ -11585,6 +11653,7 @@ bool PhysicsServerCommandProcessor::processRemoveBodyCommand(const struct Shared
InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(bodyUniqueId);
if (bodyHandle)
{
#ifndef USE_DISCRETE_DYNAMICS_WORLD
if (bodyHandle->m_multiBody)
{
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;
serverCmd.m_type = CMD_REMOVE_BODY_COMPLETED;
}
#endif
if (bodyHandle->m_rigidBody)
{
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++)
{
#ifdef USE_DISCRETE_DYNAMICS_WORLD
btWorldImporter* importer = m_data->m_worldImporters[i];
#else
btMultiBodyWorldImporter* importer = m_data->m_worldImporters[i];
#endif
for (int c = 0; c < importer->getNumCollisionShapes(); c++)
{
if (importer->getCollisionShapeByIndex(c) == handle->m_collisionShape)
@ -11747,7 +11821,11 @@ bool PhysicsServerCommandProcessor::processRemoveBodyCommand(const struct Shared
}
if (foundIndex >= 0)
{
#ifdef USE_DISCRETE_DYNAMICS_WORLD
btWorldImporter* importer = m_data->m_worldImporters[foundIndex];
#else
btMultiBodyWorldImporter* importer = m_data->m_worldImporters[foundIndex];
#endif
m_data->m_worldImporters.removeAtIndex(foundIndex);
importer->deleteAllData();
delete importer;
@ -11901,6 +11979,7 @@ bool PhysicsServerCommandProcessor::processCreateUserConstraintCommand(const str
{
btScalar defaultMaxForce = 500.0;
InternalBodyData* parentBody = m_data->m_bodyHandles.getHandle(clientCmd.m_userConstraintArguments.m_parentBodyIndex);
#ifndef USE_DISCRETE_DYNAMICS_WORLD
if (parentBody && parentBody->m_multiBody)
{
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
#endif
{
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);
if (userConstraintPtr)
{
#ifndef USE_DISCRETE_DYNAMICS_WORLD
if (userConstraintPtr->m_mbConstraint)
{
m_data->m_dynamicsWorld->removeMultiBodyConstraint(userConstraintPtr->m_mbConstraint);
delete userConstraintPtr->m_mbConstraint;
m_data->m_userConstraints.remove(userConstraintUidRemove);
}
#endif//USE_DISCRETE_DYNAMICS_WORLD
if (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)
{
BT_PROFILE("CMD_RESTORE_STATE");
bool hasStatus = true;
SharedMemoryStatus& serverCmd = serverStatusOut;
serverCmd.m_type = CMD_RESTORE_STATE_FAILED;
#ifndef USE_DISCRETE_DYNAMICS_WORLD
btMultiBodyWorldImporter* importer = new btMultiBodyWorldImporter(m_data->m_dynamicsWorld);
importer->setImporterFlags(eRESTORE_EXISTING_OBJECTS);
bool ok = false;
@ -13727,7 +13811,7 @@ bool PhysicsServerCommandProcessor::processRestoreStateCommand(const struct Shar
{
serverCmd.m_type = CMD_RESTORE_STATE_COMPLETED;
}
#endif
return hasStatus;
}
@ -13739,6 +13823,7 @@ bool PhysicsServerCommandProcessor::processLoadBulletCommand(const struct Shared
SharedMemoryStatus& serverCmd = serverStatusOut;
serverCmd.m_type = CMD_BULLET_LOADING_FAILED;
#ifndef USE_DISCRETE_DYNAMICS_WORLD
//btBulletWorldImporter* importer = new btBulletWorldImporter(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);
}
}
#endif
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
self.convertLinkFromMultiBody(bodyUid, baseLinkIndex, baseLink, physicsClientId)
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.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();
btMatrixXu& tmp = m_scratchTmp;
//Minv.printMatrix("Minv=");
{
{
BT_PROFILE("J*Minv");
@ -543,7 +543,7 @@ void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal)
m_A = tmp * J_transpose;
}
}
//J.printMatrix("J");
if (1)
{
// add cfm to the diagonal of m_A

2
src/BulletSoftBody/btDeformableMousePickingForce.h
View File

@ -29,7 +29,7 @@ class btDeformableMousePickingForce : public btDeformableLagrangianForce
public:
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)
{
}