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

Code-style consistency improvement:

Browse Source 
Apply clang-format-all.sh using the _clang-format file through all the cpp/.h files.
make sure not to apply it to certain serialization structures, since some parser expects the * as part of the name, instead of type.
This commit contains no other changes aside from adding and applying clang-format-all.sh
This commit is contained in:
erwincoumans
2018-09-23 14:17:31 -07:00
parent b73b05e9fb
commit ab8f16961e
1773 changed files with 1081087 additions and 474249 deletions
Download Patch File Download Diff File Expand all files Collapse all files

19
test/SharedMemory/gtestwrap.cpp
View File

@@ -6,18 +6,15 @@ void myprintf(const char* bla)
{
}
int main(int argc, char **argv) {
b3SetCustomPrintfFunc(myprintf);
b3SetCustomWarningMessageFunc(myprintf);
int main(int argc, char** argv)
{
b3SetCustomPrintfFunc(myprintf);
b3SetCustomWarningMessageFunc(myprintf);
#if _MSC_VER
_CrtSetDbgFlag ( _CRTDBG_ALLOC_MEM_DF | _CRTDBG_LEAK_CHECK_DF );
//void *testWhetherMemoryLeakDetectionWorks = malloc(1);
_CrtSetDbgFlag(_CRTDBG_ALLOC_MEM_DF | _CRTDBG_LEAK_CHECK_DF);
//void *testWhetherMemoryLeakDetectionWorks = malloc(1);
#endif
::testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
::testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}

363
test/SharedMemory/test.c
View File

@@ -2,80 +2,79 @@
//#include "SharedMemoryCommands.h"
#ifdef PHYSICS_SHARED_MEMORY
#include "SharedMemory/PhysicsClientC_API.h"
#endif //PHYSICS_SHARED_MEMORY
#endif //PHYSICS_SHARED_MEMORY
#ifdef PHYSICS_UDP
#include "SharedMemory/PhysicsClientUDP_C_API.h"
#endif//PHYSICS_UDP
#endif //PHYSICS_UDP
#ifdef PHYSICS_TCP
#include "SharedMemory/PhysicsClientTCP_C_API.h"
#endif//PHYSICS_TCP
#endif //PHYSICS_TCP
#ifdef PHYSICS_LOOP_BACK
#include "SharedMemory/PhysicsLoopBackC_API.h"
#endif //PHYSICS_LOOP_BACK
#endif //PHYSICS_LOOP_BACK
#ifdef PHYSICS_SERVER_DIRECT
#include "SharedMemory/PhysicsDirectC_API.h"
#endif //PHYSICS_SERVER_DIRECT
#endif //PHYSICS_SERVER_DIRECT
#ifdef PHYSICS_IN_PROCESS_EXAMPLE_BROWSER
#include "SharedMemory/SharedMemoryInProcessPhysicsC_API.h"
#endif//PHYSICS_IN_PROCESS_EXAMPLE_BROWSER
#endif //PHYSICS_IN_PROCESS_EXAMPLE_BROWSER
#include "SharedMemory/SharedMemoryPublic.h"
#include "Bullet3Common/b3Logging.h"
#include <string.h>
#include <stdio.h>
#ifndef ENABLE_GTEST
#include <assert.h>
#define ASSERT_EQ(a,b) assert((a)==(b));
#define ASSERT_EQ(a, b) assert((a) == (b));
#else
#define printf
#endif
void testSharedMemory(b3PhysicsClientHandle sm)
{
int i, dofCount , posVarCount, ret ,numJoints ;
int sensorJointIndexLeft=-1;
int sensorJointIndexRight=-1;
int i, dofCount, posVarCount, ret, numJoints;
int sensorJointIndexLeft = -1;
int sensorJointIndexRight = -1;
const char* urdfFileName = "r2d2.urdf";
const char* sdfFileName = "kuka_iiwa/model.sdf";
double gravx=0, gravy=0, gravz=-9.8;
double timeStep = 1./60.;
double startPosX, startPosY,startPosZ;
double gravx = 0, gravy = 0, gravz = -9.8;
double timeStep = 1. / 60.;
double startPosX, startPosY, startPosZ;
int imuLinkIndex = -1;
int bodyIndex = -1;
if (b3CanSubmitCommand(sm))
{
{
b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm);
b3SharedMemoryStatusHandle statusHandle;
ret = b3PhysicsParamSetGravity(command, gravx,gravy, gravz);
ret = b3PhysicsParamSetTimeStep(command, timeStep);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED);
}
{
b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm);
b3SharedMemoryStatusHandle statusHandle;
ret = b3PhysicsParamSetGravity(command, gravx, gravy, gravz);
ret = b3PhysicsParamSetTimeStep(command, timeStep);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED);
}
{
b3SharedMemoryStatusHandle statusHandle;
int statusType;
int bodyIndicesOut[10];//MAX_SDF_BODIES = 10
int numJoints, numBodies;
{
b3SharedMemoryStatusHandle statusHandle;
int statusType;
int bodyIndicesOut[10]; //MAX_SDF_BODIES = 10
int numJoints, numBodies;
int bodyUniqueId;
b3SharedMemoryCommandHandle command = b3LoadSdfCommandInit(sm, sdfFileName);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
b3SharedMemoryCommandHandle command = b3LoadSdfCommandInit(sm, sdfFileName);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
statusType = b3GetStatusType(statusHandle);
ASSERT_EQ(statusType, CMD_SDF_LOADING_COMPLETED);
numBodies = b3GetStatusBodyIndices(statusHandle, bodyIndicesOut, 10);
ASSERT_EQ(numBodies,1);
bodyUniqueId = bodyIndicesOut[0];
ASSERT_EQ(numBodies, 1);
bodyUniqueId = bodyIndicesOut[0];
{
{
b3SharedMemoryStatusHandle statusHandle;
@@ -90,9 +89,9 @@ void testSharedMemory(b3PhysicsClientHandle sm)
}
}
}
numJoints = b3GetNumJoints(sm,bodyUniqueId);
ASSERT_EQ(numJoints,7);
numJoints = b3GetNumJoints(sm, bodyUniqueId);
ASSERT_EQ(numJoints, 7);
#if 0
b3Printf("numJoints: %d\n", numJoints);
@@ -105,122 +104,116 @@ void testSharedMemory(b3PhysicsClientHandle sm)
}
}
#endif
{
b3SharedMemoryStatusHandle statusHandle;
b3SharedMemoryCommandHandle commandHandle;
double jointAngle = 0.f;
int jointIndex;
commandHandle = b3CreatePoseCommandInit(sm, bodyUniqueId);
for (jointIndex=0;jointIndex<numJoints;jointIndex++)
{
b3CreatePoseCommandSetJointPosition(sm, commandHandle, jointIndex, jointAngle);
}
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
{
b3SharedMemoryStatusHandle statusHandle;
b3SharedMemoryCommandHandle commandHandle;
double jointAngle = 0.f;
int jointIndex;
commandHandle = b3CreatePoseCommandInit(sm, bodyUniqueId);
for (jointIndex = 0; jointIndex < numJoints; jointIndex++)
{
b3CreatePoseCommandSetJointPosition(sm, commandHandle, jointIndex, jointAngle);
}
ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED);
}
}
{
b3SharedMemoryStatusHandle statusHandle;
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED);
}
}
{
b3SharedMemoryStatusHandle statusHandle;
int statusType;
b3SharedMemoryCommandHandle command = b3LoadUrdfCommandInit(sm, urdfFileName);
//setting the initial position, orientation and other arguments are optional
startPosX =2;
startPosY =0;
startPosZ = 1;
ret = b3LoadUrdfCommandSetStartPosition(command, startPosX,startPosY,startPosZ);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
//setting the initial position, orientation and other arguments are optional
startPosX = 2;
startPosY = 0;
startPosZ = 1;
ret = b3LoadUrdfCommandSetStartPosition(command, startPosX, startPosY, startPosZ);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
statusType = b3GetStatusType(statusHandle);
ASSERT_EQ(statusType, CMD_URDF_LOADING_COMPLETED);
bodyIndex = b3GetStatusBodyIndex(statusHandle);
}
if (bodyIndex>=0)
{
}
numJoints = b3GetNumJoints(sm,bodyIndex);
for (i=0;i<numJoints;i++)
if (bodyIndex >= 0)
{
numJoints = b3GetNumJoints(sm, bodyIndex);
for (i = 0; i < numJoints; i++)
{
struct b3JointInfo jointInfo;
b3GetJointInfo(sm,bodyIndex, i,&jointInfo);
// printf("jointInfo[%d].m_jointName=%s\n",i,jointInfo.m_jointName);
b3GetJointInfo(sm, bodyIndex, i, &jointInfo);
// printf("jointInfo[%d].m_jointName=%s\n",i,jointInfo.m_jointName);
//pick the IMU link index based on torso name
if (strstr(jointInfo.m_linkName,"base_link"))
if (strstr(jointInfo.m_linkName, "base_link"))
{
imuLinkIndex = i;
}
//pick the joint index based on joint name
if (strstr(jointInfo.m_jointName,"base_to_left_leg"))
if (strstr(jointInfo.m_jointName, "base_to_left_leg"))
{
sensorJointIndexLeft = i;
}
if (strstr(jointInfo.m_jointName,"base_to_right_leg"))
if (strstr(jointInfo.m_jointName, "base_to_right_leg"))
{
sensorJointIndexRight = i;
}
}
if ((sensorJointIndexLeft>=0) || (sensorJointIndexRight>=0))
if ((sensorJointIndexLeft >= 0) || (sensorJointIndexRight >= 0))
{
b3SharedMemoryCommandHandle command = b3CreateSensorCommandInit(sm, bodyIndex);
b3SharedMemoryStatusHandle statusHandle;
if (imuLinkIndex>=0)
if (imuLinkIndex >= 0)
{
ret = b3CreateSensorEnableIMUForLink(command, imuLinkIndex, 1);
ret = b3CreateSensorEnableIMUForLink(command, imuLinkIndex, 1);
}
if (sensorJointIndexLeft>=0)
if (sensorJointIndexLeft >= 0)
{
ret = b3CreateSensorEnable6DofJointForceTorqueSensor(command, sensorJointIndexLeft, 1);
ret = b3CreateSensorEnable6DofJointForceTorqueSensor(command, sensorJointIndexLeft, 1);
}
if(sensorJointIndexRight>=0)
if (sensorJointIndexRight >= 0)
{
ret = b3CreateSensorEnable6DofJointForceTorqueSensor(command, sensorJointIndexRight, 1);
}
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED);
ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED);
}
}
{
b3SharedMemoryStatusHandle statusHandle;
b3SharedMemoryCommandHandle command = b3CreateBoxShapeCommandInit(sm);
ret = b3CreateBoxCommandSetStartPosition(command, 0,0,-1);
ret = b3CreateBoxCommandSetStartOrientation(command,0,0,0,1);
ret = b3CreateBoxCommandSetHalfExtents(command, 10,10,1);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
ASSERT_EQ(b3GetStatusType(statusHandle), CMD_RIGID_BODY_CREATION_COMPLETED);
}
{
b3SharedMemoryStatusHandle statusHandle;
b3SharedMemoryCommandHandle command = b3CreateBoxShapeCommandInit(sm);
ret = b3CreateBoxCommandSetStartPosition(command, 0, 0, -1);
ret = b3CreateBoxCommandSetStartOrientation(command, 0, 0, 0, 1);
ret = b3CreateBoxCommandSetHalfExtents(command, 10, 10, 1);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
ASSERT_EQ(b3GetStatusType(statusHandle), CMD_RIGID_BODY_CREATION_COMPLETED);
}
{
int statusType;
b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(sm,bodyIndex);
b3SharedMemoryStatusHandle statusHandle;
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
statusType = b3GetStatusType(statusHandle);
ASSERT_EQ(statusType, CMD_ACTUAL_STATE_UPDATE_COMPLETED);
if (statusType == CMD_ACTUAL_STATE_UPDATE_COMPLETED)
{
b3GetStatusActualState(statusHandle,
0, &posVarCount, &dofCount,
0, 0, 0, 0);
ASSERT_EQ(posVarCount,15);
ASSERT_EQ(dofCount,14);
}
}
{
{
int statusType;
b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(sm, bodyIndex);
b3SharedMemoryStatusHandle statusHandle;
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
statusType = b3GetStatusType(statusHandle);
ASSERT_EQ(statusType, CMD_ACTUAL_STATE_UPDATE_COMPLETED);
if (statusType == CMD_ACTUAL_STATE_UPDATE_COMPLETED)
{
b3GetStatusActualState(statusHandle,
0, &posVarCount, &dofCount,
0, 0, 0, 0);
ASSERT_EQ(posVarCount, 15);
ASSERT_EQ(dofCount, 14);
}
}
{
#if 0
b3SharedMemoryStatusHandle statusHandle;
b3SharedMemoryCommandHandle command = b3JointControlCommandInit( sm, CONTROL_MODE_VELOCITY);
@@ -231,134 +224,126 @@ void testSharedMemory(b3PhysicsClientHandle sm)
}
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
#endif
}
///perform some simulation steps for testing
for ( i=0;i<1000;i++)
{
}
///perform some simulation steps for testing
for (i = 0; i < 1000; i++)
{
b3SharedMemoryStatusHandle statusHandle;
int statusType;
if (b3CanSubmitCommand(sm))
{
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, b3InitStepSimulationCommand(sm));
statusType = b3GetStatusType(statusHandle);
ASSERT_EQ(statusType, CMD_STEP_FORWARD_SIMULATION_COMPLETED);
} else
{
break;
}
}
if (b3CanSubmitCommand(sm))
{
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, b3InitStepSimulationCommand(sm));
statusType = b3GetStatusType(statusHandle);
ASSERT_EQ(statusType, CMD_STEP_FORWARD_SIMULATION_COMPLETED);
}
else
{
break;
}
}
{
b3SharedMemoryCommandHandle command;
b3SharedMemoryStatusHandle statusHandle;
int width = 1024;
int width = 1024;
int height = 1024;
command = b3InitRequestCameraImage(sm);
b3RequestCameraImageSetPixelResolution(command, width, height);
b3RequestCameraImageSelectRenderer(command,ER_BULLET_HARDWARE_OPENGL);
b3RequestCameraImageSelectRenderer(command, ER_BULLET_HARDWARE_OPENGL);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
}
if (b3CanSubmitCommand(sm))
{
b3SharedMemoryStatusHandle state = b3SubmitClientCommandAndWaitStatus(sm, b3RequestActualStateCommandInit(sm,bodyIndex));
if (sensorJointIndexLeft>=0)
{
struct b3JointSensorState sensorState;
b3GetJointState(sm,state,sensorJointIndexLeft,&sensorState);
if (b3CanSubmitCommand(sm))
{
b3SharedMemoryStatusHandle state = b3SubmitClientCommandAndWaitStatus(sm, b3RequestActualStateCommandInit(sm, bodyIndex));
if (sensorJointIndexLeft >= 0)
{
struct b3JointSensorState sensorState;
b3GetJointState(sm, state, sensorJointIndexLeft, &sensorState);
b3Printf("Sensor for joint [%d] = %f,%f,%f\n", sensorJointIndexLeft,
sensorState.m_jointForceTorque[0],
sensorState.m_jointForceTorque[1],
sensorState.m_jointForceTorque[2]);
sensorState.m_jointForceTorque[0],
sensorState.m_jointForceTorque[1],
sensorState.m_jointForceTorque[2]);
}
if (sensorJointIndexRight>=0)
if (sensorJointIndexRight >= 0)
{
struct b3JointSensorState sensorState;
b3GetJointState(sm,state,sensorJointIndexRight,&sensorState);
struct b3JointSensorState sensorState;
b3GetJointState(sm, state, sensorJointIndexRight, &sensorState);
b3Printf("Sensor for joint [%d] = %f,%f,%f\n", sensorJointIndexRight,
sensorState.m_jointForceTorque[0],
sensorState.m_jointForceTorque[1],
sensorState.m_jointForceTorque[2]);
}
{
b3SharedMemoryStatusHandle statusHandle;
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, b3InitResetSimulationCommand(sm));
ASSERT_EQ(b3GetStatusType(statusHandle), CMD_RESET_SIMULATION_COMPLETED);
}
}
} else
{
b3Warning("Cannot submit commands.\n");
}
{
b3SharedMemoryStatusHandle statusHandle;
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, b3InitResetSimulationCommand(sm));
ASSERT_EQ(b3GetStatusType(statusHandle), CMD_RESET_SIMULATION_COMPLETED);
}
}
}
else
{
b3Warning("Cannot submit commands.\n");
}
b3DisconnectSharedMemory(sm);
}
#ifdef ENABLE_GTEST
TEST(BulletPhysicsClientServerTest, DirectConnection) {
b3PhysicsClientHandle sm = b3ConnectPhysicsDirect();
testSharedMemory(sm);
TEST(BulletPhysicsClientServerTest, DirectConnection)
{
b3PhysicsClientHandle sm = b3ConnectPhysicsDirect();
testSharedMemory(sm);
}
TEST(BulletPhysicsClientServerTest, LoopBackSharedMemory) {
b3PhysicsClientHandle sm = b3ConnectPhysicsLoopback(SHARED_MEMORY_KEY);
testSharedMemory(sm);
TEST(BulletPhysicsClientServerTest, LoopBackSharedMemory)
{
b3PhysicsClientHandle sm = b3ConnectPhysicsLoopback(SHARED_MEMORY_KEY);
testSharedMemory(sm);
}
#else
int main(int argc, char* argv[])
{
#ifdef PHYSICS_LOOP_BACK
b3PhysicsClientHandle sm = b3ConnectPhysicsLoopback(SHARED_MEMORY_KEY);
b3PhysicsClientHandle sm = b3ConnectPhysicsLoopback(SHARED_MEMORY_KEY);
#endif
#ifdef PHYSICS_SERVER_DIRECT
b3PhysicsClientHandle sm = b3ConnectPhysicsDirect();
b3PhysicsClientHandle sm = b3ConnectPhysicsDirect();
#endif
#ifdef PHYSICS_IN_PROCESS_EXAMPLE_BROWSER
#ifdef __APPLE__
b3PhysicsClientHandle sm = b3CreateInProcessPhysicsServerAndConnectMainThread(argc,argv);
b3PhysicsClientHandle sm = b3CreateInProcessPhysicsServerAndConnectMainThread(argc, argv);
#else
b3PhysicsClientHandle sm = b3CreateInProcessPhysicsServerAndConnect(argc,argv);
#endif //__APPLE__
b3PhysicsClientHandle sm = b3CreateInProcessPhysicsServerAndConnect(argc, argv);
#endif //__APPLE__
#endif
#ifdef PHYSICS_SHARED_MEMORY
b3PhysicsClientHandle sm = b3ConnectSharedMemory(SHARED_MEMORY_KEY);
#endif //PHYSICS_SHARED_MEMORY
b3PhysicsClientHandle sm = b3ConnectSharedMemory(SHARED_MEMORY_KEY);
#endif //PHYSICS_SHARED_MEMORY
#ifdef PHYSICS_UDP
b3PhysicsClientHandle sm = b3ConnectPhysicsUDP("localhost",1234);
#endif //PHYSICS_UDP
b3PhysicsClientHandle sm = b3ConnectPhysicsUDP("localhost", 1234);
#endif //PHYSICS_UDP
#ifdef PHYSICS_TCP
b3PhysicsClientHandle sm = b3ConnectPhysicsTCP("localhost",6667);
#endif //PHYSICS_UDP
b3PhysicsClientHandle sm = b3ConnectPhysicsTCP("localhost", 6667);
#endif //PHYSICS_UDP
testSharedMemory(sm);
}
#endif