From 4688540a9846e20151a0c77fa464b267cae13d3d Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sun, 21 Jun 2015 13:24:36 -0700 Subject: [PATCH] minor update for shared memory interface --- .../GwenGUISupport/GwenParameterInterface.cpp | 5 +- examples/SharedMemory/PhysicsClient.cpp | 65 +++++++++++- examples/SharedMemory/PhysicsServer.cpp | 99 ++++++++++++++++--- examples/SharedMemory/SharedMemoryCommands.h | 76 ++++++++++++++ examples/SharedMemory/SharedMemoryInterface.h | 56 ++--------- 5 files changed, 235 insertions(+), 66 deletions(-) create mode 100644 examples/SharedMemory/SharedMemoryCommands.h diff --git a/examples/ExampleBrowser/GwenGUISupport/GwenParameterInterface.cpp b/examples/ExampleBrowser/GwenGUISupport/GwenParameterInterface.cpp index 322fca33c..4389f9e03 100644 --- a/examples/ExampleBrowser/GwenGUISupport/GwenParameterInterface.cpp +++ b/examples/ExampleBrowser/GwenGUISupport/GwenParameterInterface.cpp @@ -129,7 +129,6 @@ void GwenParameterInterface::setSliderValue(int sliderIndex, double sliderValue) void GwenParameterInterface::registerButtonParameter(ButtonParams& params) { - Gwen::Controls::TextBox* label = new Gwen::Controls::TextBox(m_gwenInternalData->m_demoPage->GetPage()); Gwen::Controls::Button* button = new Gwen::Controls::Button(m_gwenInternalData->m_demoPage->GetPage()); MyButtonEventHandler* handler = new MyButtonEventHandler(params.m_callback,params.m_buttonId,params.m_userPointer); @@ -139,7 +138,9 @@ void GwenParameterInterface::registerButtonParameter(ButtonParams& params) m_paramInternalData->m_buttons.push_back(button); m_paramInternalData->m_buttonEventHandlers.push_back(handler); - button->SetPos( 10, m_gwenInternalData->m_curYposition ); + button->SetPos( 5, m_gwenInternalData->m_curYposition ); + button->SetWidth(120); + m_gwenInternalData->m_curYposition+=22; } diff --git a/examples/SharedMemory/PhysicsClient.cpp b/examples/SharedMemory/PhysicsClient.cpp index 37d01c739..7ba0e22bc 100644 --- a/examples/SharedMemory/PhysicsClient.cpp +++ b/examples/SharedMemory/PhysicsClient.cpp @@ -59,7 +59,11 @@ void MyCallback(int buttonId, bool buttonState, void* userPtr) cl->submitCommand(CMD_LOAD_URDF); break; } - + case CMD_REQUEST_ACTUAL_STATE: + { + cl->submitCommand(CMD_REQUEST_ACTUAL_STATE); + break; + } case CMD_STEP_FORWARD_SIMULATION: { cl->submitCommand(CMD_STEP_FORWARD_SIMULATION); @@ -129,6 +133,22 @@ void PhysicsClient::initPhysics() m_guiHelper->getParameterInterface()->registerButtonParameter(button); } + { + bool isTrigger = false; + ButtonParams button("Get State",CMD_REQUEST_ACTUAL_STATE, isTrigger); + button.m_callback = MyCallback; + button.m_userPointer = this; + m_guiHelper->getParameterInterface()->registerButtonParameter(button); + } + /* + { + bool isTrigger = false; + ButtonParams button("Send Desired State",CMD_SEND_DESIRED_STATE, isTrigger); + button.m_callback = MyCallback; + button.m_userPointer = this; + m_guiHelper->getParameterInterface()->registerButtonParameter(button); + } + */ { bool isTrigger = false; ButtonParams button("Shut Down",CMD_SHUTDOWN, isTrigger); @@ -190,6 +210,33 @@ void PhysicsClient::stepSimulation(float deltaTime) m_serverLoadUrdfOK = false; break; } + case CMD_ACTUAL_STATE_UPDATE_COMPLETED: + { + b3Printf("Received actual state"); + + int numQ = m_testBlock1->m_serverCommands[0].m_sendActualStateArgs.m_numDegreeOfFreedomQ; + int numU = m_testBlock1->m_serverCommands[0].m_sendActualStateArgs.m_numDegreeOfFreedomU; + b3Printf("size Q = %d, size U = %d\n", numQ,numU); + char msg[1024]; + + sprintf(msg,"Q=["); + + for (int i=0;im_actualStateQ[i]); + } else + { + sprintf(msg,"%s%f",msg,m_testBlock1->m_actualStateQ[i]); + } + } + sprintf(msg,"%s]",msg); + + b3Printf(msg); + + break; + } default: { b3Error("Unknown server command"); @@ -237,12 +284,26 @@ void PhysicsClient::stepSimulation(float deltaTime) } break; } + case CMD_REQUEST_ACTUAL_STATE: + { + if (m_serverLoadUrdfOK) + { + b3Printf("Requesting actual state"); + m_testBlock1->m_clientCommands[0].m_type =CMD_REQUEST_ACTUAL_STATE; + m_testBlock1->m_numClientCommands++; + + } else + { + b3Warning("No URDF loaded"); + } + break; + } case CMD_STEP_FORWARD_SIMULATION: { if (m_serverLoadUrdfOK) { - m_testBlock1->m_clientCommands[0].m_type =CMD_STEP_FORWARD_SIMULATION; + m_testBlock1->m_clientCommands[0].m_type =CMD_STEP_FORWARD_SIMULATION; m_testBlock1->m_clientCommands[0].m_stepSimulationArguments.m_deltaTimeInSeconds = 1./60.; m_testBlock1->m_numClientCommands++; b3Printf("client created CMD_STEP_FORWARD_SIMULATION %d\n", m_counter++); diff --git a/examples/SharedMemory/PhysicsServer.cpp b/examples/SharedMemory/PhysicsServer.cpp index 1f0165d4f..f252438c2 100644 --- a/examples/SharedMemory/PhysicsServer.cpp +++ b/examples/SharedMemory/PhysicsServer.cpp @@ -217,19 +217,75 @@ void PhysicsServer::stepSimulation(float deltaTime) 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++; + 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; - //now we send back the actual q, q' and force/torque and IMU sensor values + 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;lgetNumLinks();l++) + { + for (int d=0;dgetLink(l).m_posVarCount;d++) + { + m_testBlock1->m_actualStateQ[totalDegreeOfFreedomQ++] = mb->getJointPosMultiDof(l)[d]; + } + for (int d=0;dgetLink(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;im_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: diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h new file mode 100644 index 000000000..320a322fe --- /dev/null +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -0,0 +1,76 @@ + +#ifndef SHARED_MEMORY_COMMANDS_H +#define SHARED_MEMORY_COMMANDS_H + +//this is a very experimental draft of commands. We will iterate on this API (commands, arguments etc) + +enum SharedMemoryServerCommand{ + CMD_URDF_LOADING_COMPLETED, + CMD_URDF_LOADING_FAILED, + CMD_ACTUAL_STATE_UPDATE_COMPLETED, +// CMD_DESIRED_STATE_RECEIVED_COMPLETED, + CMD_STEP_FORWARD_SIMULATION_COMPLETED, + CMD_MAX_SERVER_COMMANDS +}; + +enum SharedMemoryClientCommand{ + CMD_LOAD_URDF, + CMD_REQUEST_ACTUAL_STATE, +// CMD_SEND_DESIRED_STATE, + CMD_STEP_FORWARD_SIMULATION, //includes CMD_REQUEST_STATE + CMD_SHUTDOWN, + CMD_MAX_CLIENT_COMMANDS +}; + +#define SHARED_MEMORY_SERVER_TEST_C +#define MAX_DEGREE_OF_FREEDOM 1024 +#define MAX_NUM_SENSORS 1024 +#define MAX_URDF_FILENAME_LENGTH 1024 + + +struct UrdfArgs +{ + char m_urdfFileName[MAX_URDF_FILENAME_LENGTH]; + bool m_useMultiBody; + bool m_useFixedBase; +}; + +struct SendDesiredStateArgs +{ + int m_bodyUniqueId; +}; + +struct RequestActualStateArgs +{ + int m_bodyUniqueId; +}; + +struct SendActualStateArgs +{ + int m_bodyUniqueId; + int m_numDegreeOfFreedomQ; + int m_numDegreeOfFreedomU; +}; + + +struct StepSimulationArgs +{ + double m_deltaTimeInSeconds; +}; + +struct SharedMemoryCommand +{ + int m_type; + + union + { + UrdfArgs m_urdfArguments; + StepSimulationArgs m_stepSimulationArguments; + SendDesiredStateArgs m_sendDesiredQandUStateCommandArgument; + RequestActualStateArgs m_requestActualStateInformationCommandArgument; + SendActualStateArgs m_sendActualStateArgs; + }; +}; + + +#endif //SHARED_MEMORY_COMMANDS_H diff --git a/examples/SharedMemory/SharedMemoryInterface.h b/examples/SharedMemory/SharedMemoryInterface.h index c69dad4ec..6a31a6e94 100644 --- a/examples/SharedMemory/SharedMemoryInterface.h +++ b/examples/SharedMemory/SharedMemoryInterface.h @@ -5,51 +5,7 @@ #define SHARED_MEMORY_MAGIC_NUMBER 64738 #define SHARED_MEMORY_MAX_COMMANDS 64 -enum SharedMemoryServerCommand{ - CMD_URDF_LOADING_COMPLETED, - CMD_URDF_LOADING_FAILED, - - CMD_SERVER_STATE_UPDATE_COMPLETED, - CMD_STEP_FORWARD_SIMULATION_COMPLETED, - CMD_MAX_SERVER_COMMANDS -}; - -enum SharedMemoryClientCommand{ - CMD_LOAD_URDF, - CMD_STATE_UPDATED, - CMD_STEP_FORWARD_SIMULATION, //includes CMD_REQUEST_STATE - CMD_SHUTDOWN, - CMD_MAX_CLIENT_COMMANDS -}; - -#define SHARED_MEMORY_SERVER_TEST_C -#define MAX_DEGREE_OF_FREEDOM 1024 -#define MAX_NUM_SENSORS 1024 -#define MAX_URDF_FILENAME_LENGTH 1024 - - -struct UrdfCommandArgument -{ - char m_urdfFileName[MAX_URDF_FILENAME_LENGTH]; - bool m_useMultiBody; - bool m_useFixedBase; -}; - -struct StepSimulationCommandArgument -{ - double m_deltaTimeInSeconds; -}; - -struct SharedMemoryCommand -{ - int m_type; - - union - { - UrdfCommandArgument m_urdfArguments; - StepSimulationCommandArgument m_stepSimulationArguments; - }; -}; +#include "SharedMemoryCommands.h" struct SharedMemoryExampleData { @@ -62,19 +18,19 @@ 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]; - double m_desiredStateSensors[MAX_NUM_SENSORS];//these are force sensors and IMU information //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 - - + }; #define SHARED_MEMORY_SIZE sizeof(SharedMemoryExampleData)