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

2
data/cube.urdf
View File

@@ -1,5 +1,5 @@
<?xml version="0.0" ?>
<robot name="cube.urdf">
<robot name="cube">
<link name="baseLink">
<contact>
<lateral_friction value="1.0"/>

2
data/plane.urdf
View File

@@ -1,5 +1,5 @@
<?xml version="0.0" ?>
<robot name="cube.urdf">
<robot name="plane">
<link name="planeLink">
<contact>
<lateral_friction value="1"/>

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);
@@ -309,6 +312,8 @@ int b3CreateSensorEnable6DofJointForceTorqueSensor(b3SharedMemoryCommandHandle c
///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);
int b3GetLinkState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int linkIndex, struct b3LinkState *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");

13
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -356,13 +356,20 @@ struct LoadBunnyArgs
double m_collisionMargin;
};
struct RequestActualStateArgs
struct RequestBodyNameArgs
{
int m_bodyUniqueId;
};
struct SendBodyNameArgs
{
char m_bodyName[MAX_FILENAME_LENGTH];
};
struct RequestActualStateArgs
{
int m_bodyUniqueId;
};
struct SendActualStateArgs
{
@@ -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
};

3
examples/pybullet/kuka_with_cube.py
View File

@@ -49,6 +49,9 @@ trailDuration = 15
logId = p.startStateLogging(p.STATE_LOGGING_GENERIC_ROBOT,"LOG0001.txt",[0,1,2])
for i in xrange(5):
print "Body %d's name is %s." % (i, p.getBodyName(i)[0])
while 1:
if (useRealTimeSimulation):
dt = datetime.now()

66
examples/pybullet/pybullet.c
View File

@@ -2232,6 +2232,69 @@ static PyObject* pybullet_getJointState(PyObject* self, PyObject* args, PyObject
return Py_None;
}
// Returns the name of a body given the bodyIndex
//
// Args:
// bodyIndex - integer indicating body in simulation
//
// The returned pylist includes the body name in string format
static PyObject* pybullet_getBodyName(PyObject* self, PyObject* args, PyObject* keywds)
{
{
int bodyUniqueId = -1;
b3PhysicsClientHandle sm = 0;
int physicsClientId = 0;
static char* kwlist[] = {"bodyUniqueId", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist, &bodyUniqueId, &physicsClientId))
{
return NULL;
}
sm = getPhysicsClient(physicsClientId);
if (sm == 0)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
{
int status_type = 0;
b3SharedMemoryCommandHandle cmd_handle;
b3SharedMemoryStatusHandle status_handle;
if (bodyUniqueId < 0)
{
PyErr_SetString(SpamError, "getBodyName failed; invalid bodyIndex");
return NULL;
}
cmd_handle =
b3RequestBodyNameCommandInit(sm, bodyUniqueId);
status_handle =
b3SubmitClientCommandAndWaitStatus(sm, cmd_handle);
status_type = b3GetStatusType(status_handle);
if (status_type != CMD_REQUEST_BODY_NAME_COMPLETED)
{
PyErr_SetString(SpamError, "getBodyName failed; invalid return status");
return NULL;
}
struct b3BodyInfo info;
if (b3GetBodyName(status_handle, &info))
{
PyObject* pyBodyNameInfo = PyTuple_New(1);
PyTuple_SetItem(pyBodyNameInfo, 0, PyString_FromString(info.m_bodyName));
return pyBodyNameInfo;
}
}
}
PyErr_SetString(SpamError, "Couldn't get body name");
return NULL;
}
static PyObject* pybullet_getLinkState(PyObject* self, PyObject* args, PyObject* keywds)
{
PyObject* pyLinkState;
@@ -5101,6 +5164,9 @@ static PyMethodDef SpamMethods[] = {
{"getJointState", (PyCFunction)pybullet_getJointState, METH_VARARGS | METH_KEYWORDS,
"Get the state (position, velocity etc) for a joint on a body."},
{"getBodyName", (PyCFunction)pybullet_getBodyName, METH_VARARGS | METH_KEYWORDS,
"Get the name of a body."},
{"getLinkState", (PyCFunction)pybullet_getLinkState, METH_VARARGS | METH_KEYWORDS,
"Provides extra information such as the Cartesian world coordinates"
" center of mass (COM) of the link, relative to the world reference"