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

Add the API to get the body name and pybullet example call.

Browse Source
This commit is contained in:
yunfeibai
2017-03-23 13:54:44 -07:00
parent 50f0cfca9e
commit d717b1db84
10 changed files with 146 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

25
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -539,6 +539,17 @@ int b3JointControlSetDesiredForceTorque(b3SharedMemoryCommandHandle commandHandl
return 0;
}
b3SharedMemoryCommandHandle b3RequestBodyNameCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_REQUEST_BODY_NAME;
command->m_requestBodyNameArguments.m_bodyUniqueId = bodyUniqueId;
return (b3SharedMemoryCommandHandle) command;
}
b3SharedMemoryCommandHandle b3RequestActualStateCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId)
{
@@ -1019,6 +1030,20 @@ int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle,
return true;
}
int b3GetBodyName(b3SharedMemoryStatusHandle statusHandle,
struct b3BodyInfo* info)
{
const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
const SendBodyNameArgs &args = status->m_sendBodyNameArgs;
btAssert(status->m_type == CMD_REQUEST_BODY_NAME_COMPLETED);
if (status->m_type != CMD_REQUEST_BODY_NAME_COMPLETED)
return false;
info->m_bodyName = args.m_bodyName;
return true;
}
int b3CanSubmitCommand(b3PhysicsClientHandle physClient)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;

5
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -57,6 +57,9 @@ int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle,
const double* actualStateQdot[],
const double* jointReactionForces[]);
int b3GetBodyName(b3SharedMemoryStatusHandle statusHandle,
struct b3BodyInfo* info);
///If you re-connected to an existing server, or server changed otherwise, sync the body info and user constraints etc.
b3SharedMemoryCommandHandle b3InitSyncBodyInfoCommand(b3PhysicsClientHandle physClient);
@@ -308,6 +311,8 @@ int b3CreateSensorEnable6DofJointForceTorqueSensor(b3SharedMemoryCommandHandle c
///b3CreateSensorEnableIMUForLink is not implemented yet.
///For now, if the IMU is located in the root link, use the root world transform to mimic an IMU.
int b3CreateSensorEnableIMUForLink(b3SharedMemoryCommandHandle commandHandle, int linkIndex, int enable);
b3SharedMemoryCommandHandle b3RequestBodyNameCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId);
b3SharedMemoryCommandHandle b3RequestActualStateCommandInit(b3PhysicsClientHandle physClient,int bodyUniqueId);
int b3GetJointState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int jointIndex, struct b3JointSensorState *state);

9
examples/SharedMemory/PhysicsClientSharedMemory.cpp
View File

@@ -939,6 +939,15 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
b3Warning("State Logging failed");
break;
}
case CMD_REQUEST_BODY_NAME_COMPLETED:
{
break;
}
case CMD_REQUEST_BODY_NAME_FAILED:
{
b3Warning("Request body name failed");
break;
}
default: {
b3Error("Unknown server status %d\n", serverCmd.m_type);
btAssert(0);

22
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -4906,8 +4906,26 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
break;
}
case CMD_REQUEST_BODY_NAME:
{
int bodyUniqueId = clientCmd.m_requestBodyNameArguments.m_bodyUniqueId;
InteralBodyData* body = m_data->getHandle(bodyUniqueId);
if (body)
{
SharedMemoryStatus& serverCmd = serverStatusOut;
serverCmd.m_type = CMD_REQUEST_BODY_NAME_COMPLETED;
strcpy(serverCmd.m_sendBodyNameArgs.m_bodyName, body->m_bodyName.c_str());
hasStatus = true;
}
else
{
b3Warning("The body name requested is not available");
SharedMemoryStatus& serverCmd = serverStatusOut;
serverCmd.m_type = CMD_REQUEST_BODY_NAME_FAILED;
hasStatus = true;
}
break;
}
default:
{
b3Error("Unknown command encountered");

15
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -356,14 +356,21 @@ struct LoadBunnyArgs
double m_collisionMargin;
};
struct RequestBodyNameArgs
{
int m_bodyUniqueId;
};
struct SendBodyNameArgs
{
char m_bodyName[MAX_FILENAME_LENGTH];
};
struct RequestActualStateArgs
{
int m_bodyUniqueId;
};
struct SendActualStateArgs
{
int m_bodyUniqueId;
@@ -713,6 +720,7 @@ struct SharedMemoryCommand
struct VRCameraState m_vrCameraStateArguments;
struct StateLoggingRequest m_stateLoggingArguments;
struct ConfigureOpenGLVisualizerRequest m_configureOpenGLVisualizerArguments;
struct RequestBodyNameArgs m_requestBodyNameArguments;
};
};
@@ -775,6 +783,7 @@ struct SharedMemoryStatus
struct SendKeyboardEvents m_sendKeyboardEvents;
struct SendRaycastHits m_raycastHits;
struct StateLoggingResultArgs m_stateLoggingResultArgs;
struct SendBodyNameArgs m_sendBodyNameArgs;
};
};

4
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -54,6 +54,7 @@ enum EnumSharedMemoryClientCommand
CMD_STATE_LOGGING,
CMD_CONFIGURE_OPENGL_VISUALIZER,
CMD_REQUEST_KEYBOARD_EVENTS_DATA,
CMD_REQUEST_BODY_NAME,
//don't go beyond this command!
CMD_MAX_CLIENT_COMMANDS,
@@ -133,6 +134,8 @@ enum EnumSharedMemoryServerStatus
CMD_STATE_LOGGING_FAILED,
CMD_REQUEST_KEYBOARD_EVENTS_DATA_COMPLETED,
CMD_REQUEST_KEYBOARD_EVENTS_DATA_FAILED,
CMD_REQUEST_BODY_NAME_COMPLETED,
CMD_REQUEST_BODY_NAME_FAILED,
//don't go beyond 'CMD_MAX_SERVER_COMMANDS!
CMD_MAX_SERVER_COMMANDS
};
@@ -197,6 +200,7 @@ struct b3UserConstraint
struct b3BodyInfo
{
const char* m_baseName;
const char* m_bodyName; // for btRigidBody, it does not have a base, but can still have a body name from urdf
};