diff --git a/examples/SharedMemory/PhysicsClient.cpp b/examples/SharedMemory/PhysicsClient.cpp index db3040c7d..644581976 100644 --- a/examples/SharedMemory/PhysicsClient.cpp +++ b/examples/SharedMemory/PhysicsClient.cpp @@ -132,7 +132,7 @@ bool PhysicsClientSharedMemory::processServerStatus(ServerStatus& serverStatus) { btAssert(m_data->m_testBlock1->m_numServerCommands==m_data->m_testBlock1->m_numProcessedServerCommands+1); - const SharedMemoryCommand& serverCmd =m_data->m_testBlock1->m_serverCommands[0]; + const SharedMemoryStatus& serverCmd =m_data->m_testBlock1->m_serverCommands[0]; hasStatus = true; serverStatus = serverCmd; @@ -272,7 +272,7 @@ bool PhysicsClientSharedMemory::processServerStatus(ServerStatus& serverStatus) case CMD_ACTUAL_STATE_UPDATE_COMPLETED: { b3Printf("Received actual state\n"); - SharedMemoryCommand& command = m_data->m_testBlock1->m_serverCommands[0]; + SharedMemoryStatus& command = m_data->m_testBlock1->m_serverCommands[0]; int numQ = command.m_sendActualStateArgs.m_numDegreeOfFreedomQ; int numU = command.m_sendActualStateArgs.m_numDegreeOfFreedomU; @@ -432,6 +432,26 @@ bool PhysicsClientSharedMemory::submitClientCommand(const SharedMemoryCommand& c break; } + + case CMD_INIT_POSE: + { + if (m_data->m_serverLoadUrdfOK) + { + b3Printf("Initialize Pose"); + m_data->m_testBlock1->m_clientCommands[0] = command; + m_data->m_testBlock1->m_numClientCommands++; + + } + break; + } + case CMD_SEND_PHYSICS_SIMULATION_PARAMETERS: + { + b3Printf("Send Physics Simulation Parameters"); + m_data->m_testBlock1->m_clientCommands[0] = command; + m_data->m_testBlock1->m_numClientCommands++; + break; + } + case CMD_REQUEST_ACTUAL_STATE: { if (m_data->m_serverLoadUrdfOK) diff --git a/examples/SharedMemory/PhysicsServer.cpp b/examples/SharedMemory/PhysicsServer.cpp index a2ba50be4..7f6c77bb9 100644 --- a/examples/SharedMemory/PhysicsServer.cpp +++ b/examples/SharedMemory/PhysicsServer.cpp @@ -316,7 +316,7 @@ void PhysicsServerSharedMemory::processClientCommands() m_data->m_worldImporters.push_back(worldImporter); bool completedOk = worldImporter->loadFileFromMemory(m_data->m_testBlock1->m_bulletStreamDataClientToServer,clientCmd.m_dataStreamArguments.m_streamChunkLength); - SharedMemoryCommand& serverCmd =m_data->m_testBlock1->m_serverCommands[0]; + SharedMemoryStatus& serverCmd =m_data->m_testBlock1->m_serverCommands[0]; if (completedOk) { @@ -345,7 +345,7 @@ void PhysicsServerSharedMemory::processClientCommands() urdfArgs.m_initialOrientation[2], urdfArgs.m_initialOrientation[3]), urdfArgs.m_useMultiBody, urdfArgs.m_useFixedBase); - SharedMemoryCommand& serverCmd =m_data->m_testBlock1->m_serverCommands[0]; + SharedMemoryStatus& serverCmd =m_data->m_testBlock1->m_serverCommands[0]; if (completedOk) { @@ -450,7 +450,7 @@ void PhysicsServerSharedMemory::processClientCommands() - SharedMemoryCommand& serverCmd = m_data->m_testBlock1->m_serverCommands[0]; + SharedMemoryStatus& serverCmd = m_data->m_testBlock1->m_serverCommands[0]; serverCmd.m_type = CMD_DESIRED_STATE_RECEIVED_COMPLETED; m_data->m_testBlock1->m_numServerCommands++; break; @@ -461,7 +461,7 @@ void PhysicsServerSharedMemory::processClientCommands() if (m_data->m_dynamicsWorld->getNumMultibodies()>0) { btMultiBody* mb = m_data->m_dynamicsWorld->getMultiBody(0); - SharedMemoryCommand& serverCmd = m_data->m_testBlock1->m_serverCommands[0]; + SharedMemoryStatus& serverCmd = m_data->m_testBlock1->m_serverCommands[0]; serverCmd.m_type = CMD_ACTUAL_STATE_UPDATE_COMPLETED; serverCmd.m_sendActualStateArgs.m_bodyUniqueId = 0; @@ -553,13 +553,49 @@ void PhysicsServerSharedMemory::processClientCommands() b3Printf("Step simulation request"); m_data->m_dynamicsWorld->stepSimulation(m_data->m_physicsDeltaTime); - SharedMemoryCommand& serverCmd =m_data->m_testBlock1->m_serverCommands[0]; + SharedMemoryStatus& serverCmd =m_data->m_testBlock1->m_serverCommands[0]; serverCmd.m_type =CMD_STEP_FORWARD_SIMULATION_COMPLETED; m_data->m_testBlock1->m_numServerCommands++; break; } + + case CMD_SEND_PHYSICS_SIMULATION_PARAMETERS: + { + if (clientCmd.m_physSimParamArgs.m_updateFlags&SIM_PARAM_UPDATE_GRAVITY) + { + btVector3 grav(clientCmd.m_physSimParamArgs.m_gravityAcceleration[0], + clientCmd.m_physSimParamArgs.m_gravityAcceleration[1], + clientCmd.m_physSimParamArgs.m_gravityAcceleration[2]); + this->m_data->m_dynamicsWorld->setGravity(grav); + b3Printf("Updated Gravity: %f,%f,%f",grav[0],grav[1],grav[2]); + + } + + + SharedMemoryStatus& serverCmd =m_data->m_testBlock1->m_serverCommands[0]; + serverCmd.m_type =CMD_STEP_FORWARD_SIMULATION_COMPLETED; + m_data->m_testBlock1->m_numServerCommands++; + + + break; + + }; + case CMD_INIT_POSE: + { + b3Printf("Server Init Pose not implemented yet"); + ///@todo: implement this + m_data->m_dynamicsWorld->setGravity(btVector3(0,0,0)); + + SharedMemoryStatus& serverCmd =m_data->m_testBlock1->m_serverCommands[0]; + serverCmd.m_type =CMD_STEP_FORWARD_SIMULATION_COMPLETED; + m_data->m_testBlock1->m_numServerCommands++; + break; + } + + + case CMD_SHUTDOWN: { btAssert(0); @@ -581,7 +617,7 @@ void PhysicsServerSharedMemory::processClientCommands() bool isDynamic = (mass>0); worldImporter->createRigidBody(isDynamic,mass,startTrans,shape,0); m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld); - SharedMemoryCommand& serverCmd =m_data->m_testBlock1->m_serverCommands[0]; + SharedMemoryStatus& serverCmd =m_data->m_testBlock1->m_serverCommands[0]; serverCmd.m_type =CMD_STEP_FORWARD_SIMULATION_COMPLETED; m_data->m_testBlock1->m_numServerCommands++; break; diff --git a/examples/SharedMemory/RobotControlExample.cpp b/examples/SharedMemory/RobotControlExample.cpp index b38f0a0ef..73d757816 100644 --- a/examples/SharedMemory/RobotControlExample.cpp +++ b/examples/SharedMemory/RobotControlExample.cpp @@ -97,6 +97,27 @@ void MyCallback2(int buttonId, bool buttonState, void* userPtr) cl->enqueueCommand(command); break; } + + case CMD_SEND_PHYSICS_SIMULATION_PARAMETERS: + { + command.m_type = CMD_SEND_PHYSICS_SIMULATION_PARAMETERS; + command.m_physSimParamArgs.m_gravityAcceleration[0] = 0; + command.m_physSimParamArgs.m_gravityAcceleration[1] = 0; + command.m_physSimParamArgs.m_gravityAcceleration[2] = -10; + command.m_physSimParamArgs.m_updateFlags = SIM_PARAM_UPDATE_GRAVITY; + cl->enqueueCommand(command); + break; + + }; + case CMD_INIT_POSE: + { + ///@todo: implement this + command.m_type = CMD_INIT_POSE; + cl->enqueueCommand(command); + break; + } + + case CMD_CREATE_BOX_COLLISION_SHAPE: { command.m_type =CMD_CREATE_BOX_COLLISION_SHAPE; @@ -225,7 +246,8 @@ void RobotControlExample::initPhysics() createButton("Get State",CMD_REQUEST_ACTUAL_STATE, isTrigger); createButton("Send Desired State",CMD_SEND_DESIRED_STATE, isTrigger); createButton("Create Box Collider",CMD_CREATE_BOX_COLLISION_SHAPE,isTrigger); - + createButton("Set Physics Params",CMD_SEND_PHYSICS_SIMULATION_PARAMETERS,isTrigger); + createButton("Init Pose",CMD_INIT_POSE,isTrigger); } else { /* diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 7299d692b..a2921e69c 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -22,10 +22,28 @@ typedef unsigned long long int smUint64_t; #endif -enum SharedMemoryServerCommand +enum EnumSharedMemoryClientCommand { - CMD_URDF_LOADING_COMPLETED, - CMD_URDF_LOADING_FAILED, + 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_SET_JOINT_FEEDBACK,///enable or disable joint feedback for force/torque sensors + CMD_INIT_POSE, + CMD_SEND_PHYSICS_SIMULATION_PARAMETERS, + CMD_SEND_DESIRED_STATE, + CMD_REQUEST_ACTUAL_STATE, + CMD_STEP_FORWARD_SIMULATION, //includes CMD_REQUEST_STATE + CMD_SHUTDOWN, + CMD_MAX_CLIENT_COMMANDS +}; + +enum EnumSharedMemoryServerStatus +{ + 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, @@ -33,24 +51,8 @@ enum SharedMemoryServerCommand CMD_SET_JOINT_FEEDBACK_COMPLETED, 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_SEND_BULLET_DATA_STREAM, - CMD_CREATE_BOX_COLLISION_SHAPE, - CMD_DELETE_BOX_COLLISION_SHAPE, - CMD_CREATE_RIGID_BODY, - CMD_DELETE_RIGID_BODY, - CMD_SET_JOINT_FEEDBACK,///enable or disable joint feedback - CMD_SEND_DESIRED_STATE, - CMD_REQUEST_ACTUAL_STATE, - CMD_STEP_FORWARD_SIMULATION, //includes CMD_REQUEST_STATE - CMD_SHUTDOWN, - CMD_MAX_CLIENT_COMMANDS + CMD_STEP_FORWARD_SIMULATION_COMPLETED, + CMD_MAX_SERVER_COMMANDS }; #define SHARED_MEMORY_SERVER_TEST_C @@ -89,6 +91,19 @@ enum { CONTROL_MODE_TORQUE, }; +///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 +///moving a robot to a starting place, while it is switched off. It is only called +///at the start of a robot control session. All velocities and control forces are cleared to zero. +struct InitPoseArgs +{ + int m_bodyUniqueId; + double m_initialStateQ[MAX_DEGREE_OF_FREEDOM]; +}; + + +///Controlling a robot involves sending the desired state to its joint motor controllers. +///The control mode determines the state variables used for motor control. struct SendDesiredStateArgs { int m_bodyUniqueId; @@ -104,6 +119,29 @@ struct SendDesiredStateArgs }; + +enum EnumUpdateFlags +{ + SIM_PARAM_UPDATE_DELTA_TIME=1, + SIM_PARAM_UPDATE_GRAVITY=2, + SIM_PARAM_UPDATE_NUM_SOLVER_ITERATIONS=4, + SIM_PARAM_UPDATE_NUM_SIMULATION_SUB_STEPS=8, +}; + +///Controlling a robot involves sending the desired state to its joint motor controllers. +///The control mode determines the state variables used for motor control. +struct SendPhysicsSimulationParameters +{ + //a bit fields to tell which parameters need updating, see SIM_PARAM_UPDATE_DELTA_TIME etc. + //for example m_updateFlags = SIM_PARAM_UPDATE_DELTA_TIME | SIM_PARAM_UPDATE_NUM_SOLVER_ITERATIONS; + int m_updateFlags; + + double m_deltaTime; + double m_gravityAcceleration[3]; + int m_numSimulationSubSteps; + int m_numSolverIterations; +}; + struct RequestActualStateArgs { int m_bodyUniqueId; @@ -138,13 +176,29 @@ struct SharedMemoryCommand union { UrdfArgs m_urdfArguments; + InitPoseArgs m_initPoseArgs; + SendPhysicsSimulationParameters m_physSimParamArgs; BulletDataStreamArgs m_dataStreamArguments; SendDesiredStateArgs m_sendDesiredStateCommandArgument; RequestActualStateArgs m_requestActualStateInformationCommandArgument; - SendActualStateArgs m_sendActualStateArgs; }; }; -typedef SharedMemoryCommand ServerStatus; + +struct SharedMemoryStatus +{ + int m_type; + + smUint64_t m_timeStamp; + int m_sequenceNumber; + + union + { + BulletDataStreamArgs m_dataStreamArguments; + SendActualStateArgs m_sendActualStateArgs; + }; +}; + +typedef SharedMemoryStatus ServerStatus; #endif //SHARED_MEMORY_COMMANDS_H diff --git a/examples/SharedMemory/SharedMemoryInterface.h b/examples/SharedMemory/SharedMemoryInterface.h index 43af44496..084559451 100644 --- a/examples/SharedMemory/SharedMemoryInterface.h +++ b/examples/SharedMemory/SharedMemoryInterface.h @@ -12,7 +12,7 @@ struct SharedMemoryExampleData { int m_magicId; SharedMemoryCommand m_clientCommands[SHARED_MEMORY_MAX_COMMANDS]; - SharedMemoryCommand m_serverCommands[SHARED_MEMORY_MAX_COMMANDS]; + SharedMemoryStatus m_serverCommands[SHARED_MEMORY_MAX_COMMANDS]; int m_numClientCommands; int m_numProcessedClientCommands;