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

minor update for shared memory interface

Browse Source
This commit is contained in:
Erwin Coumans
2015-06-21 13:24:36 -07:00
parent 6e9eb13235
commit 4688540a98
5 changed files with 235 additions and 66 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
examples/ExampleBrowser/GwenGUISupport/GwenParameterInterface.cpp
View File

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

65
examples/SharedMemory/PhysicsClient.cpp
View File

@@ -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;i<numQ;i++)
{
if (i<numQ-1)
{
sprintf(msg,"%s%f,",msg,m_testBlock1->m_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++);

99
examples/SharedMemory/PhysicsServer.cpp
View File

@@ -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;l<mb->getNumLinks();l++)
{
for (int d=0;d<mb->getLink(l).m_posVarCount;d++)
{
m_testBlock1->m_actualStateQ[totalDegreeOfFreedomQ++] = mb->getJointPosMultiDof(l)[d];
}
for (int d=0;d<mb->getLink(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;i<m_jointFeedbacks.size();i++)
{
printf("Applied force A:(%f,%f,%f), torque A:(%f,%f,%f)\nForce B:(%f,%f,%f), torque B:(%f,%f,%f)\n",
@@ -246,6 +302,25 @@ void PhysicsServer::stepSimulation(float deltaTime)
m_jointFeedbacks[i]->m_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:

76
examples/SharedMemory/SharedMemoryCommands.h Normal file
View File

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

56
examples/SharedMemory/SharedMemoryInterface.h
View File

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