Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Merge pull request #1254 from erwincoumans/master

Browse Source 
Expose optional "globalScaling" factor to pybullet.loadURDF/SDF, and add pybullet.connect(p.GUI_SERVER option, that allows shared memory connections.
This commit is contained in:
erwincoumans
2017-08-16 09:30:23 -07:00
committed by GitHub
parent dfaa717fed 2e0a987750
commit b7579cbdda
24 changed files with 213 additions and 82 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
Extras/InverseDynamics/btMultiBodyFromURDF.hpp
View File

@@ -44,7 +44,7 @@ public:
void init() {
this->createEmptyDynamicsWorld();
m_dynamicsWorld->setGravity(m_gravity);
BulletURDFImporter urdf_importer(&m_nogfx,0);
BulletURDFImporter urdf_importer(&m_nogfx,0,1);
URDFImporterInterface &u2b(urdf_importer);
bool loadOk = u2b.loadURDF(m_filename.c_str(), m_base_fixed);

23
data/two_cubes.sdf
View File

@@ -142,7 +142,7 @@
</link>
</model>
<model name='unit_box_0_clone'>
<pose frame=''>0.105158 -4.55002 0.499995 -2.89297 -0.988287 -3.14159</pose>
<pose frame=''>0.105158 -4.55002 1.499995 -2.89297 -0.988287 -3.14159</pose>
<link name='unit_box_0::link'>
<inertial>
<mass>1</mass>
@@ -155,7 +155,26 @@
<izz>0.166667</izz>
</inertia>
</inertial>
<collision name='collision1'>
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='visual'>
<geometry>
<mesh>

9
examples/ExampleBrowser/InProcessExampleBrowser.cpp
View File

@@ -316,11 +316,12 @@ struct btInProcessExampleBrowserInternalData
btInProcessExampleBrowserInternalData* btCreateInProcessExampleBrowser(int argc,char** argv2)
btInProcessExampleBrowserInternalData* btCreateInProcessExampleBrowser(int argc,char** argv2, bool useInProcessMemory)
{
btInProcessExampleBrowserInternalData* data = new btInProcessExampleBrowserInternalData;
data->m_sharedMem = new InProcessMemory;
data->m_sharedMem = useInProcessMemory ? new InProcessMemory : 0;
int numThreads = 1;
int i;
@@ -410,12 +411,12 @@ struct btInProcessExampleBrowserMainThreadInternalData
b3Clock m_clock;
};
btInProcessExampleBrowserMainThreadInternalData* btCreateInProcessExampleBrowserMainThread(int argc,char** argv)
btInProcessExampleBrowserMainThreadInternalData* btCreateInProcessExampleBrowserMainThread(int argc,char** argv, bool useInProcessMemory)
{
btInProcessExampleBrowserMainThreadInternalData* data = new btInProcessExampleBrowserMainThreadInternalData;
data->m_examples.initExampleEntries();
data->m_exampleBrowser = new DefaultBrowser(&data->m_examples);
data->m_sharedMem = new InProcessMemory;
data->m_sharedMem = useInProcessMemory ? new InProcessMemory : 0;
data->m_exampleBrowser->setSharedMemoryInterface(data->m_sharedMem );
bool init;
init = data->m_exampleBrowser->init(argc,argv);

4
examples/ExampleBrowser/InProcessExampleBrowser.h
View File

@@ -3,7 +3,7 @@
struct btInProcessExampleBrowserInternalData;
btInProcessExampleBrowserInternalData* btCreateInProcessExampleBrowser(int argc,char** argv2);
btInProcessExampleBrowserInternalData* btCreateInProcessExampleBrowser(int argc,char** argv2, bool useInProcessMemory);
bool btIsExampleBrowserTerminated(btInProcessExampleBrowserInternalData* data);
@@ -17,7 +17,7 @@ class SharedMemoryInterface* btGetSharedMemoryInterface(btInProcessExampleBrowse
struct btInProcessExampleBrowserMainThreadInternalData;
btInProcessExampleBrowserMainThreadInternalData* btCreateInProcessExampleBrowserMainThread(int argc,char** argv2);
btInProcessExampleBrowserMainThreadInternalData* btCreateInProcessExampleBrowserMainThread(int argc,char** argv2, bool useInProcessMemory);
bool btIsExampleBrowserMainThreadTerminated(btInProcessExampleBrowserMainThreadInternalData* data);

2
examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp
View File

@@ -187,7 +187,7 @@ void ImportSDFSetup::initPhysics()
m_dynamicsWorld->setGravity(gravity);
BulletURDFImporter u2b(m_guiHelper, 0);
BulletURDFImporter u2b(m_guiHelper, 0,1);
bool loadOk = u2b.loadSDF(m_fileName);

10
examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
View File

@@ -69,6 +69,12 @@ ATTRIBUTE_ALIGNED16(struct) BulletURDFInternalData
{
m_pathPrefix[0] = 0;
}
void setGlobalScaling(btScalar scaling)
{
m_urdfParser.setGlobalScaling(scaling);
}
};
void BulletURDFImporter::printTree()
@@ -76,10 +82,10 @@ void BulletURDFImporter::printTree()
// btAssert(0);
}
BulletURDFImporter::BulletURDFImporter(struct GUIHelperInterface* helper, LinkVisualShapesConverter* customConverter)
BulletURDFImporter::BulletURDFImporter(struct GUIHelperInterface* helper, LinkVisualShapesConverter* customConverter, btScalar globalScaling)
{
m_data = new BulletURDFInternalData;
m_data->setGlobalScaling(globalScaling);
m_data->m_guiHelper = helper;
m_data->m_customVisualShapesConverter = customConverter;

15
examples/Importers/ImportURDFDemo/BulletUrdfImporter.h
View File

@@ -15,18 +15,19 @@ class BulletURDFImporter : public URDFImporterInterface
public:
BulletURDFImporter(struct GUIHelperInterface* guiHelper, LinkVisualShapesConverter* customConverter);
BulletURDFImporter(struct GUIHelperInterface* helper, LinkVisualShapesConverter* customConverter, btScalar globalScaling);
virtual ~BulletURDFImporter();
virtual bool loadURDF(const char* fileName, bool forceFixedBase = false);
//warning: some quick test to load SDF: we 'activate' a model, so we can re-use URDF code path
virtual bool loadSDF(const char* fileName, bool forceFixedBase = false);
virtual int getNumModels() const;
virtual void activateModel(int modelIndex);
virtual void setBodyUniqueId(int bodyId);
virtual int getBodyUniqueId() const;
//warning: some quick test to load SDF: we 'activate' a model, so we can re-use URDF code path
virtual bool loadSDF(const char* fileName, bool forceFixedBase = false);
virtual int getNumModels() const;
virtual void activateModel(int modelIndex);
virtual void setBodyUniqueId(int bodyId);
virtual int getBodyUniqueId() const;
const char* getPathPrefix();
void printTree(); //for debugging

2
examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp
View File

@@ -194,7 +194,7 @@ void ImportUrdfSetup::initPhysics()
}
BulletURDFImporter u2b(m_guiHelper, 0);
BulletURDFImporter u2b(m_guiHelper, 0,1);
bool loadOk = u2b.loadURDF(m_fileName);

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

@@ -173,6 +173,7 @@ void processContactParameters(const URDFLinkContactInfo& contactInfo, btCollisio
}
btScalar tmpUrdfScaling=2;
void ConvertURDF2BulletInternal(

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

@@ -6,7 +6,8 @@
UrdfParser::UrdfParser()
:m_parseSDF(false),
m_activeSdfModel(-1)
m_activeSdfModel(-1),
m_urdfScaling(1)
{
m_urdf2Model.m_sourceFile = "IN_MEMORY_STRING"; // if loadUrdf() called later, source file name will be replaced with real
}
@@ -132,22 +133,24 @@ bool UrdfParser::parseMaterial(UrdfMaterial& material, TiXmlElement *config, Err
}
bool parseTransform(btTransform& tr, TiXmlElement* xml, ErrorLogger* logger, bool parseSDF = false)
bool UrdfParser::parseTransform(btTransform& tr, TiXmlElement* xml, ErrorLogger* logger, bool parseSDF )
{
tr.setIdentity();
btVector3 vec(0,0,0);
if (parseSDF)
{
parseVector3(tr.getOrigin(),std::string(xml->GetText()),logger);
parseVector3(vec,std::string(xml->GetText()),logger);
}
else
{
const char* xyz_str = xml->Attribute("xyz");
if (xyz_str)
{
parseVector3(tr.getOrigin(),std::string(xyz_str),logger);
parseVector3(vec,std::string(xyz_str),logger);
}
}
tr.setOrigin(vec*m_urdfScaling);
if (parseSDF)
{
@@ -344,7 +347,7 @@ bool UrdfParser::parseGeometry(UrdfGeometry& geom, TiXmlElement* g, ErrorLogger*
return false;
} else
{
geom.m_sphereRadius = urdfLexicalCast<double>(shape->Attribute("radius"));
geom.m_sphereRadius = m_urdfScaling * urdfLexicalCast<double>(shape->Attribute("radius"));
}
}
else if (type_name == "box")
@@ -359,6 +362,7 @@ bool UrdfParser::parseGeometry(UrdfGeometry& geom, TiXmlElement* g, ErrorLogger*
return false;
}
parseVector3(geom.m_boxSize,size->GetText(),logger);
geom.m_boxSize *= m_urdfScaling;
}
else
{
@@ -369,6 +373,7 @@ bool UrdfParser::parseGeometry(UrdfGeometry& geom, TiXmlElement* g, ErrorLogger*
} else
{
parseVector3(geom.m_boxSize,shape->Attribute("size"),logger);
geom.m_boxSize *= m_urdfScaling;
}
}
}
@@ -382,8 +387,8 @@ bool UrdfParser::parseGeometry(UrdfGeometry& geom, TiXmlElement* g, ErrorLogger*
return false;
}
geom.m_hasFromTo = false;
geom.m_capsuleRadius = urdfLexicalCast<double>(shape->Attribute("radius"));
geom.m_capsuleHeight = urdfLexicalCast<double>(shape->Attribute("length"));
geom.m_capsuleRadius = m_urdfScaling * urdfLexicalCast<double>(shape->Attribute("radius"));
geom.m_capsuleHeight = m_urdfScaling * urdfLexicalCast<double>(shape->Attribute("length"));
}
else if (type_name == "capsule")
@@ -396,8 +401,8 @@ bool UrdfParser::parseGeometry(UrdfGeometry& geom, TiXmlElement* g, ErrorLogger*
return false;
}
geom.m_hasFromTo = false;
geom.m_capsuleRadius = urdfLexicalCast<double>(shape->Attribute("radius"));
geom.m_capsuleHeight = urdfLexicalCast<double>(shape->Attribute("length"));
geom.m_capsuleRadius = m_urdfScaling * urdfLexicalCast<double>(shape->Attribute("radius"));
geom.m_capsuleHeight = m_urdfScaling * urdfLexicalCast<double>(shape->Attribute("length"));
}
else if (type_name == "mesh")
{
@@ -438,6 +443,8 @@ bool UrdfParser::parseGeometry(UrdfGeometry& geom, TiXmlElement* g, ErrorLogger*
}
}
geom.m_meshScale *= m_urdfScaling;
if (fn.empty())
{
logger->reportError("Mesh filename is empty");
@@ -1031,6 +1038,11 @@ bool UrdfParser::parseJointLimits(UrdfJoint& joint, TiXmlElement* config, ErrorL
joint.m_upperLimit = urdfLexicalCast<double>(upper_str);
}
if (joint.m_type == URDFPrismaticJoint)
{
joint.m_lowerLimit *= m_urdfScaling;
joint.m_upperLimit *= m_urdfScaling;
}
// Get joint effort limit
const char* effort_str = config->Attribute("effort");

8
examples/Importers/ImportURDFDemo/UrdfParser.h
View File

@@ -251,7 +251,8 @@ protected:
bool m_parseSDF;
int m_activeSdfModel;
btScalar m_urdfScaling;
bool parseTransform(btTransform& tr, class TiXmlElement* xml, ErrorLogger* logger, bool parseSDF = false);
bool parseInertia(UrdfInertia& inertia, class TiXmlElement* config, ErrorLogger* logger);
bool parseGeometry(UrdfGeometry& geom, class TiXmlElement* g, ErrorLogger* logger);
bool parseVisual(UrdfModel& model, UrdfVisual& visual, class TiXmlElement* config, ErrorLogger* logger);
@@ -277,6 +278,11 @@ public:
{
return m_parseSDF;
}
void setGlobalScaling(btScalar scaling)
{
m_urdfScaling = scaling;
}
bool loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceFixedBase);
bool loadSDF(const char* sdfText, ErrorLogger* logger);

2
examples/InverseDynamics/InverseDynamicsExample.cpp
View File

@@ -156,7 +156,7 @@ void InverseDynamicsExample::initPhysics()
BulletURDFImporter u2b(m_guiHelper,0);
BulletURDFImporter u2b(m_guiHelper,0,1);
bool loadOk = u2b.loadURDF("kuka_iiwa/model.urdf");// lwr / kuka.urdf");
if (loadOk)
{

15
examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp
View File

@@ -129,9 +129,20 @@ bool b3RobotSimulatorClientAPI::connect(int mode, const std::string& hostName, i
int argc = 0;
char* argv[1] = {0};
#ifdef __APPLE__
sm = b3CreateInProcessPhysicsServerAndConnectMainThread(argc, argv);
sm = b3CreateInProcessPhysicsServerAndConnectMainThread(argc, argv,1);
#else
sm = b3CreateInProcessPhysicsServerAndConnect(argc, argv);
sm = b3CreateInProcessPhysicsServerAndConnect(argc, argv,1);
#endif
break;
}
case eCONNECT_GUI_SERVER:
{
int argc = 0;
char* argv[1] = {0};
#ifdef __APPLE__
sm = b3CreateInProcessPhysicsServerAndConnectMainThread(argc, argv,0);
#else
sm = b3CreateInProcessPhysicsServerAndConnect(argc, argv,0);
#endif
break;
}

25
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -223,6 +223,18 @@ int b3LoadUrdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle,
return 0;
}
int b3LoadUrdfCommandSetGlobalScaling(b3SharedMemoryCommandHandle commandHandle, double globalScaling)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_LOAD_URDF);
command->m_updateFlags |=URDF_ARGS_USE_GLOBAL_SCALING;
command->m_urdfArguments.m_globalScaling = globalScaling;
return 0;
}
int b3LoadSdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
@@ -234,6 +246,19 @@ int b3LoadSdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, i
return 0;
}
int b3LoadSdfCommandSetUseGlobalScaling(b3SharedMemoryCommandHandle commandHandle, double globalScaling)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_LOAD_SDF);
command->m_updateFlags |=URDF_ARGS_USE_GLOBAL_SCALING;
command->m_sdfArguments.m_globalScaling = globalScaling;
return 0;
}
int b3LoadUrdfCommandSetUseFixedBase(b3SharedMemoryCommandHandle commandHandle, int useFixedBase)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;

6
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -267,6 +267,9 @@ int b3LoadUrdfCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHand
int b3LoadUrdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody);
int b3LoadUrdfCommandSetUseFixedBase(b3SharedMemoryCommandHandle commandHandle, int useFixedBase);
int b3LoadUrdfCommandSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags);
int b3LoadUrdfCommandSetGlobalScaling(b3SharedMemoryCommandHandle commandHandle, double globalScaling);
b3SharedMemoryCommandHandle b3LoadBulletCommandInit(b3PhysicsClientHandle physClient, const char* fileName);
b3SharedMemoryCommandHandle b3SaveBulletCommandInit(b3PhysicsClientHandle physClient, const char* fileName);
@@ -301,6 +304,9 @@ int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle status
b3SharedMemoryCommandHandle b3LoadSdfCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName);
int b3LoadSdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody);
int b3LoadSdfCommandSetUseGlobalScaling(b3SharedMemoryCommandHandle commandHandle, double globalScaling);
b3SharedMemoryCommandHandle b3SaveWorldCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName);

18
examples/SharedMemory/PhysicsClientSharedMemory.cpp
View File

@@ -251,17 +251,23 @@ void PhysicsClientSharedMemory::resetData()
m_data->m_userConstraintInfoMap.clear();
}
void PhysicsClientSharedMemory::setSharedMemoryKey(int key) { m_data->m_sharedMemoryKey = key; }
void PhysicsClientSharedMemory::setSharedMemoryKey(int key)
{
m_data->m_sharedMemoryKey = key;
}
void PhysicsClientSharedMemory::setSharedMemoryInterface(class SharedMemoryInterface* sharedMem)
{
if (m_data->m_sharedMemory && m_data->m_ownsSharedMemory)
if (sharedMem)
{
delete m_data->m_sharedMemory;
}
m_data->m_ownsSharedMemory = false;
m_data->m_sharedMemory = sharedMem;
if (m_data->m_sharedMemory && m_data->m_ownsSharedMemory)
{
delete m_data->m_sharedMemory;
}
m_data->m_ownsSharedMemory = false;
m_data->m_sharedMemory = sharedMem;
};
}
void PhysicsClientSharedMemory::disconnectSharedMemory() {

40
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -2284,7 +2284,7 @@ bool PhysicsServerCommandProcessor::loadMjcf(const char* fileName, char* bufferS
return loadOk;
}
bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes, bool useMultiBody, int flags)
bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes, bool useMultiBody, int flags, btScalar globalScaling)
{
btAssert(m_data->m_dynamicsWorld);
if (!m_data->m_dynamicsWorld)
@@ -2295,7 +2295,7 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe
m_data->m_sdfRecentLoadedBodies.clear();
BulletURDFImporter u2b(m_data->m_guiHelper, &m_data->m_visualConverter);
BulletURDFImporter u2b(m_data->m_guiHelper, &m_data->m_visualConverter, globalScaling);
bool forceFixedBase = false;
bool loadOk =u2b.loadSDF(fileName,forceFixedBase);
@@ -2311,7 +2311,7 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe
bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVector3& pos, const btQuaternion& orn,
bool useMultiBody, bool useFixedBase, int* bodyUniqueIdPtr, char* bufferServerToClient, int bufferSizeInBytes, int flags)
bool useMultiBody, bool useFixedBase, int* bodyUniqueIdPtr, char* bufferServerToClient, int bufferSizeInBytes, int flags, btScalar globalScaling)
{
m_data->m_sdfRecentLoadedBodies.clear();
*bodyUniqueIdPtr = -1;
@@ -2326,7 +2326,7 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto
BulletURDFImporter u2b(m_data->m_guiHelper, &m_data->m_visualConverter);
BulletURDFImporter u2b(m_data->m_guiHelper, &m_data->m_visualConverter, globalScaling);
bool loadOk = u2b.loadURDF(fileName, useFixedBase);
@@ -3579,7 +3579,12 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
bool useMultiBody=(clientCmd.m_updateFlags & URDF_ARGS_USE_MULTIBODY) ? (sdfArgs.m_useMultiBody!=0) : true;
int flags = CUF_USE_SDF; //CUF_USE_URDF_INERTIA
bool completedOk = loadSdf(sdfArgs.m_sdfFileName,bufferServerToClient, bufferSizeInBytes, useMultiBody, flags);
btScalar globalScaling = 1.f;
if (clientCmd.m_updateFlags & URDF_ARGS_USE_GLOBAL_SCALING)
{
globalScaling = sdfArgs.m_globalScaling;
}
bool completedOk = loadSdf(sdfArgs.m_sdfFileName,bufferServerToClient, bufferSizeInBytes, useMultiBody, flags, globalScaling);
if (completedOk)
{
m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld);
@@ -4016,10 +4021,15 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
bool useMultiBody=(clientCmd.m_updateFlags & URDF_ARGS_USE_MULTIBODY) ? (urdfArgs.m_useMultiBody!=0) : true;
bool useFixedBase = (clientCmd.m_updateFlags & URDF_ARGS_USE_FIXED_BASE) ? (urdfArgs.m_useFixedBase!=0): false;
int bodyUniqueId;
btScalar globalScaling = 1.f;
if (clientCmd.m_updateFlags & URDF_ARGS_USE_GLOBAL_SCALING)
{
globalScaling = urdfArgs.m_globalScaling;
}
//load the actual URDF and send a report: completed or failed
bool completedOk = loadUrdf(urdfArgs.m_urdfFileName,
initialPos,initialOrn,
useMultiBody, useFixedBase,&bodyUniqueId, bufferServerToClient, bufferSizeInBytes, urdfFlags);
useMultiBody, useFixedBase,&bodyUniqueId, bufferServerToClient, bufferSizeInBytes, urdfFlags, globalScaling);
if (completedOk && bodyUniqueId>=0)
{
@@ -6070,28 +6080,30 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
if ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_FORCE)!=0)
{
btVector3 forceLocal(clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+0],
btVector3 tmpForce(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(
btVector3 tmpPosition(
clientCmd.m_externalForceArguments.m_positions[i*3+0],
clientCmd.m_externalForceArguments.m_positions[i*3+1],
clientCmd.m_externalForceArguments.m_positions[i*3+2]);
if (clientCmd.m_externalForceArguments.m_linkIds[i] == -1)
{
btVector3 forceWorld = isLinkFrame ? forceLocal : mb->getBaseWorldTransform().getBasis()*forceLocal;
btVector3 relPosWorld = isLinkFrame ? positionLocal : mb->getBaseWorldTransform().getBasis()*positionLocal;
btVector3 forceWorld = isLinkFrame ? mb->getBaseWorldTransform().getBasis()*tmpForce : tmpForce;
btVector3 relPosWorld = isLinkFrame ? mb->getBaseWorldTransform().getBasis()*tmpPosition : tmpPosition - mb->getBaseWorldTransform().getOrigin();
mb->addBaseForce(forceWorld);
mb->addBaseTorque(relPosWorld.cross(forceWorld));
//b3Printf("apply base force of %f,%f,%f at %f,%f,%f\n", forceWorld[0],forceWorld[1],forceWorld[2],positionLocal[0],positionLocal[1],positionLocal[2]);
} else
{
int link = clientCmd.m_externalForceArguments.m_linkIds[i];
btVector3 forceWorld = mb->getLink(link).m_cachedWorldTransform.getBasis()*forceLocal;
btVector3 relPosWorld = mb->getLink(link).m_cachedWorldTransform.getBasis()*positionLocal;
mb->addLinkForce(link, forceWorld);
mb->addLinkTorque(link,relPosWorld.cross(forceWorld));
btVector3 forceWorld = isLinkFrame ? mb->getLink(link).m_cachedWorldTransform.getBasis()*tmpForce : tmpForce;
btVector3 relPosWorld = isLinkFrame ? mb->getLink(link).m_cachedWorldTransform.getBasis()*tmpPosition : tmpPosition - mb->getBaseWorldTransform().getOrigin();
mb->addLinkForce(link, forceWorld);
mb->addLinkTorque(link,relPosWorld.cross(forceWorld));
//b3Printf("apply link force of %f,%f,%f at %f,%f,%f\n", forceWorld[0],forceWorld[1],forceWorld[2], positionLocal[0],positionLocal[1],positionLocal[2]);
}
}

4
examples/SharedMemory/PhysicsServerCommandProcessor.h
View File

@@ -27,10 +27,10 @@ protected:
bool loadSdf(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes, bool useMultiBody, int flags);
bool loadSdf(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes, bool useMultiBody, int flags, btScalar globalScaling);
bool loadUrdf(const char* fileName, const class btVector3& pos, const class btQuaternion& orn,
bool useMultiBody, bool useFixedBase, int* bodyUniqueIdPtr, char* bufferServerToClient, int bufferSizeInBytes, int flags=0);
bool useMultiBody, bool useFixedBase, int* bodyUniqueIdPtr, char* bufferServerToClient, int bufferSizeInBytes, int flags, btScalar globalScaling);
bool loadMjcf(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes, bool useMultiBody, int flags);

5
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -65,6 +65,7 @@ struct SdfArgs
{
char m_sdfFileName[MAX_URDF_FILENAME_LENGTH];
int m_useMultiBody;
double m_globalScaling;
};
struct FileArgs
@@ -79,7 +80,8 @@ enum EnumUrdfArgsUpdateFlags
URDF_ARGS_INITIAL_ORIENTATION=4,
URDF_ARGS_USE_MULTIBODY=8,
URDF_ARGS_USE_FIXED_BASE=16,
URDF_ARGS_HAS_CUSTOM_URDF_FLAGS = 32
URDF_ARGS_HAS_CUSTOM_URDF_FLAGS = 32,
URDF_ARGS_USE_GLOBAL_SCALING =64,
};
@@ -91,6 +93,7 @@ struct UrdfArgs
int m_useMultiBody;
int m_useFixedBase;
int m_urdfFlags;
double m_globalScaling;
};

22
examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp
View File

@@ -5,7 +5,7 @@
#include "PhysicsClientSharedMemory.h"
#include"../ExampleBrowser/InProcessExampleBrowser.h"
#include <stdio.h>
#include <string.h>
#include "PhysicsServerExampleBullet2.h"
#include "../CommonInterfaces/CommonExampleInterface.h"
@@ -19,7 +19,7 @@ class InProcessPhysicsClientSharedMemoryMainThread : public PhysicsClientSharedM
public:
InProcessPhysicsClientSharedMemoryMainThread(int argc, char* argv[])
InProcessPhysicsClientSharedMemoryMainThread(int argc, char* argv[], bool useInProcessMemory)
{
int newargc = argc+2;
char** newargv = (char**)malloc(sizeof(void*)*newargc);
@@ -30,7 +30,7 @@ public:
char* t1 = (char*)"--start_demo_name=Physics Server";
newargv[argc] = t0;
newargv[argc+1] = t1;
m_data = btCreateInProcessExampleBrowserMainThread(newargc,newargv);
m_data = btCreateInProcessExampleBrowserMainThread(newargc,newargv, useInProcessMemory);
SharedMemoryInterface* shMem = btGetSharedMemoryInterfaceMainThread(m_data);
setSharedMemoryInterface(shMem);
@@ -83,9 +83,9 @@ public:
};
b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnectMainThread(int argc, char* argv[])
b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnectMainThread(int argc, char* argv[], int useInProcessMemory)
{
InProcessPhysicsClientSharedMemoryMainThread* cl = new InProcessPhysicsClientSharedMemoryMainThread(argc, argv);
InProcessPhysicsClientSharedMemoryMainThread* cl = new InProcessPhysicsClientSharedMemoryMainThread(argc, argv, useInProcessMemory);
cl->setSharedMemoryKey(SHARED_MEMORY_KEY);
cl->connect();
return (b3PhysicsClientHandle ) cl;
@@ -98,7 +98,7 @@ class InProcessPhysicsClientSharedMemory : public PhysicsClientSharedMemory
public:
InProcessPhysicsClientSharedMemory(int argc, char* argv[])
InProcessPhysicsClientSharedMemory(int argc, char* argv[], bool useInProcessMemory)
{
int newargc = argc+2;
m_newargv = (char**)malloc(sizeof(void*)*newargc);
@@ -109,7 +109,7 @@ public:
char* t1 = (char*)"--start_demo_name=Physics Server";
m_newargv[argc] = t0;
m_newargv[argc+1] = t1;
m_data = btCreateInProcessExampleBrowser(newargc,m_newargv);
m_data = btCreateInProcessExampleBrowser(newargc,m_newargv, useInProcessMemory);
SharedMemoryInterface* shMem = btGetSharedMemoryInterface(m_data);
setSharedMemoryInterface(shMem);
}
@@ -123,11 +123,11 @@ public:
};
b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnect(int argc, char* argv[])
{
b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnect(int argc, char* argv[], int useInProcessMemory)
{
InProcessPhysicsClientSharedMemory* cl = new InProcessPhysicsClientSharedMemory(argc, argv);
cl->setSharedMemoryKey(SHARED_MEMORY_KEY);
InProcessPhysicsClientSharedMemory* cl = new InProcessPhysicsClientSharedMemory(argc, argv, useInProcessMemory);
cl->setSharedMemoryKey(SHARED_MEMORY_KEY+1);
cl->connect();
return (b3PhysicsClientHandle ) cl;
}

4
examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.h
View File

@@ -10,9 +10,9 @@ extern "C" {
///think more about naming. The b3ConnectPhysicsLoopback
b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnect(int argc, char* argv[]);
b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnect(int argc, char* argv[], int useInProcessMemory);
b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnectMainThread(int argc, char* argv[]);
b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnectMainThread(int argc, char* argv[], int useInProcessMemory);
b3PhysicsClientHandle b3CreateInProcessPhysicsServerFromExistingExampleBrowserAndConnect(void* guiHelperPtr);

1
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -577,6 +577,7 @@ enum eCONNECT_METHOD {
eCONNECT_UDP = 4,
eCONNECT_TCP = 5,
eCONNECT_EXISTING_EXAMPLE_BROWSER=6,
eCONNECT_GUI_SERVER=7,
};
enum eURDF_Flags

43
examples/pybullet/pybullet.c
View File

@@ -294,6 +294,7 @@ static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args, P
int method = eCONNECT_GUI;
int i;
char* options="";
b3PhysicsClientHandle sm = 0;
if (sNumPhysicsClients >= MAX_PHYSICS_CLIENTS)
@@ -337,10 +338,10 @@ static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args, P
int i;
for (i = 0; i < MAX_PHYSICS_CLIENTS; i++)
{
if (sPhysicsClientsGUI[i] == eCONNECT_GUI)
if ((sPhysicsClientsGUI[i] == eCONNECT_GUI) || (sPhysicsClientsGUI[i] == eCONNECT_GUI_SERVER))
{
PyErr_SetString(SpamError,
"Only one local in-process GUI connection allowed. Use DIRECT connection mode or start a separate GUI physics server (ExampleBrowser, App_SharedMemoryPhysics_GUI, App_SharedMemoryPhysics_VR) and connect over SHARED_MEMORY, UDP or TCP instead.");
"Only one local in-process GUI/GUI_SERVER connection allowed. Use DIRECT connection mode or start a separate GUI physics server (ExampleBrowser, App_SharedMemoryPhysics_GUI, App_SharedMemoryPhysics_VR) and connect over SHARED_MEMORY, UDP or TCP instead.");
return NULL;
}
}
@@ -354,9 +355,21 @@ static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args, P
char* argv[2] = {"unused",options};
#ifdef __APPLE__
sm = b3CreateInProcessPhysicsServerAndConnectMainThread(argc, argv);
sm = b3CreateInProcessPhysicsServerAndConnectMainThread(argc, argv,1);
#else
sm = b3CreateInProcessPhysicsServerAndConnect(argc, argv);
sm = b3CreateInProcessPhysicsServerAndConnect(argc, argv,1);
#endif
break;
}
case eCONNECT_GUI_SERVER:
{
int argc = 2;
char* argv[2] = {"unused",options};
#ifdef __APPLE__
sm = b3CreateInProcessPhysicsServerAndConnectMainThread(argc, argv,0);
#else
sm = b3CreateInProcessPhysicsServerAndConnect(argc, argv,0);
#endif
break;
}
@@ -929,14 +942,14 @@ static PyObject* pybullet_loadURDF(PyObject* self, PyObject* args, PyObject* key
int physicsClientId = 0;
int flags = 0;
static char* kwlist[] = {"fileName", "basePosition", "baseOrientation", "useMaximalCoordinates", "useFixedBase", "flags","physicsClientId", NULL};
static char* kwlist[] = {"fileName", "basePosition", "baseOrientation", "useMaximalCoordinates", "useFixedBase", "flags","globalScaling", "physicsClientId", NULL};
static char* kwlistBackwardCompatible4[] = {"fileName", "startPosX", "startPosY", "startPosZ", NULL};
static char* kwlistBackwardCompatible8[] = {"fileName", "startPosX", "startPosY", "startPosZ", "startOrnX", "startOrnY", "startOrnZ", "startOrnW", NULL};
int bodyUniqueId= -1;
const char* urdfFileName = "";
double globalScaling = -1;
double startPosX = 0.0;
double startPosY = 0.0;
double startPosZ = 0.0;
@@ -976,7 +989,7 @@ static PyObject* pybullet_loadURDF(PyObject* self, PyObject* args, PyObject* key
double basePos[3];
double baseOrn[4];
if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|OOiiii", kwlist, &urdfFileName, &basePosObj, &baseOrnObj, &useMaximalCoordinates, &useFixedBase, &flags, &physicsClientId))
if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|OOiiidi", kwlist, &urdfFileName, &basePosObj, &baseOrnObj, &useMaximalCoordinates, &useFixedBase, &flags, &globalScaling, &physicsClientId))
{
return NULL;
}
@@ -1040,7 +1053,10 @@ static PyObject* pybullet_loadURDF(PyObject* self, PyObject* args, PyObject* key
{
b3LoadUrdfCommandSetUseFixedBase(command, 1);
}
if (globalScaling>0)
{
b3LoadUrdfCommandSetGlobalScaling(command,globalScaling);
}
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
statusType = b3GetStatusType(statusHandle);
if (statusType != CMD_URDF_LOADING_COMPLETED)
@@ -1071,10 +1087,11 @@ static PyObject* pybullet_loadSDF(PyObject* self, PyObject* args, PyObject* keyw
int statusType;
b3SharedMemoryCommandHandle commandHandle;
b3PhysicsClientHandle sm = 0;
double globalScaling = -1;
int physicsClientId = 0;
static char* kwlist[] = {"sdfFileName", "useMaximalCoordinates", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|ii", kwlist, &sdfFileName, &useMaximalCoordinates, &physicsClientId))
static char* kwlist[] = {"sdfFileName", "useMaximalCoordinates", "globalScaling", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|idi", kwlist, &sdfFileName, &useMaximalCoordinates, &globalScaling, &physicsClientId))
{
return NULL;
}
@@ -1090,7 +1107,10 @@ static PyObject* pybullet_loadSDF(PyObject* self, PyObject* args, PyObject* keyw
{
b3LoadSdfCommandSetUseMultiBody(commandHandle,0);
}
if (globalScaling > 0)
{
b3LoadSdfCommandSetUseGlobalScaling(commandHandle,globalScaling);
}
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
statusType = b3GetStatusType(statusHandle);
if (statusType != CMD_SDF_LOADING_COMPLETED)
@@ -7254,6 +7274,7 @@ initpybullet(void)
PyModule_AddIntConstant(m, "GUI", eCONNECT_GUI); // user read
PyModule_AddIntConstant(m, "UDP", eCONNECT_UDP); // user read
PyModule_AddIntConstant(m, "TCP", eCONNECT_TCP); // user read
PyModule_AddIntConstant(m, "GUI_SERVER", eCONNECT_GUI_SERVER); // user read
PyModule_AddIntConstant(m, "JOINT_REVOLUTE", eRevoluteType); // user read
PyModule_AddIntConstant(m, "JOINT_PRISMATIC", ePrismaticType); // user read

4
test/SharedMemory/test.c
View File

@@ -335,9 +335,9 @@ int main(int argc, char* argv[])
#ifdef PHYSICS_IN_PROCESS_EXAMPLE_BROWSER
#ifdef __APPLE__
b3PhysicsClientHandle sm = b3CreateInProcessPhysicsServerAndConnectMainThread(argc,argv);
b3PhysicsClientHandle sm = b3CreateInProcessPhysicsServerAndConnectMainThread(argc,argv,1);
#else
b3PhysicsClientHandle sm = b3CreateInProcessPhysicsServerAndConnect(argc,argv);
b3PhysicsClientHandle sm = b3CreateInProcessPhysicsServerAndConnect(argc,argv,1);
#endif //__APPLE__
#endif