diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt index dc0fb873b..317b89dbc 100644 --- a/examples/ExampleBrowser/CMakeLists.txt +++ b/examples/ExampleBrowser/CMakeLists.txt @@ -20,7 +20,6 @@ SET(App_ExampleBrowser_SRCS ../SharedMemory/PhysicsClientC_API.cpp ../SharedMemory/PhysicsServerExample.cpp ../SharedMemory/PhysicsClientExample.cpp - ../SharedMemory/RobotControlExample.cpp ../SharedMemory/PosixSharedMemory.cpp ../SharedMemory/Win32SharedMemory.cpp ../BasicDemo/BasicExample.cpp diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index 230b4216e..28cf7c0a8 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/examples/ExampleBrowser/ExampleEntries.cpp @@ -34,7 +34,6 @@ #include "../DynamicControlDemo/MotorDemo.h" #include "../RollingFrictionDemo/RollingFrictionDemo.h" #include "../SharedMemory/PhysicsServerExample.h" -#include "../SharedMemory/RobotControlExample.h" #include "../SharedMemory/PhysicsClientExample.h" #include "../Constraints/TestHingeTorque.h" #include "../RenderingExamples/TimeSeriesExample.h" @@ -204,12 +203,12 @@ static ExampleEntry gDefaultExamples[]= ExampleEntry(0,"Experiments"), - ExampleEntry(1,"Robot Control (Velocity)", "Perform some robot control tasks, using physics server and client that communicate over shared memory", - RobotControlExampleCreateFunc,ROBOT_VELOCITY_CONTROL), - ExampleEntry(1,"Robot Control (PD)", "Perform some robot control tasks, using physics server and client that communicate over shared memory", - RobotControlExampleCreateFunc,ROBOT_PD_CONTROL), - ExampleEntry(1,"Robot Joint Feedback", "Apply some small ping-pong target velocity jitter, and read the joint reaction forces, using physics server and client that communicate over shared memory.", - RobotControlExampleCreateFunc,ROBOT_PING_PONG_JOINT_FEEDBACK), +// ExampleEntry(1,"Robot Control (Velocity)", "Perform some robot control tasks, using physics server and client that communicate over shared memory", +// RobotControlExampleCreateFunc,ROBOT_VELOCITY_CONTROL), +// ExampleEntry(1,"Robot Control (PD)", "Perform some robot control tasks, using physics server and client that communicate over shared memory", +// RobotControlExampleCreateFunc,ROBOT_PD_CONTROL), +// ExampleEntry(1,"Robot Joint Feedback", "Apply some small ping-pong target velocity jitter, and read the joint reaction forces, using physics server and client that communicate over shared memory.", + // RobotControlExampleCreateFunc,ROBOT_PING_PONG_JOINT_FEEDBACK), ExampleEntry(1,"Physics Server", "Create a physics server that communicates with a physics client over shared memory", PhysicsServerCreateFunc), diff --git a/examples/ExampleBrowser/premake4.lua b/examples/ExampleBrowser/premake4.lua index fb650075c..fcc3330ab 100644 --- a/examples/ExampleBrowser/premake4.lua +++ b/examples/ExampleBrowser/premake4.lua @@ -56,7 +56,6 @@ "../SharedMemory/PhysicsClientC_API.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 413c87a26..4fe6067d7 100644 --- a/examples/SharedMemory/PhysicsClient.cpp +++ b/examples/SharedMemory/PhysicsClient.cpp @@ -18,6 +18,19 @@ enum JointType ePrismaticType = 1, }; +struct TmpFloat3 +{ + float m_x; + float m_y; + float m_z; + +}; + +TmpFloat3 CreateTmpFloat3(float x,float y, float z) +{ + TmpFloat3 tmp; tmp.m_x = x;tmp.m_y = y;tmp.m_z = z; return tmp; +} + struct PhysicsClientSharedMemoryInternalData { SharedMemoryInterface* m_sharedMemory; @@ -25,10 +38,12 @@ struct PhysicsClientSharedMemoryInternalData btAlignedObjectArray m_robotMultiBodyData; btAlignedObjectArray m_jointInfo; - btAlignedObjectArray m_debugLinesFrom; - btAlignedObjectArray m_debugLinesTo; - btAlignedObjectArray m_debugLinesColor; + btAlignedObjectArray m_debugLinesFrom; + btAlignedObjectArray m_debugLinesTo; + btAlignedObjectArray m_debugLinesColor; + SharedMemoryStatus m_lastServerStatus; + int m_counter; bool m_serverLoadUrdfOK; bool m_isConnected; @@ -146,20 +161,18 @@ bool PhysicsClientSharedMemory::connect() -bool PhysicsClientSharedMemory::processServerStatus(SharedMemoryStatus& serverStatus) +const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { - bool hasStatus = false; - + SharedMemoryStatus* stat = 0; + if (!m_data->m_testBlock1) { - serverStatus.m_type = CMD_SHARED_MEMORY_NOT_INITIALIZED; - return true; + return 0; } if (!m_data->m_waitingForServer) { - serverStatus.m_type = CMD_WAITING_FOR_CLIENT_COMMAND; - return true; + return 0; } if (m_data->m_testBlock1->m_numServerCommands> m_data->m_testBlock1->m_numProcessedServerCommands) @@ -167,8 +180,8 @@ bool PhysicsClientSharedMemory::processServerStatus(SharedMemoryStatus& serverSt btAssert(m_data->m_testBlock1->m_numServerCommands==m_data->m_testBlock1->m_numProcessedServerCommands+1); const SharedMemoryStatus& serverCmd =m_data->m_testBlock1->m_serverCommands[0]; - hasStatus = true; - serverStatus = serverCmd; + m_data->m_lastServerStatus = serverCmd; + EnumSharedMemoryServerStatus s = (EnumSharedMemoryServerStatus)serverCmd.m_type; //consume the command switch (serverCmd.m_type) @@ -424,6 +437,21 @@ bool PhysicsClientSharedMemory::processServerStatus(SharedMemoryStatus& serverSt } break; } + case CMD_RESET_SIMULATION_COMPLETED: + { + if (m_data->m_verboseOutput) + { + b3Printf("CMD_RESET_SIMULATION_COMPLETED clean data\n"); + } + for (int i=0;im_robotMultiBodyData.size();i++) + { + delete m_data->m_robotMultiBodyData[i]; + } + m_data->m_robotMultiBodyData.clear(); + + m_data->m_jointInfo.clear(); + break; + } case CMD_DEBUG_LINES_COMPLETED: { if (m_data->m_verboseOutput) @@ -442,14 +470,15 @@ bool PhysicsClientSharedMemory::processServerStatus(SharedMemoryStatus& serverSt for (int i=0;im_debugLinesFrom[serverCmd.m_sendDebugLinesArgs.m_startingLineIndex+i] = from; m_data->m_debugLinesTo[serverCmd.m_sendDebugLinesArgs.m_startingLineIndex+i] = to; m_data->m_debugLinesColor[serverCmd.m_sendDebugLinesArgs.m_startingLineIndex+i] = color; } + break; } case CMD_DEBUG_LINES_OVERFLOW_FAILED: @@ -481,6 +510,20 @@ bool PhysicsClientSharedMemory::processServerStatus(SharedMemoryStatus& serverSt { m_data->m_waitingForServer = true; } + + if ((serverCmd.m_type == CMD_DEBUG_LINES_COMPLETED) && (serverCmd.m_sendDebugLinesArgs.m_numRemainingDebugLines>0)) + { + SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0]; + + //continue requesting debug lines for drawing + command.m_type =CMD_REQUEST_DEBUG_LINES; + command.m_requestDebugLinesArguments.m_startingLineIndex = serverCmd.m_sendDebugLinesArgs.m_numDebugLines+serverCmd.m_sendDebugLinesArgs.m_startingLineIndex; + submitClientCommand(command); + return 0; + } + + return &m_data->m_lastServerStatus; + } else { if (m_data->m_verboseOutput) @@ -489,7 +532,7 @@ bool PhysicsClientSharedMemory::processServerStatus(SharedMemoryStatus& serverSt m_data->m_testBlock1->m_numProcessedServerCommands); } } - return hasStatus; + return 0; } bool PhysicsClientSharedMemory::canSubmitCommand() const @@ -497,6 +540,11 @@ bool PhysicsClientSharedMemory::canSubmitCommand() const return (m_data->m_isConnected && !m_data->m_waitingForServer); } +struct SharedMemoryCommand* PhysicsClientSharedMemory::getAvailableSharedMemoryCommand() +{ + return &m_data->m_testBlock1->m_clientCommands[0]; +} + bool PhysicsClientSharedMemory::submitClientCommand(const SharedMemoryCommand& command) { ///at the moment we allow a maximum of 1 outstanding command, so we check for this @@ -505,19 +553,10 @@ bool PhysicsClientSharedMemory::submitClientCommand(const SharedMemoryCommand& c if (!m_data->m_waitingForServer) { - //a reset simulation command needs special attention, cleanup state - if (command.m_type==CMD_RESET_SIMULATION) - { - for (int i=0;im_robotMultiBodyData.size();i++) - { - delete m_data->m_robotMultiBodyData[i]; - } - m_data->m_robotMultiBodyData.clear(); - - m_data->m_jointInfo.clear(); - } - - m_data->m_testBlock1->m_clientCommands[0] = command; + if (&m_data->m_testBlock1->m_clientCommands[0] != &command) + { + m_data->m_testBlock1->m_clientCommands[0] = command; + } m_data->m_testBlock1->m_numClientCommands++; m_data->m_waitingForServer = true; return true; @@ -541,27 +580,27 @@ void PhysicsClientSharedMemory::uploadBulletFileToSharedMemory(const char* data, } -const btVector3* PhysicsClientSharedMemory::getDebugLinesFrom() const +const float* PhysicsClientSharedMemory::getDebugLinesFrom() const { if (m_data->m_debugLinesFrom.size()) { - return &m_data->m_debugLinesFrom[0]; + return &m_data->m_debugLinesFrom[0].m_x; } return 0; } -const btVector3* PhysicsClientSharedMemory::getDebugLinesTo() const +const float* PhysicsClientSharedMemory::getDebugLinesTo() const { if (m_data->m_debugLinesTo.size()) { - return &m_data->m_debugLinesTo[0]; + return &m_data->m_debugLinesTo[0].m_x; } return 0; } -const btVector3* PhysicsClientSharedMemory::getDebugLinesColor() const +const float* PhysicsClientSharedMemory::getDebugLinesColor() const { if (m_data->m_debugLinesColor.size()) { - return &m_data->m_debugLinesColor[0]; + return &m_data->m_debugLinesColor[0].m_x; } return 0; } diff --git a/examples/SharedMemory/PhysicsClient.h b/examples/SharedMemory/PhysicsClient.h index 094a81b38..1b9f8dd52 100644 --- a/examples/SharedMemory/PhysicsClient.h +++ b/examples/SharedMemory/PhysicsClient.h @@ -1,7 +1,7 @@ #ifndef BT_PHYSICS_CLIENT_API_H #define BT_PHYSICS_CLIENT_API_H -#include "SharedMemoryCommands.h" +//#include "SharedMemoryCommands.h" #include "LinearMath/btVector3.h" class PhysicsClientSharedMemory @@ -22,15 +22,17 @@ public: virtual bool isConnected() const; // return true if there is a status, and fill in 'serverStatus' - virtual bool processServerStatus(SharedMemoryStatus& serverStatus); + virtual const struct SharedMemoryStatus* processServerStatus(); + virtual struct SharedMemoryCommand* getAvailableSharedMemoryCommand(); + virtual bool canSubmitCommand() const; - virtual bool submitClientCommand(const SharedMemoryCommand& command); + virtual bool submitClientCommand(const struct SharedMemoryCommand& command); virtual int getNumJoints() const; - virtual void getJointInfo(int index, b3JointInfo& info) const; + virtual void getJointInfo(int index, struct b3JointInfo& info) const; virtual void setSharedMemoryKey(int key); @@ -38,9 +40,9 @@ public: virtual int getNumDebugLines() const; - virtual const btVector3* getDebugLinesFrom() const; - virtual const btVector3* getDebugLinesTo() const; - virtual const btVector3* getDebugLinesColor() const; + virtual const float* getDebugLinesFrom() const; + virtual const float* getDebugLinesTo() const; + virtual const float* getDebugLinesColor() const; }; diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 08b1df5dc..fb714d984 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -2,10 +2,19 @@ #include "PhysicsClient.h" #include "Bullet3Common/b3Scalar.h" #include +#include "SharedMemoryCommands.h" -int b3LoadUrdfCommandInit(struct SharedMemoryCommand* command, const char* urdfFileName) + + +b3SharedMemoryCommandHandle b3LoadUrdfCommandInit(b3PhysicsClientHandle physClient, const char* urdfFileName) { - b3Assert(command); + PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient; + b3Assert(cl); + b3Assert(cl->canSubmitCommand()); + + + struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); + b3Assert(command); command->m_type = CMD_LOAD_URDF; int len = strlen(urdfFileName); if (lenm_updateFlags = URDF_ARGS_FILE_NAME; - return 0; + return (b3SharedMemoryCommandHandle) command; +} + + +int b3LoadUrdfCommandSetUseFixedBase(b3SharedMemoryCommandHandle commandHandle, int useFixedBase) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_LOAD_URDF); + command->m_updateFlags |=URDF_ARGS_USE_FIXED_BASE; + command->m_urdfArguments.m_useFixedBase = useFixedBase; + + return 0; } - -int b3LoadUrdfCommandSetStartPosition(struct SharedMemoryCommand* command, double startPosX,double startPosY,double startPosZ) +int b3LoadUrdfCommandSetStartPosition(b3SharedMemoryCommandHandle commandHandle, double startPosX,double startPosY,double startPosZ) { + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); b3Assert(command->m_type == CMD_LOAD_URDF); command->m_urdfArguments.m_initialPosition[0] = startPosX; @@ -33,8 +54,10 @@ int b3LoadUrdfCommandSetStartPosition(struct SharedMemoryCommand* command, doubl command->m_updateFlags|=URDF_ARGS_INITIAL_POSITION; return 0; } -int b3LoadUrdfCommandSetStartOrientation(struct SharedMemoryCommand* command, double startOrnX,double startOrnY,double startOrnZ, double startOrnW) + +int b3LoadUrdfCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX,double startOrnY,double startOrnZ, double startOrnW) { + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); b3Assert(command->m_type == CMD_LOAD_URDF); command->m_urdfArguments.m_initialOrientation[0] = startOrnX; @@ -45,16 +68,21 @@ int b3LoadUrdfCommandSetStartOrientation(struct SharedMemoryCommand* command, do return 0; } -int b3InitPhysicsParamCommand(struct SharedMemoryCommand* command) +b3SharedMemoryCommandHandle b3InitPhysicsParamCommand(b3PhysicsClientHandle physClient) { - b3Assert(command); + PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient; + b3Assert(cl); + b3Assert(cl->canSubmitCommand()); + struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); + b3Assert(command); command->m_type = CMD_SEND_PHYSICS_SIMULATION_PARAMETERS; command->m_updateFlags = 0; - return 0; + return (b3SharedMemoryCommandHandle) command; } -int b3PhysicsParamSetGravity(struct SharedMemoryCommand* command, double gravx,double gravy, double gravz) +int b3PhysicsParamSetGravity(b3SharedMemoryCommandHandle commandHandle, double gravx,double gravy, double gravz) { + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS); command->m_physSimParamArgs.m_gravityAcceleration[0] = gravx; command->m_physSimParamArgs.m_gravityAcceleration[1] = gravy; @@ -63,83 +91,130 @@ int b3PhysicsParamSetGravity(struct SharedMemoryCommand* command, double gra return 0; } -int b3PhysicsParamSetTimeStep(struct SharedMemoryCommand* command, double timeStep) +int b3PhysicsParamSetTimeStep(b3SharedMemoryCommandHandle commandHandle, double timeStep) { + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS); command->m_physSimParamArgs.m_deltaTime = timeStep; return 0; } -int b3InitStepSimulationCommand(struct SharedMemoryCommand* command) +b3SharedMemoryCommandHandle b3InitStepSimulationCommand(b3PhysicsClientHandle physClient) { - b3Assert(command); + PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient; + b3Assert(cl); + b3Assert(cl->canSubmitCommand()); + struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); + b3Assert(command); command->m_type = CMD_STEP_FORWARD_SIMULATION; command->m_updateFlags = 0; - - return 0; - + return (b3SharedMemoryCommandHandle) command; } -int b3InitResetSimulationCommand(struct SharedMemoryCommand* command) +b3SharedMemoryCommandHandle b3InitResetSimulationCommand(b3PhysicsClientHandle physClient) { - b3Assert(command); - command->m_type = CMD_RESET_SIMULATION; - command->m_updateFlags = 0; + PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient; + b3Assert(cl); + b3Assert(cl->canSubmitCommand()); + struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); + b3Assert(command); + command->m_type = CMD_RESET_SIMULATION; + command->m_updateFlags = 0; - return 0; + return (b3SharedMemoryCommandHandle) command; } -int b3JointControlCommandInit(struct SharedMemoryCommand* command, int controlMode) +b3SharedMemoryCommandHandle b3JointControlCommandInit( b3PhysicsClientHandle physClient, int controlMode) { + PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient; + b3Assert(cl); + b3Assert(cl->canSubmitCommand()); + struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); b3Assert(command); command->m_type = CMD_SEND_DESIRED_STATE; command->m_sendDesiredStateCommandArgument.m_controlMode = controlMode; command->m_updateFlags = 0; - return 0; + return (b3SharedMemoryCommandHandle) command; } -int b3JointControlSetDesiredVelocity(struct SharedMemoryCommand* command, int dofIndex, double value) +int b3JointControlSetDesiredPosition(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value) { + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); - command->m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex] = 1; + command->m_sendDesiredStateCommandArgument.m_desiredStateQ[dofIndex] = value; + return 0; +} + +int b3JointControlSetKp(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + command->m_sendDesiredStateCommandArgument.m_Kp[dofIndex] = value; + return 0; +} + +int b3JointControlSetKd(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + command->m_sendDesiredStateCommandArgument.m_Kd[dofIndex] = value; + return 0; +} + +int b3JointControlSetDesiredVelocity(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + command->m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex] = value; return 0; } -int b3JointControlSetMaximumForce(struct SharedMemoryCommand* command, int dofIndex, double value) +int b3JointControlSetMaximumForce(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value) { + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); command->m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex] = value; return 0; } -int b3JointControlSetDesiredForceTorque(struct SharedMemoryCommand* command, int dofIndex, double value) +int b3JointControlSetDesiredForceTorque(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value) { + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); command->m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex] = value; return 0; } -int b3RequestActualStateCommandInit(struct SharedMemoryCommand* command) +b3SharedMemoryCommandHandle b3RequestActualStateCommandInit(b3PhysicsClientHandle physClient) { + PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient; + b3Assert(cl); + b3Assert(cl->canSubmitCommand()); + struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); b3Assert(command); command->m_type =CMD_REQUEST_ACTUAL_STATE; - return 0; + return (b3SharedMemoryCommandHandle) command; } -int b3CreateBoxShapeCommandInit(struct SharedMemoryCommand* command) +b3SharedMemoryCommandHandle b3CreateBoxShapeCommandInit(b3PhysicsClientHandle physClient) { + PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient; + b3Assert(cl); + b3Assert(cl->canSubmitCommand()); + struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); b3Assert(command); command->m_type = CMD_CREATE_BOX_COLLISION_SHAPE; command->m_updateFlags =0; - return 0; + return (b3SharedMemoryCommandHandle) command; } -int b3CreateBoxCommandSetStartPosition(struct SharedMemoryCommand* command, double startPosX,double startPosY,double startPosZ) +int b3CreateBoxCommandSetStartPosition(b3SharedMemoryCommandHandle commandHandle, double startPosX,double startPosY,double startPosZ) { + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); b3Assert(command->m_type == CMD_CREATE_BOX_COLLISION_SHAPE); command->m_updateFlags |=BOX_SHAPE_HAS_INITIAL_POSITION; @@ -150,8 +225,9 @@ int b3CreateBoxCommandSetStartPosition(struct SharedMemoryCommand* command, doub return 0; } -int b3CreateBoxCommandSetStartOrientation(struct SharedMemoryCommand* command, double startOrnX,double startOrnY,double startOrnZ, double startOrnW) +int b3CreateBoxCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX,double startOrnY,double startOrnZ, double startOrnW) { + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); b3Assert(command->m_type == CMD_CREATE_BOX_COLLISION_SHAPE); command->m_updateFlags |=BOX_SHAPE_HAS_INITIAL_ORIENTATION; @@ -163,9 +239,9 @@ int b3CreateBoxCommandSetStartOrientation(struct SharedMemoryCommand* command, d return 0; } -int b3CreateBoxCommandSetHalfExtents(struct SharedMemoryCommand* command, double halfExtentsX,double halfExtentsY,double halfExtentsZ) +int b3CreateBoxCommandSetHalfExtents(b3SharedMemoryCommandHandle commandHandle, double halfExtentsX,double halfExtentsY,double halfExtentsZ) { - + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); b3Assert(command->m_type == CMD_CREATE_BOX_COLLISION_SHAPE); command->m_updateFlags |=BOX_SHAPE_HAS_HALF_EXTENTS; @@ -179,17 +255,24 @@ int b3CreateBoxCommandSetHalfExtents(struct SharedMemoryCommand* command, double -int b3CreateSensorCommandInit(struct SharedMemoryCommand* command) +b3SharedMemoryCommandHandle b3CreateSensorCommandInit(b3PhysicsClientHandle physClient) { + PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient; + b3Assert(cl); + b3Assert(cl->canSubmitCommand()); + struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); b3Assert(command); + command->m_type = CMD_CREATE_SENSOR; command->m_updateFlags = 0; command->m_createSensorArguments.m_numJointSensorChanges = 0; - return 0; + return (b3SharedMemoryCommandHandle) command; + } -int b3CreateSensorEnable6DofJointForceTorqueSensor(struct SharedMemoryCommand* command, int jointIndex, int enable) +int b3CreateSensorEnable6DofJointForceTorqueSensor(b3SharedMemoryCommandHandle commandHandle, int jointIndex, int enable) { + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); b3Assert(command->m_type == CMD_CREATE_SENSOR); int curIndex = command->m_createSensorArguments.m_numJointSensorChanges; @@ -201,8 +284,9 @@ int b3CreateSensorEnable6DofJointForceTorqueSensor(struct SharedMemoryCommand* c return 0; } -int b3CreateSensorEnableIMUForLink(struct SharedMemoryCommand* command, int linkIndex, int enable) +int b3CreateSensorEnableIMUForLink(b3SharedMemoryCommandHandle commandHandle, int linkIndex, int enable) { + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); b3Assert(command->m_type == CMD_CREATE_SENSOR); int curIndex = command->m_createSensorArguments.m_numJointSensorChanges; @@ -214,11 +298,13 @@ int b3CreateSensorEnableIMUForLink(struct SharedMemoryCommand* command, int link } -b3PhysicsClientHandle b3ConnectSharedMemory() +b3PhysicsClientHandle b3ConnectSharedMemory(int key) { + PhysicsClientSharedMemory* cl = new PhysicsClientSharedMemory(); ///client should never create shared memory, only the server does - cl->connect(); + cl->setSharedMemoryKey(key); + cl->connect(); return (b3PhysicsClientHandle ) cl; } @@ -228,22 +314,37 @@ void b3DisconnectSharedMemory(b3PhysicsClientHandle physClient) delete cl; } -int b3ProcessServerStatus(b3PhysicsClientHandle physClient, struct SharedMemoryStatus* status) +b3SharedMemoryStatusHandle b3ProcessServerStatus(b3PhysicsClientHandle physClient) { PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient; - return (int)cl->processServerStatus(*status); + const SharedMemoryStatus* stat = cl->processServerStatus(); + return (b3SharedMemoryStatusHandle) stat; + } +int b3GetStatusType(b3SharedMemoryStatusHandle statusHandle) +{ + const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle; + b3Assert(status); + if (status) + { + return status->m_type; + } + return 0; +} + + int b3CanSubmitCommand(b3PhysicsClientHandle physClient) { PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient; return (int)cl->canSubmitCommand(); } -int b3SubmitClientCommand(b3PhysicsClientHandle physClient, struct SharedMemoryCommand* command) +int b3SubmitClientCommand(b3PhysicsClientHandle physClient, const b3SharedMemoryCommandHandle commandHandle) { - PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient; - return (int)cl->submitClientCommand(*command); + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient; + return (int)cl->submitClientCommand(*command); } @@ -262,3 +363,33 @@ void b3GetJointInfo(b3PhysicsClientHandle physClient, int linkIndex, struct b3Jo } + +b3SharedMemoryCommandHandle b3InitRequestDebugLinesCommand(b3PhysicsClientHandle physClient, int debugMode) +{ + PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient; + b3Assert(cl); + b3Assert(cl->canSubmitCommand()); + struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); + b3Assert(command); + + command->m_type =CMD_REQUEST_DEBUG_LINES; + command->m_requestDebugLinesArguments.m_debugMode = debugMode; + command->m_requestDebugLinesArguments.m_startingLineIndex = 0; + return (b3SharedMemoryCommandHandle) command; +} +void b3GetDebugLines(b3PhysicsClientHandle physClient, struct b3DebugLines* lines) +{ + PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient; + + b3Assert(lines); + if (lines) + { + lines->m_numDebugLines = cl->getNumDebugLines(); + lines->m_linesFrom = cl->getDebugLinesFrom(); + lines->m_linesTo = cl->getDebugLinesTo(); + lines->m_linesColor = cl->getDebugLinesColor(); + + } + +} + diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 04edf21b9..54a417540 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -1,55 +1,68 @@ #ifndef PHYSICS_CLIENT_C_API_H #define PHYSICS_CLIENT_C_API_H -#include "SharedMemoryBlock.h" +//#include "SharedMemoryBlock.h" +#include "SharedMemoryPublic.h" #define B3_DECLARE_HANDLE(name) typedef struct name##__ { int unused; } *name B3_DECLARE_HANDLE(b3PhysicsClientHandle); -B3_DECLARE_HANDLE(b3PhysicsRobotHandle); +B3_DECLARE_HANDLE(b3SharedMemoryCommandHandle); +B3_DECLARE_HANDLE(b3SharedMemoryStatusHandle); + #ifdef __cplusplus extern "C" { #endif ///make sure to start the server first! -b3PhysicsClientHandle b3ConnectSharedMemory(); +b3PhysicsClientHandle b3ConnectSharedMemory(int key); void b3DisconnectSharedMemory(b3PhysicsClientHandle physClient); -int b3ProcessServerStatus(b3PhysicsClientHandle physClient, struct SharedMemoryStatus* status); - +b3SharedMemoryStatusHandle b3ProcessServerStatus(b3PhysicsClientHandle physClient); +int b3GetStatusType(b3SharedMemoryStatusHandle statusHandle); + int b3CanSubmitCommand(b3PhysicsClientHandle physClient); -int b3SubmitClientCommand(b3PhysicsClientHandle physClient, struct SharedMemoryCommand* command); +int b3SubmitClientCommand(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle); int b3GetNumJoints(b3PhysicsClientHandle physClient); void b3GetJointInfo(b3PhysicsClientHandle physClient, int linkIndex, struct b3JointInfo* info); -int b3InitPhysicsParamCommand(struct SharedMemoryCommand* command); -int b3PhysicsParamSetGravity(struct SharedMemoryCommand* command, double gravx,double gravy, double gravz); -int b3PhysicsParamSetTimeStep(struct SharedMemoryCommand* command, double timeStep); +b3SharedMemoryCommandHandle b3InitRequestDebugLinesCommand(b3PhysicsClientHandle physClient, int debugMode); + +void b3GetDebugLines(b3PhysicsClientHandle physClient, struct b3DebugLines* lines); + -int b3InitStepSimulationCommand(struct SharedMemoryCommand* command); +b3SharedMemoryCommandHandle b3InitPhysicsParamCommand(b3PhysicsClientHandle physClient); +int b3PhysicsParamSetGravity(b3SharedMemoryCommandHandle commandHandle, double gravx,double gravy, double gravz); +int b3PhysicsParamSetTimeStep(b3SharedMemoryCommandHandle commandHandle, double timeStep); -int b3InitResetSimulationCommand(struct SharedMemoryCommand* command); +b3SharedMemoryCommandHandle b3InitStepSimulationCommand(b3PhysicsClientHandle physClient); -int b3LoadUrdfCommandInit(struct SharedMemoryCommand* command, const char* urdfFileName); +b3SharedMemoryCommandHandle b3InitResetSimulationCommand(b3PhysicsClientHandle physClient); + +b3SharedMemoryCommandHandle b3LoadUrdfCommandInit(b3PhysicsClientHandle physClient, const char* urdfFileName); ///all those commands are optional, except for the *Init -int b3LoadUrdfCommandSetStartPosition(struct SharedMemoryCommand* command, double startPosX,double startPosY,double startPosZ); -int b3LoadUrdfCommandSetStartOrientation(struct SharedMemoryCommand* command, double startOrnX,double startOrnY,double startOrnZ, double startOrnW); -int b3LoadUrdfCommandSetUseMultiBody(struct SharedMemoryCommand* command, int useMultiBody); -int b3LoadUrdfCommandSetUseFixedBase(struct SharedMemoryCommand* command, int useFixedBase); +int b3LoadUrdfCommandSetStartPosition(b3SharedMemoryCommandHandle commandHandle, double startPosX,double startPosY,double startPosZ); +int b3LoadUrdfCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX,double startOrnY,double startOrnZ, double startOrnW); +int b3LoadUrdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody); +int b3LoadUrdfCommandSetUseFixedBase(b3SharedMemoryCommandHandle commandHandle, int useFixedBase); ///Set joint control variables such as desired position/angle, desired velocity, ///applied joint forces, dependent on the control mode (CONTROL_MODE_VELOCITY or CONTROL_MODE_TORQUE) -int b3JointControlCommandInit(struct SharedMemoryCommand* command, int controlMode); +b3SharedMemoryCommandHandle b3JointControlCommandInit(b3PhysicsClientHandle physClient, int controlMode); +///Only use when controlMode is CONTROL_MODE_POSITION_VELOCITY_PD +int b3JointControlSetDesiredPosition(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); +int b3JointControlSetKp(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); +int b3JointControlSetKd(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); //Only use when controlMode is CONTROL_MODE_VELOCITY -int b3JointControlSetDesiredVelocity(struct SharedMemoryCommand* command, int dofIndex, double value); -int b3JointControlSetMaximumForce(struct SharedMemoryCommand* command, int dofIndex, double value); +int b3JointControlSetDesiredVelocity(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); +int b3JointControlSetMaximumForce(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); ///Only use if when controlMode is CONTROL_MODE_TORQUE, -int b3JointControlSetDesiredForceTorque(struct SharedMemoryCommand* command, int dofIndex, double value); +int b3JointControlSetDesiredForceTorque(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); ///the creation of collision shapes and rigid bodies etc is likely going to change, @@ -57,19 +70,20 @@ int b3JointControlSetDesiredForceTorque(struct SharedMemoryCommand* command, int //create a box of size (1,1,1) at world origin (0,0,0) at orientation quat (0,0,0,1) //after that, you can optionally adjust the initial position, orientation and size -int b3CreateBoxShapeCommandInit(struct SharedMemoryCommand* command); -int b3CreateBoxCommandSetStartPosition(struct SharedMemoryCommand* command, double startPosX,double startPosY,double startPosZ); -int b3CreateBoxCommandSetStartOrientation(struct SharedMemoryCommand* command, double startOrnX,double startOrnY,double startOrnZ, double startOrnW); -int b3CreateBoxCommandSetHalfExtents(struct SharedMemoryCommand* command, double halfExtentsX,double halfExtentsY,double halfExtentsZ); +b3SharedMemoryCommandHandle b3CreateBoxShapeCommandInit(b3PhysicsClientHandle physClient); +int b3CreateBoxCommandSetStartPosition(b3SharedMemoryCommandHandle commandHandle, double startPosX,double startPosY,double startPosZ); +int b3CreateBoxCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX,double startOrnY,double startOrnZ, double startOrnW); +int b3CreateBoxCommandSetHalfExtents(b3SharedMemoryCommandHandle commandHandle, double halfExtentsX,double halfExtentsY,double halfExtentsZ); -int b3CreateSensorCommandInit(struct SharedMemoryCommand* command); -int b3CreateSensorEnable6DofJointForceTorqueSensor(struct SharedMemoryCommand* command, int dofIndex, int enable); -int b3CreateSensorEnableIMUForLink(struct SharedMemoryCommand* command, int linkIndex, int enable); +b3SharedMemoryCommandHandle b3CreateSensorCommandInit(b3PhysicsClientHandle physClient); +int b3CreateSensorEnable6DofJointForceTorqueSensor(b3SharedMemoryCommandHandle commandHandle, int dofIndex, int enable); +int b3CreateSensorEnableIMUForLink(b3SharedMemoryCommandHandle commandHandle, int linkIndex, int enable); -int b3RequestActualStateCommandInit(struct SharedMemoryCommand* command); +b3SharedMemoryCommandHandle b3RequestActualStateCommandInit(b3PhysicsClientHandle physClient); + #ifdef __cplusplus } #endif diff --git a/examples/SharedMemory/PhysicsClientExample.cpp b/examples/SharedMemory/PhysicsClientExample.cpp index e9631db16..9e2cfb7a5 100644 --- a/examples/SharedMemory/PhysicsClientExample.cpp +++ b/examples/SharedMemory/PhysicsClientExample.cpp @@ -7,7 +7,9 @@ #include "../CommonInterfaces/CommonParameterInterface.h" #include "PhysicsClient.h" -#include "SharedMemoryCommands.h" +//#include "SharedMemoryCommands.h" + +#include "PhysicsClientC_API.h" struct MyMotorInfo2 { @@ -16,17 +18,18 @@ struct MyMotorInfo2 int m_uIndex; }; + #define MAX_NUM_MOTORS 128 class PhysicsClientExample : public SharedMemoryCommon { protected: - PhysicsClientSharedMemory m_physicsClient; - + b3PhysicsClientHandle m_physicsClientHandle; bool m_wantsTermination; - btAlignedObjectArray m_userCommandRequests; - + btAlignedObjectArray m_userCommandRequests; + int m_sharedMemoryKey; + void createButton(const char* name, int id, bool isTrigger ); void createButtons(); @@ -62,18 +65,22 @@ public: virtual bool isConnected() { - return m_physicsClient.isConnected(); + return (m_physicsClientHandle!=0); } - void enqueueCommand(const SharedMemoryCommand& orgCommand); - + + + void enqueueCommand(int commandId); + + void prepareAndSubmitCommand(int commandId); + + virtual void exitPhysics(){}; virtual void renderScene() { - int numLines = m_physicsClient.getNumDebugLines(); - const btVector3* fromLines = m_physicsClient.getDebugLinesFrom(); - const btVector3* toLines = m_physicsClient.getDebugLinesTo(); - const btVector3* colorLines = m_physicsClient.getDebugLinesColor(); + b3DebugLines debugLines; + b3GetDebugLines(m_physicsClientHandle,&debugLines); + int numLines = debugLines.m_numDebugLines; int lineWidth = 1; @@ -86,12 +93,12 @@ public: for (int i=0;igetRenderInterface()->drawLine(fromLines[i],toLines[i],colorLines[i],lineWidth); + m_guiHelper->getRenderInterface()->drawLine(debugLines.m_linesFrom,debugLines.m_linesTo,debugLines.m_linesColor,lineWidth); } } - } - void prepareControlCommand(SharedMemoryCommand& command) + void prepareControlCommand(b3SharedMemoryCommandHandle commandHandle) { - int controlMode = CONTROL_MODE_VELOCITY;//CONTROL_MODE_TORQUE; - - command.m_sendDesiredStateCommandArgument.m_controlMode = controlMode; - - for (int i=0;ienqueueCommand(buttonId); + } + - SharedMemoryCommand command; - - switch (buttonId) - { - case CMD_LOAD_URDF: - { - command.m_type =CMD_LOAD_URDF; - sprintf(command.m_urdfArguments.m_urdfFileName,"r2d2.urdf");//kuka_lwr/kuka.urdf"); - command.m_urdfArguments.m_initialPosition[0] = 0.0; - command.m_updateFlags = - URDF_ARGS_FILE_NAME| URDF_ARGS_INITIAL_POSITION|URDF_ARGS_INITIAL_ORIENTATION|URDF_ARGS_USE_MULTIBODY|URDF_ARGS_USE_FIXED_BASE; - - 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; - command.m_updateFlags = BOX_SHAPE_HAS_INITIAL_POSITION; - command.m_createBoxShapeArguments.m_initialPosition[0] = 0; - command.m_createBoxShapeArguments.m_initialPosition[1] = 0; - command.m_createBoxShapeArguments.m_initialPosition[2] = -3; - - 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: - { - command.m_type =CMD_STEP_FORWARD_SIMULATION; - cl->enqueueCommand(command); - command.m_type =CMD_REQUEST_DEBUG_LINES; - command.m_requestDebugLinesArguments.m_debugMode = btIDebugDraw::DBG_DrawWireframe;//:DBG_DrawConstraints; - command.m_requestDebugLinesArguments.m_startingLineIndex = 0; - cl->enqueueCommand(command); - - break; - } - - case CMD_SEND_DESIRED_STATE: - { - - command.m_type =CMD_SEND_DESIRED_STATE; - - cl->prepareControlCommand(command); - - cl->enqueueCommand(command); - break; - } - case CMD_RESET_SIMULATION: - { - command.m_type = CMD_RESET_SIMULATION; - cl->enqueueCommand(command); - break; - } - case CMD_SEND_BULLET_DATA_STREAM: - { - command.m_type = buttonId; - cl->enqueueCommand(command); - break; - } - - default: - { - b3Error("Unknown buttonId"); - btAssert(0); - } - }; } - void PhysicsClientExample::enqueueCommand(const SharedMemoryCommand& orgCommand) - { - m_userCommandRequests.push_back(orgCommand); - SharedMemoryCommand& cmd = m_userCommandRequests[m_userCommandRequests.size()-1]; +void PhysicsClientExample::enqueueCommand(int commandId) +{ + m_userCommandRequests.push_back(commandId); +} - //b3Printf("User put command request %d on queue (queue length = %d)\n",cmd.m_type, m_userCommandRequests.size()); - } +void PhysicsClientExample::prepareAndSubmitCommand(int commandId) +{ + + switch (commandId) + { + case CMD_LOAD_URDF: + { + + b3SharedMemoryCommandHandle commandHandle = b3LoadUrdfCommandInit(m_physicsClientHandle, "r2d2.urdf");//kuka_lwr/kuka.urdf"); + + //setting the initial position, orientation and other arguments are optional + double startPosX = 0; + double startPosY = 0; + double startPosZ = 0; + int ret = b3LoadUrdfCommandSetStartPosition(commandHandle, startPosX,startPosY,startPosZ); +// ret = b3LoadUrdfCommandSetUseFixedBase(commandHandle, 1); + ret = b3SubmitClientCommand(m_physicsClientHandle, commandHandle); + break; + } + case CMD_CREATE_BOX_COLLISION_SHAPE: + { + b3SharedMemoryCommandHandle commandHandle = b3CreateBoxShapeCommandInit(m_physicsClientHandle); + b3CreateBoxCommandSetStartPosition(commandHandle,0,0,-3); + b3SubmitClientCommand(m_physicsClientHandle, commandHandle); + break; + } + case CMD_REQUEST_ACTUAL_STATE: + { + b3SharedMemoryCommandHandle commandHandle = b3RequestActualStateCommandInit(m_physicsClientHandle); + b3SubmitClientCommand(m_physicsClientHandle, commandHandle); + break; + }; + + case CMD_STEP_FORWARD_SIMULATION: + { + + b3SharedMemoryCommandHandle commandHandle = b3InitStepSimulationCommand(m_physicsClientHandle); + b3SubmitClientCommand(m_physicsClientHandle, commandHandle); + break; + } + + case CMD_REQUEST_DEBUG_LINES: + { + b3SharedMemoryCommandHandle commandHandle = b3InitRequestDebugLinesCommand(m_physicsClientHandle, btIDebugDraw::DBG_DrawWireframe); + b3SubmitClientCommand(m_physicsClientHandle, commandHandle); + break; + } + case CMD_SEND_DESIRED_STATE: + { + b3SharedMemoryCommandHandle command = b3JointControlCommandInit( m_physicsClientHandle, CONTROL_MODE_VELOCITY); + prepareControlCommand(command); + b3SubmitClientCommand(m_physicsClientHandle, command); + break; + } + case CMD_RESET_SIMULATION: + { + b3SharedMemoryCommandHandle commandHandle = b3InitResetSimulationCommand(m_physicsClientHandle); + b3SubmitClientCommand(m_physicsClientHandle, commandHandle); + break; + } + case CMD_SEND_BULLET_DATA_STREAM: + { +#if 0 + + //this worked, but needs C-API and a streaming options, similar to debug lines + command.m_type = buttonId; + cl->enqueueCommand(command); +#endif + + break; + } + default: + { + b3Error("Unknown buttonId"); + btAssert(0); + } + }; +} PhysicsClientExample::PhysicsClientExample(GUIHelperInterface* helper) :SharedMemoryCommon(helper), m_wantsTermination(false), +m_sharedMemoryKey(SHARED_MEMORY_KEY), m_numMotors(0) { b3Printf("Started PhysicsClientExample\n"); @@ -299,27 +296,11 @@ void PhysicsClientExample::initPhysics() MyCallback(CMD_STEP_FORWARD_SIMULATION,true,this); MyCallback(CMD_STEP_FORWARD_SIMULATION,true,this); MyCallback(CMD_RESET_SIMULATION,true,this); - // MyCallback(CMD_LOAD_URDF, true, this); - // MyCallback(CMD_STEP_FORWARD_SIMULATION,true,this); - // MyCallback(CMD_RESET_SIMULATION,true,this); - - /* - m_userCommandRequests.push_back(CMD_LOAD_URDF); - m_userCommandRequests.push_back(CMD_REQUEST_ACTUAL_STATE); - m_userCommandRequests.push_back(CMD_SEND_DESIRED_STATE); - m_userCommandRequests.push_back(CMD_REQUEST_ACTUAL_STATE); - //m_userCommandRequests.push_back(CMD_SET_JOINT_FEEDBACK); - m_userCommandRequests.push_back(CMD_CREATE_BOX_COLLISION_SHAPE); - //m_userCommandRequests.push_back(CMD_CREATE_RIGID_BODY); - 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()) - { + m_physicsClientHandle = b3ConnectSharedMemory(m_sharedMemoryKey); + if (!b3CanSubmitCommand(m_physicsClientHandle)) + { b3Warning("Cannot connect to physics client"); } @@ -328,113 +309,96 @@ void PhysicsClientExample::initPhysics() void PhysicsClientExample::stepSimulation(float deltaTime) { - - if (m_physicsClient.isConnected()) + b3SharedMemoryStatusHandle status = b3ProcessServerStatus(m_physicsClientHandle); + bool hasStatus = (status != 0); + + if (hasStatus) { - SharedMemoryStatus status; - bool hasStatus = m_physicsClient.processServerStatus(status); - if (hasStatus && status.m_type == CMD_URDF_LOADING_COMPLETED) + + int statusType = b3GetStatusType(status); + + if (statusType == CMD_URDF_LOADING_COMPLETED) { - for (int i=0;im_velTarget = 0.f; + motorInfo->m_uIndex = info.m_uIndex; + + SliderParams slider(motorName,&motorInfo->m_velTarget); + slider.m_minVal=-4; + slider.m_maxVal=4; + if (m_guiHelper && m_guiHelper->getParameterInterface()) { - char motorName[1024]; - sprintf(motorName,"%s q'", info.m_jointName); - MyMotorInfo2* motorInfo = &m_motorTargetVelocities[m_numMotors]; - motorInfo->m_velTarget = 0.f; - motorInfo->m_uIndex = info.m_uIndex; - - SliderParams slider(motorName,&motorInfo->m_velTarget); - slider.m_minVal=-4; - slider.m_maxVal=4; - if (m_guiHelper && m_guiHelper->getParameterInterface()) - { - m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); - } - m_numMotors++; + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } + m_numMotors++; } - } - } } - if (hasStatus && status.m_type ==CMD_DEBUG_LINES_COMPLETED) - { - SharedMemoryCommand command; - if (status.m_sendDebugLinesArgs.m_numRemainingDebugLines>0) - { - //continue requesting debug lines for drawing - command.m_type =CMD_REQUEST_DEBUG_LINES; - command.m_requestDebugLinesArguments.m_debugMode = btIDebugDraw::DBG_DrawWireframe;//DBG_DrawConstraints; - command.m_requestDebugLinesArguments.m_startingLineIndex = status.m_sendDebugLinesArgs.m_numDebugLines+status.m_sendDebugLinesArgs.m_startingLineIndex; - enqueueCommand(command); - } - } - if (m_physicsClient.canSubmitCommand()) - { - if (m_userCommandRequests.size()) - { - //b3Printf("Outstanding user command requests: %d\n", m_userCommandRequests.size()); - SharedMemoryCommand command = m_userCommandRequests[0]; + } + if (b3CanSubmitCommand(m_physicsClientHandle)) + { + if (m_userCommandRequests.size()) + { + //b3Printf("Outstanding user command requests: %d\n", m_userCommandRequests.size()); + int commandId = m_userCommandRequests[0]; - //a manual 'pop_front', we don't use 'remove' because it will re-order the commands - for (int i=1;igetParameterInterface()) - { - m_guiHelper->getParameterInterface()->removeAllParameters(); - } - m_numMotors=0; - createButtons(); - } - - m_physicsClient.submitClientCommand(command); - } else - { - if (m_numMotors) - { - SharedMemoryCommand command; - command.m_type =CMD_SEND_DESIRED_STATE; - prepareControlCommand(command); - enqueueCommand(command); + m_userCommandRequests.pop_back(); + + //for the CMD_RESET_SIMULATION we need to do something special: clear the GUI sliders + if (commandId ==CMD_RESET_SIMULATION) + { + if (m_guiHelper->getParameterInterface()) + { + m_guiHelper->getParameterInterface()->removeAllParameters(); + } + m_numMotors=0; + createButtons(); + } + + prepareAndSubmitCommand(commandId); + + } else + { + if (m_numMotors) + { + enqueueCommand(CMD_SEND_DESIRED_STATE); + enqueueCommand(CMD_STEP_FORWARD_SIMULATION); + enqueueCommand(CMD_REQUEST_DEBUG_LINES); + enqueueCommand(CMD_REQUEST_ACTUAL_STATE); + } + } + } - command.m_type =CMD_STEP_FORWARD_SIMULATION; - enqueueCommand(command); - - command.m_type = CMD_REQUEST_ACTUAL_STATE; - enqueueCommand(command); - } - } - } - } - } diff --git a/examples/SharedMemory/PhysicsServer.cpp b/examples/SharedMemory/PhysicsServer.cpp index ea8cd7ed1..7d64dbfc9 100644 --- a/examples/SharedMemory/PhysicsServer.cpp +++ b/examples/SharedMemory/PhysicsServer.cpp @@ -1087,7 +1087,7 @@ void PhysicsServerSharedMemory::processClientCommands() deleteDynamicsWorld(); createEmptyDynamicsWorld(); - SharedMemoryStatus& serverCmd =m_data->createServerStatus(CMD_CLIENT_COMMAND_COMPLETED,clientCmd.m_sequenceNumber,timeStamp); + SharedMemoryStatus& serverCmd =m_data->createServerStatus(CMD_RESET_SIMULATION_COMPLETED,clientCmd.m_sequenceNumber,timeStamp); m_data->submitServerStatus(serverCmd); break; diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index d6065407e..68de98e67 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -5,7 +5,6 @@ - #include "PhysicsServer.h" #include "SharedMemoryCommon.h" diff --git a/examples/SharedMemory/RobotControlExample.cpp b/examples/SharedMemory/RobotControlExample.cpp index c08ddc3fb..1a0dad330 100644 --- a/examples/SharedMemory/RobotControlExample.cpp +++ b/examples/SharedMemory/RobotControlExample.cpp @@ -3,7 +3,7 @@ #include "RobotControlExample.h" - +#if 0 #include "../CommonInterfaces/CommonParameterInterface.h" #include "PhysicsServer.h" @@ -15,7 +15,7 @@ #include //const char* blaatnaam = "basename"; - +#define SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE 1024 struct MyMotorInfo { @@ -665,5 +665,6 @@ class CommonExampleInterface* RobotControlExampleCreateFunc(struct CommonExam } return example; } +#endif diff --git a/examples/SharedMemory/SharedMemoryBlock.h b/examples/SharedMemory/SharedMemoryBlock.h index ba7faccb5..36cd60f35 100644 --- a/examples/SharedMemory/SharedMemoryBlock.h +++ b/examples/SharedMemory/SharedMemoryBlock.h @@ -1,7 +1,6 @@ #ifndef SHARED_MEMORY_BLOCK_H #define SHARED_MEMORY_BLOCK_H -#define SHARED_MEMORY_KEY 12347 #define SHARED_MEMORY_MAGIC_NUMBER 64738 #define SHARED_MEMORY_MAX_COMMANDS 32 #define SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE (256*1024) diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 746255366..9c8d2cb1d 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -4,6 +4,8 @@ //this is a very experimental draft of commands. We will iterate on this API (commands, arguments etc) +#include "SharedMemoryPublic.h" + #ifdef __GNUC__ #include typedef int32_t smInt32_t; @@ -22,51 +24,7 @@ typedef unsigned long long int smUint64_t; #endif -enum EnumSharedMemoryClientCommand -{ - CMD_LOAD_URDF, - CMD_SEND_BULLET_DATA_STREAM, - CMD_CREATE_BOX_COLLISION_SHAPE, -// CMD_DELETE_BOX_COLLISION_SHAPE, -// CMD_CREATE_RIGID_BODY, -// CMD_DELETE_RIGID_BODY, - CMD_CREATE_SENSOR,///enable or disable joint feedback for force/torque sensors -// CMD_REQUEST_SENSOR_MEASUREMENTS,//see CMD_REQUEST_ACTUAL_STATE/CMD_ACTUAL_STATE_UPDATE_COMPLETED - CMD_INIT_POSE, - CMD_SEND_PHYSICS_SIMULATION_PARAMETERS, - CMD_SEND_DESIRED_STATE,//todo: reconsider naming, for example SET_JOINT_CONTROL_VARIABLE? - CMD_REQUEST_ACTUAL_STATE, - CMD_REQUEST_DEBUG_LINES, - CMD_STEP_FORWARD_SIMULATION, - CMD_RESET_SIMULATION, - CMD_MAX_CLIENT_COMMANDS -}; -enum EnumSharedMemoryServerStatus -{ - CMD_SHARED_MEMORY_NOT_INITIALIZED=0, - CMD_WAITING_FOR_CLIENT_COMMAND, - - //CMD_CLIENT_COMMAND_COMPLETED is a generic 'completed' status that doesn't need special handling on the client - CMD_CLIENT_COMMAND_COMPLETED, - //the server will skip unknown command and report a status 'CMD_UNKNOWN_COMMAND_FLUSHED' - CMD_UNKNOWN_COMMAND_FLUSHED, - - CMD_URDF_LOADING_COMPLETED, - CMD_URDF_LOADING_FAILED, - CMD_BULLET_DATA_STREAM_RECEIVED_COMPLETED, - CMD_BULLET_DATA_STREAM_RECEIVED_FAILED, - CMD_BOX_COLLISION_SHAPE_CREATION_COMPLETED, - CMD_RIGID_BODY_CREATION_COMPLETED, - CMD_SET_JOINT_FEEDBACK_COMPLETED, - CMD_ACTUAL_STATE_UPDATE_COMPLETED, - CMD_ACTUAL_STATE_UPDATE_FAILED, - CMD_DEBUG_LINES_COMPLETED, - CMD_DEBUG_LINES_OVERFLOW_FAILED, - CMD_DESIRED_STATE_RECEIVED_COMPLETED, - CMD_STEP_FORWARD_SIMULATION_COMPLETED, - CMD_MAX_SERVER_COMMANDS -}; #define SHARED_MEMORY_SERVER_TEST_C #define MAX_DEGREE_OF_FREEDOM 256 @@ -108,13 +66,7 @@ struct SetJointFeedbackArgs int m_isEnabled; }; -//todo: discuss and decide about control mode and combinations -enum { -// POSITION_CONTROL=0, - CONTROL_MODE_VELOCITY=0, - CONTROL_MODE_TORQUE, - CONTROL_MODE_POSITION_VELOCITY_PD, -}; + ///InitPoseArgs is mainly to initialize (teleport) the robot in a particular position ///No motors or controls are needed to initialize the pose. It is similar to @@ -294,22 +246,6 @@ struct SharedMemoryStatus typedef struct SharedMemoryStatus SharedMemoryStatus_t; -enum JointInfoFlags -{ - JOINT_HAS_MOTORIZED_POWER=1, -}; -struct b3JointInfo -{ - char* m_linkName; - char* m_jointName; - int m_jointType; - int m_qIndex; - int m_uIndex; - /// - int m_flags; -}; - - #endif //SHARED_MEMORY_COMMANDS_H diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h new file mode 100644 index 000000000..c73670b70 --- /dev/null +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -0,0 +1,84 @@ +#ifndef SHARED_MEMORY_PUBLIC_H +#define SHARED_MEMORY_PUBLIC_H + +#define SHARED_MEMORY_KEY 12347 + +enum EnumSharedMemoryClientCommand +{ + CMD_LOAD_URDF, + CMD_SEND_BULLET_DATA_STREAM, + CMD_CREATE_BOX_COLLISION_SHAPE, +// CMD_DELETE_BOX_COLLISION_SHAPE, +// CMD_CREATE_RIGID_BODY, +// CMD_DELETE_RIGID_BODY, + CMD_CREATE_SENSOR,///enable or disable joint feedback for force/torque sensors +// CMD_REQUEST_SENSOR_MEASUREMENTS,//see CMD_REQUEST_ACTUAL_STATE/CMD_ACTUAL_STATE_UPDATE_COMPLETED + CMD_INIT_POSE, + CMD_SEND_PHYSICS_SIMULATION_PARAMETERS, + CMD_SEND_DESIRED_STATE,//todo: reconsider naming, for example SET_JOINT_CONTROL_VARIABLE? + CMD_REQUEST_ACTUAL_STATE, + CMD_REQUEST_DEBUG_LINES, + CMD_STEP_FORWARD_SIMULATION, + CMD_RESET_SIMULATION, + CMD_MAX_CLIENT_COMMANDS +}; + +enum EnumSharedMemoryServerStatus +{ + CMD_SHARED_MEMORY_NOT_INITIALIZED=0, + CMD_WAITING_FOR_CLIENT_COMMAND, + + //CMD_CLIENT_COMMAND_COMPLETED is a generic 'completed' status that doesn't need special handling on the client + CMD_CLIENT_COMMAND_COMPLETED, + //the server will skip unknown command and report a status 'CMD_UNKNOWN_COMMAND_FLUSHED' + CMD_UNKNOWN_COMMAND_FLUSHED, + + CMD_URDF_LOADING_COMPLETED, + CMD_URDF_LOADING_FAILED, + CMD_BULLET_DATA_STREAM_RECEIVED_COMPLETED, + CMD_BULLET_DATA_STREAM_RECEIVED_FAILED, + CMD_BOX_COLLISION_SHAPE_CREATION_COMPLETED, + CMD_RIGID_BODY_CREATION_COMPLETED, + CMD_SET_JOINT_FEEDBACK_COMPLETED, + CMD_ACTUAL_STATE_UPDATE_COMPLETED, + CMD_ACTUAL_STATE_UPDATE_FAILED, + CMD_DEBUG_LINES_COMPLETED, + CMD_DEBUG_LINES_OVERFLOW_FAILED, + CMD_DESIRED_STATE_RECEIVED_COMPLETED, + CMD_STEP_FORWARD_SIMULATION_COMPLETED, + CMD_RESET_SIMULATION_COMPLETED, + CMD_MAX_SERVER_COMMANDS +}; + +enum JointInfoFlags +{ + JOINT_HAS_MOTORIZED_POWER=1, +}; +struct b3JointInfo +{ + char* m_linkName; + char* m_jointName; + int m_jointType; + int m_qIndex; + int m_uIndex; + /// + int m_flags; +}; + +struct b3DebugLines +{ + int m_numDebugLines; + const float* m_linesFrom;//float x,y,z times 'm_numDebugLines'. + const float* m_linesTo;//float x,y,z times 'm_numDebugLines'. + const float* m_linesColor;//float red,green,blue times 'm_numDebugLines'. +}; + +//todo: discuss and decide about control mode and combinations +enum { + // POSITION_CONTROL=0, + CONTROL_MODE_VELOCITY=0, + CONTROL_MODE_TORQUE, + CONTROL_MODE_POSITION_VELOCITY_PD, +}; + +#endif//SHARED_MEMORY_PUBLIC_H diff --git a/examples/SharedMemory/premake4.lua b/examples/SharedMemory/premake4.lua index 292276d06..63a5d0e90 100644 --- a/examples/SharedMemory/premake4.lua +++ b/examples/SharedMemory/premake4.lua @@ -16,8 +16,14 @@ links { language "C++" files { - "**.cpp", - "**.h", + "PhysicsClient.cpp", + "PhysicsClientExample.cpp", + "PhysicsServerExample.cpp", + "main.cpp", + "PhysicsClientC_API.cpp", + "PhysicsServer.cpp", + "PosixSharedMemory.cpp", + "Win32SharedMemory.cpp", "../Importers/ImportURDFDemo/ConvertRigidBodies2MultiBody.h", "../Importers/ImportURDFDemo/MultiBodyCreationInterface.h", "../Importers/ImportURDFDemo/MyMultiBodyCreator.cpp", diff --git a/test/SharedMemory/test.c b/test/SharedMemory/test.c index 05dd847d7..3c2855d4f 100644 --- a/test/SharedMemory/test.c +++ b/test/SharedMemory/test.c @@ -1,6 +1,5 @@ -#include "SharedMemoryBlock.h" +//#include "SharedMemoryCommands.h" #include "PhysicsClientC_API.h" -#include "SharedMemoryCommands.h" #include "Bullet3Common/b3Logging.h" #include @@ -24,39 +23,39 @@ int main(int argc, char* argv[]) double startPosX, startPosY,startPosZ; int imuLinkIndex = -1; - SharedMemoryCommand_t command; - SharedMemoryStatus_t status; + b3PhysicsClientHandle sm; b3Printf("timeout = %d\n",timeout); - printf("hello world\n"); - sm = b3ConnectSharedMemory( allowSharedMemoryInitialization); + sm = b3ConnectSharedMemory(SHARED_MEMORY_KEY); if (b3CanSubmitCommand(sm)) { - ret = b3InitPhysicsParamCommand(&command); - ret = b3PhysicsParamSetGravity(&command, gravx,gravy, gravz); - ret = b3PhysicsParamSetTimeStep(&command, timeStep); - ret = b3SubmitClientCommand(sm, &command); + { + b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm); + ret = b3PhysicsParamSetGravity(command, gravx,gravy, gravz); + ret = b3PhysicsParamSetTimeStep(command, timeStep); + ret = b3SubmitClientCommand(sm, command); timeout = MAX_TIMEOUT; - while ((timeout-- > 0) && b3ProcessServerStatus(sm, &status)==0) {} + while ((timeout-- > 0) && b3ProcessServerStatus(sm)==0) {} b3Printf("timeout = %d\n",timeout); - + } - ret = b3LoadUrdfCommandInit(&command, urdfFileName); - //setting the initial position, orientation and other arguments are optional - startPosX =2; - startPosY =3; - startPosZ = 1; - ret = b3LoadUrdfCommandSetStartPosition(&command, startPosX,startPosY,startPosZ); - - ret = b3SubmitClientCommand(sm, &command); - timeout = MAX_TIMEOUT; - while ((timeout-- > 0) && b3ProcessServerStatus(sm, &status)==0) {} - b3Printf("timeout = %d\n",timeout); - + { + b3SharedMemoryCommandHandle command = b3LoadUrdfCommandInit(sm, urdfFileName); + //setting the initial position, orientation and other arguments are optional + startPosX =2; + startPosY =3; + startPosZ = 1; + ret = b3LoadUrdfCommandSetStartPosition(command, startPosX,startPosY,startPosZ); + + ret = b3SubmitClientCommand(sm, command); + timeout = MAX_TIMEOUT; + while ((timeout-- > 0) && b3ProcessServerStatus(sm)==0) {} + b3Printf("timeout = %d\n",timeout); + } numJoints = b3GetNumJoints(sm); for (i=0;i=0) || (sensorJointIndexRight>=0)) { - ret = b3CreateSensorCommandInit(&command); + b3SharedMemoryCommandHandle command = b3CreateSensorCommandInit(sm); if (imuLinkIndex>=0) { - ret = b3CreateSensorEnableIMUForLink(&command, imuLinkIndex, 1); + ret = b3CreateSensorEnableIMUForLink(command, imuLinkIndex, 1); } if (sensorJointIndexLeft>=0) { - ret = b3CreateSensorEnable6DofJointForceTorqueSensor(&command, sensorJointIndexLeft, 1); + ret = b3CreateSensorEnable6DofJointForceTorqueSensor(command, sensorJointIndexLeft, 1); } if(sensorJointIndexRight>=0) { - ret = b3CreateSensorEnable6DofJointForceTorqueSensor(&command, sensorJointIndexRight, 1); + ret = b3CreateSensorEnable6DofJointForceTorqueSensor(command, sensorJointIndexRight, 1); } - ret = b3SubmitClientCommand(sm, &command); + ret = b3SubmitClientCommand(sm, command); timeout = MAX_TIMEOUT; - while ((timeout-- > 0) && b3ProcessServerStatus(sm, &status)==0) {} + while ((timeout-- > 0) && b3ProcessServerStatus(sm)==0) {} } - ret = b3CreateBoxShapeCommandInit(&command); - ret = b3CreateBoxCommandSetStartPosition(&command, 0,0,-1); - ret = b3CreateBoxCommandSetStartOrientation(&command,0,0,0,1); - ret = b3CreateBoxCommandSetHalfExtents(&command, 10,10,1); - ret = b3SubmitClientCommand(sm, &command); - timeout = MAX_TIMEOUT; - while ((timeout-- > 0) && b3ProcessServerStatus(sm, &status)==0) {} - - b3RequestActualStateCommandInit(&command); - ret = b3SubmitClientCommand(sm, &command); - timeout = MAX_TIMEOUT; - while ((timeout-- > 0) && b3ProcessServerStatus(sm, &status)==0) {} + { + b3SharedMemoryCommandHandle command = b3CreateBoxShapeCommandInit(sm); + ret = b3CreateBoxCommandSetStartPosition(command, 0,0,-1); + ret = b3CreateBoxCommandSetStartOrientation(command,0,0,0,1); + ret = b3CreateBoxCommandSetHalfExtents(command, 10,10,1); + ret = b3SubmitClientCommand(sm, command); + timeout = MAX_TIMEOUT; + while ((timeout-- > 0) && b3ProcessServerStatus(sm)==0) {} + } + { + b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(sm); + ret = b3SubmitClientCommand(sm, command); + timeout = MAX_TIMEOUT; + while ((timeout-- > 0) && b3ProcessServerStatus(sm)==0) {} + } + +#if 0 posVarCount =status.m_sendActualStateArgs.m_numDegreeOfFreedomQ; dofCount =status.m_sendActualStateArgs.m_numDegreeOfFreedomU; b3Printf("posVarCount = %d\n",posVarCount); printf("dofCount = %d\n",dofCount); +#endif - b3JointControlCommandInit(&command, CONTROL_MODE_VELOCITY); - for ( dofIndex=0;dofIndex 0) && b3ProcessServerStatus(sm)==0) {} } - ret = b3SubmitClientCommand(sm, &command); - timeout = MAX_TIMEOUT; - while ((timeout-- > 0) && b3ProcessServerStatus(sm, &status)==0) {} - ///perform some simulation steps for testing for ( i=0;i<100;i++) { - ret = b3InitStepSimulationCommand(&command); - ret = b3SubmitClientCommand(sm, &command); + + b3SharedMemoryCommandHandle command = b3InitStepSimulationCommand(sm); + ret = b3SubmitClientCommand(sm, command); timeout = MAX_TIMEOUT; - while ((timeout-- > 0) && b3ProcessServerStatus(sm, &status)==0) {} + while ((timeout-- > 0) && b3ProcessServerStatus(sm)==0) {} } - b3RequestActualStateCommandInit(&command); - ret = b3SubmitClientCommand(sm, &command); - timeout = MAX_TIMEOUT; - while ((timeout-- > 0) && b3ProcessServerStatus(sm, &status)==0) {} - + { + b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(sm); + ret = b3SubmitClientCommand(sm, command); + timeout = MAX_TIMEOUT; + while ((timeout-- > 0) && b3ProcessServerStatus(sm)==0) {} + } +#if 0 if (sensorJointIndexLeft>=0) { b3Printf("Sensor for joint [%d] = %f,%f,%f\n", sensorJointIndexLeft, @@ -164,12 +173,13 @@ int main(int argc, char* argv[]) status.m_sendActualStateArgs.m_jointReactionForces[6*sensorJointIndexRight+1], status.m_sendActualStateArgs.m_jointReactionForces[6*sensorJointIndexRight+2]); } - - ret = b3InitResetSimulationCommand(&command); - ret = b3SubmitClientCommand(sm, &command); +#endif + { + b3SharedMemoryCommandHandle command = b3InitResetSimulationCommand(sm); + ret = b3SubmitClientCommand(sm, command); timeout = MAX_TIMEOUT; - while ((timeout-- > 0) && b3ProcessServerStatus(sm, &status)==0) {} - + while ((timeout-- > 0) && b3ProcessServerStatus(sm)==0) {} + } }