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

Merge remote-tracking branch 'upstream/master'

Browse Source
This commit is contained in:
yunfeibai
2017-03-29 15:06:15 -07:00
parent 64573c38e4 30c1923b00
commit 66a8685816
77 changed files with 2213 additions and 1218 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
examples/SharedMemory/BodyJointInfoUtility.h
View File

@@ -61,6 +61,10 @@ template <typename T, typename U> void addJointInfoFromMultiBodyData(const T* mb
info.m_jointType = mb->m_links[link].m_jointType;
info.m_jointDamping = mb->m_links[link].m_jointDamping;
info.m_jointFriction = mb->m_links[link].m_jointFriction;
info.m_jointLowerLimit = mb->m_links[link].m_jointLowerLimit;
info.m_jointUpperLimit = mb->m_links[link].m_jointUpperLimit;
info.m_jointMaxForce = mb->m_links[link].m_jointMaxForce;
info.m_jointMaxVelocity = mb->m_links[link].m_jointMaxVelocity;
if ((mb->m_links[link].m_jointType == eRevoluteType) ||
(mb->m_links[link].m_jointType == ePrismaticType)) {
@@ -76,4 +80,4 @@ template <typename T, typename U> void addJointInfoFromMultiBodyData(const T* mb
#endif //BODY_JOINT_INFO_UTILITY_H
#endif //BODY_JOINT_INFO_UTILITY_H

44
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -238,6 +238,19 @@ int b3LoadUrdfCommandSetUseFixedBase(b3SharedMemoryCommandHandle commandHandle,
return -1;
}
int b3LoadUrdfCommandSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_LOAD_URDF);
if (command && (command->m_type == CMD_LOAD_URDF))
{
command->m_updateFlags |= URDF_ARGS_HAS_CUSTOM_URDF_FLAGS;
command->m_urdfArguments.m_urdfFlags = flags;
}
return 0;
}
int b3LoadUrdfCommandSetStartPosition(b3SharedMemoryCommandHandle commandHandle, double startPosX,double startPosY,double startPosZ)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
@@ -547,10 +560,24 @@ b3SharedMemoryCommandHandle b3RequestActualStateCommandInit(b3PhysicsClientHandl
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type =CMD_REQUEST_ACTUAL_STATE;
command->m_updateFlags = 0;
command->m_requestActualStateInformationCommandArgument.m_bodyUniqueId = bodyUniqueId;
return (b3SharedMemoryCommandHandle) command;
}
int b3RequestActualStateCommandComputeLinkVelocity(b3SharedMemoryCommandHandle commandHandle, int computeLinkVelocity)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
btAssert(command->m_type == CMD_REQUEST_ACTUAL_STATE);
if (computeLinkVelocity && command->m_type == CMD_REQUEST_ACTUAL_STATE)
{
command->m_updateFlags |= ACTUAL_STATE_COMPUTE_LINKVELOCITY;
}
return 0;
}
int b3GetJointState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int jointIndex, b3JointSensorState *state)
{
const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
@@ -604,6 +631,8 @@ int b3GetLinkState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle
{
state->m_worldPosition[i] = status->m_sendActualStateArgs.m_linkState[7 * linkIndex + i];
state->m_localInertialPosition[i] = status->m_sendActualStateArgs.m_linkLocalInertialFrames[7 * linkIndex + i];
state->m_worldLinearVelocity[i] = status->m_sendActualStateArgs.m_linkWorldVelocities[6*linkIndex+i];
state->m_worldAngularVelocity[i] = status->m_sendActualStateArgs.m_linkWorldVelocities[6*linkIndex+i+3];
}
for (int i = 0; i < 4; ++i)
{
@@ -1085,6 +1114,7 @@ int b3SubmitClientCommand(b3PhysicsClientHandle physClient, const b3SharedMemory
b3SharedMemoryStatusHandle b3SubmitClientCommandAndWaitStatus(b3PhysicsClientHandle physClient, const b3SharedMemoryCommandHandle commandHandle)
{
B3_PROFILE("b3SubmitClientCommandAndWaitStatus");
b3Clock clock;
double startTime = clock.getTimeInSeconds();
@@ -1098,11 +1128,17 @@ b3SharedMemoryStatusHandle b3SubmitClientCommandAndWaitStatus(b3PhysicsClientHan
double timeOutInSeconds = cl->getTimeOut();
b3SubmitClientCommand(physClient, commandHandle);
while (cl->isConnected() && (statusHandle == 0) && (clock.getTimeInSeconds()-startTime < timeOutInSeconds))
{
statusHandle = b3ProcessServerStatus(physClient);
B3_PROFILE("b3SubmitClientCommand");
b3SubmitClientCommand(physClient, commandHandle);
}
{
B3_PROFILE("b3ProcessServerStatus");
while (cl->isConnected() && (statusHandle == 0) && (clock.getTimeInSeconds()-startTime < timeOutInSeconds))
{
clock.usleep(0);
statusHandle = b3ProcessServerStatus(physClient);
}
}
return (b3SharedMemoryStatusHandle)statusHandle;
}

3
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -217,6 +217,7 @@ int b3LoadUrdfCommandSetStartPosition(b3SharedMemoryCommandHandle commandHandle,
int b3LoadUrdfCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX,double startOrnY,double startOrnZ, double startOrnW);
int b3LoadUrdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody);
int b3LoadUrdfCommandSetUseFixedBase(b3SharedMemoryCommandHandle commandHandle, int useFixedBase);
int b3LoadUrdfCommandSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags);
b3SharedMemoryCommandHandle b3LoadBulletCommandInit(b3PhysicsClientHandle physClient, const char* fileName);
b3SharedMemoryCommandHandle b3SaveBulletCommandInit(b3PhysicsClientHandle physClient, const char* fileName);
@@ -313,6 +314,8 @@ int b3CreateSensorEnable6DofJointForceTorqueSensor(b3SharedMemoryCommandHandle c
int b3CreateSensorEnableIMUForLink(b3SharedMemoryCommandHandle commandHandle, int linkIndex, int enable);
b3SharedMemoryCommandHandle b3RequestActualStateCommandInit(b3PhysicsClientHandle physClient,int bodyUniqueId);
int b3RequestActualStateCommandComputeLinkVelocity(b3SharedMemoryCommandHandle commandHandle, int computeLinkVelocity);
int b3GetJointState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int jointIndex, struct b3JointSensorState *state);
int b3GetLinkState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int linkIndex, struct b3LinkState *state);

169
examples/SharedMemory/PhysicsClientSharedMemory.cpp
View File

@@ -341,7 +341,7 @@ void PhysicsClientSharedMemory::processBodyJointInfo(int bodyUniqueId, const Sha
const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
// SharedMemoryStatus* stat = 0;
if (!m_data->m_testBlock1) {
m_data->m_lastServerStatus.m_type = CMD_SHARED_MEMORY_NOT_INITIALIZED;
return &m_data->m_lastServerStatus;
@@ -358,7 +358,9 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
}
if (m_data->m_testBlock1->m_numServerCommands >
m_data->m_testBlock1->m_numProcessedServerCommands) {
m_data->m_testBlock1->m_numProcessedServerCommands)
{
B3_PROFILE("processServerCMD");
btAssert(m_data->m_testBlock1->m_numServerCommands ==
m_data->m_testBlock1->m_numProcessedServerCommands + 1);
@@ -368,8 +370,13 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
// EnumSharedMemoryServerStatus s = (EnumSharedMemoryServerStatus)serverCmd.m_type;
// consume the command
switch (serverCmd.m_type) {
case CMD_CLIENT_COMMAND_COMPLETED: {
switch (serverCmd.m_type)
{
case CMD_CLIENT_COMMAND_COMPLETED:
{
B3_PROFILE("CMD_CLIENT_COMMAND_COMPLETED");
if (m_data->m_verboseOutput) {
b3Printf("Server completed command");
}
@@ -378,13 +385,17 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
case CMD_MJCF_LOADING_COMPLETED:
{
B3_PROFILE("CMD_MJCF_LOADING_COMPLETED");
if (m_data->m_verboseOutput) {
b3Printf("Server loading the MJCF OK\n");
}
break;
}
case CMD_SDF_LOADING_COMPLETED: {
case CMD_SDF_LOADING_COMPLETED:
{
B3_PROFILE("CMD_SDF_LOADING_COMPLETED");
if (m_data->m_verboseOutput) {
b3Printf("Server loading the SDF OK\n");
}
@@ -392,7 +403,9 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
break;
}
case CMD_URDF_LOADING_COMPLETED: {
case CMD_URDF_LOADING_COMPLETED:
{
B3_PROFILE("CMD_URDF_LOADING_COMPLETED");
if (m_data->m_verboseOutput) {
b3Printf("Server loading the URDF OK\n");
@@ -444,19 +457,28 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
}
break;
}
case CMD_DESIRED_STATE_RECEIVED_COMPLETED: {
case CMD_DESIRED_STATE_RECEIVED_COMPLETED:
{
B3_PROFILE("CMD_DESIRED_STATE_RECEIVED_COMPLETED");
if (m_data->m_verboseOutput) {
b3Printf("Server received desired state");
}
break;
}
case CMD_STEP_FORWARD_SIMULATION_COMPLETED: {
case CMD_STEP_FORWARD_SIMULATION_COMPLETED:
{
B3_PROFILE("CMD_STEP_FORWARD_SIMULATION_COMPLETED");
if (m_data->m_verboseOutput) {
b3Printf("Server completed step simulation");
}
break;
}
case CMD_URDF_LOADING_FAILED: {
case CMD_URDF_LOADING_FAILED:
{
B3_PROFILE("CMD_URDF_LOADING_FAILED");
if (m_data->m_verboseOutput) {
b3Printf("Server failed loading the URDF...\n");
}
@@ -465,24 +487,30 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
}
case CMD_USER_CONSTRAINT_INFO_COMPLETED:
{
int cid = serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId;
B3_PROFILE("CMD_USER_CONSTRAINT_INFO_COMPLETED");
int cid = serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId;
m_data->m_userConstraintInfoMap.insert(cid,serverCmd.m_userConstraintResultArgs);
break;
}
case CMD_USER_CONSTRAINT_COMPLETED:
{
B3_PROFILE("CMD_USER_CONSTRAINT_COMPLETED");
int cid = serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId;
m_data->m_userConstraintInfoMap.insert(cid,serverCmd.m_userConstraintResultArgs);
break;
}
case CMD_REMOVE_USER_CONSTRAINT_COMPLETED:
{
B3_PROFILE("CMD_REMOVE_USER_CONSTRAINT_COMPLETED");
int cid = serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId;
m_data->m_userConstraintInfoMap.remove(cid);
break;
}
case CMD_CHANGE_USER_CONSTRAINT_COMPLETED:
{
B3_PROFILE("CMD_CHANGE_USER_CONSTRAINT_COMPLETED");
int cid = serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId;
b3UserConstraint* userConstraintPtr = m_data->m_userConstraintInfoMap[cid];
if (userConstraintPtr)
@@ -511,26 +539,31 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
case CMD_USER_CONSTRAINT_FAILED:
{
B3_PROFILE("CMD_USER_CONSTRAINT_FAILED");
b3Warning("createConstraint failed");
break;
}
case CMD_REMOVE_USER_CONSTRAINT_FAILED:
{
B3_PROFILE("CMD_REMOVE_USER_CONSTRAINT_FAILED");
b3Warning("removeConstraint failed");
break;
}
case CMD_CHANGE_USER_CONSTRAINT_FAILED:
{
B3_PROFILE("CMD_CHANGE_USER_CONSTRAINT_FAILED");
b3Warning("changeConstraint failed");
break;
}
case CMD_ACTUAL_STATE_UPDATE_FAILED:
{
B3_PROFILE("CMD_ACTUAL_STATE_UPDATE_FAILED");
b3Warning("request actual state failed");
break;
}
case CMD_BODY_INFO_COMPLETED:
{
B3_PROFILE("CMD_BODY_INFO_COMPLETED");
if (m_data->m_verboseOutput) {
b3Printf("Received body info\n");
}
@@ -541,12 +574,14 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
}
case CMD_MJCF_LOADING_FAILED:
{
B3_PROFILE("CMD_MJCF_LOADING_FAILED");
if (m_data->m_verboseOutput) {
b3Printf("Server failed loading the MJCF...\n");
}
break;
}
case CMD_SDF_LOADING_FAILED: {
B3_PROFILE("CMD_SDF_LOADING_FAILED");
if (m_data->m_verboseOutput) {
b3Printf("Server failed loading the SDF...\n");
}
@@ -555,6 +590,7 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
}
case CMD_BULLET_DATA_STREAM_RECEIVED_COMPLETED: {
B3_PROFILE("CMD_BULLET_DATA_STREAM_RECEIVED_COMPLETED");
if (m_data->m_verboseOutput) {
b3Printf("Server received bullet data stream OK\n");
}
@@ -562,6 +598,7 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
break;
}
case CMD_BULLET_DATA_STREAM_RECEIVED_FAILED: {
B3_PROFILE("CMD_BULLET_DATA_STREAM_RECEIVED_FAILED");
if (m_data->m_verboseOutput) {
b3Printf("Server failed receiving bullet data stream\n");
}
@@ -570,37 +607,33 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
}
case CMD_ACTUAL_STATE_UPDATE_COMPLETED: {
if (m_data->m_verboseOutput) {
B3_PROFILE("CMD_ACTUAL_STATE_UPDATE_COMPLETED");
if (m_data->m_verboseOutput)
{
b3Printf("Received actual state\n");
}
SharedMemoryStatus& 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;
if (m_data->m_verboseOutput) {
int numQ = command.m_sendActualStateArgs.m_numDegreeOfFreedomQ;
int numU = command.m_sendActualStateArgs.m_numDegreeOfFreedomU;
b3Printf("size Q = %d, size U = %d\n", numQ, numU);
}
char msg[1024];
char msg[1024];
{
sprintf(msg, "Q=[");
{
sprintf(msg, "Q=[");
for (int i = 0; i < numQ; i++) {
if (i < numQ - 1) {
sprintf(msg, "%s%f,", msg,
command.m_sendActualStateArgs.m_actualStateQ[i]);
} else {
sprintf(msg, "%s%f", msg,
command.m_sendActualStateArgs.m_actualStateQ[i]);
}
}
sprintf(msg, "%s]", msg);
}
if (m_data->m_verboseOutput) {
for (int i = 0; i < numQ; i++) {
if (i < numQ - 1) {
sprintf(msg, "%s%f,", msg,
command.m_sendActualStateArgs.m_actualStateQ[i]);
} else {
sprintf(msg, "%s%f", msg,
command.m_sendActualStateArgs.m_actualStateQ[i]);
}
}
sprintf(msg, "%s]", msg);
}
b3Printf(msg);
}
{
sprintf(msg, "U=[");
for (int i = 0; i < numU; i++) {
@@ -613,18 +646,15 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
}
}
sprintf(msg, "%s]", msg);
}
if (m_data->m_verboseOutput) {
b3Printf(msg);
}
if (m_data->m_verboseOutput) {
b3Printf("\n");
}
break;
}
case CMD_RESET_SIMULATION_COMPLETED: {
if (m_data->m_verboseOutput) {
B3_PROFILE("CMD_RESET_SIMULATION_COMPLETED");
if (m_data->m_verboseOutput) {
b3Printf("CMD_RESET_SIMULATION_COMPLETED clean data\n");
}
resetData();
@@ -632,6 +662,7 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
break;
}
case CMD_DEBUG_LINES_COMPLETED: {
B3_PROFILE("CMD_DEBUG_LINES_COMPLETED");
if (m_data->m_verboseOutput) {
b3Printf("Success receiving %d debug lines",
serverCmd.m_sendDebugLinesArgs.m_numDebugLines);
@@ -675,11 +706,13 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
}
case CMD_RIGID_BODY_CREATION_COMPLETED:
{
B3_PROFILE("CMD_RIGID_BODY_CREATION_COMPLETED");
break;
}
case CMD_DEBUG_LINES_OVERFLOW_FAILED: {
b3Warning("Error receiving debug lines");
B3_PROFILE("CMD_DEBUG_LINES_OVERFLOW_FAILED");
b3Warning("Error receiving debug lines");
m_data->m_debugLinesFrom.resize(0);
m_data->m_debugLinesTo.resize(0);
m_data->m_debugLinesColor.resize(0);
@@ -689,6 +722,7 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
case CMD_CAMERA_IMAGE_COMPLETED:
{
B3_PROFILE("CMD_CAMERA_IMAGE_COMPLETED");
if (m_data->m_verboseOutput)
{
b3Printf("Camera image OK\n");
@@ -738,7 +772,8 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
case CMD_CAMERA_IMAGE_FAILED:
{
b3Warning("Camera image FAILED\n");
B3_PROFILE("CMD_CAMERA_IMAGE_FAILED");
b3Warning("Camera image FAILED\n");
break;
}
case CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED:
@@ -772,6 +807,7 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
case CMD_REQUEST_VR_EVENTS_DATA_COMPLETED:
{
B3_PROFILE("CMD_REQUEST_VR_EVENTS_DATA_COMPLETED");
if (m_data->m_verboseOutput)
{
b3Printf("Request VR Events completed");
@@ -786,6 +822,7 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
case CMD_REQUEST_KEYBOARD_EVENTS_DATA_COMPLETED:
{
B3_PROFILE("CMD_REQUEST_KEYBOARD_EVENTS_DATA_COMPLETED");
if (m_data->m_verboseOutput)
{
b3Printf("Request keyboard events completed");
@@ -800,6 +837,7 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
case CMD_REQUEST_AABB_OVERLAP_COMPLETED:
{
B3_PROFILE("CMD_REQUEST_AABB_OVERLAP_COMPLETED");
if (m_data->m_verboseOutput)
{
b3Printf("Overlapping object request completed");
@@ -819,7 +857,8 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
}
case CMD_CONTACT_POINT_INFORMATION_COMPLETED:
{
if (m_data->m_verboseOutput)
B3_PROFILE("CMD_CONTACT_POINT_INFORMATION_COMPLETED");
if (m_data->m_verboseOutput)
{
b3Printf("Contact Point Information Request OK\n");
}
@@ -839,30 +878,38 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
}
case CMD_CONTACT_POINT_INFORMATION_FAILED:
{
b3Warning("Contact Point Information Request failed");
B3_PROFILE("CMD_CONTACT_POINT_INFORMATION_FAILED");
b3Warning("Contact Point Information Request failed");
break;
}
case CMD_SAVE_WORLD_COMPLETED:
break;
{
B3_PROFILE("CMD_SAVE_WORLD_COMPLETED");
break;
}
case CMD_SAVE_WORLD_FAILED:
{
b3Warning("Saving world failed");
B3_PROFILE("CMD_SAVE_WORLD_FAILED");
b3Warning("Saving world failed");
break;
}
case CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED:
{
break;
B3_PROFILE("CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED");
break;
}
case CMD_CALCULATE_INVERSE_KINEMATICS_FAILED:
{
b3Warning("Calculate Inverse Kinematics Request failed");
B3_PROFILE("CMD_CALCULATE_INVERSE_KINEMATICS_FAILED");
b3Warning("Calculate Inverse Kinematics Request failed");
break;
}
case CMD_VISUAL_SHAPE_INFO_COMPLETED:
{
if (m_data->m_verboseOutput)
B3_PROFILE("CMD_VISUAL_SHAPE_INFO_COMPLETED");
if (m_data->m_verboseOutput)
{
b3Printf("Visual Shape Information Request OK\n");
}
@@ -949,6 +996,7 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
}
};
m_data->m_testBlock1->m_numProcessedServerCommands++;
// we don't have more than 1 command outstanding (in total, either server or client)
btAssert(m_data->m_testBlock1->m_numProcessedServerCommands ==
@@ -964,6 +1012,7 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
if ((serverCmd.m_type == CMD_SDF_LOADING_COMPLETED) || (serverCmd.m_type == CMD_MJCF_LOADING_COMPLETED) || (serverCmd.m_type == CMD_SYNC_BODY_INFO_COMPLETED))
{
B3_PROFILE("CMD_LOADING_COMPLETED");
int numConstraints = serverCmd.m_sdfLoadedArgs.m_numUserConstraints;
for (int i=0;i<numConstraints;i++)
{
@@ -994,6 +1043,8 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
if (serverCmd.m_type == CMD_USER_CONSTRAINT_INFO_COMPLETED)
{
B3_PROFILE("CMD_USER_CONSTRAINT_INFO_COMPLETED");
if (m_data->m_constraintIdsRequestInfo.size())
{
int cid = m_data->m_constraintIdsRequestInfo[m_data->m_constraintIdsRequestInfo.size()-1];
@@ -1013,6 +1064,7 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
if (serverCmd.m_type == CMD_BODY_INFO_COMPLETED)
{
B3_PROFILE("CMD_BODY_INFO_COMPLETED");
//are there any bodies left to be processed?
if (m_data->m_bodyIdsRequestInfo.size())
{
@@ -1046,6 +1098,7 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
if (serverCmd.m_type == CMD_REQUEST_AABB_OVERLAP_COMPLETED)
{
B3_PROFILE("CMD_REQUEST_AABB_OVERLAP_COMPLETED2");
SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
if (serverCmd.m_sendOverlappingObjectsArgs.m_numRemainingOverlappingObjects > 0 && serverCmd.m_sendOverlappingObjectsArgs.m_numOverlappingObjectsCopied)
{
@@ -1058,6 +1111,7 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
if (serverCmd.m_type == CMD_CONTACT_POINT_INFORMATION_COMPLETED)
{
B3_PROFILE("CMD_CONTACT_POINT_INFORMATION_COMPLETED2");
SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
if (serverCmd.m_sendContactPointArgs.m_numRemainingContactPoints>0 && serverCmd.m_sendContactPointArgs.m_numContactPointsCopied)
{
@@ -1072,6 +1126,7 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
if (serverCmd.m_type == CMD_VISUAL_SHAPE_INFO_COMPLETED)
{
B3_PROFILE("CMD_VISUAL_SHAPE_INFO_COMPLETED2");
SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
if (serverCmd.m_sendVisualShapeArgs.m_numRemainingVisualShapes >0 && serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied)
{
@@ -1087,6 +1142,7 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
if (serverCmd.m_type == CMD_CAMERA_IMAGE_COMPLETED)
{
B3_PROFILE("CMD_CAMERA_IMAGE_COMPLETED2");
SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
if (serverCmd.m_sendPixelDataArguments.m_numRemainingPixels > 0 && serverCmd.m_sendPixelDataArguments.m_numPixelsCopied)
@@ -1110,7 +1166,9 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
}
if ((serverCmd.m_type == CMD_DEBUG_LINES_COMPLETED) &&
(serverCmd.m_sendDebugLinesArgs.m_numRemainingDebugLines > 0)) {
(serverCmd.m_sendDebugLinesArgs.m_numRemainingDebugLines > 0))
{
B3_PROFILE("CMD_DEBUG_LINES_COMPLETED2");
SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
// continue requesting debug lines for drawing
@@ -1125,7 +1183,10 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
return &m_data->m_lastServerStatus;
} else {
if (m_data->m_verboseOutput) {
if (m_data->m_verboseOutput)
{
B3_PROFILE("m_verboseOutput");
b3Printf("m_numServerStatus = %d, processed = %d\n",
m_data->m_testBlock1->m_numServerCommands,
m_data->m_testBlock1->m_numProcessedServerCommands);

166
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -1399,8 +1399,8 @@ bool PhysicsServerCommandProcessor::processImportedObjects(const char* fileName,
//todo: move these internal API called inside the 'ConvertURDF2Bullet' call, hidden from the user
int rootLinkIndex = u2b.getRootLinkIndex();
b3Printf("urdf root link index = %d\n",rootLinkIndex);
//int rootLinkIndex = u2b.getRootLinkIndex();
//b3Printf("urdf root link index = %d\n",rootLinkIndex);
MyMultiBodyCreator creation(m_data->m_guiHelper);
u2b.getRootTransformInWorld(rootTrans);
@@ -1540,7 +1540,7 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe
bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVector3& pos, const btQuaternion& orn,
bool useMultiBody, bool useFixedBase, int* bodyUniqueIdPtr, char* bufferServerToClient, int bufferSizeInBytes)
bool useMultiBody, bool useFixedBase, int* bodyUniqueIdPtr, char* bufferServerToClient, int bufferSizeInBytes, int flags)
{
BT_PROFILE("loadURDF");
btAssert(m_data->m_dynamicsWorld);
@@ -1598,7 +1598,7 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto
// printf("urdf root link index = %d\n",rootLinkIndex);
MyMultiBodyCreator creation(m_data->m_guiHelper);
ConvertURDF2Bullet(u2b,creation, tr,m_data->m_dynamicsWorld,useMultiBody,u2b.getPathPrefix());
ConvertURDF2Bullet(u2b,creation, tr,m_data->m_dynamicsWorld,useMultiBody,u2b.getPathPrefix(),flags);
for (int i=0;i<u2b.getNumAllocatedCollisionShapes();i++)
{
@@ -1761,6 +1761,7 @@ int PhysicsServerCommandProcessor::createBodyInfoStream(int bodyUniqueId, char*
bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes )
{
BT_PROFILE("processCommand");
bool hasStatus = false;
@@ -1821,6 +1822,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
#endif
case CMD_STATE_LOGGING:
{
BT_PROFILE("CMD_STATE_LOGGING");
serverStatusOut.m_type = CMD_STATE_LOGGING_FAILED;
hasStatus = true;
@@ -1937,6 +1940,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
case CMD_SET_VR_CAMERA_STATE:
{
BT_PROFILE("CMD_SET_VR_CAMERA_STATE");
if (clientCmd.m_updateFlags & VR_CAMERA_ROOT_POSITION)
{
@@ -1963,6 +1967,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
case CMD_REQUEST_VR_EVENTS_DATA:
{
BT_PROFILE("CMD_REQUEST_VR_EVENTS_DATA");
serverStatusOut.m_sendVREvents.m_numVRControllerEvents = 0;
for (int i=0;i<MAX_VR_CONTROLLERS;i++)
{
@@ -1984,6 +1990,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
case CMD_REQUEST_KEYBOARD_EVENTS_DATA:
{
BT_PROFILE("CMD_REQUEST_KEYBOARD_EVENTS_DATA");
serverStatusOut.m_sendKeyboardEvents.m_numKeyboardEvents = m_data->m_keyboardEvents.size();
if (serverStatusOut.m_sendKeyboardEvents.m_numKeyboardEvents>MAX_KEYBOARD_EVENTS)
{
@@ -2019,6 +2027,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
case CMD_REQUEST_RAY_CAST_INTERSECTIONS:
{
BT_PROFILE("CMD_REQUEST_RAY_CAST_INTERSECTIONS");
btVector3 rayFromWorld(clientCmd.m_requestRaycastIntersections.m_rayFromPosition[0],
clientCmd.m_requestRaycastIntersections.m_rayFromPosition[1],
clientCmd.m_requestRaycastIntersections.m_rayFromPosition[2]);
@@ -2078,6 +2088,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
};
case CMD_REQUEST_DEBUG_LINES:
{
BT_PROFILE("CMD_REQUEST_DEBUG_LINES");
int curFlags =m_data->m_remoteDebugDrawer->getDebugMode();
int debugMode = clientCmd.m_requestDebugLinesArguments.m_debugMode;//clientCmd.btIDebugDraw::DBG_DrawWireframe|btIDebugDraw::DBG_DrawAabb;
@@ -2147,7 +2159,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
case CMD_REQUEST_CAMERA_IMAGE_DATA:
{
BT_PROFILE("CMD_REQUEST_CAMERA_IMAGE_DATA");
int startPixelIndex = clientCmd.m_requestPixelDataArguments.m_startPixelIndex;
int width = clientCmd.m_requestPixelDataArguments.m_pixelWidth;
int height = clientCmd.m_requestPixelDataArguments.m_pixelHeight;
@@ -2276,6 +2288,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
case CMD_SYNC_BODY_INFO:
{
BT_PROFILE("CMD_SYNC_BODY_INFO");
int numHandles = m_data->getNumHandles();
int actualNumBodies = 0;
for (int i=0;i<numHandles;i++)
@@ -2304,6 +2318,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
case CMD_REQUEST_BODY_INFO:
{
BT_PROFILE("CMD_REQUEST_BODY_INFO");
const SdfRequestInfoArgs& sdfInfoArgs = clientCmd.m_sdfRequestInfoArgs;
//stream info into memory
int streamSizeInBytes = createBodyInfoStream(sdfInfoArgs.m_bodyUniqueId, bufferServerToClient, bufferSizeInBytes);
@@ -2317,6 +2333,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
case CMD_SAVE_WORLD:
{
BT_PROFILE("CMD_SAVE_WORLD");
///this is a very rudimentary way to save the state of the world, for scene authoring
///many todo's, for example save the state of motor controllers etc.
@@ -2552,6 +2570,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
case CMD_LOAD_SDF:
{
BT_PROFILE("CMD_LOAD_SDF");
const SdfArgs& sdfArgs = clientCmd.m_sdfArguments;
if (m_data->m_verboseOutput)
{
@@ -2584,7 +2604,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
case CMD_LOAD_URDF:
{
BT_PROFILE("CMD_LOAD_URDF");
const UrdfArgs& urdfArgs = clientCmd.m_urdfArguments;
if (m_data->m_verboseOutput)
{
@@ -2600,6 +2620,11 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
initialPos[1] = urdfArgs.m_initialPosition[1];
initialPos[2] = urdfArgs.m_initialPosition[2];
}
int urdfFlags = 0;
if (clientCmd.m_updateFlags & URDF_ARGS_HAS_CUSTOM_URDF_FLAGS)
{
urdfFlags = urdfArgs.m_urdfFlags;
}
if (clientCmd.m_updateFlags & URDF_ARGS_INITIAL_ORIENTATION)
{
initialOrn[0] = urdfArgs.m_initialOrientation[0];
@@ -2613,8 +2638,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
//load the actual URDF and send a report: completed or failed
bool completedOk = loadUrdf(urdfArgs.m_urdfFileName,
initialPos,initialOrn,
useMultiBody, useFixedBase,&bodyUniqueId, bufferServerToClient, bufferSizeInBytes);
useMultiBody, useFixedBase,&bodyUniqueId, bufferServerToClient, bufferSizeInBytes, urdfFlags);
if (completedOk)
{
@@ -2691,6 +2715,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
case CMD_CREATE_SENSOR:
{
BT_PROFILE("CMD_CREATE_SENSOR");
if (m_data->m_verboseOutput)
{
b3Printf("Processed CMD_CREATE_SENSOR");
@@ -2759,6 +2785,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
case CMD_SEND_DESIRED_STATE:
{
BT_PROFILE("CMD_SEND_DESIRED_STATE");
if (m_data->m_verboseOutput)
{
b3Printf("Processed CMD_SEND_DESIRED_STATE");
@@ -2951,6 +2978,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
case CMD_REQUEST_ACTUAL_STATE:
{
BT_PROFILE("CMD_REQUEST_ACTUAL_STATE");
if (m_data->m_verboseOutput)
{
b3Printf("Sending the actual state (Q,U)");
@@ -3024,6 +3052,20 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
serverCmd.m_sendActualStateArgs.m_actualStateQdot[5] = mb->getBaseOmega()[2];
totalDegreeOfFreedomU += 6;//3 linear and 3 angular DOF
}
btAlignedObjectArray<btVector3> omega;
btAlignedObjectArray<btVector3> linVel;
bool computeLinkVelocities = ((clientCmd.m_updateFlags & ACTUAL_STATE_COMPUTE_LINKVELOCITY)!=0);
if (computeLinkVelocities)
{
omega.resize(mb->getNumLinks()+1);
linVel.resize(mb->getNumLinks()+1);
{
B3_PROFILE("compTreeLinkVelocities");
mb->compTreeLinkVelocities(&omega[0], &linVel[0]);
}
}
for (int l=0;l<mb->getNumLinks();l++)
{
for (int d=0;d<mb->getLink(l).m_posVarCount;d++)
@@ -3073,28 +3115,46 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
//}
}
}
btVector3 linkLocalInertialOrigin = body->m_linkLocalInertialFrames[l].getOrigin();
btQuaternion linkLocalInertialRotation = body->m_linkLocalInertialFrames[l].getRotation();
btVector3 linkLocalInertialOrigin = body->m_linkLocalInertialFrames[l].getOrigin();
btQuaternion linkLocalInertialRotation = body->m_linkLocalInertialFrames[l].getRotation();
btVector3 linkCOMOrigin = mb->getLink(l).m_cachedWorldTransform.getOrigin();
btQuaternion linkCOMRotation = mb->getLink(l).m_cachedWorldTransform.getRotation();
btVector3 linkCOMOrigin = mb->getLink(l).m_cachedWorldTransform.getOrigin();
btQuaternion linkCOMRotation = mb->getLink(l).m_cachedWorldTransform.getRotation();
serverCmd.m_sendActualStateArgs.m_linkState[l*7+0] = linkCOMOrigin.getX();
serverCmd.m_sendActualStateArgs.m_linkState[l*7+1] = linkCOMOrigin.getY();
serverCmd.m_sendActualStateArgs.m_linkState[l*7+2] = linkCOMOrigin.getZ();
serverCmd.m_sendActualStateArgs.m_linkState[l*7+3] = linkCOMRotation.x();
serverCmd.m_sendActualStateArgs.m_linkState[l*7+4] = linkCOMRotation.y();
serverCmd.m_sendActualStateArgs.m_linkState[l*7+5] = linkCOMRotation.z();
serverCmd.m_sendActualStateArgs.m_linkState[l*7+6] = linkCOMRotation.w();
serverCmd.m_sendActualStateArgs.m_linkState[l*7+0] = linkCOMOrigin.getX();
serverCmd.m_sendActualStateArgs.m_linkState[l*7+1] = linkCOMOrigin.getY();
serverCmd.m_sendActualStateArgs.m_linkState[l*7+2] = linkCOMOrigin.getZ();
serverCmd.m_sendActualStateArgs.m_linkState[l*7+3] = linkCOMRotation.x();
serverCmd.m_sendActualStateArgs.m_linkState[l*7+4] = linkCOMRotation.y();
serverCmd.m_sendActualStateArgs.m_linkState[l*7+5] = linkCOMRotation.z();
serverCmd.m_sendActualStateArgs.m_linkState[l*7+6] = linkCOMRotation.w();
serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+0] = linkLocalInertialOrigin.getX();
serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+1] = linkLocalInertialOrigin.getY();
serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+2] = linkLocalInertialOrigin.getZ();
serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+3] = linkLocalInertialRotation.x();
serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+4] = linkLocalInertialRotation.y();
serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+5] = linkLocalInertialRotation.z();
serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+6] = linkLocalInertialRotation.w();
btVector3 worldLinVel(0,0,0);
btVector3 worldAngVel(0,0,0);
if (computeLinkVelocities)
{
const btMatrix3x3& linkRotMat = mb->getLink(l).m_cachedWorldTransform.getBasis();
worldLinVel = linkRotMat * linVel[l+1];
worldAngVel = linkRotMat * omega[l+1];
}
serverCmd.m_sendActualStateArgs.m_linkWorldVelocities[l*6+0] = worldLinVel[0];
serverCmd.m_sendActualStateArgs.m_linkWorldVelocities[l*6+1] = worldLinVel[1];
serverCmd.m_sendActualStateArgs.m_linkWorldVelocities[l*6+2] = worldLinVel[2];
serverCmd.m_sendActualStateArgs.m_linkWorldVelocities[l*6+3] = worldAngVel[0];
serverCmd.m_sendActualStateArgs.m_linkWorldVelocities[l*6+4] = worldAngVel[1];
serverCmd.m_sendActualStateArgs.m_linkWorldVelocities[l*6+5] = worldAngVel[2];
serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+0] = linkLocalInertialOrigin.getX();
serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+1] = linkLocalInertialOrigin.getY();
serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+2] = linkLocalInertialOrigin.getZ();
serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+3] = linkLocalInertialRotation.x();
serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+4] = linkLocalInertialRotation.y();
serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+5] = linkLocalInertialRotation.z();
serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+6] = linkLocalInertialRotation.w();
}
@@ -3157,6 +3217,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
case CMD_STEP_FORWARD_SIMULATION:
{
BT_PROFILE("CMD_STEP_FORWARD_SIMULATION");
if (m_data->m_verboseOutput)
{
@@ -3191,6 +3253,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
case CMD_REQUEST_INTERNAL_DATA:
{
BT_PROFILE("CMD_REQUEST_INTERNAL_DATA");
//todo: also check version etc?
SharedMemoryStatus& serverCmd = serverStatusOut;
@@ -3213,6 +3277,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
};
case CMD_SEND_PHYSICS_SIMULATION_PARAMETERS:
{
BT_PROFILE("CMD_SEND_PHYSICS_SIMULATION_PARAMETERS");
if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DELTA_TIME)
{
m_data->m_physicsDeltaTime = clientCmd.m_physSimParamArgs.m_deltaTime;
@@ -3288,6 +3354,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
};
case CMD_INIT_POSE:
{
BT_PROFILE("CMD_INIT_POSE");
if (m_data->m_verboseOutput)
{
b3Printf("Server Init Pose not implemented yet");
@@ -3436,7 +3504,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
case CMD_RESET_SIMULATION:
{
BT_PROFILE("CMD_RESET_SIMULATION");
resetSimulation();
SharedMemoryStatus& serverCmd =serverStatusOut;
@@ -3447,6 +3516,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
case CMD_CREATE_RIGID_BODY:
case CMD_CREATE_BOX_COLLISION_SHAPE:
{
BT_PROFILE("CMD_CREATE_RIGID_BODY");
btVector3 halfExtents(1,1,1);
if (clientCmd.m_updateFlags & BOX_SHAPE_HAS_HALF_EXTENTS)
{
@@ -3581,6 +3652,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
case CMD_PICK_BODY:
{
BT_PROFILE("CMD_PICK_BODY");
pickBody(btVector3(clientCmd.m_pickBodyArguments.m_rayFromWorld[0],
clientCmd.m_pickBodyArguments.m_rayFromWorld[1],
clientCmd.m_pickBodyArguments.m_rayFromWorld[2]),
@@ -3596,6 +3669,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
case CMD_MOVE_PICKED_BODY:
{
BT_PROFILE("CMD_MOVE_PICKED_BODY");
movePickedBody(btVector3(clientCmd.m_pickBodyArguments.m_rayFromWorld[0],
clientCmd.m_pickBodyArguments.m_rayFromWorld[1],
clientCmd.m_pickBodyArguments.m_rayFromWorld[2]),
@@ -3610,6 +3685,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
case CMD_REMOVE_PICKING_CONSTRAINT_BODY:
{
BT_PROFILE("CMD_REMOVE_PICKING_CONSTRAINT_BODY");
removePickingConstraint();
SharedMemoryStatus& serverCmd =serverStatusOut;
@@ -3619,6 +3695,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
case CMD_REQUEST_AABB_OVERLAP:
{
BT_PROFILE("CMD_REQUEST_AABB_OVERLAP");
SharedMemoryStatus& serverCmd = serverStatusOut;
int curObjectIndex = clientCmd.m_requestOverlappingObjectsArgs.m_startingOverlappingObjectIndex;
@@ -3676,6 +3753,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
case CMD_CONFIGURE_OPENGL_VISUALIZER:
{
BT_PROFILE("CMD_CONFIGURE_OPENGL_VISUALIZER");
SharedMemoryStatus& serverCmd = serverStatusOut;
serverCmd.m_type =CMD_CLIENT_COMMAND_COMPLETED;
@@ -3699,6 +3777,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
case CMD_REQUEST_CONTACT_POINT_INFORMATION:
{
BT_PROFILE("CMD_REQUEST_CONTACT_POINT_INFORMATION");
SharedMemoryStatus& serverCmd =serverStatusOut;
serverCmd.m_sendContactPointArgs.m_numContactPointsCopied = 0;
@@ -3738,6 +3817,12 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
{
linkIndexB = mblB->m_link;
objectIndexB = mblB->m_multiBody->getUserIndex2();
if (
(clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_B_FILTER) &&
clientCmd.m_requestContactPointArguments.m_linkIndexBIndexFilter != linkIndexB)
{
continue;
}
}
int objectIndexA = -1;
@@ -3750,8 +3835,13 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
if (mblA && mblA->m_multiBody)
{
linkIndexA = mblA->m_link;
objectIndexA = mblA->m_multiBody->getUserIndex2();
if (
(clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_A_FILTER) &&
clientCmd.m_requestContactPointArguments.m_linkIndexAIndexFilter != linkIndexA)
{
continue;
}
}
btAssert(bodyA || mblA);
@@ -4009,6 +4099,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
case CMD_CALCULATE_INVERSE_DYNAMICS:
{
BT_PROFILE("CMD_CALCULATE_INVERSE_DYNAMICS");
SharedMemoryStatus& serverCmd = serverStatusOut;
InternalBodyHandle* bodyHandle = m_data->getHandle(clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId);
if (bodyHandle && bodyHandle->m_multiBody)
@@ -4059,6 +4150,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
case CMD_CALCULATE_JACOBIAN:
{
BT_PROFILE("CMD_CALCULATE_JACOBIAN");
SharedMemoryStatus& serverCmd = serverStatusOut;
InternalBodyHandle* bodyHandle = m_data->getHandle(clientCmd.m_calculateJacobianArguments.m_bodyUniqueId);
if (bodyHandle && bodyHandle->m_multiBody)
@@ -4115,6 +4208,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
case CMD_APPLY_EXTERNAL_FORCE:
{
BT_PROFILE("CMD_APPLY_EXTERNAL_FORCE");
if (m_data->m_verboseOutput)
{
b3Printf("CMD_APPLY_EXTERNAL_FORCE clientCmd = %d\n", clientCmd.m_sequenceNumber);
@@ -4214,6 +4309,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
case CMD_USER_CONSTRAINT:
{
BT_PROFILE("CMD_USER_CONSTRAINT");
SharedMemoryStatus& serverCmd =serverStatusOut;
serverCmd.m_type = CMD_USER_CONSTRAINT_FAILED;
hasStatus = true;
@@ -4444,6 +4541,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
case CMD_CALCULATE_INVERSE_KINEMATICS:
{
BT_PROFILE("CMD_CALCULATE_INVERSE_KINEMATICS");
SharedMemoryStatus& serverCmd = serverStatusOut;
serverCmd.m_type = CMD_CALCULATE_INVERSE_KINEMATICS_FAILED;
@@ -4613,7 +4711,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
case CMD_REQUEST_VISUAL_SHAPE_INFO:
{
BT_PROFILE("CMD_REQUEST_VISUAL_SHAPE_INFO");
SharedMemoryStatus& serverCmd = serverStatusOut;
serverCmd.m_type = CMD_VISUAL_SHAPE_INFO_FAILED;
//retrieve the visual shape information for a specific body
@@ -4642,6 +4740,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
case CMD_UPDATE_VISUAL_SHAPE:
{
BT_PROFILE("CMD_UPDATE_VISUAL_SHAPE");
SharedMemoryStatus& serverCmd = serverStatusOut;
serverCmd.m_type = CMD_VISUAL_SHAPE_UPDATE_FAILED;
@@ -4654,6 +4753,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
case CMD_LOAD_TEXTURE:
{
BT_PROFILE("CMD_LOAD_TEXTURE");
SharedMemoryStatus& serverCmd = serverStatusOut;
serverCmd.m_type = CMD_LOAD_TEXTURE_FAILED;
@@ -4673,7 +4773,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
case CMD_LOAD_BULLET:
{
BT_PROFILE("CMD_LOAD_BULLET");
SharedMemoryStatus& serverCmd = serverStatusOut;
btBulletWorldImporter* importer = new btBulletWorldImporter(m_data->m_dynamicsWorld);
@@ -4744,6 +4844,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
case CMD_SAVE_BULLET:
{
BT_PROFILE("CMD_SAVE_BULLET");
SharedMemoryStatus& serverCmd = serverStatusOut;
FILE* f = fopen(clientCmd.m_fileArguments.m_fileName, "wb");
@@ -4763,6 +4864,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
case CMD_LOAD_MJCF:
{
BT_PROFILE("CMD_LOAD_MJCF");
SharedMemoryStatus& serverCmd = serverStatusOut;
serverCmd.m_type = CMD_MJCF_LOADING_FAILED;
const MjcfArgs& mjcfArgs = clientCmd.m_mjcfArguments;
@@ -4797,6 +4899,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
case CMD_USER_DEBUG_DRAW:
{
BT_PROFILE("CMD_USER_DEBUG_DRAW");
SharedMemoryStatus& serverCmd = serverStatusOut;
serverCmd.m_type = CMD_USER_DEBUG_DRAW_FAILED;
hasStatus = true;
@@ -4919,6 +5022,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
default:
{
BT_PROFILE("CMD_UNKNOWN");
b3Error("Unknown command encountered");
SharedMemoryStatus& serverCmd =serverStatusOut;
@@ -5313,7 +5417,7 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets()
btVector3 spawnDir = mat.getColumn(0);
btVector3 shiftPos = spawnDir*spawnDistance;
btVector3 spawnPos = gVRGripperPos + shiftPos;
loadUrdf("sphere_small.urdf", spawnPos, gVRGripperOrn, true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("sphere_small.urdf", spawnPos, gVRGripperOrn, true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size(),0);
//loadUrdf("lego/lego.urdf", spawnPos, gVRGripperOrn, true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
m_data->m_sphereId = bodyId;
InteralBodyData* parentBody = m_data->getHandle(bodyId);

2
examples/SharedMemory/PhysicsServerCommandProcessor.h
View File

@@ -35,7 +35,7 @@ protected:
bool loadSdf(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes, bool useMultiBody, int flags);
bool loadUrdf(const char* fileName, const class btVector3& pos, const class btQuaternion& orn,
bool useMultiBody, bool useFixedBase, int* bodyUniqueIdPtr, char* bufferServerToClient, int bufferSizeInBytes);
bool useMultiBody, bool useFixedBase, int* bodyUniqueIdPtr, char* bufferServerToClient, int bufferSizeInBytes, int flags=0);
bool loadMjcf(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes, bool useMultiBody, int flags);

42
examples/SharedMemory/PhysicsServerExample.cpp
View File

@@ -23,7 +23,6 @@
#include "../CommonInterfaces/CommonParameterInterface.h"
//@todo(erwincoumans) those globals are hacks for a VR demo, move this to Python/pybullet!
extern btVector3 gLastPickPos;
@@ -305,37 +304,46 @@ void MotionThreadFunc(void* userPtr,void* lsMemory)
double deltaTimeInSeconds = 0;
int numCmdSinceSleep1ms = 0;
unsigned long long int prevTime = clock.getTimeMicroseconds();
do
{
BT_PROFILE("loop");
{
BT_PROFILE("usleep(0)");
b3Clock::usleep(0);
}
if (gMaxNumCmdPer1ms>0)
{
if (numCmdSinceSleep1ms>gMaxNumCmdPer1ms)
if (gMaxNumCmdPer1ms>0)
{
if (numCmdSinceSleep1ms>gMaxNumCmdPer1ms)
{
BT_PROFILE("usleep(10)");
b3Clock::usleep(10);
numCmdSinceSleep1ms = 0;
sleepClock.reset();
}
}
if (sleepClock.getTimeMilliseconds()>1)
{
BT_PROFILE("usleep(10)");
b3Clock::usleep(10);
numCmdSinceSleep1ms = 0;
sleepClock.reset();
numCmdSinceSleep1ms = 0;
}
}
if (sleepClock.getTimeMilliseconds()>1)
{
sleepClock.reset();
numCmdSinceSleep1ms = 0;
}
unsigned long long int curTime = clock.getTimeMicroseconds();
unsigned long long int dtMicro = curTime - prevTime;
prevTime = curTime;
#if 1
double dt = double(dtMicro)/1000000.;
#else
double dt = double(clock.getTimeMicroseconds())/1000000.;
clock.reset();
#endif
deltaTimeInSeconds+= dt;
{
//process special controller commands, such as
//VR controller button press/release and controller motion
@@ -440,7 +448,6 @@ void MotionThreadFunc(void* userPtr,void* lsMemory)
args->m_csGUI->unlock();
{
BT_PROFILE("stepSimulationRealTime");
args->m_physicsServerPtr->stepSimulationRealTime(deltaTimeInSeconds, args->m_sendVrControllerEvents,numSendVrControllers, keyEvents, args->m_sendKeyEvents.size());
}
deltaTimeInSeconds = 0;
@@ -478,7 +485,6 @@ void MotionThreadFunc(void* userPtr,void* lsMemory)
args->m_csGUI->unlock();
{
BT_PROFILE("processClientCommands");
args->m_physicsServerPtr->processClientCommands();
numCmdSinceSleep1ms++;
}

1
examples/SharedMemory/PhysicsServerSharedMemory.cpp
View File

@@ -252,6 +252,7 @@ void PhysicsServerSharedMemory::processClientCommands()
if (m_data->m_testBlocks[block]->m_numClientCommands> m_data->m_testBlocks[block]->m_numProcessedClientCommands)
{
BT_PROFILE("processClientCommand");
//until we implement a proper ring buffer, we assume always maximum of 1 outstanding commands
btAssert(m_data->m_testBlocks[block]->m_numClientCommands==m_data->m_testBlocks[block]->m_numProcessedClientCommands+1);

3
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -79,6 +79,7 @@ enum EnumUrdfArgsUpdateFlags
URDF_ARGS_INITIAL_ORIENTATION=4,
URDF_ARGS_USE_MULTIBODY=8,
URDF_ARGS_USE_FIXED_BASE=16,
URDF_ARGS_HAS_CUSTOM_URDF_FLAGS = 32
};
@@ -89,6 +90,7 @@ struct UrdfArgs
double m_initialOrientation[4];
int m_useMultiBody;
int m_useFixedBase;
int m_urdfFlags;
};
struct MjcfArgs
@@ -381,6 +383,7 @@ struct SendActualStateArgs
double m_jointMotorForce[MAX_DEGREE_OF_FREEDOM];
double m_linkState[7*MAX_NUM_LINKS];
double m_linkWorldVelocities[6*MAX_NUM_LINKS];//linear velocity and angular velocity in world space (x/y/z each).
double m_linkLocalInertialFrames[7*MAX_NUM_LINKS];
};

39
examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp
View File

@@ -5,7 +5,7 @@
#include "PhysicsClientSharedMemory.h"
#include"../ExampleBrowser/InProcessExampleBrowser.h"
#include "Bullet3Common/b3Logging.h"
class InProcessPhysicsClientSharedMemoryMainThread : public PhysicsClientSharedMemory
{
btInProcessExampleBrowserMainThreadInternalData* m_data;
@@ -39,18 +39,33 @@ public:
// return non-null if there is a status, nullptr otherwise
virtual const struct SharedMemoryStatus* processServerStatus()
{
if (btIsExampleBrowserMainThreadTerminated(m_data))
{
PhysicsClientSharedMemory::disconnectSharedMemory();
}
unsigned long int ms = m_clock.getTimeMilliseconds();
if (ms>20)
{
m_clock.reset();
btUpdateInProcessExampleBrowserMainThread(m_data);
{
if (btIsExampleBrowserMainThreadTerminated(m_data))
{
PhysicsClientSharedMemory::disconnectSharedMemory();
}
}
b3Clock::usleep(0);
return PhysicsClientSharedMemory::processServerStatus();
{
unsigned long int ms = m_clock.getTimeMilliseconds();
if (ms>20)
{
B3_PROFILE("m_clock.reset()");
m_clock.reset();
btUpdateInProcessExampleBrowserMainThread(m_data);
}
}
{
b3Clock::usleep(0);
}
const SharedMemoryStatus* stat = 0;
{
stat = PhysicsClientSharedMemory::processServerStatus();
}
return stat;
}

26
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -4,7 +4,7 @@
#define SHARED_MEMORY_KEY 12347
///increase the SHARED_MEMORY_MAGIC_NUMBER whenever incompatible changes are made in the structures
///my convention is year/month/day/rev
#define SHARED_MEMORY_MAGIC_NUMBER 201703010
#define SHARED_MEMORY_MAGIC_NUMBER 201703024
enum EnumSharedMemoryClientCommand
{
@@ -175,9 +175,13 @@ struct b3JointInfo
int m_flags;
double m_jointDamping;
double m_jointFriction;
double m_parentFrame[7]; // position and orientation (quaternion)
double m_childFrame[7]; // ^^^
double m_jointAxis[3]; // joint axis in parent local frame
double m_jointLowerLimit;
double m_jointUpperLimit;
double m_jointMaxForce;
double m_jointMaxVelocity;
double m_parentFrame[7]; // position and orientation (quaternion)
double m_childFrame[7]; // ^^^
double m_jointAxis[3]; // joint axis in parent local frame
};
struct b3UserConstraint
@@ -378,6 +382,11 @@ struct b3VisualShapeInformation
struct b3VisualShapeData* m_visualShapeData;
};
enum eLinkStateFlags
{
ACTUAL_STATE_COMPUTE_LINKVELOCITY=1
};
///b3LinkState provides extra information such as the Cartesian world coordinates
///center of mass (COM) of the link, relative to the world reference frame.
///Orientation is a quaternion x,y,z,w
@@ -395,6 +404,10 @@ struct b3LinkState
///world position and orientation of the (URDF) link frame
double m_worldLinkFramePosition[3];
double m_worldLinkFrameOrientation[4];
double m_worldLinearVelocity[3]; //only valid when ACTUAL_STATE_COMPUTE_LINKVELOCITY is set (b3RequestActualStateCommandComputeLinkVelocity)
double m_worldAngularVelocity[3]; //only valid when ACTUAL_STATE_COMPUTE_LINKVELOCITY is set (b3RequestActualStateCommandComputeLinkVelocity)
};
//todo: discuss and decide about control mode and combinations
@@ -435,4 +448,9 @@ enum eCONNECT_METHOD {
eCONNECT_TCP = 5,
};
enum eURDF_Flags
{
URDF_USE_INERTIA_FROM_FILE=2,//sync with URDF2Bullet.h 'ConvertURDFFlags'
};
#endif//SHARED_MEMORY_PUBLIC_H

7
examples/SharedMemory/TinyRendererVisualShapeConverter.cpp
View File

@@ -220,7 +220,7 @@ void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPathPrefi
rad = visual->m_geometry.m_capsuleRadius;
} else {
tr = visual->m_linkLocalFrame;
len = visual->m_geometry.m_capsuleHalfHeight;
len = visual->m_geometry.m_capsuleHeight;
rad = visual->m_geometry.m_capsuleRadius;
}
visualShapeOut.m_localVisualFrame[0] = tr.getOrigin()[0];
@@ -414,6 +414,11 @@ void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPathPrefi
break;
} // case mesh
case URDF_GEOM_PLANE:
// TODO: plane in tiny renderer
// TODO: export visualShapeOut for external render
break;
default:
{
b3Warning("TinyRenderer: unknown visual geometry type %i\n", visual->m_geometry.m_type);