mirror of
https://github.com/bulletphysics/bullet3
synced 2024-12-15 06:00:12 +00:00
358 lines
11 KiB
C++
358 lines
11 KiB
C++
|
|
#include "PhysicsServer.h"
|
|
|
|
#include "PosixSharedMemory.h"
|
|
#include "Win32SharedMemory.h"
|
|
|
|
#include "../Importers/ImportURDFDemo/MyURDFImporter.h"
|
|
#include "../Importers/ImportURDFDemo/MyMultiBodyCreator.h"
|
|
#include "../Importers/ImportURDFDemo/URDF2Bullet.h"
|
|
|
|
#include "SharedMemoryCommon.h"
|
|
|
|
|
|
class PhysicsServer : public SharedMemoryCommon
|
|
{
|
|
SharedMemoryInterface* m_sharedMemory;
|
|
SharedMemoryExampleData* m_testBlock1;
|
|
|
|
btAlignedObjectArray<btJointFeedback*> m_jointFeedbacks;
|
|
bool m_wantsShutdown;
|
|
|
|
public:
|
|
|
|
PhysicsServer(GUIHelperInterface* helper);
|
|
|
|
virtual ~PhysicsServer();
|
|
|
|
virtual void initPhysics();
|
|
|
|
virtual void stepSimulation(float deltaTime);
|
|
|
|
void releaseSharedMemory();
|
|
|
|
bool loadUrdf(const char* fileName, const btVector3& pos, const btQuaternion& orn,
|
|
bool useMultiBody, bool useFixedBase);
|
|
|
|
virtual void resetCamera()
|
|
{
|
|
float dist = 5;
|
|
float pitch = 50;
|
|
float yaw = 35;
|
|
float targetPos[3]={0,0,0};//-3,2.8,-2.5};
|
|
m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]);
|
|
}
|
|
|
|
virtual bool wantsTermination();
|
|
};
|
|
|
|
PhysicsServer::PhysicsServer(GUIHelperInterface* helper)
|
|
:SharedMemoryCommon(helper),
|
|
m_testBlock1(0),
|
|
m_wantsShutdown(false)
|
|
{
|
|
b3Printf("Started PhysicsServer\n");
|
|
bool useServer = true;
|
|
#ifdef _WIN32
|
|
m_sharedMemory = new Win32SharedMemoryServer();
|
|
#else
|
|
m_sharedMemory = new PosixSharedMemory();
|
|
#endif
|
|
}
|
|
|
|
void PhysicsServer::releaseSharedMemory()
|
|
{
|
|
if (m_testBlock1)
|
|
{
|
|
m_testBlock1->m_magicId = 0;
|
|
b3Printf("magic id = %d\n",m_testBlock1->m_magicId);
|
|
btAssert(m_sharedMemory);
|
|
m_sharedMemory->releaseSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE);
|
|
}
|
|
if (m_sharedMemory)
|
|
{
|
|
|
|
delete m_sharedMemory;
|
|
m_sharedMemory = 0;
|
|
m_testBlock1 = 0;
|
|
}
|
|
}
|
|
|
|
PhysicsServer::~PhysicsServer()
|
|
{
|
|
releaseSharedMemory();
|
|
}
|
|
|
|
void PhysicsServer::initPhysics()
|
|
{
|
|
///for this testing we use Z-axis up
|
|
int upAxis = 2;
|
|
m_guiHelper->setUpAxis(upAxis);
|
|
|
|
createEmptyDynamicsWorld();
|
|
//todo: create a special debug drawer that will cache the lines, so we can send the debug info over the wire
|
|
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
|
|
btVector3 grav(0,0,0);
|
|
grav[upAxis] = -9.8;
|
|
this->m_dynamicsWorld->setGravity(grav);
|
|
|
|
m_testBlock1 = (SharedMemoryExampleData*) m_sharedMemory->allocateSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE);
|
|
|
|
// btAssert(m_testBlock1);
|
|
if (m_testBlock1)
|
|
{
|
|
// btAssert(m_testBlock1->m_magicId != SHARED_MEMORY_MAGIC_NUMBER);
|
|
if (m_testBlock1->m_magicId == SHARED_MEMORY_MAGIC_NUMBER)
|
|
{
|
|
b3Printf("Warning: shared memory is already initialized, did you already spawn a server?\n");
|
|
}
|
|
|
|
m_testBlock1->m_numClientCommands = 0;
|
|
m_testBlock1->m_numServerCommands = 0;
|
|
m_testBlock1->m_numProcessedClientCommands=0;
|
|
m_testBlock1->m_numProcessedServerCommands=0;
|
|
|
|
m_testBlock1->m_magicId = SHARED_MEMORY_MAGIC_NUMBER;
|
|
b3Printf("Shared memory succesfully allocated\n");
|
|
} else
|
|
{
|
|
b3Error("Couldn't allocated shared memory, is it implemented on your operating system?\n");
|
|
}
|
|
}
|
|
|
|
|
|
bool PhysicsServer::wantsTermination()
|
|
{
|
|
return m_wantsShutdown;
|
|
}
|
|
|
|
bool PhysicsServer::loadUrdf(const char* fileName, const btVector3& pos, const btQuaternion& orn,
|
|
bool useMultiBody, bool useFixedBase)
|
|
{
|
|
|
|
MyURDFImporter u2b(m_guiHelper);
|
|
bool loadOk = u2b.loadURDF(fileName);
|
|
if (loadOk)
|
|
{
|
|
b3Printf("loaded %s OK!", fileName);
|
|
|
|
btTransform tr;
|
|
tr.setIdentity();
|
|
tr.setOrigin(pos);
|
|
tr.setRotation(orn);
|
|
int rootLinkIndex = u2b.getRootLinkIndex();
|
|
// printf("urdf root link index = %d\n",rootLinkIndex);
|
|
MyMultiBodyCreator creation(m_guiHelper);
|
|
|
|
ConvertURDF2Bullet(u2b,creation, tr,m_dynamicsWorld,useMultiBody,u2b.getPathPrefix());
|
|
btMultiBody* mb = creation.getBulletMultiBody();
|
|
if (useMultiBody)
|
|
{
|
|
if (mb)
|
|
{
|
|
return true;
|
|
} else
|
|
{
|
|
b3Warning("No multibody loaded from URDF");
|
|
return false;
|
|
}
|
|
} else
|
|
{
|
|
for (int i=0;i<m_dynamicsWorld->getNumConstraints();i++)
|
|
{
|
|
btTypedConstraint* c = m_dynamicsWorld->getConstraint(i);
|
|
btJointFeedback* fb = new btJointFeedback();
|
|
m_jointFeedbacks.push_back(fb);
|
|
c->setJointFeedback(fb);
|
|
|
|
}
|
|
return true;
|
|
}
|
|
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
void PhysicsServer::stepSimulation(float deltaTime)
|
|
{
|
|
|
|
bool wantsShutdown = false;
|
|
|
|
if (m_testBlock1)
|
|
{
|
|
///we ignore overflow of integer for now
|
|
if (m_testBlock1->m_numClientCommands> m_testBlock1->m_numProcessedClientCommands)
|
|
{
|
|
|
|
//until we implement a proper ring buffer, we assume always maximum of 1 outstanding commands
|
|
btAssert(m_testBlock1->m_numClientCommands==m_testBlock1->m_numProcessedClientCommands+1);
|
|
|
|
const SharedMemoryCommand& clientCmd =m_testBlock1->m_clientCommands[0];
|
|
m_testBlock1->m_numProcessedClientCommands++;
|
|
|
|
//consume the command
|
|
switch (clientCmd.m_type)
|
|
{
|
|
case CMD_LOAD_URDF:
|
|
{
|
|
b3Printf("Processed CMD_LOAD_URDF:%s",clientCmd.m_urdfArguments.m_urdfFileName);
|
|
|
|
//load the actual URDF and send a report: completed or failed
|
|
|
|
|
|
|
|
bool completedOk = loadUrdf(clientCmd.m_urdfArguments.m_urdfFileName,
|
|
btVector3(0,0,0), btQuaternion(0,0,0,1),clientCmd.m_urdfArguments.m_useMultiBody,clientCmd.m_urdfArguments.m_useFixedBase);
|
|
SharedMemoryCommand& serverCmd =m_testBlock1->m_serverCommands[0];
|
|
|
|
if (completedOk)
|
|
{
|
|
serverCmd.m_type =CMD_URDF_LOADING_COMPLETED;
|
|
} else
|
|
{
|
|
serverCmd.m_type =CMD_URDF_LOADING_FAILED;
|
|
|
|
}
|
|
m_testBlock1->m_numServerCommands++;
|
|
break;
|
|
}
|
|
case CMD_REQUEST_ACTUAL_STATE:
|
|
{
|
|
b3Printf("Sending the actual state (Q,U)");
|
|
if (m_dynamicsWorld->getNumMultibodies()>0)
|
|
{
|
|
btMultiBody* mb = m_dynamicsWorld->getMultiBody(0);
|
|
SharedMemoryCommand& serverCmd = m_testBlock1->m_serverCommands[0];
|
|
serverCmd.m_type = CMD_ACTUAL_STATE_UPDATE_COMPLETED;
|
|
|
|
serverCmd.m_sendActualStateArgs.m_bodyUniqueId = 0;
|
|
int totalDegreeOfFreedomQ = 0;
|
|
int totalDegreeOfFreedomU = 0;
|
|
|
|
//always add the base, even for static (non-moving objects)
|
|
//so that we can easily move the 'fixed' base when needed
|
|
//do we don't use this conditional "if (!mb->hasFixedBase())"
|
|
{
|
|
btTransform tr;
|
|
tr.setOrigin(mb->getBasePos());
|
|
tr.setRotation(mb->getWorldToBaseRot().inverse());
|
|
|
|
//base position in world space, carthesian
|
|
m_testBlock1->m_actualStateQ[0] = tr.getOrigin()[0];
|
|
m_testBlock1->m_actualStateQ[1] = tr.getOrigin()[1];
|
|
m_testBlock1->m_actualStateQ[2] = tr.getOrigin()[2];
|
|
|
|
//base orientation, quaternion x,y,z,w, in world space, carthesian
|
|
m_testBlock1->m_actualStateQ[3] = tr.getRotation()[0];
|
|
m_testBlock1->m_actualStateQ[4] = tr.getRotation()[1];
|
|
m_testBlock1->m_actualStateQ[5] = tr.getRotation()[2];
|
|
m_testBlock1->m_actualStateQ[6] = tr.getRotation()[3];
|
|
totalDegreeOfFreedomQ +=7;//pos + quaternion
|
|
|
|
//base linear velocity (in world space, carthesian)
|
|
m_testBlock1->m_actualStateQdot[0] = mb->getBaseVel()[0];
|
|
m_testBlock1->m_actualStateQdot[1] = mb->getBaseVel()[1];
|
|
m_testBlock1->m_actualStateQdot[2] = mb->getBaseVel()[2];
|
|
|
|
//base angular velocity (in world space, carthesian)
|
|
m_testBlock1->m_actualStateQdot[3] = mb->getBaseOmega()[0];
|
|
m_testBlock1->m_actualStateQdot[4] = mb->getBaseOmega()[1];
|
|
m_testBlock1->m_actualStateQdot[5] = mb->getBaseOmega()[2];
|
|
totalDegreeOfFreedomU += 6;//3 linear and 3 angular DOF
|
|
}
|
|
for (int l=0;l<mb->getNumLinks();l++)
|
|
{
|
|
for (int d=0;d<mb->getLink(l).m_posVarCount;d++)
|
|
{
|
|
m_testBlock1->m_actualStateQ[totalDegreeOfFreedomQ++] = mb->getJointPosMultiDof(l)[d];
|
|
}
|
|
for (int d=0;d<mb->getLink(l).m_dofCount;d++)
|
|
{
|
|
m_testBlock1->m_actualStateQdot[totalDegreeOfFreedomU++] = mb->getJointVelMultiDof(l)[d];
|
|
}
|
|
|
|
}
|
|
|
|
serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomQ = totalDegreeOfFreedomQ;
|
|
serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomU = totalDegreeOfFreedomU;
|
|
|
|
|
|
} else
|
|
{
|
|
b3Warning("Request state but no multibody available");
|
|
//rigid bodies?
|
|
}
|
|
/*
|
|
|
|
//now we send back the actual q, q' and force/torque and IMU sensor values
|
|
for (int i=0;i<m_jointFeedbacks.size();i++)
|
|
{
|
|
printf("Applied force A:(%f,%f,%f), torque A:(%f,%f,%f)\nForce B:(%f,%f,%f), torque B:(%f,%f,%f)\n",
|
|
m_jointFeedbacks[i]->m_appliedForceBodyA.x(),
|
|
m_jointFeedbacks[i]->m_appliedForceBodyA.y(),
|
|
m_jointFeedbacks[i]->m_appliedForceBodyA.z(),
|
|
m_jointFeedbacks[i]->m_appliedTorqueBodyA.x(),
|
|
m_jointFeedbacks[i]->m_appliedTorqueBodyA.y(),
|
|
m_jointFeedbacks[i]->m_appliedTorqueBodyA.z(),
|
|
m_jointFeedbacks[i]->m_appliedForceBodyB.x(),
|
|
m_jointFeedbacks[i]->m_appliedForceBodyB.y(),
|
|
m_jointFeedbacks[i]->m_appliedForceBodyB.z(),
|
|
m_jointFeedbacks[i]->m_appliedTorqueBodyB.x(),
|
|
m_jointFeedbacks[i]->m_appliedTorqueBodyB.y(),
|
|
m_jointFeedbacks[i]->m_appliedTorqueBodyB.z());
|
|
}
|
|
*/
|
|
|
|
m_testBlock1->m_numServerCommands++;
|
|
|
|
|
|
break;
|
|
}
|
|
case CMD_STEP_FORWARD_SIMULATION:
|
|
{
|
|
|
|
b3Printf("Step simulation request");
|
|
double timeStep = clientCmd.m_stepSimulationArguments.m_deltaTimeInSeconds;
|
|
m_dynamicsWorld->stepSimulation(timeStep);
|
|
|
|
SharedMemoryCommand& serverCmd =m_testBlock1->m_serverCommands[0];
|
|
|
|
serverCmd.m_type =CMD_STEP_FORWARD_SIMULATION_COMPLETED;
|
|
m_testBlock1->m_numServerCommands++;
|
|
|
|
break;
|
|
}
|
|
case CMD_SHUTDOWN:
|
|
{
|
|
wantsShutdown = true;
|
|
break;
|
|
}
|
|
default:
|
|
{
|
|
b3Error("Unsupported command encountered");
|
|
btAssert(0);
|
|
}
|
|
};
|
|
|
|
|
|
|
|
//process the command right now
|
|
|
|
}
|
|
}
|
|
if (wantsShutdown)
|
|
{
|
|
m_wantsShutdown = true;
|
|
releaseSharedMemory();
|
|
}
|
|
}
|
|
|
|
|
|
class CommonExampleInterface* PhysicsServerCreateFunc(struct CommonExampleOptions& options)
|
|
{
|
|
return new PhysicsServer(options.m_guiHelper);
|
|
}
|
|
|
|
|