Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Separate SharedMemoryStatus from SharedMemoryCommand

Browse Source 
Added CMD_CMD_SEND_PHYSICS_SIMULATION_PARAMETERS (set gravity as example)
and CMD_INIT_POSE, not fully implemented yet.
This commit is contained in:
Erwin Coumans
2015-07-21 21:46:28 -07:00
parent 8e163c984d
commit 05fc203ec1
5 changed files with 165 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files

24
examples/SharedMemory/PhysicsClient.cpp
View File

@@ -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)

48
examples/SharedMemory/PhysicsServer.cpp
View File

@@ -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;

24
examples/SharedMemory/RobotControlExample.cpp
View File

@@ -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
{
/*

100
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -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

2
examples/SharedMemory/SharedMemoryInterface.h
View File

@@ -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;