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

Merge remote-tracking branch 'upstream/master' into multibody_cleanup

Browse Source
This commit is contained in:
Jeongseok Lee
2018-07-09 14:58:23 -07:00
parent 04556502f0 3d5c921495
commit 15258fbdcc
39 changed files with 5812 additions and 605 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
data/mjcf/capsule.xml
View File

@@ -5,7 +5,7 @@
<worldbody>
<light diffuse=".5 .5 .5" pos="0 0 3" dir="0 0 -1"/>
<geom type="plane" size="1 1 0.1" rgba=".9 0 0 1"/>
<body pos="0 0 1">
<body name="capsule" pos="0 0 1">
<joint type="free"/>
<geom name="aux_1_geom" size="0.05 0.1" type="capsule"/>
</body>

8
examples/SharedMemory/PhysicsClient.h
View File

@@ -75,10 +75,10 @@ public:
virtual void setTimeOut(double timeOutInSeconds) = 0;
virtual double getTimeOut() const = 0;
virtual bool getCachedUserData(int bodyUniqueId, int linkIndex, int userDataId, struct b3UserDataValue &valueOut) const = 0;
virtual int getCachedUserDataId(int bodyUniqueId, int linkIndex, const char *key) const = 0;
virtual int getNumUserData(int bodyUniqueId, int linkIndex) const = 0;
virtual void getUserDataInfo(int bodyUniqueId, int linkIndex, int userDataIndex, const char **keyOut, int *userDataIdOut) const = 0;
virtual bool getCachedUserData(int userDataId, struct b3UserDataValue &valueOut) const = 0;
virtual int getCachedUserDataId(int bodyUniqueId, int linkIndex, int visualShapeIndex, const char *key) const = 0;
virtual int getNumUserData(int bodyUniqueId) const = 0;
virtual void getUserDataInfo(int bodyUniqueId, int userDataIndex, const char **keyOut, int *userDataIdOut, int *linkIndexOut, int *visualShapeIndexOut) const = 0;
virtual void pushProfileTiming(const char* timingName)=0;
virtual void popProfileTiming()=0;

25
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -2848,7 +2848,7 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3InitSyncUserDataCommand(b3PhysicsCl
return (b3SharedMemoryCommandHandle) command;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3InitAddUserDataCommand(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, const char* key, UserDataValueType valueType, int valueLength, const void *valueData) {
B3_SHARED_API b3SharedMemoryCommandHandle b3InitAddUserDataCommand(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, int visualShapeIndex, const char* key, UserDataValueType valueType, int valueLength, const void *valueData) {
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(strlen(key) < MAX_USER_DATA_KEY_LENGTH);
b3Assert(cl);
@@ -2860,6 +2860,7 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3InitAddUserDataCommand(b3PhysicsCli
command->m_type = CMD_ADD_USER_DATA;
command->m_addUserDataRequestArgs.m_bodyUniqueId = bodyUniqueId;
command->m_addUserDataRequestArgs.m_linkIndex = linkIndex;
command->m_addUserDataRequestArgs.m_visualShapeIndex = visualShapeIndex;
command->m_addUserDataRequestArgs.m_valueType = valueType;
command->m_addUserDataRequestArgs.m_valueLength = valueLength;
strcpy(command->m_addUserDataRequestArgs.m_key, key);
@@ -2868,7 +2869,7 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3InitAddUserDataCommand(b3PhysicsCli
return (b3SharedMemoryCommandHandle) command;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3InitRemoveUserDataCommand(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, int userDataId) {
B3_SHARED_API b3SharedMemoryCommandHandle b3InitRemoveUserDataCommand(b3PhysicsClientHandle physClient, int userDataId) {
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
@@ -2876,29 +2877,27 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3InitRemoveUserDataCommand(b3Physics
b3Assert(command);
command->m_type = CMD_REMOVE_USER_DATA;
command->m_removeUserDataRequestArgs.m_bodyUniqueId = bodyUniqueId;
command->m_removeUserDataRequestArgs.m_linkIndex = linkIndex;
command->m_removeUserDataRequestArgs.m_userDataId = userDataId;
return (b3SharedMemoryCommandHandle) command;
}
B3_SHARED_API int b3GetUserData(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, int userDataId, struct b3UserDataValue *valueOut)
B3_SHARED_API int b3GetUserData(b3PhysicsClientHandle physClient, int userDataId, struct b3UserDataValue *valueOut)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
if (cl)
{
return cl->getCachedUserData(bodyUniqueId, linkIndex, userDataId, *valueOut);
return cl->getCachedUserData(userDataId, *valueOut);
}
return false;
}
B3_SHARED_API int b3GetUserDataId(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, const char *key)
B3_SHARED_API int b3GetUserDataId(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, int visualShapeIndex, const char *key)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
if (cl)
{
return cl->getCachedUserDataId(bodyUniqueId, linkIndex, key);
return cl->getCachedUserDataId(bodyUniqueId, linkIndex, visualShapeIndex, key);
}
return -1;
}
@@ -2909,27 +2908,27 @@ B3_SHARED_API int b3GetUserDataIdFromStatus(b3SharedMemoryStatusHandle statusHan
if (status)
{
btAssert(status->m_type == CMD_ADD_USER_DATA_COMPLETED);
return status->m_userDataResponseArgs.m_userDataGlobalId.m_userDataId;
return status->m_userDataResponseArgs.m_userDataId;
}
return -1;
}
B3_SHARED_API int b3GetNumUserData(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex)
B3_SHARED_API int b3GetNumUserData(b3PhysicsClientHandle physClient, int bodyUniqueId)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
if (cl)
{
return cl->getNumUserData(bodyUniqueId, linkIndex);
return cl->getNumUserData(bodyUniqueId);
}
return 0;
}
B3_SHARED_API void b3GetUserDataInfo(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, int userDataIndex, const char **keyOut, int *userDataIdOut)
B3_SHARED_API void b3GetUserDataInfo(b3PhysicsClientHandle physClient, int bodyUniqueId, int userDataIndex, const char **keyOut, int *userDataIdOut, int *linkIndexOut, int *visualShapeIndexOut)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
if (cl)
{
cl->getUserDataInfo(bodyUniqueId, linkIndex, userDataIndex, keyOut, userDataIdOut);
cl->getUserDataInfo(bodyUniqueId, userDataIndex, keyOut, userDataIdOut, linkIndexOut, visualShapeIndexOut);
}
}

12
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -115,14 +115,14 @@ B3_SHARED_API int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyUniqu
///user data handling
B3_SHARED_API b3SharedMemoryCommandHandle b3InitSyncUserDataCommand(b3PhysicsClientHandle physClient);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitAddUserDataCommand(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, const char* key, enum UserDataValueType valueType, int valueLength, const void *valueData);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitRemoveUserDataCommand(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, int userDataId);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitAddUserDataCommand(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, int visualShapeIndex, const char* key, enum UserDataValueType valueType, int valueLength, const void *valueData);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitRemoveUserDataCommand(b3PhysicsClientHandle physClient, int userDataId);
B3_SHARED_API int b3GetUserData(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, int userDataId, struct b3UserDataValue *valueOut);
B3_SHARED_API int b3GetUserDataId(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, const char *key);
B3_SHARED_API int b3GetUserData(b3PhysicsClientHandle physClient, int userDataId, struct b3UserDataValue *valueOut);
B3_SHARED_API int b3GetUserDataId(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, int visualShapeIndex, const char *key);
B3_SHARED_API int b3GetUserDataIdFromStatus(b3SharedMemoryStatusHandle statusHandle);
B3_SHARED_API int b3GetNumUserData(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex);
B3_SHARED_API void b3GetUserDataInfo(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, int userDataIndex, const char **keyOut, int *userDataIdOut);
B3_SHARED_API int b3GetNumUserData(b3PhysicsClientHandle physClient, int bodyUniqueId);
B3_SHARED_API void b3GetUserDataInfo(b3PhysicsClientHandle physClient, int bodyUniqueId, int userDataIndex, const char **keyOut, int *userDataIdOut, int *linkIndexOut, int *visualShapeIndexOut);
B3_SHARED_API b3SharedMemoryCommandHandle b3GetDynamicsInfoCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex);
///given a body unique id and link index, return the dynamics information. See b3DynamicsInfo in SharedMemoryPublic.h

155
examples/SharedMemory/PhysicsClientSharedMemory.cpp
View File

@@ -15,26 +15,12 @@
#include "LinearMath/btQuickprof.h"
struct UserDataCache
{
btHashMap<btHashInt, SharedMemoryUserData> m_userDataMap;
btHashMap<btHashString, int> m_keyToUserDataIdMap;
UserDataCache()
{
}
~UserDataCache()
{
}
};
struct BodyJointInfoCache
{
std::string m_baseName;
b3AlignedObjectArray<b3JointInfo> m_jointInfo;
std::string m_bodyName;
// Joint index -> user data.
btHashMap<btHashInt, UserDataCache> m_jointToUserDataMap;
btAlignedObjectArray<int> m_userDataIds;
~BodyJointInfoCache()
{
@@ -78,7 +64,10 @@ struct PhysicsClientSharedMemoryInternalData {
btAlignedObjectArray<int> m_bodyIdsRequestInfo;
btAlignedObjectArray<int> m_constraintIdsRequestInfo;
btAlignedObjectArray<b3UserDataGlobalIdentifier> m_userDataIdsRequestInfo;
btAlignedObjectArray<int> m_userDataIdsRequestInfo;
btHashMap<btHashInt, SharedMemoryUserData> m_userDataMap;
btHashMap<SharedMemoryUserDataHashKey, int> m_userDataHandleLookup;
SharedMemoryStatus m_tempBackupServerStatus;
@@ -243,6 +232,12 @@ void PhysicsClientSharedMemory::removeCachedBody(int bodyUniqueId)
BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId];
if (bodyJointsPtr && *bodyJointsPtr)
{
for(int i=0; i<(*bodyJointsPtr)->m_userDataIds.size(); i++) {
const int userDataId = (*bodyJointsPtr)->m_userDataIds[i];
SharedMemoryUserData *userData = m_data->m_userDataMap[userDataId];
m_data->m_userDataHandleLookup.remove(SharedMemoryUserDataHashKey(userData));
m_data->m_userDataMap.remove(userDataId);
}
delete (*bodyJointsPtr);
m_data->m_bodyJointMap.remove(bodyUniqueId);
}
@@ -264,6 +259,8 @@ void PhysicsClientSharedMemory::resetData()
}
m_data->m_bodyJointMap.clear();
m_data->m_userConstraintInfoMap.clear();
m_data->m_userDataHandleLookup.clear();
m_data->m_userDataMap.clear();
}
void PhysicsClientSharedMemory::setSharedMemoryKey(int key)
@@ -1409,25 +1406,26 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap.getAtIndex(i);
if (bodyJointsPtr && *bodyJointsPtr)
{
(*bodyJointsPtr)->m_jointToUserDataMap.clear();
(*bodyJointsPtr)->m_userDataIds.clear();
}
m_data->m_userDataMap.clear();
m_data->m_userDataHandleLookup.clear();
}
const int numIdentifiers = serverCmd.m_syncUserDataArgs.m_numUserDataIdentifiers;
if (numIdentifiers > 0) {
m_data->m_tempBackupServerStatus = m_data->m_lastServerStatus;
const b3UserDataGlobalIdentifier *identifiers = (b3UserDataGlobalIdentifier *)m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor;
for (int i=0; i<numIdentifiers; i++) {
const int *identifiers = (int *)m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor;
m_data->m_userDataIdsRequestInfo.reserve(numIdentifiers - 1);
// Store the identifiers that still need to be requested.
for (int i=0; i<numIdentifiers - 1; i++) {
m_data->m_userDataIdsRequestInfo.push_back(identifiers[i]);
}
// Request individual user data entries.
const b3UserDataGlobalIdentifier userDataGlobalId = m_data->m_userDataIdsRequestInfo[m_data->m_userDataIdsRequestInfo.size()-1];
m_data->m_userDataIdsRequestInfo.pop_back();
// Request individual user data entries, start with last identifier.
SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
command.m_type = CMD_REQUEST_USER_DATA;
command.m_userDataRequestArgs = userDataGlobalId;
command.m_userDataRequestArgs.m_userDataId = identifiers[numIdentifiers - 1];
submitClientCommand(command);
return 0;
}
@@ -1435,31 +1433,23 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
if (serverCmd.m_type == CMD_ADD_USER_DATA_COMPLETED || serverCmd.m_type == CMD_REQUEST_USER_DATA_COMPLETED) {
B3_PROFILE("CMD_ADD_USER_DATA_COMPLETED");
const b3UserDataGlobalIdentifier userDataGlobalId = serverCmd.m_userDataResponseArgs.m_userDataGlobalId;
BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[userDataGlobalId.m_bodyUniqueId];
const UserDataResponseArgs response = serverCmd.m_userDataResponseArgs;
BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[response.m_bodyUniqueId];
if (bodyJointsPtr && *bodyJointsPtr) {
UserDataCache* userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[userDataGlobalId.m_linkIndex];
if (!userDataCachePtr)
{
UserDataCache cache;
(*bodyJointsPtr)->m_jointToUserDataMap.insert(userDataGlobalId.m_linkIndex, cache);
}
userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[userDataGlobalId.m_linkIndex];
const char *dataStream = m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor;
SharedMemoryUserData* userDataPtr = (userDataCachePtr)->m_userDataMap[userDataGlobalId.m_userDataId];
if (userDataPtr) {
SharedMemoryUserData* userData = m_data->m_userDataMap[response.m_userDataId];
if (userData) {
// Only replace the value.
userDataPtr->replaceValue(dataStream, serverCmd.m_userDataResponseArgs.m_valueLength, serverCmd.m_userDataResponseArgs.m_valueType);
userData->replaceValue(dataStream, response.m_valueLength, response.m_valueType);
}
else {
// Add a new user data entry.
const char *key = serverCmd.m_userDataResponseArgs.m_key;
(userDataCachePtr)->m_userDataMap.insert(userDataGlobalId.m_userDataId, SharedMemoryUserData(key));
(userDataCachePtr)->m_keyToUserDataIdMap.insert(key, userDataGlobalId.m_userDataId);
userDataPtr = (userDataCachePtr)->m_userDataMap[userDataGlobalId.m_userDataId];
userDataPtr->replaceValue(dataStream, serverCmd.m_userDataResponseArgs.m_valueLength, serverCmd.m_userDataResponseArgs.m_valueType);
const char *key = response.m_key;
m_data->m_userDataMap.insert(response.m_userDataId, SharedMemoryUserData(key, response.m_bodyUniqueId, response.m_linkIndex, response.m_visualShapeIndex));
userData = m_data->m_userDataMap[response.m_userDataId];
userData->replaceValue(dataStream, response.m_valueLength, response.m_valueType);
m_data->m_userDataHandleLookup.insert(SharedMemoryUserDataHashKey(userData), response.m_userDataId);
(*bodyJointsPtr)->m_userDataIds.push_back(response.m_userDataId);
}
}
}
@@ -1468,12 +1458,12 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
if (m_data->m_userDataIdsRequestInfo.size() > 0) {
// Request individual user data entries.
const b3UserDataGlobalIdentifier userDataGlobalId = m_data->m_userDataIdsRequestInfo[m_data->m_userDataIdsRequestInfo.size()-1];
const int userDataId = m_data->m_userDataIdsRequestInfo[m_data->m_userDataIdsRequestInfo.size()-1];
m_data->m_userDataIdsRequestInfo.pop_back();
SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
command.m_type = CMD_REQUEST_USER_DATA;
command.m_userDataRequestArgs = userDataGlobalId;
command.m_userDataRequestArgs.m_userDataId = userDataId;
submitClientCommand(command);
return 0;
}
@@ -1482,19 +1472,15 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
if (serverCmd.m_type == CMD_REMOVE_USER_DATA_COMPLETED) {
B3_PROFILE("CMD_REMOVE_USER_DATA_COMPLETED");
const b3UserDataGlobalIdentifier userDataGlobalId = serverCmd.m_removeUserDataResponseArgs;
BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[userDataGlobalId.m_bodyUniqueId];
if (bodyJointsPtr && *bodyJointsPtr) {
UserDataCache* userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[userDataGlobalId.m_linkIndex];
if (userDataCachePtr)
{
SharedMemoryUserData *userDataPtr = (userDataCachePtr)->m_userDataMap[userDataGlobalId.m_userDataId];
if (userDataPtr)
{
(userDataCachePtr)->m_keyToUserDataIdMap.remove((userDataPtr)->m_key.c_str());
(userDataCachePtr)->m_userDataMap.remove(userDataGlobalId.m_userDataId);
}
const int userDataId = serverCmd.m_removeUserDataResponseArgs.m_userDataId;
SharedMemoryUserData *userData = m_data->m_userDataMap[userDataId];
if (userData) {
BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[userData->m_bodyUniqueId];
if (bodyJointsPtr && *bodyJointsPtr) {
(*bodyJointsPtr)->m_userDataIds.remove(userDataId);
}
m_data->m_userDataHandleLookup.remove(SharedMemoryUserDataHashKey(userData));
m_data->m_userDataMap.remove(userDataId);
}
}
@@ -1852,16 +1838,8 @@ double PhysicsClientSharedMemory::getTimeOut() const
return m_data->m_timeOutInSeconds;
}
bool PhysicsClientSharedMemory::getCachedUserData(int bodyUniqueId, int linkIndex, int userDataId, struct b3UserDataValue &valueOut) const {
BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId];
if (!bodyJointsPtr || !(*bodyJointsPtr)) {
return false;
}
UserDataCache* userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[linkIndex];
if (!userDataCachePtr) {
return false;
}
SharedMemoryUserData *userDataPtr = (userDataCachePtr)->m_userDataMap[userDataId];
bool PhysicsClientSharedMemory::getCachedUserData(int userDataId, struct b3UserDataValue &valueOut) const {
SharedMemoryUserData *userDataPtr = m_data->m_userDataMap[userDataId];
if (!userDataPtr)
{
return false;
@@ -1872,54 +1850,37 @@ bool PhysicsClientSharedMemory::getCachedUserData(int bodyUniqueId, int linkInde
return true;
}
int PhysicsClientSharedMemory::getCachedUserDataId(int bodyUniqueId, int linkIndex, const char *key) const {
BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId];
if (!bodyJointsPtr || !(*bodyJointsPtr)) {
return -1;
}
UserDataCache* userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[linkIndex];
if (!userDataCachePtr)
{
return -1;
}
int *userDataId = (userDataCachePtr)->m_keyToUserDataIdMap[key];
int PhysicsClientSharedMemory::getCachedUserDataId(int bodyUniqueId, int linkIndex, int visualShapeIndex, const char *key) const {
int* userDataId = m_data->m_userDataHandleLookup.find(SharedMemoryUserDataHashKey(key, bodyUniqueId, linkIndex, visualShapeIndex));
if (!userDataId) {
return -1;
}
return *userDataId;
}
int PhysicsClientSharedMemory::getNumUserData(int bodyUniqueId, int linkIndex) const {
int PhysicsClientSharedMemory::getNumUserData(int bodyUniqueId) const {
BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId];
if (!bodyJointsPtr || !(*bodyJointsPtr)) {
return 0;
}
UserDataCache* userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[linkIndex];
if (!userDataCachePtr)
{
return 0;
}
return (userDataCachePtr)->m_userDataMap.size();
return (*bodyJointsPtr)->m_userDataIds.size();
}
void PhysicsClientSharedMemory::getUserDataInfo(int bodyUniqueId, int linkIndex, int userDataIndex, const char **keyOut, int *userDataIdOut) const {
void PhysicsClientSharedMemory::getUserDataInfo(int bodyUniqueId, int userDataIndex, const char **keyOut, int *userDataIdOut, int *linkIndexOut, int *visualShapeIndexOut) const {
BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId];
if (!bodyJointsPtr || !(*bodyJointsPtr))
if (!bodyJointsPtr || !(*bodyJointsPtr) || userDataIndex < 0 || userDataIndex > (*bodyJointsPtr)->m_userDataIds.size())
{
*keyOut = 0;
*userDataIdOut = -1;
return;
}
UserDataCache* userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[linkIndex];
if (!userDataCachePtr || userDataIndex >= (userDataCachePtr)->m_userDataMap.size())
{
*keyOut = 0;
*userDataIdOut = -1;
return;
}
*userDataIdOut = (userDataCachePtr)->m_userDataMap.getKeyAtIndex(userDataIndex).getUid1();
SharedMemoryUserData *userDataPtr = (userDataCachePtr)->m_userDataMap.getAtIndex(userDataIndex);
*keyOut = (userDataPtr)->m_key.c_str();
int userDataId = (*bodyJointsPtr)->m_userDataIds[userDataIndex];
SharedMemoryUserData *userData = m_data->m_userDataMap[userDataId];
*userDataIdOut = userDataId;
*keyOut = userData->m_key.c_str();
*linkIndexOut = userData->m_linkIndex;
*visualShapeIndexOut = userData->m_visualShapeIndex;
}

8
examples/SharedMemory/PhysicsClientSharedMemory.h
View File

@@ -85,10 +85,10 @@ public:
virtual void setTimeOut(double timeOutInSeconds);
virtual double getTimeOut() const;
virtual bool getCachedUserData(int bodyUniqueId, int linkIndex, int userDataId, struct b3UserDataValue &valueOut) const;
virtual int getCachedUserDataId(int bodyUniqueId, int linkIndex, const char *key) const;
virtual int getNumUserData(int bodyUniqueId, int linkIndex) const;
virtual void getUserDataInfo(int bodyUniqueId, int linkIndex, int userDataIndex, const char **keyOut, int *userDataIdOut) const;
virtual bool getCachedUserData(int userDataId, struct b3UserDataValue &valueOut) const;
virtual int getCachedUserDataId(int bodyUniqueId, int linkIndex, int visualShapeIndex, const char *key) const;
virtual int getNumUserData(int bodyUniqueId) const;
virtual void getUserDataInfo(int bodyUniqueId, int userDataIndex, const char **keyOut, int *userDataIdOut, int *linkIndexOut, int *visualShapeIndexOut) const;
virtual void pushProfileTiming(const char* timingName);
virtual void popProfileTiming();

13
examples/SharedMemory/PhysicsClientTCP.cpp
View File

@@ -181,8 +181,17 @@ bool TcpNetworkedPhysicsProcessor::processCommand(const struct SharedMemoryComma
}
else
{
sz = sizeof(SharedMemoryCommand);
data = (unsigned char*)&clientCmd;
if (clientCmd.m_type == CMD_REQUEST_VR_EVENTS_DATA)
{
sz = 3 * sizeof(int) + sizeof(smUint64_t) + 16;
data = (unsigned char*)&clientCmd;
}
else
{
sz = sizeof(SharedMemoryCommand);
data = (unsigned char*)&clientCmd;
}
}
m_data->m_tcpSocket.Send((const uint8 *)data,sz);

141
examples/SharedMemory/PhysicsDirect.cpp
View File

@@ -16,24 +16,13 @@
#include "SharedMemoryUserData.h"
#include "LinearMath/btQuickprof.h"
struct UserDataCache {
btHashMap<btHashInt, SharedMemoryUserData> m_userDataMap;
btHashMap<btHashString, int> m_keyToUserDataIdMap;
~UserDataCache()
{
}
};
struct BodyJointInfoCache2
{
std::string m_baseName;
btAlignedObjectArray<b3JointInfo> m_jointInfo;
std::string m_bodyName;
// Joint index -> user data.
btHashMap<btHashInt, UserDataCache> m_jointToUserDataMap;
btAlignedObjectArray<int> m_userDataIds;
~BodyJointInfoCache2() {
}
@@ -85,6 +74,9 @@ struct PhysicsDirectInternalData
btAlignedObjectArray<b3RayHitInfo> m_raycastHits;
btHashMap<btHashInt, SharedMemoryUserData> m_userDataMap;
btHashMap<SharedMemoryUserDataHashKey, int> m_userDataHandleLookup;
PhysicsCommandProcessorInterface* m_commandProcessor;
bool m_ownsCommandProcessor;
double m_timeOutInSeconds;
@@ -685,34 +677,23 @@ void PhysicsDirect::processBodyJointInfo(int bodyUniqueId, const SharedMemorySta
}
void PhysicsDirect::processAddUserData(const struct SharedMemoryStatus& serverCmd) {
const b3UserDataGlobalIdentifier userDataGlobalId = serverCmd.m_userDataResponseArgs.m_userDataGlobalId;
BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[userDataGlobalId.m_bodyUniqueId];
if (bodyJointsPtr && *bodyJointsPtr)
{
UserDataCache* userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[userDataGlobalId.m_linkIndex];
if (!userDataCachePtr)
{
UserDataCache cache;
(*bodyJointsPtr)->m_jointToUserDataMap.insert(userDataGlobalId.m_linkIndex, cache);
}
userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[userDataGlobalId.m_linkIndex];
const UserDataResponseArgs response = serverCmd.m_userDataResponseArgs;
BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[response.m_bodyUniqueId];
if (bodyJointsPtr && *bodyJointsPtr) {
const char *dataStream = m_data->m_bulletStreamDataServerToClient;
b3UserDataValue userDataValue;
userDataValue.m_type = serverCmd.m_userDataResponseArgs.m_valueType;
userDataValue.m_length = serverCmd.m_userDataResponseArgs.m_valueLength;
SharedMemoryUserData *userDataPtr = userDataCachePtr->m_userDataMap[userDataGlobalId.m_userDataId];
if (userDataPtr) {
SharedMemoryUserData* userData = m_data->m_userDataMap[response.m_userDataId];
if (userData) {
// Only replace the value.
(userDataPtr)->replaceValue(dataStream,serverCmd.m_userDataResponseArgs.m_valueLength,userDataValue.m_type);
userData->replaceValue(dataStream, response.m_valueLength, response.m_valueType);
}
else {
// Add a new user data entry.
(userDataCachePtr)->m_userDataMap.insert(userDataGlobalId.m_userDataId, SharedMemoryUserData(serverCmd.m_userDataResponseArgs.m_key));
userDataPtr = (userDataCachePtr)->m_userDataMap[userDataGlobalId.m_userDataId];
userDataPtr->replaceValue(dataStream,serverCmd.m_userDataResponseArgs.m_valueLength,userDataValue.m_type);
(userDataCachePtr)->m_keyToUserDataIdMap.insert(serverCmd.m_userDataResponseArgs.m_key, userDataGlobalId.m_userDataId);
const char *key = response.m_key;
m_data->m_userDataMap.insert(response.m_userDataId, SharedMemoryUserData(key, response.m_bodyUniqueId, response.m_linkIndex, response.m_visualShapeIndex));
userData = m_data->m_userDataMap[response.m_userDataId];
userData->replaceValue(dataStream, response.m_valueLength, response.m_valueType);
m_data->m_userDataHandleLookup.insert(SharedMemoryUserDataHashKey(userData), response.m_userDataId);
(*bodyJointsPtr)->m_userDataIds.push_back(response.m_userDataId);
}
}
}
@@ -1152,16 +1133,18 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd
BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap.getAtIndex(i);
if (bodyJointsPtr && *bodyJointsPtr)
{
(*bodyJointsPtr)->m_jointToUserDataMap.clear();
(*bodyJointsPtr)->m_userDataIds.clear();
}
m_data->m_userDataMap.clear();
m_data->m_userDataHandleLookup.clear();
}
const int numIdentifiers = serverCmd.m_syncUserDataArgs.m_numUserDataIdentifiers;
b3UserDataGlobalIdentifier *identifiers = new b3UserDataGlobalIdentifier[numIdentifiers];
memcpy(identifiers, &m_data->m_bulletStreamDataServerToClient[0], numIdentifiers * sizeof(b3UserDataGlobalIdentifier));
int *identifiers = new int[numIdentifiers];
memcpy(identifiers, &m_data->m_bulletStreamDataServerToClient[0], numIdentifiers * sizeof(int));
for (int i=0; i<numIdentifiers; i++) {
m_data->m_tmpInfoRequestCommand.m_type = CMD_REQUEST_USER_DATA;
m_data->m_tmpInfoRequestCommand.m_userDataRequestArgs = identifiers[i];
m_data->m_tmpInfoRequestCommand.m_userDataRequestArgs.m_userDataId = identifiers[i];
bool hasStatus = m_data->m_commandProcessor->processCommand(m_data->m_tmpInfoRequestCommand, m_data->m_tmpInfoStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
@@ -1184,18 +1167,15 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd
}
case CMD_REMOVE_USER_DATA_COMPLETED:
{
const b3UserDataGlobalIdentifier userDataGlobalId = serverCmd.m_removeUserDataResponseArgs;
BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[userDataGlobalId.m_bodyUniqueId];
if (bodyJointsPtr && *bodyJointsPtr) {
UserDataCache *userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[userDataGlobalId.m_linkIndex];
if (userDataCachePtr)
{
SharedMemoryUserData* userDataPtr = (userDataCachePtr)->m_userDataMap[userDataGlobalId.m_userDataId];
if (userDataPtr) {
(userDataCachePtr)->m_keyToUserDataIdMap.remove((userDataPtr)->m_key.c_str());
(userDataCachePtr)->m_userDataMap.remove(userDataGlobalId.m_userDataId);
}
const int userDataId = serverCmd.m_removeUserDataResponseArgs.m_userDataId;
SharedMemoryUserData *userData = m_data->m_userDataMap[userDataId];
if (userData) {
BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[userData->m_bodyUniqueId];
if (bodyJointsPtr && *bodyJointsPtr) {
(*bodyJointsPtr)->m_userDataIds.remove(userDataId);
}
m_data->m_userDataHandleLookup.remove(SharedMemoryUserDataHashKey(userData));
m_data->m_userDataMap.remove(userDataId);
}
break;
}
@@ -1253,6 +1233,12 @@ void PhysicsDirect::removeCachedBody(int bodyUniqueId)
BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId];
if (bodyJointsPtr && *bodyJointsPtr)
{
for(int i=0; i<(*bodyJointsPtr)->m_userDataIds.size(); i++) {
const int userDataId = (*bodyJointsPtr)->m_userDataIds[i];
SharedMemoryUserData *userData = m_data->m_userDataMap[userDataId];
m_data->m_userDataHandleLookup.remove(SharedMemoryUserDataHashKey(userData));
m_data->m_userDataMap.remove(userDataId);
}
delete (*bodyJointsPtr);
m_data->m_bodyJointMap.remove(bodyUniqueId);
}
@@ -1499,72 +1485,49 @@ double PhysicsDirect::getTimeOut() const
return m_data->m_timeOutInSeconds;
}
bool PhysicsDirect::getCachedUserData(int bodyUniqueId, int linkIndex, int userDataId, struct b3UserDataValue &valueOut) const {
BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId];
if (!bodyJointsPtr || !(*bodyJointsPtr)) {
return false;
}
UserDataCache* userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[linkIndex];
if (!userDataCachePtr)
{
return false;
}
SharedMemoryUserData* userDataPtr = (userDataCachePtr)->m_userDataMap[userDataId];
bool PhysicsDirect::getCachedUserData(int userDataId, struct b3UserDataValue &valueOut) const {
SharedMemoryUserData *userDataPtr = m_data->m_userDataMap[userDataId];
if (!userDataPtr)
{
return false;
}
valueOut.m_type = userDataPtr->m_type;
valueOut.m_type = (userDataPtr)->m_type;
valueOut.m_length = userDataPtr->m_bytes.size();
valueOut.m_data1 = userDataPtr->m_bytes.size()? &userDataPtr->m_bytes[0] : 0;
return true;
}
int PhysicsDirect::getCachedUserDataId(int bodyUniqueId, int linkIndex, const char *key) const {
BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId];
if (!bodyJointsPtr || !(*bodyJointsPtr)) {
return -1;
}
UserDataCache* userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[linkIndex];
if (!userDataCachePtr) {
return -1;
}
int *userDataId = (userDataCachePtr)->m_keyToUserDataIdMap[key];
int PhysicsDirect::getCachedUserDataId(int bodyUniqueId, int linkIndex, int visualShapeIndex, const char *key) const {
int* userDataId = m_data->m_userDataHandleLookup.find(SharedMemoryUserDataHashKey(key, bodyUniqueId, linkIndex, visualShapeIndex));
if (!userDataId) {
return -1;
}
return *userDataId;
}
int PhysicsDirect::getNumUserData(int bodyUniqueId, int linkIndex) const {
int PhysicsDirect::getNumUserData(int bodyUniqueId) const {
BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId];
if (!bodyJointsPtr || !(*bodyJointsPtr)) {
return 0;
}
UserDataCache* userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[linkIndex];
if (!userDataCachePtr) {
return 0;
}
return (userDataCachePtr)->m_userDataMap.size();
return (*bodyJointsPtr)->m_userDataIds.size();
}
void PhysicsDirect::getUserDataInfo(int bodyUniqueId, int linkIndex, int userDataIndex, const char **keyOut, int *userDataIdOut) const {
void PhysicsDirect::getUserDataInfo(int bodyUniqueId, int userDataIndex, const char **keyOut, int *userDataIdOut, int *linkIndexOut, int *visualShapeIndexOut) const {
BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId];
if (!bodyJointsPtr || !(*bodyJointsPtr)) {
*keyOut = 0;
*userDataIdOut = -1;
return;
}
UserDataCache* userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[linkIndex];
if (!userDataCachePtr || userDataIndex >= (userDataCachePtr)->m_userDataMap.size())
if (!bodyJointsPtr || !(*bodyJointsPtr) || userDataIndex <= 0 || userDataIndex > (*bodyJointsPtr)->m_userDataIds.size())
{
*keyOut = 0;
*userDataIdOut = -1;
return;
}
*userDataIdOut = (userDataCachePtr)->m_userDataMap.getKeyAtIndex(userDataIndex).getUid1();
SharedMemoryUserData* userDataPtr = (userDataCachePtr)->m_userDataMap.getAtIndex(userDataIndex);
*keyOut = (userDataPtr)->m_key.c_str();
int userDataId = (*bodyJointsPtr)->m_userDataIds[userDataIndex];
SharedMemoryUserData *userData = m_data->m_userDataMap[userDataId];
*userDataIdOut = userDataId;
*keyOut = userData->m_key.c_str();
*linkIndexOut = userData->m_linkIndex;
*visualShapeIndexOut = userData->m_visualShapeIndex;
}

8
examples/SharedMemory/PhysicsDirect.h
View File

@@ -115,10 +115,10 @@ public:
virtual void setTimeOut(double timeOutInSeconds);
virtual double getTimeOut() const;
virtual bool getCachedUserData(int bodyUniqueId, int linkIndex, int userDataId, struct b3UserDataValue &valueOut) const;
virtual int getCachedUserDataId(int bodyUniqueId, int linkIndex, const char *key) const;
virtual int getNumUserData(int bodyUniqueId, int linkIndex) const;
virtual void getUserDataInfo(int bodyUniqueId, int linkIndex, int userDataIndex, const char **keyOut, int *userDataIdOut) const;
virtual bool getCachedUserData(int userDataId, struct b3UserDataValue &valueOut) const;
virtual int getCachedUserDataId(int bodyUniqueId, int linkIndex, int visualShapeIndex, const char *key) const;
virtual int getNumUserData(int bodyUniqueId) const;
virtual void getUserDataInfo(int bodyUniqueId, int userDataIndex, const char **keyOut, int *userDataIdOut, int *linkIndexOut, int *visualShapeIndexOut) const;
virtual void pushProfileTiming(const char* timingName);
virtual void popProfileTiming();

18
examples/SharedMemory/PhysicsLoopBack.cpp
View File

@@ -231,20 +231,20 @@ double PhysicsLoopBack::getTimeOut() const
return m_data->m_physicsClient->getTimeOut();
}
bool PhysicsLoopBack::getCachedUserData(int bodyUniqueId, int linkIndex, int userDataId, struct b3UserDataValue &valueOut) const {
return m_data->m_physicsClient->getCachedUserData(bodyUniqueId, linkIndex, userDataId, valueOut);
bool PhysicsLoopBack::getCachedUserData(int userDataId, struct b3UserDataValue &valueOut) const {
return m_data->m_physicsClient->getCachedUserData(userDataId, valueOut);
}
int PhysicsLoopBack::getCachedUserDataId(int bodyUniqueId, int linkIndex, const char *key) const {
return m_data->m_physicsClient->getCachedUserDataId(bodyUniqueId, linkIndex, key);
int PhysicsLoopBack::getCachedUserDataId(int bodyUniqueId, int linkIndex, int visualShapeIndex, const char *key) const {
return m_data->m_physicsClient->getCachedUserDataId(bodyUniqueId, linkIndex, visualShapeIndex, key);
}
int PhysicsLoopBack::getNumUserData(int bodyUniqueId, int linkIndex) const {
return m_data->m_physicsClient->getNumUserData(bodyUniqueId, linkIndex);
int PhysicsLoopBack::getNumUserData(int bodyUniqueId) const {
return m_data->m_physicsClient->getNumUserData(bodyUniqueId);
}
void PhysicsLoopBack::getUserDataInfo(int bodyUniqueId, int linkIndex, int userDataIndex, const char **keyOut, int *userDataIdOut) const {
m_data->m_physicsClient->getUserDataInfo(bodyUniqueId, linkIndex, userDataIndex, keyOut, userDataIdOut);
void PhysicsLoopBack::getUserDataInfo(int bodyUniqueId, int userDataIndex, const char **keyOut, int *userDataIdOut, int *linkIndexOut, int *visualShapeIndexOut) const {
m_data->m_physicsClient->getUserDataInfo(bodyUniqueId, userDataIndex, keyOut, userDataIdOut, linkIndexOut, visualShapeIndexOut);
}
void PhysicsLoopBack::pushProfileTiming(const char* timingName)
@@ -254,4 +254,4 @@ void PhysicsLoopBack::pushProfileTiming(const char* timingName)
void PhysicsLoopBack::popProfileTiming()
{
m_data->m_physicsClient->popProfileTiming();
}
}

8
examples/SharedMemory/PhysicsLoopBack.h
View File

@@ -89,10 +89,10 @@ public:
virtual void setTimeOut(double timeOutInSeconds);
virtual double getTimeOut() const;
virtual bool getCachedUserData(int bodyUniqueId, int linkIndex, int userDataId, struct b3UserDataValue &valueOut) const;
virtual int getCachedUserDataId(int bodyUniqueId, int linkIndex, const char *key) const;
virtual int getNumUserData(int bodyUniqueId, int linkIndex) const;
virtual void getUserDataInfo(int bodyUniqueId, int linkIndex, int userDataIndex, const char **keyOut, int *userDataIdOut) const;
virtual bool getCachedUserData(int userDataId, struct b3UserDataValue &valueOut) const;
virtual int getCachedUserDataId(int bodyUniqueId, int linkIndex, int visualShapeIndex, const char *key) const;
virtual int getNumUserData(int bodyUniqueId) const;
virtual void getUserDataInfo(int bodyUniqueId, int userDataIndex, const char **keyOut, int *userDataIdOut, int *linkIndexOut, int *visualShapeIndexOut) const;
virtual void pushProfileTiming(const char* timingName);
virtual void popProfileTiming();

267
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -222,72 +222,6 @@ struct InternalCollisionShapeData
#include "SharedMemoryUserData.h"
/**
* Holds all custom user data entries for a link.
*/
struct InternalLinkUserData {
// Used to look up user data entry handles for string keys.
btHashMap<btHashString, int> m_keyToHandleMap;
b3ResizablePool<b3PoolBodyHandle<SharedMemoryUserData> > m_userData;
// Adds or replaces a user data entry.
// Returns the user data handle.
const int add(const char* key, const char* bytes, int len, int type)
{
// If an entry with the key already exists, just update the value.
int userDataId = getUserDataId(key);
if (userDataId != -1) {
SharedMemoryUserData* userData = m_userData.getHandle(userDataId);
b3Assert(userData);
userData->replaceValue(bytes, len, type);
return userDataId;
}
userDataId = m_userData.allocHandle();
SharedMemoryUserData* userData = m_userData.getHandle(userDataId);
userData->m_key = key;
userData->replaceValue(bytes, len, type);
m_keyToHandleMap.insert(userData->m_key.c_str(), userDataId);
return userDataId;
}
// Returns the user data handle for a specified key or -1 if not found.
const int getUserDataId(const char* key) const
{
const int* userDataIdPtr = m_keyToHandleMap.find(key);
if (userDataIdPtr)
{
return *userDataIdPtr;
}
return -1;
}
// Removes a user data entry given the handle.
// Returns true when the entry was removed, false otherwise.
const bool remove(int userDataId)
{
const SharedMemoryUserData* userData = m_userData.getHandle(userDataId);
if (!userData)
{
return false;
}
m_keyToHandleMap.remove(userData->m_key.c_str());
m_userData.freeHandle(userDataId);
return true;
}
// Returns the user data given the user data id. null otherwise.
const SharedMemoryUserData* getUserData(const int userDataId) const
{
return m_userData.getHandle(userDataId);
}
void getUserDataIds(b3AlignedObjectArray<int> &userDataIds) const
{
m_userData.getUsedHandles(userDataIds);
}
};
struct InternalBodyData
{
btMultiBody* m_multiBody;
@@ -303,7 +237,7 @@ struct InternalBodyData
btAlignedObjectArray<btGeneric6DofSpring2Constraint*> m_rigidBodyJoints;
btAlignedObjectArray<std::string> m_rigidBodyJointNames;
btAlignedObjectArray<std::string> m_rigidBodyLinkNames;
btHashMap<btHashInt, InternalLinkUserData*> m_linkUserDataMap;
btAlignedObjectArray<int> m_userDataHandles;
#ifdef B3_ENABLE_TINY_AUDIO
b3HashMap<btHashInt, SDFAudioSource> m_audioSources;
@@ -328,10 +262,7 @@ struct InternalBodyData
m_rigidBodyJoints.clear();
m_rigidBodyJointNames.clear();
m_rigidBodyLinkNames.clear();
for(int i=0; i<m_linkUserDataMap.size(); i++) {
delete *m_linkUserDataMap.getAtIndex(i);
}
m_linkUserDataMap.clear();
m_userDataHandles.clear();
}
};
@@ -1607,8 +1538,8 @@ struct PhysicsServerCommandProcessorInternalData
b3ResizablePool< InternalBodyHandle > m_bodyHandles;
b3ResizablePool<InternalCollisionShapeHandle> m_userCollisionShapeHandles;
b3ResizablePool<InternalVisualShapeHandle> m_userVisualShapeHandles;
b3ResizablePool<b3PoolBodyHandle<SharedMemoryUserData> > m_userDataHandles;
btHashMap<SharedMemoryUserDataHashKey, int> m_userDataHandleLookup;
b3PluginManager m_pluginManager;
@@ -2719,7 +2650,8 @@ bool PhysicsServerCommandProcessor::processImportedObjects(const char* fileName,
SaveWorldObjectData sd;
sd.m_fileName = fileName;
int currentOpenGLTextureIndex = 0;
for (int m =0; m<u2b.getNumModels();m++)
{
@@ -2885,6 +2817,44 @@ bool PhysicsServerCommandProcessor::processImportedObjects(const char* fileName,
}
}
{
int startShapeIndex = 0;
if (m_data->m_pluginManager.getRenderInterface())
{
int totalNumVisualShapes = m_data->m_pluginManager.getRenderInterface()->getNumVisualShapes(bodyUniqueId);
//int totalBytesPerVisualShape = sizeof (b3VisualShapeData);
//int visualShapeStorage = bufferSizeInBytes / totalBytesPerVisualShape - 1;
b3VisualShapeData tmpShape;
int remain = totalNumVisualShapes - startShapeIndex;
int shapeIndex = startShapeIndex;
int success = m_data->m_pluginManager.getRenderInterface()->getVisualShapesData(bodyUniqueId,shapeIndex,&tmpShape);
if (success)
{
if (tmpShape.m_tinyRendererTextureId>=0)
{
int openglTextureUniqueId = -1;
//find companion opengl texture unique id and create a 'textureUid'
if (currentOpenGLTextureIndex<u2b.getNumAllocatedTextures())
{
openglTextureUniqueId = u2b.getAllocatedTexture(currentOpenGLTextureIndex);
currentOpenGLTextureIndex++;
}
int texHandle = m_data->m_textureHandles.allocHandle();
InternalTextureHandle* texH = m_data->m_textureHandles.getHandle(texHandle);
if(texH)
{
texH->m_tinyRendererTextureId = tmpShape.m_tinyRendererTextureId;
texH->m_openglTextureId = openglTextureUniqueId;
}
}
}
}
}
}
@@ -2893,8 +2863,16 @@ bool PhysicsServerCommandProcessor::processImportedObjects(const char* fileName,
int texId = u2b.getAllocatedTexture(i);
m_data->m_allocatedTextures.push_back(texId);
}
/*
int texHandle = m_data->m_textureHandles.allocHandle();
InternalTextureHandle* texH = m_data->m_textureHandles.getHandle(texHandle);
if(texH)
{
texH->m_tinyRendererTextureId = -1;
texH->m_openglTextureId = -1;
*/
for (int i=0;i<u2b.getNumAllocatedMeshInterfaces();i++)
{
@@ -5042,34 +5020,11 @@ bool PhysicsServerCommandProcessor::processSyncUserDataCommand(const struct Shar
bool hasStatus = true;
BT_PROFILE("CMD_SYNC_USER_DATA");
b3UserDataGlobalIdentifier *userDataIdentifiers = (b3UserDataGlobalIdentifier *)bufferServerToClient;
int numIdentifiers = 0;
b3AlignedObjectArray<int> bodyHandles;
m_data->m_bodyHandles.getUsedHandles(bodyHandles);
for (int i=0; i<bodyHandles.size(); i++) {
int bodyHandle = bodyHandles[i];
InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyHandle);
if (!body) {
continue;
}
for (int j=0; j<body->m_linkUserDataMap.size(); j++) {
const int linkIndex = body->m_linkUserDataMap.getKeyAtIndex(j).getUid1();
InternalLinkUserData **userDataPtr = body->m_linkUserDataMap.getAtIndex(j);
if (!userDataPtr) {
continue;
}
b3AlignedObjectArray<int> userDataIds;
(*userDataPtr)->getUserDataIds(userDataIds);
for (int k=0; k<userDataIds.size(); k++) {
b3UserDataGlobalIdentifier &identifier = userDataIdentifiers[numIdentifiers++];
identifier.m_bodyUniqueId = bodyHandle;
identifier.m_linkIndex = linkIndex;
identifier.m_userDataId = userDataIds[k];
}
}
}
b3AlignedObjectArray<int> userDataHandles;
m_data->m_userDataHandles.getUsedHandles(userDataHandles);
memcpy(bufferServerToClient, &userDataHandles[0], sizeof(int) * userDataHandles.size());
serverStatusOut.m_syncUserDataArgs.m_numUserDataIdentifiers = numIdentifiers;
serverStatusOut.m_syncUserDataArgs.m_numUserDataIdentifiers = userDataHandles.size();
serverStatusOut.m_type = CMD_SYNC_USER_DATA_COMPLETED;
return hasStatus;
}
@@ -5080,21 +5035,16 @@ bool PhysicsServerCommandProcessor::processRequestUserDataCommand(const struct S
BT_PROFILE("CMD_REQUEST_USER_DATA");
serverStatusOut.m_type = CMD_REQUEST_USER_DATA_FAILED;
InternalBodyData *body = m_data->m_bodyHandles.getHandle(clientCmd.m_userDataRequestArgs.m_bodyUniqueId);
if (!body) {
return hasStatus;
}
const int linkIndex = clientCmd.m_userDataRequestArgs.m_linkIndex;
InternalLinkUserData **userDataPtr = body->m_linkUserDataMap[linkIndex];
if (!userDataPtr) {
return hasStatus;
}
const SharedMemoryUserData *userData = (*userDataPtr)->getUserData(clientCmd.m_userDataRequestArgs.m_userDataId);
SharedMemoryUserData *userData = m_data->m_userDataHandles.getHandle(clientCmd.m_userDataRequestArgs.m_userDataId);
if (!userData) {
return hasStatus;
}
btAssert(bufferSizeInBytes >= userData->m_bytes.size());
serverStatusOut.m_userDataResponseArgs.m_userDataGlobalId = clientCmd.m_userDataRequestArgs;
btAssert(bufferSizeInBytes >= userData->m_bytes.size());
serverStatusOut.m_userDataResponseArgs.m_userDataId = clientCmd.m_userDataRequestArgs.m_userDataId;
serverStatusOut.m_userDataResponseArgs.m_bodyUniqueId = userData->m_bodyUniqueId;
serverStatusOut.m_userDataResponseArgs.m_linkIndex = userData->m_linkIndex;
serverStatusOut.m_userDataResponseArgs.m_visualShapeIndex = userData->m_visualShapeIndex;
serverStatusOut.m_userDataResponseArgs.m_valueType = userData->m_type;
serverStatusOut.m_userDataResponseArgs.m_valueLength = userData->m_bytes.size();
serverStatusOut.m_type = CMD_REQUEST_USER_DATA_COMPLETED;
@@ -5117,27 +5067,43 @@ bool PhysicsServerCommandProcessor::processAddUserDataCommand(const struct Share
{
return hasStatus;
}
InternalBodyData *body = m_data->m_bodyHandles.getHandle(clientCmd.m_addUserDataRequestArgs.m_bodyUniqueId);
if (!body) {
return hasStatus;
}
const int linkIndex = clientCmd.m_addUserDataRequestArgs.m_linkIndex;
InternalLinkUserData **userDataPtr = body->m_linkUserDataMap[linkIndex];
if (!userDataPtr) {
InternalLinkUserData *userData = new InternalLinkUserData;
userDataPtr = &userData;
body->m_linkUserDataMap.insert(linkIndex, userData);
SharedMemoryUserDataHashKey userDataIdentifier(
clientCmd.m_addUserDataRequestArgs.m_key,
clientCmd.m_addUserDataRequestArgs.m_bodyUniqueId,
clientCmd.m_addUserDataRequestArgs.m_linkIndex,
clientCmd.m_addUserDataRequestArgs.m_visualShapeIndex);
int* userDataHandlePtr = m_data->m_userDataHandleLookup.find(userDataIdentifier);
int userDataHandle = userDataHandlePtr ? *userDataHandlePtr : m_data->m_userDataHandles.allocHandle();
SharedMemoryUserData *userData = m_data->m_userDataHandles.getHandle(userDataHandle);
if (!userData) {
return hasStatus;
}
const int userDataId = (*userDataPtr)->add(clientCmd.m_addUserDataRequestArgs.m_key,
bufferServerToClient,clientCmd.m_addUserDataRequestArgs.m_valueLength,
if (!userDataHandlePtr) {
userData->m_key = clientCmd.m_addUserDataRequestArgs.m_key;
userData->m_bodyUniqueId = clientCmd.m_addUserDataRequestArgs.m_bodyUniqueId;
userData->m_linkIndex = clientCmd.m_addUserDataRequestArgs.m_linkIndex;
userData->m_visualShapeIndex = clientCmd.m_addUserDataRequestArgs.m_visualShapeIndex;
m_data->m_userDataHandleLookup.insert(userDataIdentifier, userDataHandle);
body->m_userDataHandles.push_back(userDataHandle);
}
userData->replaceValue(bufferServerToClient,
clientCmd.m_addUserDataRequestArgs.m_valueLength,
clientCmd.m_addUserDataRequestArgs.m_valueType);
serverStatusOut.m_type = CMD_ADD_USER_DATA_COMPLETED;
serverStatusOut.m_userDataResponseArgs.m_userDataGlobalId.m_userDataId = userDataId;
serverStatusOut.m_userDataResponseArgs.m_userDataGlobalId.m_bodyUniqueId = clientCmd.m_addUserDataRequestArgs.m_bodyUniqueId;
serverStatusOut.m_userDataResponseArgs.m_userDataGlobalId.m_linkIndex = clientCmd.m_addUserDataRequestArgs.m_linkIndex;
serverStatusOut.m_userDataResponseArgs.m_userDataId = userDataHandle;
serverStatusOut.m_userDataResponseArgs.m_bodyUniqueId = clientCmd.m_addUserDataRequestArgs.m_bodyUniqueId;
serverStatusOut.m_userDataResponseArgs.m_linkIndex = clientCmd.m_addUserDataRequestArgs.m_linkIndex;
serverStatusOut.m_userDataResponseArgs.m_visualShapeIndex = clientCmd.m_addUserDataRequestArgs.m_visualShapeIndex;
serverStatusOut.m_userDataResponseArgs.m_valueLength = clientCmd.m_addUserDataRequestArgs.m_valueLength;
serverStatusOut.m_userDataResponseArgs.m_valueType = clientCmd.m_addUserDataRequestArgs.m_valueType;
strcpy(serverStatusOut.m_userDataResponseArgs.m_key, clientCmd.m_addUserDataRequestArgs.m_key);
@@ -5152,19 +5118,20 @@ bool PhysicsServerCommandProcessor::processRemoveUserDataCommand(const struct Sh
BT_PROFILE("CMD_REMOVE_USER_DATA");
serverStatusOut.m_type = CMD_REMOVE_USER_DATA_FAILED;
InternalBodyData *body = m_data->m_bodyHandles.getHandle(clientCmd.m_removeUserDataRequestArgs.m_bodyUniqueId);
SharedMemoryUserData *userData = m_data->m_userDataHandles.getHandle(clientCmd.m_removeUserDataRequestArgs.m_userDataId);
if (!userData) {
return hasStatus;
}
InternalBodyData *body = m_data->m_bodyHandles.getHandle(userData->m_bodyUniqueId);
if (!body) {
return hasStatus;
}
const int linkIndex = clientCmd.m_removeUserDataRequestArgs.m_linkIndex;
InternalLinkUserData **userDataPtr = body->m_linkUserDataMap[linkIndex];
if (!userDataPtr) {
return hasStatus;
}
const bool removed = (*userDataPtr)->remove(clientCmd.m_removeUserDataRequestArgs.m_userDataId);
if (!removed) {
return hasStatus;
}
body->m_userDataHandles.remove(clientCmd.m_removeUserDataRequestArgs.m_userDataId);
m_data->m_userDataHandleLookup.remove(SharedMemoryUserDataHashKey(userData));
m_data->m_userDataHandles.freeHandle(clientCmd.m_removeUserDataRequestArgs.m_userDataId);
serverStatusOut.m_removeUserDataResponseArgs = clientCmd.m_removeUserDataRequestArgs;
serverStatusOut.m_type = CMD_REMOVE_USER_DATA_COMPLETED;
return hasStatus;
@@ -8278,6 +8245,12 @@ bool PhysicsServerCommandProcessor::processRemoveBodyCommand(const struct Shared
bodyHandle->m_rigidBody=0;
serverCmd.m_type = CMD_REMOVE_BODY_COMPLETED;
}
for (int i=0; i < bodyHandle->m_userDataHandles.size(); i++) {
int userDataHandle = bodyHandle->m_userDataHandles[i];
SharedMemoryUserData *userData = m_data->m_userDataHandles.getHandle(userDataHandle);
m_data->m_userDataHandleLookup.remove(SharedMemoryUserDataHashKey(userData));
m_data->m_userDataHandles.freeHandle(userDataHandle);
}
m_data->m_bodyHandles.freeHandle(bodyUniqueId);
}
@@ -9284,6 +9257,25 @@ bool PhysicsServerCommandProcessor::processRequestVisualShapeInfoCommand(const s
shapeIndex,
visualShapeStoragePtr);
if (success) {
//find the matching texture unique ids.
if (visualShapeStoragePtr->m_tinyRendererTextureId>=0)
{
b3AlignedObjectArray<int> usedHandles;
m_data->m_textureHandles.getUsedHandles(usedHandles);
for (int i=0;i<usedHandles.size();i++)
{
int texHandle =usedHandles[i];
InternalTextureHandle* texH = m_data->m_textureHandles.getHandle(texHandle);
if (texH && (texH->m_tinyRendererTextureId == visualShapeStoragePtr->m_tinyRendererTextureId))
{
visualShapeStoragePtr->m_openglTextureId =texH->m_openglTextureId;
visualShapeStoragePtr->m_textureUniqueId = texHandle;
}
}
}
serverCmd.m_sendVisualShapeArgs.m_numRemainingVisualShapes = remain-1;
serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied = 1;
serverCmd.m_sendVisualShapeArgs.m_startingVisualShapeIndex = clientCmd.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex;
@@ -10495,6 +10487,9 @@ void PhysicsServerCommandProcessor::resetSimulation()
m_data->m_userCollisionShapeHandles.exitHandles();
m_data->m_userCollisionShapeHandles.initHandles();
m_data->m_userDataHandles.exitHandles();
m_data->m_userDataHandles.initHandles();
m_data->m_userDataHandleLookup.clear();
}

18
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -993,13 +993,20 @@ struct b3StateSerializationArguments
struct SyncUserDataArgs
{
// User data identifiers stored in m_bulletStreamDataServerToClientRefactor
// as as array of b3UserDataGlobalIdentifier objects
// as as array of integers.
int m_numUserDataIdentifiers;
};
struct UserDataRequestArgs {
int m_userDataId;
};
struct UserDataResponseArgs
{
b3UserDataGlobalIdentifier m_userDataGlobalId;
int m_userDataId;
int m_bodyUniqueId;
int m_linkIndex;
int m_visualShapeIndex;
int m_valueType;
int m_valueLength;
char m_key[MAX_USER_DATA_KEY_LENGTH];
@@ -1010,6 +1017,7 @@ struct AddUserDataRequestArgs
{
int m_bodyUniqueId;
int m_linkIndex;
int m_visualShapeIndex;
int m_valueType;
int m_valueLength;
char m_key[MAX_USER_DATA_KEY_LENGTH];
@@ -1073,9 +1081,9 @@ struct SharedMemoryCommand
struct b3CustomCommand m_customCommandArgs;
struct b3StateSerializationArguments m_loadStateArguments;
struct RequestCollisionShapeDataArgs m_requestCollisionShapeDataArguments;
struct b3UserDataGlobalIdentifier m_userDataRequestArgs;
struct UserDataRequestArgs m_userDataRequestArgs;
struct AddUserDataRequestArgs m_addUserDataRequestArgs;
struct b3UserDataGlobalIdentifier m_removeUserDataRequestArgs;
struct UserDataRequestArgs m_removeUserDataRequestArgs;
};
};
@@ -1150,7 +1158,7 @@ struct SharedMemoryStatus
struct SendCollisionShapeDataArgs m_sendCollisionShapeArgs;
struct SyncUserDataArgs m_syncUserDataArgs;
struct UserDataResponseArgs m_userDataResponseArgs;
struct b3UserDataGlobalIdentifier m_removeUserDataResponseArgs;
struct UserDataRequestArgs m_removeUserDataResponseArgs;
};
};

21
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -7,7 +7,8 @@
//Please don't replace an existing magic number:
//instead, only ADD a new one at the top, comment-out previous one
#define SHARED_MEMORY_MAGIC_NUMBER 201806150
#define SHARED_MEMORY_MAGIC_NUMBER 201807040
//#define SHARED_MEMORY_MAGIC_NUMBER 201806150
//#define SHARED_MEMORY_MAGIC_NUMBER 201806020
//#define SHARED_MEMORY_MAGIC_NUMBER 201801170
//#define SHARED_MEMORY_MAGIC_NUMBER 201801080
@@ -287,13 +288,6 @@ struct b3UserDataValue
char* m_data1;
};
struct b3UserDataGlobalIdentifier
{
int m_bodyUniqueId;
int m_linkIndex;
int m_userDataId;
};
struct b3UserConstraint
{
int m_parentBodyIndex;
@@ -595,6 +589,11 @@ typedef union {
#define MAX_RAY_HITS MAX_RAY_INTERSECTION_BATCH_SIZE
#define VISUAL_SHAPE_MAX_PATH_LEN 1024
enum b3VisualShapeDataFlags
{
eVISUAL_SHAPE_DATA_TEXTURE_UNIQUE_IDS = 1,
};
struct b3VisualShapeData
{
int m_objectUniqueId;
@@ -605,6 +604,10 @@ struct b3VisualShapeData
double m_localVisualFrame[7];//pos[3], orn[4]
//todo: add more data if necessary (material color etc, although material can be in asset file .obj file)
double m_rgbaColor[4];
int m_tinyRendererTextureId;
int m_textureUniqueId;
int m_openglTextureId;
};
struct b3VisualShapeInformation
@@ -743,6 +746,8 @@ enum eCONNECT_METHOD {
eCONNECT_GUI_SERVER=7,
eCONNECT_GUI_MAIN_THREAD=8,
eCONNECT_SHARED_MEMORY_SERVER=9,
eCONNECT_DART=10,
eCONNECT_MUJOCO=11,
};
enum eURDF_Flags

52
examples/SharedMemory/SharedMemoryUserData.h
View File

@@ -3,6 +3,7 @@
#include <string>
#include "LinearMath/btAlignedObjectArray.h"
#include "LinearMath/btHashMap.h"
#include "SharedMemoryPublic.h"
struct SharedMemoryUserData
@@ -10,20 +11,22 @@ struct SharedMemoryUserData
std::string m_key;
int m_type;
int m_bodyUniqueId;
int m_linkIndex;
int m_visualShapeIndex;
btAlignedObjectArray<char> m_bytes;
SharedMemoryUserData()
:m_type(-1)
:m_type(-1), m_bodyUniqueId(-1), m_linkIndex(-1), m_visualShapeIndex(-1)
{
}
// Takes ownership of the passed key and value arguments.
SharedMemoryUserData(const char* key)
:m_key(key)
SharedMemoryUserData(const char* key, int bodyUniqueId, int linkIndex, int visualShapeIndex)
:m_key(key), m_type(-1), m_bodyUniqueId(bodyUniqueId), m_linkIndex(linkIndex), m_visualShapeIndex(visualShapeIndex)
{
}
// Takes ownership of the data pointed to by newValue.
void replaceValue(const char* bytes, int len, int type)
{
m_type = type;
@@ -45,4 +48,43 @@ struct SharedMemoryUserData
}
};
struct SharedMemoryUserDataHashKey {
unsigned int m_hash;
btHashString m_key;
btHashInt m_bodyUniqueId;
btHashInt m_linkIndex;
btHashInt m_visualShapeIndex;
SIMD_FORCE_INLINE unsigned int getHash()const {
return m_hash;
}
SharedMemoryUserDataHashKey() : m_hash(0) {}
SharedMemoryUserDataHashKey(const struct SharedMemoryUserData *userData)
: m_key(userData->m_key.c_str()),
m_bodyUniqueId(userData->m_bodyUniqueId),
m_linkIndex(userData->m_linkIndex),
m_visualShapeIndex(userData->m_visualShapeIndex) {
calculateHash();
}
SharedMemoryUserDataHashKey(const char *key, int bodyUniqueId, int linkIndex, int visualShapeIndex)
: m_key(key), m_bodyUniqueId(bodyUniqueId), m_linkIndex(linkIndex), m_visualShapeIndex(visualShapeIndex) {
calculateHash();
}
void calculateHash() {
m_hash = m_key.getHash() ^ m_bodyUniqueId.getHash() ^ m_linkIndex.getHash() ^ m_visualShapeIndex.getHash();
}
bool equals(const SharedMemoryUserDataHashKey& other) const {
return m_bodyUniqueId.equals(other.m_bodyUniqueId) &&
m_linkIndex.equals(other.m_linkIndex) &&
m_visualShapeIndex.equals(other.m_visualShapeIndex) &&
m_key.equals(other.m_key);
}
};
#endif //SHARED_MEMORY_USER_DATA_H

86
examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp
View File

@@ -155,6 +155,69 @@ btVector3 b3RobotSimulatorClientAPI_NoDirect::getEulerFromQuaternion(const btQua
return rpy2;
}
int b3RobotSimulatorClientAPI_NoDirect::loadTexture(const std::string& fileName)
{
if (!isConnected())
{
b3Warning("Not connected");
return -1;
}
btAssert(b3CanSubmitCommand(m_data->m_physicsClientHandle));
b3SharedMemoryCommandHandle commandHandle;
b3SharedMemoryStatusHandle statusHandle;
int statusType;
{
commandHandle = b3InitLoadTexture(m_data->m_physicsClientHandle, fileName.c_str());
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
statusType = b3GetStatusType(statusHandle);
if (statusType == CMD_LOAD_TEXTURE_COMPLETED)
{
return b3GetStatusTextureUniqueId(statusHandle);
}
}
return -1;
}
bool b3RobotSimulatorClientAPI_NoDirect::changeVisualShape(const struct b3RobotSimulatorChangeVisualShapeArgs& args)
{
if (!isConnected())
{
b3Warning("Not connected");
return false;
}
int objectUniqueId = args.m_objectUniqueId;
int jointIndex = args.m_linkIndex;
int shapeIndex = args.m_shapeIndex;
int textureUniqueId = args.m_textureUniqueId;
b3SharedMemoryCommandHandle commandHandle;
b3SharedMemoryStatusHandle statusHandle;
int statusType;
commandHandle = b3InitUpdateVisualShape(m_data->m_physicsClientHandle, objectUniqueId, jointIndex, shapeIndex, textureUniqueId);
if (args.m_hasSpecularColor)
{
double specularColor[3] = {args.m_specularColor[0],args.m_specularColor[1],args.m_specularColor[2]};
b3UpdateVisualShapeSpecularColor(commandHandle,specularColor);
}
if (args.m_hasRgbaColor)
{
double rgbaColor[4] = {args.m_rgbaColor[0],args.m_rgbaColor[1],args.m_rgbaColor[2],args.m_rgbaColor[3]};
b3UpdateVisualShapeRGBAColor(commandHandle,rgbaColor);
}
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
statusType = b3GetStatusType(statusHandle);
return (statusType == CMD_VISUAL_SHAPE_UPDATE_COMPLETED);
}
int b3RobotSimulatorClientAPI_NoDirect::loadURDF(const std::string& fileName, const struct b3RobotSimulatorLoadUrdfFileArgs& args)
{
int robotUniqueId = -1;
@@ -2177,7 +2240,8 @@ void b3RobotSimulatorClientAPI_NoDirect::restoreStateFromMemory(int stateId)
bool b3RobotSimulatorClientAPI_NoDirect::getVisualShapeData(int bodyUniqueId, b3VisualShapeInformation &visualShapeInfo)
{
b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
if (sm == 0) {
if (sm == 0)
{
b3Warning("Not connected");
return false;
}
@@ -2185,17 +2249,17 @@ bool b3RobotSimulatorClientAPI_NoDirect::getVisualShapeData(int bodyUniqueId, b3
b3SharedMemoryStatusHandle statusHandle;
int statusType;
{
commandHandle = b3InitRequestVisualShapeInformation(sm, bodyUniqueId);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
statusType = b3GetStatusType(statusHandle);
commandHandle = b3InitRequestVisualShapeInformation(sm, bodyUniqueId);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
statusType = b3GetStatusType(statusHandle);
btAssert(statusType == CMD_VISUAL_SHAPE_INFO_COMPLETED);
if (statusType == CMD_VISUAL_SHAPE_INFO_COMPLETED) {
b3GetVisualShapeInformation(sm, &visualShapeInfo);
}
return true;
}
btAssert(statusType == CMD_VISUAL_SHAPE_INFO_COMPLETED);
if (statusType == CMD_VISUAL_SHAPE_INFO_COMPLETED)
{
b3GetVisualShapeInformation(sm, &visualShapeInfo);
return true;
}
return false;
}
void b3RobotSimulatorClientAPI_NoDirect::setAdditionalSearchPath(const std::string& path)

30
examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h
View File

@@ -60,6 +60,30 @@ struct b3RobotSimulatorLoadFileResults
}
};
struct b3RobotSimulatorChangeVisualShapeArgs
{
int m_objectUniqueId;
int m_linkIndex;
int m_shapeIndex;
int m_textureUniqueId;
btVector4 m_rgbaColor;
bool m_hasRgbaColor;
btVector3 m_specularColor;
bool m_hasSpecularColor;
b3RobotSimulatorChangeVisualShapeArgs()
:m_objectUniqueId(-1),
m_linkIndex(-1),
m_shapeIndex(-1),
m_textureUniqueId(-1),
m_rgbaColor(0,0,0,1),
m_hasRgbaColor(false),
m_specularColor(1,1,1),
m_hasSpecularColor(false)
{
}
};
struct b3RobotSimulatorJointMotorArgs
{
int m_controlMode;
@@ -505,6 +529,10 @@ public:
bool loadBullet(const std::string& fileName, b3RobotSimulatorLoadFileResults& results);
bool saveBullet(const std::string& fileName);
int loadTexture(const std::string& fileName);
bool changeVisualShape(const struct b3RobotSimulatorChangeVisualShapeArgs& args);
bool savePythonWorld(const std::string& fileName);
bool getBodyInfo(int bodyUniqueId, struct b3BodyInfo* bodyInfo);
@@ -648,7 +676,7 @@ public:
bool getCollisionShapeData(int bodyUniqueId, int linkIndex, b3CollisionShapeInformation &collisionShapeInfo);
bool getVisualShapeData(int bodyUniqueId, b3VisualShapeInformation &visualShapeInfo);
bool getVisualShapeData(int bodyUniqueId, struct b3VisualShapeInformation &visualShapeInfo);
int saveStateToMemory();
void restoreStateFromMemory(int stateId);

16
examples/SharedMemory/dart/DARTPhysicsC_API.cpp Normal file
View File

@@ -0,0 +1,16 @@
#ifdef BT_ENABLE_DART
#include "DARTPhysicsC_API.h"
#include "DARTPhysicsServerCommandProcessor.h"
#include "DARTPhysicsClient.h"
//think more about naming. The b3ConnectPhysicsLoopback
B3_SHARED_API b3PhysicsClientHandle b3ConnectPhysicsDART()
{
DARTPhysicsServerCommandProcessor* sdk = new DARTPhysicsServerCommandProcessor;
DARTPhysicsClient* direct = new DARTPhysicsClient(sdk,true);
bool connected;
connected = direct->connect();
return (b3PhysicsClientHandle )direct;
}
#endif//BT_ENABLE_DART

20
examples/SharedMemory/dart/DARTPhysicsC_API.h Normal file
View File

@@ -0,0 +1,20 @@
#ifndef DART_PHYSICS_C_API_H
#define DART_PHYSICS_C_API_H
#ifdef BT_ENABLE_DART
#include "../PhysicsClientC_API.h"
#ifdef __cplusplus
extern "C" {
#endif
//think more about naming. The b3ConnectPhysicsLoopback
B3_SHARED_API b3PhysicsClientHandle b3ConnectPhysicsDART();
#ifdef __cplusplus
}
#endif
#endif //BT_ENABLE_DART
#endif //DART_PHYSICS_C_API_H

1596
examples/SharedMemory/dart/DARTPhysicsClient.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

124
examples/SharedMemory/dart/DARTPhysicsClient.h Normal file
View File

@@ -0,0 +1,124 @@
#ifndef DART_PHYSICS_CLIENT_H
#define DART_PHYSICS_CLIENT_H
#include "../PhysicsClient.h"
///PhysicsDirect executes the commands directly, without transporting them or having a separate server executing commands
class DARTPhysicsClient : public PhysicsClient
{
protected:
struct DARTPhysicsDirectInternalData* m_data;
bool processDebugLines(const struct SharedMemoryCommand& orgCommand);
bool processCamera(const struct SharedMemoryCommand& orgCommand);
bool processContactPointData(const struct SharedMemoryCommand& orgCommand);
bool processOverlappingObjects(const struct SharedMemoryCommand& orgCommand);
bool processVisualShapeData(const struct SharedMemoryCommand& orgCommand);
void processBodyJointInfo(int bodyUniqueId, const struct SharedMemoryStatus& serverCmd);
void processAddUserData(const struct SharedMemoryStatus& serverCmd);
void postProcessStatus(const struct SharedMemoryStatus& serverCmd);
void resetData();
void removeCachedBody(int bodyUniqueId);
public:
DARTPhysicsClient(class PhysicsCommandProcessorInterface* physSdk, bool passSdkOwnership);
virtual ~DARTPhysicsClient();
// return true if connection succesfull, can also check 'isConnected'
//it is OK to pass a null pointer for the gui helper
virtual bool connect();
////todo: rename to 'disconnect'
virtual void disconnectSharedMemory();
virtual bool isConnected() const;
// return non-null if there is a status, nullptr otherwise
virtual const SharedMemoryStatus* processServerStatus();
virtual SharedMemoryCommand* getAvailableSharedMemoryCommand();
virtual bool canSubmitCommand() const;
virtual bool submitClientCommand(const struct SharedMemoryCommand& command);
virtual int getNumBodies() const;
virtual int getBodyUniqueId(int serialIndex) const;
virtual bool getBodyInfo(int bodyUniqueId, struct b3BodyInfo& info) const;
virtual int getNumJoints(int bodyIndex) const;
virtual bool getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const;
virtual int getNumUserConstraints() const;
virtual int getUserConstraintInfo(int constraintUniqueId, struct b3UserConstraint& info) const;
virtual int getUserConstraintId(int serialIndex) const;
///todo: move this out of the
virtual void setSharedMemoryKey(int key);
void uploadBulletFileToSharedMemory(const char* data, int len);
virtual void uploadRaysToSharedMemory(struct SharedMemoryCommand& command, const double* rayFromWorldArray, const double* rayToWorldArray, int numRays);
virtual int getNumDebugLines() const;
virtual const float* getDebugLinesFrom() const;
virtual const float* getDebugLinesTo() const;
virtual const float* getDebugLinesColor() const;
virtual void getCachedCameraImage(b3CameraImageData* cameraData);
virtual void getCachedContactPointInformation(struct b3ContactInformation* contactPointData);
virtual void getCachedOverlappingObjects(struct b3AABBOverlapData* overlappingObjects);
virtual void getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo);
virtual void getCachedCollisionShapeInformation(struct b3CollisionShapeInformation* collisionShapesInfo);
virtual void getCachedVREvents(struct b3VREventsData* vrEventsData);
virtual void getCachedKeyboardEvents(struct b3KeyboardEventsData* keyboardEventsData);
virtual void getCachedMouseEvents(struct b3MouseEventsData* mouseEventsData);
virtual void getCachedRaycastHits(struct b3RaycastInformation* raycastHits);
virtual void getCachedMassMatrix(int dofCountCheck, double* massMatrix);
//the following APIs are for internal use for visualization:
virtual bool connect(struct GUIHelperInterface* guiHelper);
virtual void renderScene();
virtual void debugDraw(int debugDrawMode);
virtual void setTimeOut(double timeOutInSeconds);
virtual double getTimeOut() const;
virtual bool getCachedUserData(int bodyUniqueId, int linkIndex, int userDataId, struct b3UserDataValue &valueOut) const;
virtual int getCachedUserDataId(int bodyUniqueId, int linkIndex, const char *key) const;
virtual int getNumUserData(int bodyUniqueId, int linkIndex) const;
virtual void getUserDataInfo(int bodyUniqueId, int linkIndex, int userDataIndex, const char **keyOut, int *userDataIdOut) const;
virtual void pushProfileTiming(const char* timingName);
virtual void popProfileTiming();
};
#endif //DART_PHYSICS__H

39
examples/SharedMemory/dart/DARTPhysicsServerCommandProcessor.cpp Normal file
View File

@@ -0,0 +1,39 @@
#include "DARTPhysicsServerCommandProcessor.h"
DARTPhysicsServerCommandProcessor::DARTPhysicsServerCommandProcessor()
{
}
DARTPhysicsServerCommandProcessor::~DARTPhysicsServerCommandProcessor()
{
}
bool DARTPhysicsServerCommandProcessor::connect()
{
return false;
}
void DARTPhysicsServerCommandProcessor::disconnect()
{
}
bool DARTPhysicsServerCommandProcessor::isConnected() const
{
return false;
}
bool DARTPhysicsServerCommandProcessor::processCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
return false;
}
bool DARTPhysicsServerCommandProcessor::receiveStatus(struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
return false;
}

31
examples/SharedMemory/dart/DARTPhysicsServerCommandProcessor.h Normal file
View File

@@ -0,0 +1,31 @@
#ifndef DART_PHYSICS_SERVER_COMMAND_PROCESSOR_H
#define DART_PHYSICS_SERVER_COMMAND_PROCESSOR_H
#include "../PhysicsCommandProcessorInterface.h"
class DARTPhysicsServerCommandProcessor : public PhysicsCommandProcessorInterface
{
public:
DARTPhysicsServerCommandProcessor();
virtual ~DARTPhysicsServerCommandProcessor();
virtual bool connect();
virtual void disconnect();
virtual bool isConnected() const;
virtual bool processCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
virtual bool receiveStatus(struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
virtual void renderScene(int renderFlags){}
virtual void physicsDebugDraw(int debugDrawFlags){}
virtual void setGuiHelper(struct GUIHelperInterface* guiHelper){}
virtual void setTimeOut(double timeOutInSeconds){}
};
#endif //DART_PHYSICS_COMMAND_PROCESSOR_H

15
examples/SharedMemory/mujoco/MuJoCoPhysicsC_API.cpp Normal file
View File

@@ -0,0 +1,15 @@
#ifdef BT_ENABLE_MUJOCO
#include "MuJoCoPhysicsC_API.h"
#include "MuJoCoPhysicsServerCommandProcessor.h"
#include "MuJoCoPhysicsClient.h"
B3_SHARED_API b3PhysicsClientHandle b3ConnectPhysicsMuJoCo()
{
MuJoCoPhysicsServerCommandProcessor* sdk = new MuJoCoPhysicsServerCommandProcessor;
MuJoCoPhysicsClient* direct = new MuJoCoPhysicsClient(sdk,true);
bool connected;
connected = direct->connect();
return (b3PhysicsClientHandle )direct;
}
#endif//BT_ENABLE_MUJOCO

20
examples/SharedMemory/mujoco/MuJoCoPhysicsC_API.h Normal file
View File

@@ -0,0 +1,20 @@
#ifndef MUJOCO_PHYSICS_C_API_H
#define MUJOCO_PHYSICS_C_API_H
#ifdef BT_ENABLE_MUJOCO
#include "../PhysicsClientC_API.h"
#ifdef __cplusplus
extern "C" {
#endif
//think more about naming. The b3ConnectPhysicsLoopback
B3_SHARED_API b3PhysicsClientHandle b3ConnectPhysicsMuJoCo();
#ifdef __cplusplus
}
#endif
#endif //BT_ENABLE_MUJOCO
#endif //MUJOCO_PHYSICS_C_API_H

1598
examples/SharedMemory/mujoco/MuJoCoPhysicsClient.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

123
examples/SharedMemory/mujoco/MuJoCoPhysicsClient.h Normal file
View File

@@ -0,0 +1,123 @@
#ifndef MUJOCO_PHYSICS_CLIENT_H
#define MUJOCO_PHYSICS_CLIENT_H
#include "../PhysicsClient.h"
///PhysicsDirect executes the commands directly, without transporting them or having a separate server executing commands
class MuJoCoPhysicsClient : public PhysicsClient
{
protected:
struct MuJoCoPhysicsDirectInternalData* m_data;
bool processDebugLines(const struct SharedMemoryCommand& orgCommand);
bool processCamera(const struct SharedMemoryCommand& orgCommand);
bool processContactPointData(const struct SharedMemoryCommand& orgCommand);
bool processOverlappingObjects(const struct SharedMemoryCommand& orgCommand);
bool processVisualShapeData(const struct SharedMemoryCommand& orgCommand);
void processBodyJointInfo(int bodyUniqueId, const struct SharedMemoryStatus& serverCmd);
void processAddUserData(const struct SharedMemoryStatus& serverCmd);
void postProcessStatus(const struct SharedMemoryStatus& serverCmd);
void resetData();
void removeCachedBody(int bodyUniqueId);
public:
MuJoCoPhysicsClient(class PhysicsCommandProcessorInterface* physSdk, bool passSdkOwnership);
virtual ~MuJoCoPhysicsClient();
// return true if connection succesfull, can also check 'isConnected'
//it is OK to pass a null pointer for the gui helper
virtual bool connect();
////todo: rename to 'disconnect'
virtual void disconnectSharedMemory();
virtual bool isConnected() const;
// return non-null if there is a status, nullptr otherwise
virtual const SharedMemoryStatus* processServerStatus();
virtual SharedMemoryCommand* getAvailableSharedMemoryCommand();
virtual bool canSubmitCommand() const;
virtual bool submitClientCommand(const struct SharedMemoryCommand& command);
virtual int getNumBodies() const;
virtual int getBodyUniqueId(int serialIndex) const;
virtual bool getBodyInfo(int bodyUniqueId, struct b3BodyInfo& info) const;
virtual int getNumJoints(int bodyIndex) const;
virtual bool getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const;
virtual int getNumUserConstraints() const;
virtual int getUserConstraintInfo(int constraintUniqueId, struct b3UserConstraint& info) const;
virtual int getUserConstraintId(int serialIndex) const;
///todo: move this out of the
virtual void setSharedMemoryKey(int key);
void uploadBulletFileToSharedMemory(const char* data, int len);
virtual void uploadRaysToSharedMemory(struct SharedMemoryCommand& command, const double* rayFromWorldArray, const double* rayToWorldArray, int numRays);
virtual int getNumDebugLines() const;
virtual const float* getDebugLinesFrom() const;
virtual const float* getDebugLinesTo() const;
virtual const float* getDebugLinesColor() const;
virtual void getCachedCameraImage(b3CameraImageData* cameraData);
virtual void getCachedContactPointInformation(struct b3ContactInformation* contactPointData);
virtual void getCachedOverlappingObjects(struct b3AABBOverlapData* overlappingObjects);
virtual void getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo);
virtual void getCachedCollisionShapeInformation(struct b3CollisionShapeInformation* collisionShapesInfo);
virtual void getCachedVREvents(struct b3VREventsData* vrEventsData);
virtual void getCachedKeyboardEvents(struct b3KeyboardEventsData* keyboardEventsData);
virtual void getCachedMouseEvents(struct b3MouseEventsData* mouseEventsData);
virtual void getCachedRaycastHits(struct b3RaycastInformation* raycastHits);
virtual void getCachedMassMatrix(int dofCountCheck, double* massMatrix);
//the following APIs are for internal use for visualization:
virtual bool connect(struct GUIHelperInterface* guiHelper);
virtual void renderScene();
virtual void debugDraw(int debugDrawMode);
virtual void setTimeOut(double timeOutInSeconds);
virtual double getTimeOut() const;
virtual bool getCachedUserData(int bodyUniqueId, int linkIndex, int userDataId, struct b3UserDataValue &valueOut) const;
virtual int getCachedUserDataId(int bodyUniqueId, int linkIndex, const char *key) const;
virtual int getNumUserData(int bodyUniqueId, int linkIndex) const;
virtual void getUserDataInfo(int bodyUniqueId, int linkIndex, int userDataIndex, const char **keyOut, int *userDataIdOut) const;
virtual void pushProfileTiming(const char* timingName);
virtual void popProfileTiming();
};
#endif //MUJOCO_PHYSICS_CLIENT_H

1032
examples/SharedMemory/mujoco/MuJoCoPhysicsServerCommandProcessor.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

45
examples/SharedMemory/mujoco/MuJoCoPhysicsServerCommandProcessor.h Normal file
View File

@@ -0,0 +1,45 @@
#ifndef MUJOCO_PHYSICS_SERVER_COMMAND_PROCESSOR_H
#define MUJOCO_PHYSICS_SERVER_COMMAND_PROCESSOR_H
#include "../PhysicsCommandProcessorInterface.h"
class MuJoCoPhysicsServerCommandProcessor : public PhysicsCommandProcessorInterface
{
struct MuJoCoPhysicsServerCommandProcessorInternalData* m_data;
bool processSyncBodyInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
bool processRequestInternalDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
bool processSyncUserDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
bool processLoadMJCFCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
bool processRequestBodyInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
bool processForwardDynamicsCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
bool processSendPhysicsParametersCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
bool processRequestActualStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
bool processResetSimulationCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
void resetSimulation();
public:
MuJoCoPhysicsServerCommandProcessor();
virtual ~MuJoCoPhysicsServerCommandProcessor();
virtual bool connect();
virtual void disconnect();
virtual bool isConnected() const;
virtual bool processCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
virtual bool receiveStatus(struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
virtual void renderScene(int renderFlags){}
virtual void physicsDebugDraw(int debugDrawFlags){}
virtual void setGuiHelper(struct GUIHelperInterface* guiHelper){}
virtual void setTimeOut(double timeOutInSeconds){}
};
#endif //MUJOCO_PHYSICS_COMMAND_PROCESSOR_H

18
examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp
View File

@@ -671,13 +671,15 @@ void TinyRendererVisualShapeConverter::convertVisualShapes(
visualShape.m_rgbaColor[1] = rgbaColor[1];
visualShape.m_rgbaColor[2] = rgbaColor[2];
visualShape.m_rgbaColor[3] = rgbaColor[3];
visualShape.m_openglTextureId = -1;
visualShape.m_tinyRendererTextureId = -1;
visualShape.m_textureUniqueId = -1;
{
B3_PROFILE("convertURDFToVisualShape");
convertURDFToVisualShape(vis, pathPrefix, localInertiaFrame.inverse()*childTrans, vertices, indices, textures, visualShape);
}
m_data->m_visualShapes.push_back(visualShape);
if (vertices.size() && indices.size())
{
TinyRenderObjectData* tinyObj = new TinyRenderObjectData(m_data->m_rgbColorBuffer,m_data->m_depthBuffer, &m_data->m_shadowBuffer, &m_data->m_segmentationMaskBuffer, bodyUniqueId, linkIndex);
@@ -701,13 +703,15 @@ void TinyRendererVisualShapeConverter::convertVisualShapes(
}
visuals->m_renderObjects.push_back(tinyObj);
}
btAssert(textures.size()<=1);
for (int i=0;i<textures.size();i++)
{
if (!textures[i].m_isCached)
{
free(textures[i].textureData1);
}
visualShape.m_tinyRendererTextureId = m_data->m_textures.size();
m_data->m_textures.push_back(textures[i]);
}
m_data->m_visualShapes.push_back(visualShape);
}
}
}
@@ -1199,13 +1203,13 @@ void TinyRendererVisualShapeConverter::changeShapeTexture(int objectUniqueId, in
for (int v = 0; v < visualArray->m_renderObjects.size(); v++)
{
TinyRenderObjectData* renderObj = visualArray->m_renderObjects[v];
if ((shapeIndex < 0) || (shapeIndex == v))
{
renderObj->m_model->setDiffuseTextureFromData(m_data->m_textures[textureUniqueId].textureData1, m_data->m_textures[textureUniqueId].m_width, m_data->m_textures[textureUniqueId].m_height);
}
}
}
}
}
}

77
examples/SharedMemory/tcp/main.cpp
View File

@@ -177,17 +177,30 @@ int main(int argc, char *argv[])
SharedMemoryCommand cmd;
SharedMemoryCommand* cmdPtr = 0;
int type = *(int*)&bytesReceived[0];
//performance test
if (numBytesRec == sizeof(int))
{
cmdPtr = &cmd;
cmd.m_type = *(int*)&bytesReceived[0];
}
if (numBytesRec == sizeof(SharedMemoryCommand))
else
{
cmdPtr = (SharedMemoryCommand*)&bytesReceived[0];
if (numBytesRec == sizeof(SharedMemoryCommand))
{
cmdPtr = (SharedMemoryCommand*)&bytesReceived[0];
}
else
{
if (numBytesRec==36)
{
cmdPtr = &cmd;
memcpy(&cmd, &bytesReceived[0], numBytesRec);
}
}
}
if (cmdPtr)
{
@@ -207,7 +220,7 @@ int main(int argc, char *argv[])
}
if (gVerboseNetworkMessagesServer)
{
printf("buffer.size = %d\n", buffer.size());
//printf("buffer.size = %d\n", buffer.size());
printf("serverStatus.m_numDataStreamBytes = %d\n", serverStatus.m_numDataStreamBytes);
}
if (hasStatus)
@@ -234,25 +247,49 @@ int main(int argc, char *argv[])
}
else
{
//create packetData with [int packetSizeInBytes, status, streamBytes)
packetData.resize(4 + sizeof(SharedMemoryStatus) + serverStatus.m_numDataStreamBytes);
int sz = packetData.size();
int curPos = 0;
MySerializeInt(sz, &packetData[curPos]);
curPos += 4;
for (int i = 0; i < sizeof(SharedMemoryStatus); i++)
if (cmdPtr->m_type == CMD_REQUEST_VR_EVENTS_DATA)
{
packetData[i + curPos] = statBytes[i];
int headerSize = 16+5 * sizeof(int) + sizeof(smUint64_t) + sizeof(char*) + sizeof(b3VRControllerEvent)*serverStatus.m_sendVREvents.m_numVRControllerEvents;
packetData.resize(4 + headerSize);
int sz = packetData.size();
int curPos = 0;
MySerializeInt(sz, &packetData[curPos]);
curPos += 4;
for (int i = 0; i < headerSize; i++)
{
packetData[i + curPos] = statBytes[i];
}
curPos += headerSize;
pClient->Send(&packetData[0], packetData.size());
}
curPos += sizeof(SharedMemoryStatus);
for (int i = 0; i < serverStatus.m_numDataStreamBytes; i++)
else
{
packetData[i + curPos] = buffer[i];
//create packetData with [int packetSizeInBytes, status, streamBytes)
packetData.resize(4 + sizeof(SharedMemoryStatus) + serverStatus.m_numDataStreamBytes);
int sz = packetData.size();
int curPos = 0;
if (gVerboseNetworkMessagesServer)
{
//printf("buffer.size = %d\n", buffer.size());
printf("serverStatus packed size = %d\n", sz);
}
MySerializeInt(sz, &packetData[curPos]);
curPos += 4;
for (int i = 0; i < sizeof(SharedMemoryStatus); i++)
{
packetData[i + curPos] = statBytes[i];
}
curPos += sizeof(SharedMemoryStatus);
for (int i = 0; i < serverStatus.m_numDataStreamBytes; i++)
{
packetData[i + curPos] = buffer[i];
}
pClient->Send(&packetData[0], packetData.size());
}
pClient->Send( &packetData[0], packetData.size() );
}
}

40
examples/pybullet/examples/biped2d_pybullet.py Normal file
View File

@@ -0,0 +1,40 @@
import pybullet as p
import pybullet_data
import os
import time
GRAVITY = -9.8
dt = 1e-3
iters=2000
physicsClient = p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.resetSimulation()
#p.setRealTimeSimulation(True)
p.setGravity(0,0,GRAVITY)
p.setTimeStep(dt)
planeId = p.loadURDF("plane.urdf")
cubeStartPos = [0,0,1.13]
cubeStartOrientation = p.getQuaternionFromEuler([0.,0,0])
botId = p.loadURDF("biped/biped2d_pybullet.urdf",
cubeStartPos,
cubeStartOrientation)
#disable the default velocity motors
#and set some position control with small force to emulate joint friction/return to a rest pose
jointFrictionForce=1
for joint in range (p.getNumJoints(botId)):
p.setJointMotorControl2(botId,joint,p.POSITION_CONTROL,force=jointFrictionForce)
#for i in range(10000):
# p.setJointMotorControl2(botId, 1, p.TORQUE_CONTROL, force=1098.0)
# p.stepSimulation()
#import ipdb
#ipdb.set_trace()
import time
p.setRealTimeSimulation(1)
while (1):
#p.stepSimulation()
#p.setJointMotorControl2(botId, 1, p.TORQUE_CONTROL, force=1098.0)
p.setGravity(0,0,GRAVITY)
time.sleep(1/240.)
time.sleep(1000)

19
examples/pybullet/examples/getTextureUid.py Normal file
View File

@@ -0,0 +1,19 @@
import pybullet as p
p.connect(p.GUI)
plane = p.loadURDF("plane.urdf")
visualData = p.getVisualShapeData(plane, p.VISUAL_SHAPE_DATA_TEXTURE_UNIQUE_IDS)
print(visualData)
curTexUid = visualData[0][8]
print(curTexUid)
texUid = p.loadTexture("tex256.png")
print("texUid=",texUid)
p.changeVisualShape(plane,-1,textureUniqueId=texUid)
for i in range (100):
p.getCameraImage(320,200)
p.changeVisualShape(plane,-1,textureUniqueId=curTexUid)
for i in range (100):
p.getCameraImage(320,200)

32
examples/pybullet/examples/otherPhysicsEngine.py Normal file
View File

@@ -0,0 +1,32 @@
import pybullet as p
import time
#p.connect(p.DIRECT)
#p.connect(p.DART)
p.connect(p.MuJoCo)
#p.connect(p.GUI)
bodies = p.loadMJCF("mjcf/capsule.xml")
print("bodies=",bodies)
numBodies = p.getNumBodies()
print("numBodies=",numBodies)
for i in range (numBodies):
print("bodyInfo[",i,"]=",p.getBodyInfo(i))
p.setGravity(0,0,-10)
timeStep = 1./240.
p.setPhysicsEngineParameter(fixedTimeStep=timeStep)
#while (p.isConnected()):
for i in range (1000):
p.stepSimulation()
for b in bodies:
pos,orn=p.getBasePositionAndOrientation(b)
print("pos[",b,"]=",pos)
print("orn[",b,"]=",orn)
linvel,angvel=p.getBaseVelocity(b)
print("linvel[",b,"]=",linvel)
print("angvel[",b,"]=",angvel)
time.sleep(timeStep)

4
examples/pybullet/examples/projective_texture.py
View File

@@ -13,8 +13,8 @@ bearStartPos2 = [0,0,0]
bearStartOrientation2 = p.getQuaternionFromEuler([0,0,0])
bearId2 = p.loadURDF("teddy_large.urdf",bearStartPos2, bearStartOrientation2)
textureId = p.loadTexture("checker_grid.jpg")
p.changeVisualShape(objectUniqueId=0, linkIndex=-1, textureUniqueId=textureId)
p.changeVisualShape(objectUniqueId=1, linkIndex=-1, textureUniqueId=textureId)
#p.changeVisualShape(objectUniqueId=0, linkIndex=-1, textureUniqueId=textureId)
#p.changeVisualShape(objectUniqueId=1, linkIndex=-1, textureUniqueId=textureId)
useRealTimeSimulation = 1

66
examples/pybullet/examples/userData.py
View File

@@ -21,16 +21,16 @@ plane_id = client.loadURDF(PLANE_PATH)
print ("Plane ID: %s" % plane_id)
print ("Adding user data to plane")
MyKey1 = client.addUserData(plane_id, 0, "MyKey1", "MyValue1")
MyKey2 = client.addUserData(plane_id, 0, "MyKey2", "MyValue2")
MyKey3 = client.addUserData(plane_id, 0, "MyKey3", "MyValue3")
MyKey4 = client.addUserData(plane_id, 0, "MyKey4", "MyValue4")
MyKey1 = client.addUserData(plane_id, "MyKey1", "MyValue1")
MyKey2 = client.addUserData(plane_id, "MyKey2", "MyValue2")
MyKey3 = client.addUserData(plane_id, "MyKey3", "MyValue3")
MyKey4 = client.addUserData(plane_id, "MyKey4", "MyValue4")
print ("Retrieving cached user data")
print (client.getUserData(plane_id, 0, MyKey1))
print (client.getUserData(plane_id, 0, MyKey2))
print (client.getUserData(plane_id, 0, MyKey3))
print (client.getUserData(plane_id, 0, MyKey4))
print (client.getUserData(MyKey1))
print (client.getUserData(MyKey2))
print (client.getUserData(MyKey3))
print (client.getUserData(MyKey4))
print ("Disconnecting")
del client
@@ -39,18 +39,18 @@ print ("Reconnecting")
client = bullet_client.BulletClient(connection_mode=CONNECTION_METHOD)
print ("Retrieving synced user data")
print (client.getUserData(plane_id, 0, MyKey1))
print (client.getUserData(plane_id, 0, MyKey2))
print (client.getUserData(plane_id, 0, MyKey3))
print (client.getUserData(plane_id, 0, MyKey4))
print (client.getUserData(MyKey1))
print (client.getUserData(MyKey2))
print (client.getUserData(MyKey3))
print (client.getUserData(MyKey4))
print ("Number of user data entries: %s" % client.getNumUserData(plane_id, 0))
print ("Number of user data entries: %s" % client.getNumUserData(plane_id))
print ("Overriding user data")
client.addUserData(plane_id, 0, "MyKey1", "MyNewValue")
client.addUserData(plane_id, "MyKey1", "MyNewValue")
print ("Cached overridden data")
print (client.getUserData(plane_id, 0, MyKey1))
print (client.getUserData(MyKey1))
print ("Disconnecting")
@@ -61,50 +61,50 @@ client = bullet_client.BulletClient(connection_mode=CONNECTION_METHOD)
print ("Synced overridden data")
print (client.getUserData(plane_id, 0, MyKey1))
print (client.getUserData(MyKey1))
print ("Getting user data ID")
print ("Retrieved ID: %s, ID retrieved from addUserData: %s" % (client.getUserDataId(plane_id, 0, "MyKey2"), MyKey2))
print ("Retrieved ID: %s, ID retrieved from addUserData: %s" % (client.getUserDataId(plane_id, "MyKey2"), MyKey2))
print ("Removing user data")
client.removeUserData(plane_id, 0, MyKey2)
client.removeUserData(MyKey2)
print ("Retrieving cached removed data")
print (client.getUserData(plane_id, 0, MyKey2))
print (client.getUserData(MyKey2))
print ("Syncing")
client.syncUserData()
print ("Retrieving removed removed data")
print (client.getUserData(plane_id, 0, MyKey2))
print (client.getUserData(MyKey2))
print ("Iterating over all user data entries and printing results")
for i in range(client.getNumUserData(plane_id, 0)):
userDataId, key = client.getUserDataInfo(plane_id, 0, i)
print ("Info: (%s, %s)" % (userDataId, key))
print ("Value: %s" % client.getUserData(plane_id, 0, userDataId))
for i in range(client.getNumUserData(plane_id)):
userDataId, key, bodyId, linkIndex, visualShapeIndex = client.getUserDataInfo(plane_id, i)
print ("Info: (%s, %s, %s, %s, %s)" % (userDataId, key, bodyId, linkIndex, visualShapeIndex))
print ("Value: %s" % client.getUserData(userDataId))
print ("Removing body")
client.removeBody(plane_id)
print ("Retrieving user data")
print (client.getUserData(plane_id, 0, MyKey1))
print (client.getUserData(plane_id, 0, MyKey3))
print (client.getUserData(plane_id, 0, MyKey4))
print (client.getUserData(MyKey1))
print (client.getUserData(MyKey3))
print (client.getUserData(MyKey4))
print ("Syncing")
client.syncUserData()
print ("Retrieving user data")
print (client.getUserData(plane_id, 0, MyKey1))
print (client.getUserData(plane_id, 0, MyKey3))
print (client.getUserData(plane_id, 0, MyKey4))
print (client.getUserData(MyKey1))
print (client.getUserData(MyKey3))
print (client.getUserData(MyKey4))
plane_id2 = client.loadURDF(PLANE_PATH)
print ("Plane1: %s, plane2: %s" % (plane_id, plane_id2))
print ("Retrieving user data")
print (client.getUserData(plane_id, 0, MyKey1))
print (client.getUserData(plane_id, 0, MyKey3))
print (client.getUserData(plane_id, 0, MyKey4))
print (client.getUserData(MyKey1))
print (client.getUserData(MyKey3))
print (client.getUserData(MyKey4))

273
examples/pybullet/gym/pybullet_data/biped/biped2d_pybullet.urdf Normal file
View File

@@ -0,0 +1,273 @@
<?xml version="1.0"?>
<robot name="balance">
<material name="white">
<color rgba="1 1 1 1"/>
</material>
<material name="black">
<color rgba="0.2 0.2 0.2 1"/>
</material>
<material name="blue">
<color rgba="0 0 1 1"/>
</material>
<link name="world">
<inertial>
<mass value="0"/>
<inertia ixx="0.2125" ixy="-0.005" ixz="0.0225" iyy="0.205" iyz="0.045" izz="0.0125"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
</inertial>
</link>
<link name="y_prismatic">
<inertial>
<mass value="0.01"/>
<inertia ixx="0.2125" ixy="-0.005" ixz="0.0225" iyy="0.205" iyz="0.045" izz="0.0125"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
</inertial>
</link>
<joint name="y_to_world" type="prismatic">
<parent link="world"/>
<child link="y_prismatic"/>
<axis xyz="0 1 0"/>
<limit effort="0.0" lower="1" upper="-1" velocity="1000.0"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
</joint>
<link name="z_prismatic">
<inertial>
<mass value="0.01"/>
<inertia ixx="0.2125" ixy="-0.005" ixz="0.0225" iyy="0.205" iyz="0.045" izz="0.0125"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
</inertial>
</link>
<joint name="z_to_y" type="prismatic">
<parent link="y_prismatic"/>
<child link="z_prismatic"/>
<axis xyz="0 0 1"/>
<limit effort="0.0" lower="1" upper="-1" velocity="1000.0"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
</joint>
<joint name="torso_to_z" type="continuous">
<parent link="z_prismatic"/>
<child link="torso"/>
<axis xyz="1 0 0"/>
<origin rpy="0 0 0" xyz="0 0 1.4"/>
</joint>
<link name="torso">
<visual>
<geometry>
<box size="0.1 0.1 0.48"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0.0 0"/>
<material name="white"/>
</visual>
<collision>
<geometry>
<box size="0.1 0.1 0.48"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0.0 0"/>
<contact_coefficients mu="0.08" />
</collision>
<inertial>
<mass value="70"/>
<inertia ixx="0.2404" ixy="-0.01" ixz="-0.048" iyy="0.2404" iyz="-0.048" izz="0.02"/>
<origin rpy="0 0 0" xyz="0 0.0 0"/>
</inertial>
</link>
<link name="r_upperleg">
<visual>
<geometry>
<box size="0.05 0.1 .45"/>
</geometry>
<origin rpy="0 0 0" xyz="0.018 0. -0.21715"/>
<material name="black"/>
</visual>
<collision>
<geometry>
<box size="0.05 0.1 .45"/>
</geometry>
<origin rpy="0 0 0" xyz="0.018 -0.0 -0.21715"/>
<contact_coefficients mu="0.08" />
</collision>
<inertial>
<mass value="5"/>
<inertia ixx="0.2125" ixy="-0.005" ixz="0.0225" iyy="0.205" iyz="0.045" izz="0.0125"/>
<origin rpy="0 0 0" xyz="0.018 -0 -0.21715"/>
</inertial>
</link>
<joint name="torso_to_rightleg" type="revolute">
<parent link="torso"/>
<child link="r_upperleg"/>
<axis xyz="1 0 0"/>
<limit effort="0.0" lower="-1.57." upper="1.57" velocity="1000.0"/>
<origin rpy="0 0 0" xyz="0.05 0 -0.17"/>
</joint>
<link name="l_upperleg">
<visual>
<geometry>
<box size="0.05 0.1 .45"/>
</geometry>
<origin rpy="0 0 0" xyz="-0.018 0. -0.21715"/>
<material name="black"/>
</visual>
<collision>
<geometry>
<box size="0.05 0.1 .45"/>
</geometry>
<origin rpy="0 0 0" xyz="-0.018 0.0 -0.21715"/>
<contact_coefficients mu="0.08" />
</collision>
<inertial>
<mass value="5"/>
<inertia ixx="0.2125" ixy="-0.005" ixz="0.0225" iyy="0.205" iyz="0.045" izz="0.0125"/>
<origin rpy="0 0 0" xyz="-0.018 -0 -0.21715"/>
</inertial>
</link>
<joint name="torso_to_leftleg" type="revolute">
<parent link="torso"/>
<child link="l_upperleg"/>
<axis xyz="1 0 0"/>
<limit effort="10.0" lower="-1.57." upper="1.57" velocity="1000.0"/>
<origin rpy="0 0 0" xyz="-0.05 0 -0.17"/>
</joint>
<link name="r_lowerleg">
<visual>
<geometry>
<box size="0.05 0.1 .45"/>
</geometry>
<origin rpy="0 0 0" xyz="0.048 0. -0.21715"/>
<material name="blue"/>
</visual>
<collision>
<geometry>
<box size="0.05 0.1 .45"/>
</geometry>
<origin rpy="0 0 0" xyz="0.048 0.0 -0.21715"/>
<contact_coefficients mu="0.08" />
</collision>
<inertial>
<mass value="4"/>
<inertia ixx="0.2125" ixy="-0.005" ixz="0.0225" iyy="0.205" iyz="0.045" izz="0.0125"/>
<origin rpy="0 0 0" xyz="0.048 -0 -0.21715"/>
</inertial>
</link>
<joint name="r_knee" type="revolute">
<parent link="r_upperleg"/>
<child link="r_lowerleg"/>
<axis xyz="1 0 0"/>
<limit effort="10.0" lower="-1.57." upper="1.57" velocity="1000.0"/>
<origin rpy="0 0 0" xyz="0.015 0 -.41"/>
</joint>
<link name="l_lowerleg">
<visual>
<geometry>
<box size="0.05 0.1 .45"/>
</geometry>
<origin rpy="0 0 0" xyz="-0.048 0. -0.21715"/>
<material name="blue"/>
</visual>
<collision>
<geometry>
<box size="0.1 0.1 .45"/>
</geometry>
<origin rpy="0 0 0" xyz="-0.048 0.0 -0.21715"/>
<contact_coefficients mu="0.08" />
</collision>
<inertial>
<mass value="4"/>
<inertia ixx="0.2125" ixy="-0.005" ixz="0.0225" iyy="0.205" iyz="0.045" izz="0.0125"/>
<origin rpy="0 0 0" xyz="-0.048 -0 -0.21715"/>
</inertial>
</link>
<joint name="l_knee" type="revolute">
<parent link="l_upperleg"/>
<child link="l_lowerleg"/>
<axis xyz="1 0 0"/>
<limit effort="10.0" lower="-1.57." upper="1.57" velocity="1000.0"/>
<origin rpy="0 0 0" xyz="-0.015 0 -.41"/>
</joint>
<link name="l_foot">
<visual>
<geometry>
<box size="0.05 0.2 .04"/>
</geometry>
<origin rpy="0 0 0" xyz="0.05 0.08 -0.038"/>
<material name="blue"/>
</visual>
<collision>
<geometry>
<box size="0.05 0.2 .04"/>
</geometry>
<origin rpy="0 0 0" xyz="0.05 0.08 -0.038"/>
<contact_coefficients mu="0.5" />
</collision>
<inertial>
<mass value="1"/>
<inertia ixx="0.0416" ixy="-0.01" ixz="-0.002" iyy="0.0041" iyz="-0.008" izz="0.0425"/>
<origin rpy="0 0 0" xyz="0.05 0.08 -0.038"/>
</inertial>
</link>
<joint name="l_ankle" type="revolute">
<parent link="l_lowerleg"/>
<child link="l_foot"/>
<axis xyz="1 0 0"/>
<limit effort="10.0" lower="-2" upper="2" velocity="1000.0"/>
<origin rpy="0 0 0" xyz="-0.05 -0.03 -.459"/>
</joint>
<link name="r_foot">
<visual>
<geometry>
<box size="0.05 0.2 .04"/>
</geometry>
<origin rpy="0 0 0" xyz="-0.05 0.08 -0.038"/>
<material name="blue"/>
</visual>
<collision>
<geometry>
<box size="0.05 0.2 .04"/>
</geometry>
<origin rpy="0 0 0" xyz="-0.05 0.08 -0.038"/>
<contact_coefficients mu="0.5" />
</collision>
<inertial>
<mass value="1"/>
<inertia ixx="0.0416" ixy="-0.01" ixz="-0.002" iyy="0.0041" iyz="-0.008" izz="0.0425"/>
<origin rpy="0 0 0" xyz="-0.05 0.08 -0.038"/>
</inertial>
</link>
<joint name="r_ankle" type="revolute">
<parent link="r_lowerleg"/>
<child link="r_foot"/>
<axis xyz="1 0 0"/>
<limit effort="10.0" lower="-2." upper="2" velocity="1000.0"/>
<origin rpy="0 0 0" xyz="0.05 -0.03 -.459"/>
</joint>
</robot>

122
examples/pybullet/pybullet.c
View File

@@ -5,6 +5,14 @@
#include "../SharedMemory/PhysicsClientUDP_C_API.h"
#endif //BT_ENABLE_ENET
#ifdef BT_ENABLE_DART
#include "../SharedMemory/dart/DARTPhysicsC_API.h"
#endif
#ifdef BT_ENABLE_MUJOCO
#include "../SharedMemory/mujoco/MuJoCoPhysicsC_API.h"
#endif
#ifdef BT_ENABLE_CLSOCKET
#include "../SharedMemory/PhysicsClientTCP_C_API.h"
#endif //BT_ENABLE_CLSOCKET
@@ -407,6 +415,22 @@ static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args, P
sm = b3ConnectPhysicsDirect();
break;
}
#ifdef BT_ENABLE_DART
case eCONNECT_DART:
{
sm = b3ConnectPhysicsDART();
break;
}
#endif
#ifdef BT_ENABLE_MUJOCO
case eCONNECT_MUJOCO:
{
sm = b3ConnectPhysicsMuJoCo();
break;
}
#endif
case eCONNECT_SHARED_MEMORY:
{
sm = b3ConnectSharedMemory(key);
@@ -689,18 +713,19 @@ static PyObject* pybullet_addUserData(PyObject* self, PyObject* args, PyObject*
b3PhysicsClientHandle sm = 0;
int physicsClientId = 0;
int bodyUniqueId = -1;
int linkIndex = -2;
int linkIndex = -1;
int visualShapeIndex = -1;
const char* key = "";
const char* value = ""; // TODO: Change this to a PyObject and detect the type dynamically.
static char* kwlist[] = {"bodyUniqueId", "linkIndex", "key", "value", "physicsClientId", NULL};
static char* kwlist[] = {"bodyUniqueId", "key", "value", "linkIndex", "visualShapeIndex", "physicsClientId", NULL};
b3SharedMemoryCommandHandle command;
b3SharedMemoryStatusHandle statusHandle;
int statusType;
int userDataId;
int valueLen=-1;
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iiss|i", kwlist, &bodyUniqueId, &linkIndex, &key, &value, &physicsClientId))
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iss|iii", kwlist, &bodyUniqueId, &key, &value, &linkIndex, &visualShapeIndex, &physicsClientId))
{
return NULL;
}
@@ -712,7 +737,7 @@ static PyObject* pybullet_addUserData(PyObject* self, PyObject* args, PyObject*
}
valueLen = strlen(value)+1;
command = b3InitAddUserDataCommand(sm, bodyUniqueId, linkIndex, key, USER_DATA_VALUE_TYPE_STRING, valueLen, value);
command = b3InitAddUserDataCommand(sm, bodyUniqueId, linkIndex, visualShapeIndex, key, USER_DATA_VALUE_TYPE_STRING, valueLen, value);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
statusType = b3GetStatusType(statusHandle);
@@ -730,16 +755,14 @@ static PyObject* pybullet_removeUserData(PyObject* self, PyObject* args, PyObjec
{
b3PhysicsClientHandle sm = 0;
int physicsClientId = 0;
int bodyUniqueId = -1;
int linkIndex = -1;
int userDataId = -1;
static char* kwlist[] = {"bodyUniqueId", "linkIndex", "userDataId", "physicsClientId", NULL};
static char* kwlist[] = {"userDataId", "physicsClientId", NULL};
b3SharedMemoryCommandHandle command;
b3SharedMemoryStatusHandle statusHandle;
int statusType;
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iii|i", kwlist, &bodyUniqueId, &linkIndex, &userDataId, &physicsClientId))
if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist, &userDataId, &physicsClientId))
{
return NULL;
}
@@ -750,7 +773,7 @@ static PyObject* pybullet_removeUserData(PyObject* self, PyObject* args, PyObjec
return NULL;
}
command = b3InitRemoveUserDataCommand(sm, bodyUniqueId, linkIndex, userDataId);
command = b3InitRemoveUserDataCommand(sm, userDataId);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
statusType = b3GetStatusType(statusHandle);
@@ -770,15 +793,16 @@ static PyObject* pybullet_getUserDataId(PyObject* self, PyObject* args, PyObject
int physicsClientId = 0;
int bodyUniqueId = -1;
int linkIndex = -1;
int visualShapeIndex = -1;
const char* key = "";
int userDataId;
static char* kwlist[] = {"bodyUniqueId", "linkIndex", "key", "physicsClientId", NULL};
static char* kwlist[] = {"bodyUniqueId", "key", "linkIndex", "visualShapeIndex", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iis|i", kwlist, &bodyUniqueId, &linkIndex, &key, &physicsClientId))
if (!PyArg_ParseTupleAndKeywords(args, keywds, "is|iii", kwlist, &bodyUniqueId, &key, &linkIndex, &visualShapeIndex, &physicsClientId))
{
return NULL;
}
@@ -789,7 +813,7 @@ static PyObject* pybullet_getUserDataId(PyObject* self, PyObject* args, PyObject
return NULL;
}
userDataId = b3GetUserDataId(sm, bodyUniqueId, linkIndex, key);
userDataId = b3GetUserDataId(sm, bodyUniqueId, linkIndex, visualShapeIndex, key);
return PyInt_FromLong(userDataId);
}
@@ -797,16 +821,14 @@ static PyObject* pybullet_getUserData(PyObject* self, PyObject* args, PyObject*
{
b3PhysicsClientHandle sm = 0;
int physicsClientId = 0;
int bodyUniqueId = -1;
int linkIndex = -1;
int userDataId = -1;
static char* kwlist[] = {"bodyUniqueId", "linkIndex", "userDataId", "physicsClientId", NULL};
static char* kwlist[] = {"userDataId", "physicsClientId", NULL};
struct b3UserDataValue value;
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iii|i", kwlist, &bodyUniqueId, &linkIndex, &userDataId, &physicsClientId))
if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist, &userDataId, &physicsClientId))
{
return NULL;
}
@@ -818,7 +840,7 @@ static PyObject* pybullet_getUserData(PyObject* self, PyObject* args, PyObject*
}
if (!b3GetUserData(sm, bodyUniqueId, linkIndex, userDataId, &value)) {
if (!b3GetUserData(sm, userDataId, &value)) {
Py_INCREF(Py_None);
return Py_None;
@@ -837,15 +859,14 @@ static PyObject* pybullet_getNumUserData(PyObject* self, PyObject* args, PyObjec
b3PhysicsClientHandle sm = 0;
int physicsClientId = 0;
int bodyUniqueId = -1;
int linkIndex = -1;
static char* kwlist[] = {"bodyUniqueId", "linkIndex", "physicsClientId", NULL};
static char* kwlist[] = {"bodyUniqueId", "physicsClientId", NULL};
int numUserData;
if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|i", kwlist, &bodyUniqueId, &linkIndex, &physicsClientId))
if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist, &bodyUniqueId, &physicsClientId))
{
return NULL;
}
@@ -856,7 +877,7 @@ static PyObject* pybullet_getNumUserData(PyObject* self, PyObject* args, PyObjec
return NULL;
}
numUserData = b3GetNumUserData(sm, bodyUniqueId, linkIndex);
numUserData = b3GetNumUserData(sm, bodyUniqueId);
return PyInt_FromLong(numUserData);
}
@@ -865,16 +886,17 @@ static PyObject* pybullet_getUserDataInfo(PyObject* self, PyObject* args, PyObje
b3PhysicsClientHandle sm = 0;
int physicsClientId = 0;
int bodyUniqueId = -1;
int linkIndex = -1;
int userDataIndex = -1;
int linkIndex = -1;
int visualShapeIndex = -1;
static char* kwlist[] = {"bodyUniqueId", "linkIndex", "userDataIndex", "physicsClientId", NULL};
static char* kwlist[] = {"bodyUniqueId", "userDataIndex", "physicsClientId", NULL};
const char* key = 0;
int userDataId = -1;
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iii|i", kwlist, &bodyUniqueId, &linkIndex, &userDataIndex, &physicsClientId))
if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|i", kwlist, &bodyUniqueId, &userDataIndex, &physicsClientId))
{
return NULL;
}
@@ -885,16 +907,19 @@ static PyObject* pybullet_getUserDataInfo(PyObject* self, PyObject* args, PyObje
return NULL;
}
b3GetUserDataInfo(sm, bodyUniqueId, linkIndex, userDataIndex, &key, &userDataId);
b3GetUserDataInfo(sm, bodyUniqueId, userDataIndex, &key, &userDataId, &linkIndex, &visualShapeIndex);
if (key == 0 || userDataId == -1) {
PyErr_SetString(SpamError, "Could not get user data info.");
return NULL;
}
{
PyObject *userDataInfoTuple = PyTuple_New(2);
PyObject *userDataInfoTuple = PyTuple_New(5);
PyTuple_SetItem(userDataInfoTuple, 0, PyInt_FromLong(userDataId));
PyTuple_SetItem(userDataInfoTuple, 1, PyString_FromString(key));
PyTuple_SetItem(userDataInfoTuple, 2, PyInt_FromLong(bodyUniqueId));
PyTuple_SetItem(userDataInfoTuple, 3, PyInt_FromLong(linkIndex));
PyTuple_SetItem(userDataInfoTuple, 4, PyInt_FromLong(visualShapeIndex));
return userDataInfoTuple;
}
}
@@ -5428,8 +5453,10 @@ static PyObject* pybullet_getVisualShapeData(PyObject* self, PyObject* args, PyO
PyObject* pyResultList = 0;
int physicsClientId = 0;
b3PhysicsClientHandle sm = 0;
static char* kwlist[] = {"objectUniqueId", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist, &objectUniqueId, &physicsClientId))
int flags=0;
static char* kwlist[] = {"objectUniqueId", "flags", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|ii", kwlist, &objectUniqueId, &flags, &physicsClientId))
{
return NULL;
}
@@ -5450,7 +5477,9 @@ static PyObject* pybullet_getVisualShapeData(PyObject* self, PyObject* args, PyO
pyResultList = PyTuple_New(visualShapeInfo.m_numVisualShapes);
for (i = 0; i < visualShapeInfo.m_numVisualShapes; i++)
{
PyObject* visualShapeObList = PyTuple_New(8);
int numFields = flags&eVISUAL_SHAPE_DATA_TEXTURE_UNIQUE_IDS ? 9 : 8;
PyObject* visualShapeObList = PyTuple_New(numFields);
PyObject* item;
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_objectUniqueId);
PyTuple_SetItem(visualShapeObList, 0, item);
@@ -5511,6 +5540,11 @@ static PyObject* pybullet_getVisualShapeData(PyObject* self, PyObject* args, PyO
PyTuple_SetItem(rgba, 3, item);
PyTuple_SetItem(visualShapeObList, 7, rgba);
}
if (flags&eVISUAL_SHAPE_DATA_TEXTURE_UNIQUE_IDS)
{
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_textureUniqueId);
PyTuple_SetItem(visualShapeObList, 8, item);
}
PyTuple_SetItem(pyResultList, i, visualShapeObList);
}
@@ -9131,28 +9165,28 @@ static PyMethodDef SpamMethods[] = {
"Update user data, in case other clients made changes."},
{"addUserData", (PyCFunction)pybullet_addUserData, METH_VARARGS | METH_KEYWORDS,
"addUserData(bodyUniqueId, linkIndex, key, value, physicsClientId=0)\n"
"Adds or updates a user data entry to a link. Returns user data identifier."},
"addUserData(bodyUniqueId, key, value, linkIndex=-1, visualShapeIndex=-1, physicsClientId=0)\n"
"Adds or updates a user data entry. Returns user data identifier."},
{"getUserData", (PyCFunction)pybullet_getUserData, METH_VARARGS | METH_KEYWORDS,
"getUserData(bodyUniqueId, linkIndex, userDataId, physicsClientId=0)\n"
"getUserData(userDataId, physicsClientId=0)\n"
"Returns the user data value."},
{"removeUserData", (PyCFunction)pybullet_removeUserData, METH_VARARGS | METH_KEYWORDS,
"removeUserData(bodyUniqueId, linkIndex, userDataId, physicsClientId=0)\n"
"removeUserData(userDataId, physicsClientId=0)\n"
"Removes a user data entry."},
{"getUserDataId", (PyCFunction)pybullet_getUserDataId, METH_VARARGS | METH_KEYWORDS,
"getUserDataId(bodyUniqueId, linkIndex, key, physicsClientId=0)\n"
"Retrieves the userDataId on a link given the key."},
"getUserDataId(bodyUniqueId, key, linkIndex=-1, visualShapeIndex=-1, physicsClientId=0)\n"
"Retrieves the userDataId given the key and optionally link and visual shape index."},
{"getNumUserData", (PyCFunction)pybullet_getNumUserData, METH_VARARGS | METH_KEYWORDS,
"getNumUserData(bodyUniqueId, linkIndex, physicsClientId=0)\n"
"Retrieves the number of user data entries in a link."},
"getNumUserData(bodyUniqueId physicsClientId=0)\n"
"Retrieves the number of user data entries in a body."},
{"getUserDataInfo", (PyCFunction)pybullet_getUserDataInfo, METH_VARARGS | METH_KEYWORDS,
"getUserDataInfo(bodyUniqueId, linkIndex, userDataIndex, physicsClientId=0)\n"
"Retrieves the key and the identifier of a user data as (id, key)."},
"getUserDataInfo(bodyUniqueId, userDataIndex, physicsClientId=0)\n"
"Retrieves the key and the identifier of a user data as (userDataId, key, bodyUniqueId, linkIndex, visualShapeIndex)."},
{"removeBody", (PyCFunction)pybullet_removeBody, METH_VARARGS | METH_KEYWORDS,
"Remove a body by its body unique id."},
@@ -9549,6 +9583,14 @@ initpybullet(void)
PyModule_AddIntConstant(m, "GUI_SERVER", eCONNECT_GUI_SERVER); // user read
PyModule_AddIntConstant(m, "GUI_MAIN_THREAD", eCONNECT_GUI_MAIN_THREAD); // user read
PyModule_AddIntConstant(m, "SHARED_MEMORY_SERVER", eCONNECT_SHARED_MEMORY_SERVER); // user read
#ifdef BT_ENABLE_DART
PyModule_AddIntConstant(m, "DART", eCONNECT_DART); // user read
#endif
#ifdef BT_ENABLE_MUJOCO
PyModule_AddIntConstant(m, "MuJoCo", eCONNECT_MUJOCO); // user read
#endif
PyModule_AddIntConstant(m, "SHARED_MEMORY_KEY", SHARED_MEMORY_KEY);
PyModule_AddIntConstant(m, "SHARED_MEMORY_KEY2", SHARED_MEMORY_KEY+1);
@@ -9656,6 +9698,8 @@ initpybullet(void)
PyModule_AddIntConstant(m, "URDF_USE_SELF_COLLISION_EXCLUDE_PARENT", URDF_USE_SELF_COLLISION_EXCLUDE_PARENT);
PyModule_AddIntConstant(m, "URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS", URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS);
PyModule_AddIntConstant(m, "VISUAL_SHAPE_DATA_TEXTURE_UNIQUE_IDS", eVISUAL_SHAPE_DATA_TEXTURE_UNIQUE_IDS);
PyModule_AddIntConstant(m, "MAX_RAY_INTERSECTION_BATCH_SIZE", MAX_RAY_INTERSECTION_BATCH_SIZE_STREAMING);
PyModule_AddIntConstant(m, "B3G_F1", B3G_F1);

235
examples/pybullet/unittests/userDataTest.py
View File

@@ -26,169 +26,170 @@ class TestUserDataMethods(unittest.TestCase):
def testAddUserData(self):
plane_id = self.client.loadURDF(PLANE_PATH)
uid1 = self.client.addUserData(plane_id, 0, "MyKey1", "MyValue1")
uid2 = self.client.addUserData(plane_id, 0, "MyKey2", "MyValue2")
uid3 = self.client.addUserData(plane_id, 0, "MyKey3", "MyValue3")
uid4 = self.client.addUserData(plane_id, 0, "MyKey4", "MyValue4")
uid1 = self.client.addUserData(plane_id, "MyKey1", "MyValue1")
uid2 = self.client.addUserData(plane_id, "MyKey2", "MyValue2")
uid3 = self.client.addUserData(plane_id, "MyKey3", "MyValue3")
uid4 = self.client.addUserData(plane_id, "MyKey4", "MyValue4")
# Retrieve user data and make sure it's correct.
self.assertEqual(b"MyValue1", self.client.getUserData(plane_id, 0, uid1))
self.assertEqual(b"MyValue2", self.client.getUserData(plane_id, 0, uid2))
self.assertEqual(b"MyValue3", self.client.getUserData(plane_id, 0, uid3))
self.assertEqual(b"MyValue4", self.client.getUserData(plane_id, 0, uid4))
self.assertEqual(b"MyValue1", self.client.getUserData(uid1))
self.assertEqual(b"MyValue2", self.client.getUserData(uid2))
self.assertEqual(b"MyValue3", self.client.getUserData(uid3))
self.assertEqual(b"MyValue4", self.client.getUserData(uid4))
# Disconnect/reconnect and make sure that the user data is synced back.
del self.client
self.client = bullet_client.BulletClient(pybullet.SHARED_MEMORY)
self.assertEqual(b"MyValue1", self.client.getUserData(plane_id, 0, uid1))
self.assertEqual(b"MyValue2", self.client.getUserData(plane_id, 0, uid2))
self.assertEqual(b"MyValue3", self.client.getUserData(plane_id, 0, uid3))
self.assertEqual(b"MyValue4", self.client.getUserData(plane_id, 0, uid4))
self.assertEqual(b"MyValue1", self.client.getUserData(uid1))
self.assertEqual(b"MyValue2", self.client.getUserData(uid2))
self.assertEqual(b"MyValue3", self.client.getUserData(uid3))
self.assertEqual(b"MyValue4", self.client.getUserData(uid4))
self.client.resetSimulation()
self.assertEqual(None, self.client.getUserData(plane_id, 0, uid1))
self.assertEqual(None, self.client.getUserData(plane_id, 0, uid2))
self.assertEqual(None, self.client.getUserData(plane_id, 0, uid3))
self.assertEqual(None, self.client.getUserData(plane_id, 0, uid4))
self.assertEqual(None, self.client.getUserData(uid1))
self.assertEqual(None, self.client.getUserData(uid2))
self.assertEqual(None, self.client.getUserData(uid3))
self.assertEqual(None, self.client.getUserData(uid4))
def testGetNumUserData(self):
plane_id = self.client.loadURDF(PLANE_PATH)
uid1 = self.client.addUserData(plane_id, 0, "MyKey1", "MyValue1")
uid2 = self.client.addUserData(plane_id, 0, "MyKey2", "MyValue2")
uid3 = self.client.addUserData(plane_id, 0, "MyKey3", "MyValue3")
uid4 = self.client.addUserData(plane_id, 0, "MyKey4", "MyValue4")
uid1 = self.client.addUserData(plane_id, "MyKey1", "MyValue1")
uid2 = self.client.addUserData(plane_id, "MyKey2", "MyValue2")
uid3 = self.client.addUserData(plane_id, "MyKey3", "MyValue3")
uid4 = self.client.addUserData(plane_id, "MyKey4", "MyValue4")
self.assertEqual(4, self.client.getNumUserData(plane_id, 0))
self.assertEqual(4, self.client.getNumUserData(plane_id))
del self.client
self.client = bullet_client.BulletClient(pybullet.SHARED_MEMORY)
self.assertEqual(4, self.client.getNumUserData(plane_id, 0))
self.assertEqual(4, self.client.getNumUserData(plane_id))
def testReplaceUserData(self):
plane_id = self.client.loadURDF(PLANE_PATH)
uid = self.client.addUserData(plane_id, 0, "MyKey", "MyValue")
uid = self.client.addUserData(plane_id, "MyKey", "MyValue")
self.assertEqual(b"MyValue", self.client.getUserData(plane_id, 0, uid))
self.assertEqual(b"MyValue", self.client.getUserData(uid))
new_uid = self.client.addUserData(plane_id, 0, "MyKey", "MyNewValue")
new_uid = self.client.addUserData(plane_id, "MyKey", "MyNewValue")
self.assertEqual(uid, new_uid)
self.assertEqual(b"MyNewValue", self.client.getUserData(plane_id, 0, uid))
self.assertEqual(b"MyNewValue", self.client.getUserData(uid))
del self.client
self.client = bullet_client.BulletClient(pybullet.SHARED_MEMORY)
self.assertEqual(b"MyNewValue", self.client.getUserData(plane_id, 0, uid))
self.assertEqual(b"MyNewValue", self.client.getUserData(uid))
def testGetUserDataId(self):
plane_id = self.client.loadURDF(PLANE_PATH)
uid1 = self.client.addUserData(plane_id, 0, "MyKey1", "MyValue1")
uid2 = self.client.addUserData(plane_id, 0, "MyKey2", "MyValue2")
uid3 = self.client.addUserData(plane_id, 0, "MyKey3", "MyValue3")
uid4 = self.client.addUserData(plane_id, 0, "MyKey4", "MyValue4")
uid1 = self.client.addUserData(plane_id, "MyKey1", "MyValue1")
uid2 = self.client.addUserData(plane_id, "MyKey2", "MyValue2")
uid3 = self.client.addUserData(plane_id, "MyKey3", "MyValue3")
uid4 = self.client.addUserData(plane_id, "MyKey4", "MyValue4")
self.assertEqual(uid1, self.client.getUserDataId(plane_id, 0, "MyKey1"))
self.assertEqual(uid2, self.client.getUserDataId(plane_id, 0, "MyKey2"))
self.assertEqual(uid3, self.client.getUserDataId(plane_id, 0, "MyKey3"))
self.assertEqual(uid4, self.client.getUserDataId(plane_id, 0, "MyKey4"))
self.assertEqual(uid1, self.client.getUserDataId(plane_id, "MyKey1"))
self.assertEqual(uid2, self.client.getUserDataId(plane_id, "MyKey2"))
self.assertEqual(uid3, self.client.getUserDataId(plane_id, "MyKey3"))
self.assertEqual(uid4, self.client.getUserDataId(plane_id, "MyKey4"))
del self.client
self.client = bullet_client.BulletClient(pybullet.SHARED_MEMORY)
self.assertEqual(uid1, self.client.getUserDataId(plane_id, 0, "MyKey1"))
self.assertEqual(uid2, self.client.getUserDataId(plane_id, 0, "MyKey2"))
self.assertEqual(uid3, self.client.getUserDataId(plane_id, 0, "MyKey3"))
self.assertEqual(uid4, self.client.getUserDataId(plane_id, 0, "MyKey4"))
self.assertEqual(uid1, self.client.getUserDataId(plane_id, "MyKey1"))
self.assertEqual(uid2, self.client.getUserDataId(plane_id, "MyKey2"))
self.assertEqual(uid3, self.client.getUserDataId(plane_id, "MyKey3"))
self.assertEqual(uid4, self.client.getUserDataId(plane_id, "MyKey4"))
def testRemoveUserData(self):
plane_id = self.client.loadURDF(PLANE_PATH)
uid1 = self.client.addUserData(plane_id, 0, "MyKey1", "MyValue1")
uid2 = self.client.addUserData(plane_id, 0, "MyKey2", "MyValue2")
uid3 = self.client.addUserData(plane_id, 0, "MyKey3", "MyValue3")
uid4 = self.client.addUserData(plane_id, 0, "MyKey4", "MyValue4")
uid1 = self.client.addUserData(plane_id, "MyKey1", "MyValue1")
uid2 = self.client.addUserData(plane_id, "MyKey2", "MyValue2")
uid3 = self.client.addUserData(plane_id, "MyKey3", "MyValue3")
uid4 = self.client.addUserData(plane_id, "MyKey4", "MyValue4")
self.client.removeUserData(plane_id, 0, uid2)
self.client.removeUserData(uid2)
self.assertEqual(3, self.client.getNumUserData(plane_id, 0))
self.assertEqual(-1, self.client.getUserDataId(plane_id, 0, "MyKey2"))
self.assertEqual(None, self.client.getUserData(plane_id, 0, uid2))
self.assertEqual(b"MyValue1", self.client.getUserData(plane_id, 0, uid1))
self.assertEqual(b"MyValue3", self.client.getUserData(plane_id, 0, uid3))
self.assertEqual(b"MyValue4", self.client.getUserData(plane_id, 0, uid4))
self.assertEqual(3, self.client.getNumUserData(plane_id))
self.assertEqual(-1, self.client.getUserDataId(plane_id, "MyKey2"))
self.assertEqual(None, self.client.getUserData(uid2))
self.assertEqual(b"MyValue1", self.client.getUserData(uid1))
self.assertEqual(b"MyValue3", self.client.getUserData(uid3))
self.assertEqual(b"MyValue4", self.client.getUserData(uid4))
del self.client
self.client = bullet_client.BulletClient(pybullet.SHARED_MEMORY)
self.assertEqual(3, self.client.getNumUserData(plane_id, 0))
self.assertEqual(-1, self.client.getUserDataId(plane_id, 0, "MyKey2"))
self.assertEqual(None, self.client.getUserData(plane_id, 0, uid2))
self.assertEqual(b"MyValue1", self.client.getUserData(plane_id, 0, uid1))
self.assertEqual(b"MyValue3", self.client.getUserData(plane_id, 0, uid3))
self.assertEqual(b"MyValue4", self.client.getUserData(plane_id, 0, uid4))
self.assertEqual(3, self.client.getNumUserData(plane_id))
self.assertEqual(-1, self.client.getUserDataId(plane_id, "MyKey2"))
self.assertEqual(None, self.client.getUserData(uid2))
self.assertEqual(b"MyValue1", self.client.getUserData(uid1))
self.assertEqual(b"MyValue3", self.client.getUserData(uid3))
self.assertEqual(b"MyValue4", self.client.getUserData(uid4))
def testIterateAllUserData(self):
plane_id = self.client.loadURDF(PLANE_PATH)
uid1 = self.client.addUserData(plane_id, 0, "MyKey1", "MyValue1")
uid2 = self.client.addUserData(plane_id, 0, "MyKey2", "MyValue2")
uid3 = self.client.addUserData(plane_id, 0, "MyKey3", "MyValue3")
uid4 = self.client.addUserData(plane_id, 0, "MyKey4", "MyValue4")
uid1 = self.client.addUserData(plane_id, "MyKey1", "MyValue1")
uid2 = self.client.addUserData(plane_id, "MyKey2", "MyValue2")
uid3 = self.client.addUserData(plane_id, "MyKey3", "MyValue3")
uid4 = self.client.addUserData(plane_id, "MyKey4", "MyValue4")
entries = set()
for i in range(self.client.getNumUserData(plane_id, 0)):
userDataId, key = self.client.getUserDataInfo(plane_id, 0, i)
value = self.client.getUserData(plane_id, 0, userDataId);
entries.add((userDataId, key, value))
for i in range(self.client.getNumUserData(plane_id)):
userDataId, key, bodyId, linkIndex, visualShapeIndex = self.client.getUserDataInfo(plane_id, i)
value = self.client.getUserData(userDataId);
entries.add((userDataId, key, value, bodyId, linkIndex, visualShapeIndex))
self.assertTrue((uid1, b"MyKey1", b"MyValue1") in entries)
self.assertTrue((uid2, b"MyKey2", b"MyValue2") in entries)
self.assertTrue((uid3, b"MyKey3", b"MyValue3") in entries)
self.assertTrue((uid4, b"MyKey4", b"MyValue4") in entries)
self.assertTrue((uid1, b"MyKey1", b"MyValue1", plane_id, -1, -1) in entries)
self.assertTrue((uid2, b"MyKey2", b"MyValue2", plane_id, -1, -1) in entries)
self.assertTrue((uid3, b"MyKey3", b"MyValue3", plane_id, -1, -1) in entries)
self.assertTrue((uid4, b"MyKey4", b"MyValue4", plane_id, -1, -1) in entries)
self.assertEqual(4, len(entries))
def testRemoveBody(self):
plane_id = self.client.loadURDF(PLANE_PATH)
uid1 = self.client.addUserData(plane_id, 0, "MyKey1", "MyValue1")
uid2 = self.client.addUserData(plane_id, 0, "MyKey2", "MyValue2")
uid3 = self.client.addUserData(plane_id, 0, "MyKey3", "MyValue3")
uid4 = self.client.addUserData(plane_id, 0, "MyKey4", "MyValue4")
uid1 = self.client.addUserData(plane_id, "MyKey1", "MyValue1")
uid2 = self.client.addUserData(plane_id, "MyKey2", "MyValue2")
uid3 = self.client.addUserData(plane_id, "MyKey3", "MyValue3")
uid4 = self.client.addUserData(plane_id, "MyKey4", "MyValue4")
self.client.removeBody(plane_id)
self.assertEqual(None, self.client.getUserData(plane_id, 0, uid1))
self.assertEqual(None, self.client.getUserData(plane_id, 0, uid2))
self.assertEqual(None, self.client.getUserData(plane_id, 0, uid3))
self.assertEqual(None, self.client.getUserData(plane_id, 0, uid4))
self.assertEqual(None, self.client.getUserData(uid1))
self.assertEqual(None, self.client.getUserData(uid2))
self.assertEqual(None, self.client.getUserData(uid3))
self.assertEqual(None, self.client.getUserData(uid4))
del self.client
self.client = bullet_client.BulletClient(pybullet.SHARED_MEMORY)
self.assertEqual(None, self.client.getUserData(plane_id, 0, uid1))
self.assertEqual(None, self.client.getUserData(plane_id, 0, uid2))
self.assertEqual(None, self.client.getUserData(plane_id, 0, uid3))
self.assertEqual(None, self.client.getUserData(plane_id, 0, uid4))
self.assertEqual(None, self.client.getUserData(uid1))
self.assertEqual(None, self.client.getUserData(uid2))
self.assertEqual(None, self.client.getUserData(uid3))
self.assertEqual(None, self.client.getUserData(uid4))
def testMultipleBodies(self):
plane1 = self.client.loadURDF(PLANE_PATH)
plane2 = self.client.loadURDF(PLANE_PATH)
uid1 = self.client.addUserData(plane1, 0, "MyKey1", "This is plane 1 - 1")
uid2 = self.client.addUserData(plane1, 0, "MyKey2", "This is plane 1 - 2")
uid1 = self.client.addUserData(plane1, "MyKey1", "This is plane 1 - 1")
uid2 = self.client.addUserData(plane1, "MyKey2", "This is plane 1 - 2")
uid3 = self.client.addUserData(plane2, 0, "MyKey1", "This is plane 2 - 1")
uid4 = self.client.addUserData(plane2, 0, "MyKey2", "This is plane 2 - 2")
uid5 = self.client.addUserData(plane2, 0, "MyKey3", "This is plane 2 - 3")
uid3 = self.client.addUserData(plane2, "MyKey1", "This is plane 2 - 1")
uid4 = self.client.addUserData(plane2, "MyKey2", "This is plane 2 - 2")
uid5 = self.client.addUserData(plane2, "MyKey3", "This is plane 2 - 3")
self.assertEqual(b"This is plane 1 - 1", self.client.getUserData(plane1, 0, self.client.getUserDataId(plane1, 0, "MyKey1")))
self.assertEqual(b"This is plane 1 - 2", self.client.getUserData(plane1, 0, self.client.getUserDataId(plane1, 0, "MyKey2")))
self.assertEqual(b"This is plane 1 - 1", self.client.getUserData(self.client.getUserDataId(plane1, "MyKey1")))
self.assertEqual(b"This is plane 1 - 2", self.client.getUserData(self.client.getUserDataId(plane1, "MyKey2")))
self.assertEqual(b"This is plane 2 - 1", self.client.getUserData(plane2, 0, self.client.getUserDataId(plane2, 0, "MyKey1")))
self.assertEqual(b"This is plane 2 - 2", self.client.getUserData(plane2, 0, self.client.getUserDataId(plane2, 0, "MyKey2")))
self.assertEqual(b"This is plane 2 - 3", self.client.getUserData(plane2, 0, self.client.getUserDataId(plane2, 0, "MyKey3")))
self.assertEqual(b"This is plane 2 - 1", self.client.getUserData(self.client.getUserDataId(plane2, "MyKey1")))
self.assertEqual(b"This is plane 2 - 2", self.client.getUserData(self.client.getUserDataId(plane2, "MyKey2")))
self.assertEqual(b"This is plane 2 - 3", self.client.getUserData(self.client.getUserDataId(plane2, "MyKey3")))
def testMultipleLinks(self):
@@ -198,14 +199,15 @@ class TestUserDataMethods(unittest.TestCase):
self.assertTrue(num_links > 1)
for link_index in range(num_links):
uid1 = self.client.addUserData(body_id, link_index, "MyKey1", "Value1 for link %s" % link_index)
uid2 = self.client.addUserData(body_id, link_index, "MyKey2", "Value2 for link %s" % link_index)
uid1 = self.client.addUserData(body_id, "MyKey1", "Value1 for link %s" % link_index, link_index)
uid2 = self.client.addUserData(body_id, "MyKey2", "Value2 for link %s" % link_index, link_index)
for link_index in range(num_links):
uid1 = self.client.getUserDataId(body_id, link_index, "MyKey1")
uid2 = self.client.getUserDataId(body_id, link_index, "MyKey2")
self.assertEqual(("Value1 for link %s" % link_index).encode(), self.client.getUserData(body_id, link_index, uid1))
self.assertEqual(("Value2 for link %s" % link_index).encode(), self.client.getUserData(body_id, link_index, uid2))
uid1 = self.client.getUserDataId(body_id, "MyKey1", link_index)
uid2 = self.client.getUserDataId(body_id, "MyKey2", link_index)
self.assertEqual(("Value1 for link %s" % link_index).encode(), self.client.getUserData(uid1))
self.assertEqual(("Value2 for link %s" % link_index).encode(), self.client.getUserData(uid2))
def testMultipleClients(self):
client1 = self.client
@@ -215,25 +217,48 @@ class TestUserDataMethods(unittest.TestCase):
client2.syncBodyInfo()
# Add user data on client 1, check on client 1
uid = client1.addUserData(plane_id, 0, "MyKey", "MyValue")
self.assertEqual(None, client2.getUserData(plane_id, 0, uid))
uid = client1.addUserData(plane_id, "MyKey", "MyValue")
self.assertEqual(None, client2.getUserData(uid))
client2.syncUserData()
self.assertEqual(b"MyValue", client2.getUserData(plane_id, 0, uid))
self.assertEqual(b"MyValue", client2.getUserData(uid))
# Overwrite the value on client 2, check on client 1
client2.addUserData(plane_id, 0, "MyKey", "MyNewValue")
self.assertEqual(b"MyValue", client1.getUserData(plane_id, 0, uid))
client2.addUserData(plane_id, "MyKey", "MyNewValue")
self.assertEqual(b"MyValue", client1.getUserData(uid))
client1.syncUserData()
self.assertEqual(b"MyNewValue", client1.getUserData(plane_id, 0, uid))
self.assertEqual(b"MyNewValue", client1.getUserData(uid))
# Remove user data on client 1, check on client 2
client1.removeUserData(plane_id, 0, uid)
self.assertEqual(b"MyNewValue", client2.getUserData(plane_id, 0, uid))
client1.removeUserData(uid)
self.assertEqual(b"MyNewValue", client2.getUserData(uid))
client2.syncUserData()
self.assertEqual(None, client2.getUserData(plane_id, 0, uid))
self.assertEqual(None, client2.getUserData(uid))
del client2
def testUserDataOnVisualShapes(self):
body_id = self.client.loadURDF(ROBOT_PATH)
num_links = self.client.getNumJoints(body_id)
visual_shapes = self.client.getVisualShapeData(body_id)
self.assertTrue(num_links > 0)
self.assertTrue(len(visual_shapes) > 0)
user_data_entries = set()
for link_index in range(-1, num_links):
num_shapes = sum([1 for shape in visual_shapes if shape[1] == link_index])
for shape_index in range(num_shapes):
key = "MyKey"
value = "MyValue %s, %s" % (link_index, shape_index)
uid = self.client.addUserData(body_id, key, value, link_index, shape_index)
user_data_entries.add((uid, key, value.encode(), body_id, link_index, shape_index))
self.assertEqual(len(visual_shapes), self.client.getNumUserData(body_id))
for uid, key, value, body_id, link_index, shape_index in user_data_entries:
self.assertEqual(value, self.client.getUserData(uid))
self.assertEqual(uid, self.client.getUserDataId(body_id, key, link_index, shape_index))
if __name__ == "__main__":
unittest.main()