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

Merge branch 'master' of https://github.com/erwincoumans/bullet3

Browse Source
This commit is contained in:
Erwin Coumans
2016-11-29 19:14:20 -08:00
parent 32a3fb0b4d 746c4d0d2a
commit 898517b1e3
7 changed files with 351 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files

72
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -43,11 +43,12 @@ bool gCloseToKuka=false;
bool gEnableRealTimeSimVR=false;
bool gCreateDefaultRobotAssets = false;
int gInternalSimFlags = 0;
bool gResetSimulation = 0;
int gHuskyId = -1;
btTransform huskyTr = btTransform::getIdentity();
int gCreateObjectSimVR = -1;
int gEnableKukaControl = 0;
int gEnableKukaControl = 1;
btScalar simTimeScalingFactor = 1;
btScalar gRhsClamp = 1.f;
@@ -718,6 +719,19 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
}
void PhysicsServerCommandProcessor::deleteCachedInverseKinematicsBodies()
{
for (int i = 0; i < m_data->m_inverseKinematicsHelpers.size(); i++)
{
IKTrajectoryHelper** ikHelperPtr = m_data->m_inverseKinematicsHelpers.getAtIndex(i);
if (ikHelperPtr)
{
IKTrajectoryHelper* ikHelper = *ikHelperPtr;
delete ikHelper;
}
}
m_data->m_inverseKinematicsHelpers.clear();
}
void PhysicsServerCommandProcessor::deleteCachedInverseDynamicsBodies()
{
for (int i = 0; i < m_data->m_inverseDynamicsBodies.size(); i++)
@@ -736,7 +750,7 @@ void PhysicsServerCommandProcessor::deleteCachedInverseDynamicsBodies()
void PhysicsServerCommandProcessor::deleteDynamicsWorld()
{
deleteCachedInverseDynamicsBodies();
deleteCachedInverseKinematicsBodies();
for (int i=0;i<m_data->m_multiBodyJointFeedbacks.size();i++)
{
@@ -2439,29 +2453,12 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
case CMD_RESET_SIMULATION:
{
//clean up all data
deleteCachedInverseDynamicsBodies();
if (m_data && m_data->m_guiHelper)
{
m_data->m_guiHelper->removeAllGraphicsInstances();
}
if (m_data)
{
m_data->m_visualConverter.resetAll();
}
deleteDynamicsWorld();
createEmptyDynamicsWorld();
m_data->exitHandles();
m_data->initHandles();
resetSimulation();
SharedMemoryStatus& serverCmd =serverStatusOut;
serverCmd.m_type = CMD_RESET_SIMULATION_COMPLETED;
hasStatus = true;
m_data->m_hasGround = false;
m_data->m_gripperRigidbodyFixed = 0;
break;
}
case CMD_CREATE_RIGID_BODY:
@@ -3950,6 +3947,12 @@ double gDtInSec = 0.f;
double gSubStep = 0.f;
void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
{
if (gResetSimulation)
{
resetSimulation();
gResetSimulation = false;
}
if ((gEnableRealTimeSimVR || m_data->m_allowRealTimeSimulation) && m_data->m_guiHelper)
{
@@ -4007,7 +4010,30 @@ void PhysicsServerCommandProcessor::applyJointDamping(int bodyUniqueId)
}
}
void PhysicsServerCommandProcessor::resetSimulation()
{
//clean up all data
deleteCachedInverseDynamicsBodies();
if (m_data && m_data->m_guiHelper)
{
m_data->m_guiHelper->removeAllGraphicsInstances();
}
if (m_data)
{
m_data->m_visualConverter.resetAll();
}
deleteDynamicsWorld();
createEmptyDynamicsWorld();
m_data->exitHandles();
m_data->initHandles();
m_data->m_hasGround = false;
m_data->m_gripperRigidbodyFixed = 0;
}
//todo: move this to Python/scripting
void PhysicsServerCommandProcessor::createDefaultRobotAssets()
{
@@ -4151,7 +4177,7 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets()
btTransform objectLocalTr[] = {
btTransform(btQuaternion(0, 0, 0, 1), btVector3(0.0, 0.0, 0.0)),
btTransform(btQuaternion(0, 0, 0, 1), btVector3(0.0, 0.15, 0.64)),
btTransform(btQuaternion(btVector3(0,0,1),-SIMD_HALF_PI), btVector3(0.0, 0.15, 0.64)),
btTransform(btQuaternion(0, 0, 0, 1), btVector3(0.1, 0.15, 0.85)),
btTransform(btQuaternion(0, 0, 0, 1), btVector3(-0.4, 0.05, 0.85)),
btTransform(btQuaternion(0, 0, 0, 1), btVector3(-0.3, -0.05, 0.7)),
@@ -4178,7 +4204,7 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets()
// Table area
loadUrdf("table/table.urdf", objectWorldTr[0].getOrigin(), objectWorldTr[0].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("tray.urdf", objectWorldTr[1].getOrigin(), objectWorldTr[1].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("tray/tray_textured.urdf", objectWorldTr[1].getOrigin(), objectWorldTr[1].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
//loadUrdf("cup_small.urdf", objectWorldTr[2].getOrigin(), objectWorldTr[2].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
//loadUrdf("pitcher_small.urdf", objectWorldTr[3].getOrigin(), objectWorldTr[3].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("teddy_vhacd.urdf", objectWorldTr[4].getOrigin(), objectWorldTr[4].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());