diff --git a/examples/ExampleBrowser/premake4.lua b/examples/ExampleBrowser/premake4.lua index 1cde49ca1..e0843482e 100644 --- a/examples/ExampleBrowser/premake4.lua +++ b/examples/ExampleBrowser/premake4.lua @@ -52,6 +52,7 @@ "**.h", "../SharedMemory/PhysicsServerExample.cpp", "../SharedMemory/PhysicsClientExample.cpp", + "../SharedMemory/RobotControlExample.cpp", "../SharedMemory/PhysicsServer.cpp", "../SharedMemory/PhysicsClient.cpp", "../SharedMemory/PosixSharedMemory.cpp", diff --git a/examples/SharedMemory/PhysicsClient.cpp b/examples/SharedMemory/PhysicsClient.cpp index 797c0fd29..5bbf80376 100644 --- a/examples/SharedMemory/PhysicsClient.cpp +++ b/examples/SharedMemory/PhysicsClient.cpp @@ -205,9 +205,10 @@ void PhysicsClientSharedMemory::processServerStatus() case CMD_ACTUAL_STATE_UPDATE_COMPLETED: { b3Printf("Received actual state\n"); - - int numQ = m_data->m_testBlock1->m_serverCommands[0].m_sendActualStateArgs.m_numDegreeOfFreedomQ; - int numU = m_data->m_testBlock1->m_serverCommands[0].m_sendActualStateArgs.m_numDegreeOfFreedomU; + SharedMemoryCommand& command = m_data->m_testBlock1->m_serverCommands[0]; + + int numQ = command.m_sendActualStateArgs.m_numDegreeOfFreedomQ; + int numU = command.m_sendActualStateArgs.m_numDegreeOfFreedomU; b3Printf("size Q = %d, size U = %d\n", numQ,numU); char msg[1024]; @@ -217,10 +218,10 @@ void PhysicsClientSharedMemory::processServerStatus() { if (im_testBlock1->m_actualStateQ[i]); + sprintf(msg,"%s%f,",msg,command.m_sendActualStateArgs.m_actualStateQ[i]); } else { - sprintf(msg,"%s%f",msg,m_data->m_testBlock1->m_actualStateQ[i]); + sprintf(msg,"%s%f",msg,command.m_sendActualStateArgs.m_actualStateQ[i]); } } sprintf(msg,"%s]",msg); @@ -271,18 +272,8 @@ bool PhysicsClientSharedMemory::submitClientCommand(const SharedMemoryCommand& c { if (!m_data->m_serverLoadUrdfOK) { - m_data->m_testBlock1->m_clientCommands[0].m_type =CMD_LOAD_URDF; - sprintf(m_data->m_testBlock1->m_clientCommands[0].m_urdfArguments.m_urdfFileName,"r2d2.urdf"); - m_data->m_testBlock1->m_clientCommands[0].m_urdfArguments.m_initialPosition[0] = 0.0; - m_data->m_testBlock1->m_clientCommands[0].m_urdfArguments.m_initialPosition[1] = 0.0; - m_data->m_testBlock1->m_clientCommands[0].m_urdfArguments.m_initialPosition[2] = 0.0; - m_data->m_testBlock1->m_clientCommands[0].m_urdfArguments.m_initialOrientation[0] = 0.0; - m_data->m_testBlock1->m_clientCommands[0].m_urdfArguments.m_initialOrientation[1] = 0.0; - m_data->m_testBlock1->m_clientCommands[0].m_urdfArguments.m_initialOrientation[2] = 0.0; - m_data->m_testBlock1->m_clientCommands[0].m_urdfArguments.m_initialOrientation[3] = 1.0; - m_data->m_testBlock1->m_clientCommands[0].m_urdfArguments.m_useFixedBase = false; - m_data->m_testBlock1->m_clientCommands[0].m_urdfArguments.m_useMultiBody = true; - + m_data->m_testBlock1->m_clientCommands[0] = command; + m_data->m_testBlock1->m_numClientCommands++; b3Printf("Client created CMD_LOAD_URDF\n"); } else @@ -371,37 +362,7 @@ bool PhysicsClientSharedMemory::submitClientCommand(const SharedMemoryCommand& c if (m_data->m_serverLoadUrdfOK) { b3Printf("Sending desired state (pos, vel, torque)\n"); - m_data->m_testBlock1->m_clientCommands[0].m_type =CMD_SEND_DESIRED_STATE; - //todo: expose a drop box in the GUI for this - int controlMode = CONTROL_MODE_VELOCITY;//CONTROL_MODE_TORQUE; - - switch (controlMode) - { - case CONTROL_MODE_VELOCITY: - { - m_data->m_testBlock1->m_clientCommands[0].m_sendDesiredStateCommandArgument.m_controlMode = CONTROL_MODE_VELOCITY; - for (int i=0;im_testBlock1->m_desiredStateQdot[i] = 1; - m_data->m_testBlock1->m_desiredStateForceTorque[i] = 100; - } - break; - } - case CONTROL_MODE_TORQUE: - { - m_data->m_testBlock1->m_clientCommands[0].m_sendDesiredStateCommandArgument.m_controlMode = CONTROL_MODE_TORQUE; - for (int i=0;im_testBlock1->m_desiredStateForceTorque[i] = 100; - } - break; - } - default: - { - b3Printf("Unknown control mode in client CMD_SEND_DESIRED_STATE"); - btAssert(0); - } - } + m_data->m_testBlock1->m_clientCommands[0] = command; m_data->m_testBlock1->m_numClientCommands++; diff --git a/examples/SharedMemory/PhysicsClientExample.cpp b/examples/SharedMemory/PhysicsClientExample.cpp index 0b9d305ab..caf899980 100644 --- a/examples/SharedMemory/PhysicsClientExample.cpp +++ b/examples/SharedMemory/PhysicsClientExample.cpp @@ -16,7 +16,7 @@ protected: bool m_wantsTermination; - btAlignedObjectArray m_userCommandRequests; + btAlignedObjectArray m_userCommandRequests; void createButton(const char* name, int id, bool isTrigger ); @@ -42,7 +42,7 @@ public: { return m_wantsTermination; } - void enqueueCommand(int command); + void enqueueCommand(const SharedMemoryCommand& orgCommand); }; @@ -50,17 +50,85 @@ public: void MyCallback(int buttonId, bool buttonState, void* userPtr) { PhysicsClientExample* cl = (PhysicsClientExample*) userPtr; + + SharedMemoryCommand command; + switch (buttonId) { case CMD_LOAD_URDF: + { + command.m_type =CMD_LOAD_URDF; + sprintf(command.m_urdfArguments.m_urdfFileName,"hinge.urdf");//r2d2.urdf"); + command.m_urdfArguments.m_initialPosition[0] = 0.0; + command.m_urdfArguments.m_initialPosition[1] = 0.0; + command.m_urdfArguments.m_initialPosition[2] = 0.0; + command.m_urdfArguments.m_initialOrientation[0] = 0.0; + command.m_urdfArguments.m_initialOrientation[1] = 0.0; + command.m_urdfArguments.m_initialOrientation[2] = 0.0; + command.m_urdfArguments.m_initialOrientation[3] = 1.0; + command.m_urdfArguments.m_useFixedBase = false; + command.m_urdfArguments.m_useMultiBody = true; + cl->enqueueCommand(command); + break; + } case CMD_CREATE_BOX_COLLISION_SHAPE: + { + command.m_type =CMD_CREATE_BOX_COLLISION_SHAPE; + cl->enqueueCommand(command); + break; + } case CMD_REQUEST_ACTUAL_STATE: + { + command.m_type =CMD_REQUEST_ACTUAL_STATE; + cl->enqueueCommand(command); + break; + }; case CMD_STEP_FORWARD_SIMULATION: - case CMD_SHUTDOWN: + { + command.m_type =CMD_STEP_FORWARD_SIMULATION; + cl->enqueueCommand(command); + break; + } + case CMD_SEND_DESIRED_STATE: + { + command.m_type =CMD_SEND_DESIRED_STATE; + int controlMode = CONTROL_MODE_VELOCITY;//CONTROL_MODE_TORQUE; + + command.m_sendDesiredStateCommandArgument.m_controlMode = controlMode; + //todo: expose a drop box in the GUI for this + switch (controlMode) + { + case CONTROL_MODE_VELOCITY: + { + for (int i=0;ienqueueCommand(command); + break; + } case CMD_SEND_BULLET_DATA_STREAM: { - cl->enqueueCommand(buttonId); + command.m_type = buttonId; + cl->enqueueCommand(command); break; } @@ -72,12 +140,13 @@ void MyCallback(int buttonId, bool buttonState, void* userPtr) }; } + void PhysicsClientExample::enqueueCommand(const SharedMemoryCommand& orgCommand) + { + m_userCommandRequests.push_back(orgCommand); + SharedMemoryCommand& cmd = m_userCommandRequests[m_userCommandRequests.size()-1]; -void PhysicsClientExample::enqueueCommand(int command) -{ - m_userCommandRequests.push_back(command); - b3Printf("User put command request %d on queue (queue length = %d)\n",command, m_userCommandRequests.size()); -} + b3Printf("User put command request %d on queue (queue length = %d)\n",cmd.m_type, m_userCommandRequests.size()); + } PhysicsClientExample::PhysicsClientExample(GUIHelperInterface* helper) @@ -114,6 +183,7 @@ void PhysicsClientExample::initPhysics() } else { + /* m_userCommandRequests.push_back(CMD_LOAD_URDF); m_userCommandRequests.push_back(CMD_REQUEST_ACTUAL_STATE); m_userCommandRequests.push_back(CMD_SEND_DESIRED_STATE); @@ -124,7 +194,8 @@ void PhysicsClientExample::initPhysics() m_userCommandRequests.push_back(CMD_STEP_FORWARD_SIMULATION); m_userCommandRequests.push_back(CMD_REQUEST_ACTUAL_STATE); m_userCommandRequests.push_back(CMD_SHUTDOWN); - + */ + } if (!m_physicsClient.connect()) @@ -147,7 +218,7 @@ void PhysicsClientExample::stepSimulation(float deltaTime) if (m_userCommandRequests.size()) { b3Printf("Outstanding user command requests: %d\n", m_userCommandRequests.size()); - int command = m_userCommandRequests[0]; + SharedMemoryCommand command = m_userCommandRequests[0]; //a manual 'pop_front', we don't use 'remove' because it will re-order the commands for (int i=1;iclearForcesAndTorques(); int torqueIndex = 0; - btVector3 f(m_data->m_testBlock1->m_desiredStateForceTorque[0], - m_data->m_testBlock1->m_desiredStateForceTorque[1], - m_data->m_testBlock1->m_desiredStateForceTorque[2]); - btVector3 t(m_data->m_testBlock1->m_desiredStateForceTorque[3], - m_data->m_testBlock1->m_desiredStateForceTorque[4], - m_data->m_testBlock1->m_desiredStateForceTorque[5]); + btVector3 f(clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[0], + clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[1], + clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[2]); + btVector3 t(clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[3], + clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[4], + clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[5]); torqueIndex+=6; mb->addBaseForce(f); mb->addBaseTorque(t); @@ -399,7 +399,7 @@ void PhysicsServerSharedMemory::processClientCommands() for (int dof=0;dofgetLink(link).m_dofCount;dof++) { - double torque = m_data->m_testBlock1->m_desiredStateForceTorque[torqueIndex]; + double torque = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[torqueIndex]; mb->addJointTorqueMultiDof(link,dof,torque); torqueIndex++; } @@ -423,12 +423,12 @@ void PhysicsServerSharedMemory::processClientCommands() if (motorPtr) { btMultiBodyJointMotor* motor = *motorPtr; - btScalar desiredVelocity = m_data->m_testBlock1->m_desiredStateQdot[dofIndex]; + btScalar desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex]; motor->setVelocityTarget(desiredVelocity); btScalar physicsDeltaTime=1./60.;//todo: set the physicsDeltaTime explicitly in the API, separate from the 'stepSimulation' - btScalar maxImp = m_data->m_testBlock1->m_desiredStateForceTorque[dofIndex]*physicsDeltaTime; + btScalar maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex]*physicsDeltaTime; motor->setMaxAppliedImpulse(maxImp); numMotors++; @@ -478,37 +478,37 @@ void PhysicsServerSharedMemory::processClientCommands() tr.setRotation(mb->getWorldToBaseRot().inverse()); //base position in world space, carthesian - m_data->m_testBlock1->m_actualStateQ[0] = tr.getOrigin()[0]; - m_data->m_testBlock1->m_actualStateQ[1] = tr.getOrigin()[1]; - m_data->m_testBlock1->m_actualStateQ[2] = tr.getOrigin()[2]; + serverCmd.m_sendActualStateArgs.m_actualStateQ[0] = tr.getOrigin()[0]; + serverCmd.m_sendActualStateArgs.m_actualStateQ[1] = tr.getOrigin()[1]; + serverCmd.m_sendActualStateArgs.m_actualStateQ[2] = tr.getOrigin()[2]; //base orientation, quaternion x,y,z,w, in world space, carthesian - m_data->m_testBlock1->m_actualStateQ[3] = tr.getRotation()[0]; - m_data->m_testBlock1->m_actualStateQ[4] = tr.getRotation()[1]; - m_data->m_testBlock1->m_actualStateQ[5] = tr.getRotation()[2]; - m_data->m_testBlock1->m_actualStateQ[6] = tr.getRotation()[3]; + serverCmd.m_sendActualStateArgs.m_actualStateQ[3] = tr.getRotation()[0]; + serverCmd.m_sendActualStateArgs.m_actualStateQ[4] = tr.getRotation()[1]; + serverCmd.m_sendActualStateArgs.m_actualStateQ[5] = tr.getRotation()[2]; + serverCmd.m_sendActualStateArgs.m_actualStateQ[6] = tr.getRotation()[3]; totalDegreeOfFreedomQ +=7;//pos + quaternion //base linear velocity (in world space, carthesian) - m_data->m_testBlock1->m_actualStateQdot[0] = mb->getBaseVel()[0]; - m_data->m_testBlock1->m_actualStateQdot[1] = mb->getBaseVel()[1]; - m_data->m_testBlock1->m_actualStateQdot[2] = mb->getBaseVel()[2]; + serverCmd.m_sendActualStateArgs.m_actualStateQdot[0] = mb->getBaseVel()[0]; + serverCmd.m_sendActualStateArgs.m_actualStateQdot[1] = mb->getBaseVel()[1]; + serverCmd.m_sendActualStateArgs.m_actualStateQdot[2] = mb->getBaseVel()[2]; //base angular velocity (in world space, carthesian) - m_data->m_testBlock1->m_actualStateQdot[3] = mb->getBaseOmega()[0]; - m_data->m_testBlock1->m_actualStateQdot[4] = mb->getBaseOmega()[1]; - m_data->m_testBlock1->m_actualStateQdot[5] = mb->getBaseOmega()[2]; + serverCmd.m_sendActualStateArgs.m_actualStateQdot[3] = mb->getBaseOmega()[0]; + serverCmd.m_sendActualStateArgs.m_actualStateQdot[4] = mb->getBaseOmega()[1]; + serverCmd.m_sendActualStateArgs.m_actualStateQdot[5] = mb->getBaseOmega()[2]; totalDegreeOfFreedomU += 6;//3 linear and 3 angular DOF } for (int l=0;lgetNumLinks();l++) { for (int d=0;dgetLink(l).m_posVarCount;d++) { - m_data->m_testBlock1->m_actualStateQ[totalDegreeOfFreedomQ++] = mb->getJointPosMultiDof(l)[d]; + serverCmd.m_sendActualStateArgs.m_actualStateQ[totalDegreeOfFreedomQ++] = mb->getJointPosMultiDof(l)[d]; } for (int d=0;dgetLink(l).m_dofCount;d++) { - m_data->m_testBlock1->m_actualStateQdot[totalDegreeOfFreedomU++] = mb->getJointVelMultiDof(l)[d]; + serverCmd.m_sendActualStateArgs.m_actualStateQdot[totalDegreeOfFreedomU++] = mb->getJointVelMultiDof(l)[d]; } } diff --git a/examples/SharedMemory/RobotControlExample.cpp b/examples/SharedMemory/RobotControlExample.cpp index 442e0fa07..777181c66 100644 --- a/examples/SharedMemory/RobotControlExample.cpp +++ b/examples/SharedMemory/RobotControlExample.cpp @@ -9,6 +9,8 @@ #include "PhysicsServer.h" #include "PhysicsClient.h" #include "SharedMemoryCommon.h" +#include "../Utils/b3Clock.h" + //const char* blaatnaam = "basename"; @@ -18,10 +20,12 @@ class RobotControlExample : public SharedMemoryCommon { PhysicsServerSharedMemory m_physicsServer; PhysicsClientSharedMemory m_physicsClient; - + b3Clock m_realtimeClock; + + int m_sequenceNumberGenerator; bool m_wantsShutdown; - btAlignedObjectArray m_userCommandRequests; + btAlignedObjectArray m_userCommandRequests; void createButton(const char* name, int id, bool isTrigger ); @@ -35,10 +39,14 @@ public: virtual void stepSimulation(float deltaTime); - void enqueueCommand(int command) + void enqueueCommand(const SharedMemoryCommand& orgCommand) { - m_userCommandRequests.push_back(command); - b3Printf("User put command request %d on queue (queue length = %d)\n",command, m_userCommandRequests.size()); + m_userCommandRequests.push_back(orgCommand); + SharedMemoryCommand& cmd = m_userCommandRequests[m_userCommandRequests.size()-1]; + cmd.m_sequenceNumber = m_sequenceNumberGenerator++; + cmd.m_timeStamp = m_realtimeClock.getTimeMicroseconds(); + + b3Printf("User put command request %d on queue (queue length = %d)\n",cmd.m_type, m_userCommandRequests.size()); } virtual void resetCamera() @@ -57,17 +65,85 @@ public: void MyCallback2(int buttonId, bool buttonState, void* userPtr) { RobotControlExample* cl = (RobotControlExample*) userPtr; + + SharedMemoryCommand command; + switch (buttonId) { case CMD_LOAD_URDF: + { + command.m_type =CMD_LOAD_URDF; + sprintf(command.m_urdfArguments.m_urdfFileName,"hinge.urdf");//r2d2.urdf"); + command.m_urdfArguments.m_initialPosition[0] = 0.0; + command.m_urdfArguments.m_initialPosition[1] = 0.0; + command.m_urdfArguments.m_initialPosition[2] = 0.0; + command.m_urdfArguments.m_initialOrientation[0] = 0.0; + command.m_urdfArguments.m_initialOrientation[1] = 0.0; + command.m_urdfArguments.m_initialOrientation[2] = 0.0; + command.m_urdfArguments.m_initialOrientation[3] = 1.0; + command.m_urdfArguments.m_useFixedBase = false; + command.m_urdfArguments.m_useMultiBody = true; + cl->enqueueCommand(command); + break; + } case CMD_CREATE_BOX_COLLISION_SHAPE: + { + command.m_type =CMD_CREATE_BOX_COLLISION_SHAPE; + cl->enqueueCommand(command); + break; + } case CMD_REQUEST_ACTUAL_STATE: + { + command.m_type =CMD_REQUEST_ACTUAL_STATE; + cl->enqueueCommand(command); + break; + }; case CMD_STEP_FORWARD_SIMULATION: - case CMD_SHUTDOWN: + { + command.m_type =CMD_STEP_FORWARD_SIMULATION; + cl->enqueueCommand(command); + break; + } + case CMD_SEND_DESIRED_STATE: + { + command.m_type =CMD_SEND_DESIRED_STATE; + int controlMode = CONTROL_MODE_VELOCITY;//CONTROL_MODE_TORQUE; + + command.m_sendDesiredStateCommandArgument.m_controlMode = controlMode; + //todo: expose a drop box in the GUI for this + switch (controlMode) + { + case CONTROL_MODE_VELOCITY: + { + for (int i=0;ienqueueCommand(command); + break; + } case CMD_SEND_BULLET_DATA_STREAM: { - cl->enqueueCommand(buttonId); + command.m_type = buttonId; + cl->enqueueCommand(command); break; } @@ -88,7 +164,8 @@ void RobotControlExample::createButton(const char* name, int buttonId, bool isTr RobotControlExample::RobotControlExample(GUIHelperInterface* helper) :SharedMemoryCommon(helper), -m_wantsShutdown(false) +m_wantsShutdown(false), +m_sequenceNumberGenerator(0) { bool useServer = true; @@ -131,6 +208,7 @@ void RobotControlExample::initPhysics() } else { + /* m_userCommandRequests.push_back(CMD_LOAD_URDF); m_userCommandRequests.push_back(CMD_REQUEST_ACTUAL_STATE); m_userCommandRequests.push_back(CMD_SEND_DESIRED_STATE); @@ -141,7 +219,7 @@ void RobotControlExample::initPhysics() m_userCommandRequests.push_back(CMD_STEP_FORWARD_SIMULATION); m_userCommandRequests.push_back(CMD_REQUEST_ACTUAL_STATE); m_userCommandRequests.push_back(CMD_SHUTDOWN); - + */ } if (!m_physicsClient.connect()) @@ -172,7 +250,7 @@ void RobotControlExample::stepSimulation(float deltaTime) if (m_userCommandRequests.size()) { b3Printf("Outstanding user command requests: %d\n", m_userCommandRequests.size()); - int command = m_userCommandRequests[0]; + SharedMemoryCommand& cmd = m_userCommandRequests[0]; //a manual 'pop_front', we don't use 'remove' because it will re-order the commands for (int i=1;i + typedef int32_t smInt32_t; + typedef int64_t smInt64_t; + typedef uint32_t smUint32_t; + typedef uint64_t smUint64_t; +#elif defined(_MSC_VER) + typedef __int32 smInt32_t; + typedef __int64 smInt64_t; + typedef unsigned __int32 smUint32_t; + typedef unsigned __int64 smUint64_t; +#else + typedef int smInt32_t; + typedef long long int smInt64_t; + typedef unsigned int smUint32_t; + typedef unsigned long long int smUint64_t; +#endif + enum SharedMemoryServerCommand { CMD_URDF_LOADING_COMPLETED, @@ -74,6 +92,15 @@ struct SendDesiredStateArgs { int m_bodyUniqueId; int m_controlMode; + + //desired state is only written by the client, read-only access by server is expected + double m_desiredStateQ[MAX_DEGREE_OF_FREEDOM]; + double m_desiredStateQdot[MAX_DEGREE_OF_FREEDOM]; + + //m_desiredStateForceTorque is either the actual applied force/torque (in CONTROL_MODE_TORQUE) or + //or m_desiredStateForceTorque is the maximum applied force/torque for the motor/constraint to reach the desired velocity in CONTROL_MODE_VELOCITY mode + double m_desiredStateForceTorque[MAX_DEGREE_OF_FREEDOM]; + }; struct RequestActualStateArgs @@ -99,6 +126,12 @@ struct SendActualStateArgs int m_bodyUniqueId; int m_numDegreeOfFreedomQ; int m_numDegreeOfFreedomU; + + //actual state is only written by the server, read-only access by client is expected + double m_actualStateQ[MAX_DEGREE_OF_FREEDOM]; + double m_actualStateQdot[MAX_DEGREE_OF_FREEDOM]; + double m_actualStateSensors[MAX_NUM_SENSORS];//these are force sensors and IMU information + }; @@ -111,7 +144,10 @@ struct StepSimulationArgs struct SharedMemoryCommand { int m_type; - + + smUint64_t m_timeStamp; + int m_sequenceNumber; + union { UrdfArgs m_urdfArguments; diff --git a/examples/SharedMemory/SharedMemoryInterface.h b/examples/SharedMemory/SharedMemoryInterface.h index dfb0f5ad7..43af44496 100644 --- a/examples/SharedMemory/SharedMemoryInterface.h +++ b/examples/SharedMemory/SharedMemoryInterface.h @@ -3,7 +3,7 @@ #define SHARED_MEMORY_KEY 12347 #define SHARED_MEMORY_MAGIC_NUMBER 64738 -#define SHARED_MEMORY_MAX_COMMANDS 64 +#define SHARED_MEMORY_MAX_COMMANDS 32 #define SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE (256*1024) #include "SharedMemoryCommands.h" @@ -20,22 +20,6 @@ struct SharedMemoryExampleData int m_numServerCommands; int m_numProcessedServerCommands; - //TODO: it is better to move this single desired/actual state (m_desiredStateQ, m_actualStateQ etc) into the command, - //so we can deal with multiple bodies/robots - - //desired state is only written by the client, read-only access by server is expected - double m_desiredStateQ[MAX_DEGREE_OF_FREEDOM]; - double m_desiredStateQdot[MAX_DEGREE_OF_FREEDOM]; - - //m_desiredStateForceTorque is either the actual applied force/torque (in CONTROL_MODE_TORQUE) or - //or m_desiredStateForceTorque is the maximum applied force/torque for the motor/constraint to reach the desired velocity in CONTROL_MODE_VELOCITY mode - double m_desiredStateForceTorque[MAX_DEGREE_OF_FREEDOM]; - - //actual state is only written by the server, read-only access by client is expected - double m_actualStateQ[MAX_DEGREE_OF_FREEDOM]; - double m_actualStateQdot[MAX_DEGREE_OF_FREEDOM]; - double m_actualStateSensors[MAX_NUM_SENSORS];//these are force sensors and IMU information - //m_bulletStreamDataClientToServer is a way for the client to create collision shapes, rigid bodies and constraints //the Bullet data structures are more general purpose than the capabilities of a URDF file. char m_bulletStreamDataClientToServer[SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE]; diff --git a/examples/SharedMemory/premake4.lua b/examples/SharedMemory/premake4.lua index b0f10c9b8..292276d06 100644 --- a/examples/SharedMemory/premake4.lua +++ b/examples/SharedMemory/premake4.lua @@ -33,6 +33,7 @@ files { "../Importers/ImportURDFDemo/URDF2Bullet.cpp", "../Importers/ImportURDFDemo/URDF2Bullet.h", "../Utils/b3ResourcePath.cpp", + "../Utils/b3Clock.cpp", "../../Extras/Serialize/BulletWorldImporter/*", "../../Extras/Serialize/BulletFileLoader/*", "../Importers/ImportURDFDemo/URDFImporterInterface.h",