From b563c7c8ceb8bf385105b8b4c33af72ac363babb Mon Sep 17 00:00:00 2001 From: erwin coumans Date: Wed, 15 Jul 2015 15:23:40 -0700 Subject: [PATCH] add additional example using server and client for future simple robot control of simulated robot (load urdf, get state, set desired state, step simulation) Create SharedMemoryCommand and put that into the queue move arrays for Q, Qdot etc into the command --- examples/ExampleBrowser/premake4.lua | 1 + examples/SharedMemory/PhysicsClient.cpp | 57 ++-------- .../SharedMemory/PhysicsClientExample.cpp | 98 ++++++++++++++--- examples/SharedMemory/PhysicsServer.cpp | 48 ++++----- examples/SharedMemory/RobotControlExample.cpp | 102 +++++++++++++++--- examples/SharedMemory/SharedMemoryCommands.h | 38 ++++++- examples/SharedMemory/SharedMemoryInterface.h | 18 +--- examples/SharedMemory/premake4.lua | 1 + 8 files changed, 246 insertions(+), 117 deletions(-) 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",