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

Merge pull request #1962 from erwincoumans/master

Browse Source 
eglPlugin: remove visual shape for removeBody.
This commit is contained in:
erwincoumans
2018-10-29 16:51:32 -07:00
committed by GitHub
parent f34aec941b c2b9dc9361
commit d9fa97dd5b
13 changed files with 263 additions and 114 deletions
Download Patch File Download Diff File Expand all files Collapse all files

59
Extras/Serialize/BulletWorldImporter/btMultiBodyWorldImporter.cpp
View File

@@ -274,6 +274,7 @@ bool btMultiBodyWorldImporter::convertAllObjects(bParse::btBulletFile* bulletFil
//equal number of objects, # links etc
if ((bulletFile2->m_multiBodies.size() != m_data->m_mbDynamicsWorld->getNumMultibodies()))
{
printf("btMultiBodyWorldImporter::convertAllObjects error: expected %d multibodies, got %d.\n", m_data->m_mbDynamicsWorld->getNumMultibodies(), bulletFile2->m_multiBodies.size());
result = false;
return result;
}
@@ -287,8 +288,16 @@ bool btMultiBodyWorldImporter::convertAllObjects(bParse::btBulletFile* bulletFil
{
btMultiBodyDoubleData* mbd = (btMultiBodyDoubleData*)bulletFile2->m_multiBodies[i];
btMultiBody* mb = m_data->m_mbDynamicsWorld->getMultiBody(i);
if (mbd->m_numLinks != mb->getNumLinks())
{
printf("btMultiBodyWorldImporter::convertAllObjects error: mismatch in number of links in a body (expected %d, found %d).\n", mbd->m_numLinks, mb->getNumLinks() );
result = false;
return result;
} else
{
syncMultiBody(mbd, mb, m_data, scratchQ, scratchM);
}
}
for (int i = bulletFile2->m_rigidBodies.size() - 1; i >= 0; i--)
{
@@ -319,11 +328,13 @@ bool btMultiBodyWorldImporter::convertAllObjects(bParse::btBulletFile* bulletFil
}
else
{
printf("btMultiBodyWorldImporter::convertAllObjects error: cannot find btRigidBody with bodyUniqueId %d\n", uid);
result = false;
}
}
else
{
printf("Error in btMultiBodyWorldImporter::convertAllObjects: didn't find bodyUniqueId: %d\n", uid);
result = false;
}
}
@@ -362,8 +373,56 @@ bool btMultiBodyWorldImporter::convertAllObjects(bParse::btBulletFile* bulletFil
{
btMultiBodyFloatData* mbd = (btMultiBodyFloatData*)bulletFile2->m_multiBodies[i];
btMultiBody* mb = m_data->m_mbDynamicsWorld->getMultiBody(i);
if (mbd->m_numLinks != mb->getNumLinks())
{
printf("btMultiBodyWorldImporter::convertAllObjects error: mismatch in number of links in a body (expected %d, found %d).\n", mbd->m_numLinks, mb->getNumLinks() );
result = false;
return result;
} else
{
syncMultiBody(mbd, mb, m_data, scratchQ, scratchM);
}
}
for (int i = bulletFile2->m_rigidBodies.size() - 1; i >= 0; i--)
{
btRigidBodyFloatData* rbd = (btRigidBodyFloatData*)bulletFile2->m_rigidBodies[i];
int foundRb = -1;
int uid = rbd->m_collisionObjectData.m_uniqueId;
for (int i = 0; i < m_data->m_mbDynamicsWorld->getNumCollisionObjects(); i++)
{
if (uid == m_data->m_mbDynamicsWorld->getCollisionObjectArray()[i]->getBroadphaseHandle()->m_uniqueId)
{
foundRb = i;
break;
}
}
if (foundRb >= 0)
{
btRigidBody* rb = btRigidBody::upcast(m_data->m_mbDynamicsWorld->getCollisionObjectArray()[foundRb]);
if (rb)
{
btTransform tr;
tr.deSerializeFloat(rbd->m_collisionObjectData.m_worldTransform);
rb->setWorldTransform(tr);
btVector3 linVel, angVel;
linVel.deSerializeFloat(rbd->m_linearVelocity);
angVel.deSerializeFloat(rbd->m_angularVelocity);
rb->setLinearVelocity(linVel);
rb->setAngularVelocity(angVel);
}
else
{
printf("btMultiBodyWorldImporter::convertAllObjects error: cannot find btRigidBody with bodyUniqueId %d\n", uid);
result = false;
}
}
else
{
printf("Error in btMultiBodyWorldImporter::convertAllObjects: didn't find bodyUniqueId: %d\n", uid);
result = false;
}
}
//todo: check why body1 pointer is not properly deserialized
for (int i = 0; i < bulletFile2->m_contactManifolds.size(); i++)

9
examples/CommonInterfaces/CommonFileIOInterface.h
View File

@@ -3,6 +3,15 @@
struct CommonFileIOInterface
{
int m_fileIOType;
const char* m_pathPrefix;
CommonFileIOInterface(int fileIOType, const char* pathPrefix)
:m_fileIOType(fileIOType),
m_pathPrefix(pathPrefix)
{
}
virtual ~CommonFileIOInterface()
{
}

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

@@ -143,7 +143,7 @@ bool BulletURDFImporter::loadURDF(const char* fileName, bool forceFixedBase)
b3FileUtils fu;
//bool fileFound = fu.findFile(fileName, relativeFileName, 1024);
bool fileFound = m_data->m_fileIO->findResourcePath(fileName, relativeFileName, 1024) > 0;
bool fileFound = m_data->m_fileIO->findResourcePath(fileName, relativeFileName, 1024);
std::string xml_string;
@@ -217,7 +217,7 @@ bool BulletURDFImporter::loadSDF(const char* fileName, bool forceFixedBase)
b3FileUtils fu;
//bool fileFound = fu.findFile(fileName, relativeFileName, 1024);
bool fileFound = (m_data->m_fileIO->findResourcePath(fileName, relativeFileName, 1024)) > 0;
bool fileFound = (m_data->m_fileIO->findResourcePath(fileName, relativeFileName, 1024));
std::string xml_string;

104
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -10152,33 +10152,50 @@ bool PhysicsServerCommandProcessor::processRestoreStateCommand(const struct Shar
}
else
{
const char* prefix[] = {"", "./", "./data/", "../data/", "../../data/", "../../../data/", "../../../../data/"};
int numPrefixes = sizeof(prefix) / sizeof(const char*);
char relativeFileName[1024];
FILE* f = 0;
bool found = false;
char fileName[1024];
fileName[0] = 0;
for (int i = 0; !f && i < numPrefixes; i++)
CommonFileIOInterface* fileIO = m_data->m_pluginManager.getFileIOInterface();
b3AlignedObjectArray<char> buffer;
buffer.reserve(1024);
if (fileIO)
{
sprintf(relativeFileName, "%s%s", prefix[i], clientCmd.m_fileArguments.m_fileName);
f = fopen(relativeFileName, "rb");
if (f)
{
found = true;
break;
}
}
if (f)
{
fclose(f);
}
int fileId = -1;
found = fileIO->findResourcePath(clientCmd.m_fileArguments.m_fileName, fileName, 1024);
if (found)
{
ok = importer->loadFile(relativeFileName);
fileId = fileIO->fileOpen(fileName,"rb");
}
if (fileId>=0)
{
int size = fileIO->getFileSize(fileId);
if (size>0)
{
buffer.resize(size);
int actual = fileIO->fileRead(fileId,&buffer[0],size);
if (actual != size)
{
b3Warning("image filesize mismatch!\n");
buffer.resize(0);
} else
{
found=true;
}
}
fileIO->fileClose(fileId);
}
}
if (found && buffer.size())
{
ok = importer->loadFileFromMemory(&buffer[0], buffer.size());
} else
{
b3Error("Error in restoreState: cannot load file %s\n", clientCmd.m_fileArguments.m_fileName);
}
}
if (ok)
{
serverCmd.m_type = CMD_RESTORE_STATE_COMPLETED;
@@ -10198,30 +10215,43 @@ bool PhysicsServerCommandProcessor::processLoadBulletCommand(const struct Shared
//btBulletWorldImporter* importer = new btBulletWorldImporter(m_data->m_dynamicsWorld);
btMultiBodyWorldImporter* importer = new btMultiBodyWorldImporter(m_data->m_dynamicsWorld);
const char* prefix[] = {"", "./", "./data/", "../data/", "../../data/", "../../../data/", "../../../../data/"};
int numPrefixes = sizeof(prefix) / sizeof(const char*);
char relativeFileName[1024];
FILE* f = 0;
bool found = false;
for (int i = 0; !f && i < numPrefixes; i++)
CommonFileIOInterface* fileIO = m_data->m_pluginManager.getFileIOInterface();
b3AlignedObjectArray<char> buffer;
buffer.reserve(1024);
if (fileIO)
{
sprintf(relativeFileName, "%s%s", prefix[i], clientCmd.m_fileArguments.m_fileName);
f = fopen(relativeFileName, "rb");
if (f)
{
found = true;
break;
}
}
if (f)
{
fclose(f);
}
char fileName[1024];
int fileId = -1;
found = fileIO->findResourcePath(clientCmd.m_fileArguments.m_fileName, fileName, 1024);
if (found)
{
bool ok = importer->loadFile(relativeFileName);
fileId = fileIO->fileOpen(fileName,"rb");
}
if (fileId>=0)
{
int size = fileIO->getFileSize(fileId);
if (size>0)
{
buffer.resize(size);
int actual = fileIO->fileRead(fileId,&buffer[0],size);
if (actual != size)
{
b3Warning("image filesize mismatch!\n");
buffer.resize(0);
} else
{
found=true;
}
}
fileIO->fileClose(fileId);
}
}
if (found && buffer.size())
{
bool ok = importer->loadFileFromMemory(&buffer[0], buffer.size());
if (ok)
{
int numRb = importer->getNumRigidBodies();

1
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -924,6 +924,7 @@ enum eFileIOTypes
ePosixFileIO = 1,
eZipFileIO,
eCNSFileIO,
eInMemoryFileIO,
};
//limits for vertices/indices in PyBullet::createCollisionShape

1
examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.cpp
View File

@@ -1257,6 +1257,7 @@ void EGLRendererVisualShapeConverter::removeVisualShape(int collisionObjectUniqu
{
for (int o = 0; o < ptr->m_renderObjects.size(); o++)
{
m_data->m_instancingRenderer->removeGraphicsInstance(ptr->m_graphicsInstanceId);
delete ptr->m_renderObjects[o];
}
}

72
examples/SharedMemory/plugins/fileIOPlugin/fileIOPlugin.cpp
View File

@@ -48,6 +48,7 @@ struct InMemoryFileIO : public CommonFileIOInterface
int m_numFrees;
InMemoryFileIO()
:CommonFileIOInterface(eInMemoryFileIO,0)
{
m_numAllocs=0;
m_numFrees=0;
@@ -293,7 +294,8 @@ struct WrapperFileIO : public CommonFileIOInterface
InMemoryFileIO m_cachedFiles;
WrapperFileIO()
:m_numWrapperInterfaces(0)
:CommonFileIOInterface(0,0),
m_numWrapperInterfaces(0)
{
for (int i=0;i<B3_MAX_FILEIO_INTERFACES;i++)
{
@@ -327,6 +329,14 @@ struct WrapperFileIO : public CommonFileIOInterface
return result;
}
CommonFileIOInterface* getFileIOInterface(int fileIOIndex)
{
if (fileIOIndex>=0 && fileIOIndex<B3_MAX_FILEIO_INTERFACES)
{
return m_availableFileIOInterfaces[fileIOIndex];
}
return 0;
}
void removeFileIOInterface(int fileIOIndex)
{
if (fileIOIndex>=0 && fileIOIndex<B3_MAX_FILEIO_INTERFACES)
@@ -571,51 +581,68 @@ B3_SHARED_API int executePluginCommand_fileIOPlugin(struct b3PluginContext* cont
{
case eAddFileIOAction:
{
//create new fileIO interface
//if the fileIO already exists, skip this action
int fileIOType = arguments->m_ints[1];
bool alreadyExists = false;
for (int i=0;i<B3_MAX_FILEIO_INTERFACES;i++)
{
CommonFileIOInterface* fileIO = obj->m_fileIO.getFileIOInterface(i);
if (fileIO)
{
if (fileIO->m_fileIOType == fileIOType)
{
if (fileIO->m_pathPrefix && strcmp(fileIO->m_pathPrefix,arguments->m_text)==0)
{
result = i;
alreadyExists = true;
break;
}
}
}
}
//create new fileIO interface
if (!alreadyExists)
{
switch (fileIOType)
{
case ePosixFileIO:
{
#ifdef B3_EXCLUDE_DEFAULT_FILEIO
#ifdef B3_EXCLUDE_DEFAULT_FILEIO
printf("ePosixFileIO is not enabled in this build.\n");
#else
obj->m_fileIO.addFileIOInterface(new b3BulletDefaultFileIO());
#endif
#else
result = obj->m_fileIO.addFileIOInterface(new b3BulletDefaultFileIO(ePosixFileIO, arguments->m_text));
#endif
break;
}
case eZipFileIO:
{
#ifdef B3_USE_ZIPFILE_FILEIO
#ifdef B3_USE_ZIPFILE_FILEIO
if (arguments->m_text)
{
obj->m_fileIO.addFileIOInterface(new ZipFileIO(arguments->m_text, &obj->m_fileIO));
result = obj->m_fileIO.addFileIOInterface(new ZipFileIO(eZipFileIO, arguments->m_text, &obj->m_fileIO));
}
#else
#else
printf("eZipFileIO is not enabled in this build.\n");
#endif
#endif
break;
}
case eCNSFileIO:
{
#ifdef B3_USE_CNS_FILEIO
if (strlen(arguments->m_text))
{
obj->m_fileIO.addFileIOInterface(new CNSFileIO(arguments->m_text));
}
else
{
obj->m_fileIO.addFileIOInterface(new CNSFileIO(""));
}
#else//B3_USE_CNS_FILEIO
#ifdef B3_USE_CNS_FILEIO
result = obj->m_fileIO.addFileIOInterface(new CNSFileIO(eCNSFileIO, arguments->m_text));
#else//B3_USE_CNS_FILEIO
printf("CNSFileIO is not enabled in this build.\n");
#endif //B3_USE_CNS_FILEIO
#endif //B3_USE_CNS_FILEIO
break;
}
default:
{
}
}
}//switch (fileIOType)
}//if (!alreadyExists)
break;
}
case eRemoveFileIOAction:
@@ -624,6 +651,7 @@ B3_SHARED_API int executePluginCommand_fileIOPlugin(struct b3PluginContext* cont
//remove fileIO interface
int fileIOIndex = arguments->m_ints[1];
obj->m_fileIO.removeFileIOInterface(fileIOIndex);
result = fileIOIndex;
break;
}
default:

6
examples/SharedMemory/plugins/fileIOPlugin/zipFileIO.h
View File

@@ -10,10 +10,12 @@ struct ZipFileIO : public CommonFileIOInterface
unzFile m_fileHandles[B3_ZIP_FILEIO_MAX_FILES ];
int m_numFileHandles;
ZipFileIO(const char* zipfileName, CommonFileIOInterface* wrapperFileIO)
:m_zipfileName(zipfileName),
ZipFileIO(int fileIOType, const char* zipfileName, CommonFileIOInterface* wrapperFileIO)
:CommonFileIOInterface(fileIOType,0),
m_zipfileName(zipfileName),
m_numFileHandles(0)
{
m_pathPrefix = m_zipfileName.c_str();
for (int i=0;i<B3_ZIP_FILEIO_MAX_FILES ;i++)
{
m_fileHandles[i]=0;

15
examples/Utils/b3BulletDefaultFileIO.h
View File

@@ -16,12 +16,19 @@ struct b3BulletDefaultFileIO : public CommonFileIOInterface
return fileIo->findFile(orgFileName, relativeFileName, maxRelativeFileNameMaxLen);
}
char m_prefix[1024];
FILE* m_fileHandles[B3_FILEIO_MAX_FILES];
int m_numFileHandles;
b3BulletDefaultFileIO()
:m_numFileHandles(0)
b3BulletDefaultFileIO(int fileIOType=0, const char* pathPrefix=0)
:CommonFileIOInterface(fileIOType, m_prefix),
m_numFileHandles(0)
{
m_prefix[0] = 0;
if (pathPrefix)
{
sprintf(m_prefix,"%s", pathPrefix);
}
for (int i=0;i<B3_FILEIO_MAX_FILES;i++)
{
m_fileHandles[i]=0;
@@ -97,7 +104,7 @@ struct b3BulletDefaultFileIO : public CommonFileIOInterface
virtual bool findResourcePath(const char* fileName, char* relativeFileName, int relativeFileNameSizeInBytes)
{
return b3ResourcePath::findResourcePath(fileName, relativeFileName, relativeFileNameSizeInBytes, b3BulletDefaultFileIO::FileIOPluginFindFile, this);
return b3ResourcePath::findResourcePath(fileName, relativeFileName, relativeFileNameSizeInBytes, b3BulletDefaultFileIO::FileIOPluginFindFile, this)>0;
}
@@ -114,7 +121,7 @@ struct b3BulletDefaultFileIO : public CommonFileIOInterface
}
//printf("Trying various directories, relative to current working directory\n");
const char* prefix[] = {"./", "./data/", "../data/", "../../data/", "../../../data/", "../../../../data/"};
const char* prefix[] = {m_prefix, "./", "./data/", "../data/", "../../data/", "../../../data/", "../../../../data/"};
int numPrefixes = sizeof(prefix) / sizeof(const char*);
f = 0;

8
examples/pybullet/gym/pybullet_data/cartpole.urdf
View File

@@ -59,9 +59,15 @@
</visual>
<inertial>
<origin xyz="0 0 0.5"/>
<mass value="10"/>
<mass value="0.1"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
<collision>
<geometry>
<box size="0.05 0.05 1.0"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0.5"/>
</collision>
</link>
<joint name="cart_to_pole" type="continuous">

8
examples/pybullet/gym/pybullet_envs/__init__.py
View File

@@ -9,17 +9,17 @@ def register(id,*args,**kvargs):
# ------------bullet-------------
register(
id='CartPoleBulletEnv-v0',
id='CartPoleBulletEnv-v1',
entry_point='pybullet_envs.bullet:CartPoleBulletEnv',
timestep_limit=1000,
reward_threshold=950.0,
max_episode_steps=200,
reward_threshold=190.0,
)
register(
id='MinitaurBulletEnv-v0',
entry_point='pybullet_envs.bullet:MinitaurBulletEnv',
timestep_limit=1000,
reward_threshold=5.0,
reward_threshold=15.0,
)
register(

4
examples/pybullet/gym/pybullet_envs/baselines/enjoy_pybullet_cartpole.py
View File

@@ -5,12 +5,13 @@ parentdir = os.path.dirname(os.path.dirname(currentdir))
os.sys.path.insert(0,parentdir)
import gym
import time
from baselines import deepq
from pybullet_envs.bullet.cartpole_bullet import CartPoleBulletEnv
def main():
env = gym.make('CartPoleBulletEnv-v0')
env = gym.make('CartPoleBulletEnv-v1')
act = deepq.load("cartpole_model.pkl")
while True:
@@ -28,6 +29,7 @@ def main():
a = aa[0]
obs, rew, done, _ = env.step(a)
episode_rew += rew
time.sleep(1./240.)
print("Episode reward", episode_rew)

54
examples/pybullet/gym/pybullet_envs/bullet/cartpole_bullet.py
View File

@@ -34,24 +34,26 @@ class CartPoleBulletEnv(gym.Env):
p.connect(p.GUI)
else:
p.connect(p.DIRECT)
observation_high = np.array([
np.finfo(np.float32).max,
np.finfo(np.float32).max,
self.theta_threshold_radians = 12 * 2 * math.pi / 360
self.x_threshold = 0.4 #2.4
high = np.array([
self.x_threshold * 2,
np.finfo(np.float32).max,
self.theta_threshold_radians * 2,
np.finfo(np.float32).max])
action_high = np.array([0.1])
self.action_space = spaces.Discrete(9)
self.observation_space = spaces.Box(-observation_high, observation_high)
self.force_mag = 10
self.action_space = spaces.Discrete(2)
self.observation_space = spaces.Box(-high, high, dtype=np.float32)
self.theta_threshold_radians = 1
self.x_threshold = 2.4
self._seed()
# self.reset()
# self.reset()
self.viewer = None
self._configure()
def _configure(self, display=None):
self.display = display
@@ -60,41 +62,43 @@ class CartPoleBulletEnv(gym.Env):
return [seed]
def _step(self, action):
force = self.force_mag if action==1 else -self.force_mag
p.setJointMotorControl2(self.cartpole, 0, p.TORQUE_CONTROL, force=force)
p.stepSimulation()
# time.sleep(self.timeStep)
self.state = p.getJointState(self.cartpole, 1)[0:2] + p.getJointState(self.cartpole, 0)[0:2]
theta, theta_dot, x, x_dot = self.state
dv = 0.1
deltav = [-10.*dv,-5.*dv, -2.*dv, -0.1*dv, 0, 0.1*dv, 2.*dv,5.*dv, 10.*dv][action]
p.setJointMotorControl2(self.cartpole, 0, p.VELOCITY_CONTROL, targetVelocity=(deltav + self.state[3]))
done = x < -self.x_threshold \
or x > self.x_threshold \
or theta < -self.theta_threshold_radians \
or theta > self.theta_threshold_radians
done = bool(done)
reward = 1.0
#print("state=",self.state)
return np.array(self.state), reward, done, {}
def _reset(self):
# print("-----------reset simulation---------------")
p.resetSimulation()
self.cartpole = p.loadURDF(os.path.join(pybullet_data.getDataPath(),"cartpole.urdf"),[0,0,0])
self.timeStep = 0.01
p.changeDynamics(self.cartpole, -1, linearDamping=0, angularDamping=0)
p.changeDynamics(self.cartpole, 0, linearDamping=0, angularDamping=0)
p.changeDynamics(self.cartpole, 1, linearDamping=0, angularDamping=0)
self.timeStep = 0.02
p.setJointMotorControl2(self.cartpole, 1, p.VELOCITY_CONTROL, force=0)
p.setGravity(0,0, -10)
p.setJointMotorControl2(self.cartpole, 0, p.VELOCITY_CONTROL, force=0)
p.setGravity(0,0, -9.8)
p.setTimeStep(self.timeStep)
p.setRealTimeSimulation(0)
initialCartPos = self.np_random.uniform(low=-0.5, high=0.5, size=(1,))
initialAngle = self.np_random.uniform(low=-0.5, high=0.5, size=(1,))
p.resetJointState(self.cartpole, 1, initialAngle)
p.resetJointState(self.cartpole, 0, initialCartPos)
randstate = self.np_random.uniform(low=-0.05, high=0.05, size=(4,))
p.resetJointState(self.cartpole, 1, randstate[0], randstate[1])
p.resetJointState(self.cartpole, 0, randstate[2], randstate[3])
#print("randstate=",randstate)
self.state = p.getJointState(self.cartpole, 1)[0:2] + p.getJointState(self.cartpole, 0)[0:2]
#print("self.state=", self.state)
return np.array(self.state)
def _render(self, mode='human', close=False):