1
2 //#include "SharedMemoryCommands.h"
3 #ifdef PHYSICS_SHARED_MEMORY
4 #include "SharedMemory/PhysicsClientC_API.h"
5 #endif //PHYSICS_SHARED_MEMORY
6
7 #ifdef PHYSICS_UDP
8 #include "SharedMemory/PhysicsClientUDP_C_API.h"
9 #endif //PHYSICS_UDP
10
11 #ifdef PHYSICS_TCP
12 #include "SharedMemory/PhysicsClientTCP_C_API.h"
13 #endif //PHYSICS_TCP
14
15 #ifdef PHYSICS_LOOP_BACK
16 #include "SharedMemory/PhysicsLoopBackC_API.h"
17 #endif //PHYSICS_LOOP_BACK
18
19 #ifdef PHYSICS_SERVER_DIRECT
20 #include "SharedMemory/PhysicsDirectC_API.h"
21 #endif //PHYSICS_SERVER_DIRECT
22
23 #ifdef PHYSICS_IN_PROCESS_EXAMPLE_BROWSER
24 #include "SharedMemory/SharedMemoryInProcessPhysicsC_API.h"
25 #endif //PHYSICS_IN_PROCESS_EXAMPLE_BROWSER
26
27 #include "SharedMemory/SharedMemoryPublic.h"
28 #include "Bullet3Common/b3Logging.h"
29 #include <string.h>
30
31 #include <stdio.h>
32
33 #ifndef ENABLE_GTEST
34 #include <assert.h>
35 #define ASSERT_EQ(a, b) assert((a) == (b));
36 #else
37 #define printf
38 #endif
39
testSharedMemory(b3PhysicsClientHandle sm)40 void testSharedMemory(b3PhysicsClientHandle sm)
41 {
42 int i, dofCount, posVarCount, ret, numJoints;
43 int sensorJointIndexLeft = -1;
44 int sensorJointIndexRight = -1;
45 const char* urdfFileName = "r2d2.urdf";
46 const char* sdfFileName = "kuka_iiwa/model.sdf";
47 double gravx = 0, gravy = 0, gravz = -9.8;
48 double timeStep = 1. / 60.;
49 double startPosX, startPosY, startPosZ;
50 int imuLinkIndex = -1;
51 int bodyIndex = -1;
52
53 if (b3CanSubmitCommand(sm))
54 {
55 {
56 b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm);
57 b3SharedMemoryStatusHandle statusHandle;
58 ret = b3PhysicsParamSetGravity(command, gravx, gravy, gravz);
59 ret = b3PhysicsParamSetTimeStep(command, timeStep);
60 statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
61 ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED);
62 }
63
64 {
65 b3SharedMemoryStatusHandle statusHandle;
66 int statusType;
67 int bodyIndicesOut[10]; //MAX_SDF_BODIES = 10
68 int numJoints, numBodies;
69 int bodyUniqueId;
70 b3SharedMemoryCommandHandle command = b3LoadSdfCommandInit(sm, sdfFileName);
71 statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
72 statusType = b3GetStatusType(statusHandle);
73 ASSERT_EQ(statusType, CMD_SDF_LOADING_COMPLETED);
74
75 numBodies = b3GetStatusBodyIndices(statusHandle, bodyIndicesOut, 10);
76 ASSERT_EQ(numBodies, 1);
77 bodyUniqueId = bodyIndicesOut[0];
78 {
79 {
80 b3SharedMemoryStatusHandle statusHandle;
81 int statusType;
82 b3SharedMemoryCommandHandle command = b3InitRequestVisualShapeInformation(sm, bodyUniqueId);
83 statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
84 statusType = b3GetStatusType(statusHandle);
85 if (statusType == CMD_VISUAL_SHAPE_INFO_COMPLETED)
86 {
87 struct b3VisualShapeInformation vi;
88 b3GetVisualShapeInformation(sm, &vi);
89 }
90 }
91 }
92
93 numJoints = b3GetNumJoints(sm, bodyUniqueId);
94 ASSERT_EQ(numJoints, 7);
95
96 #if 0
97 b3Printf("numJoints: %d\n", numJoints);
98 for (i=0;i<numJoints;i++)
99 {
100 struct b3JointInfo jointInfo;
101 if (b3GetJointInfo(sm,bodyUniqueId, i,&jointInfo))
102 {
103 b3Printf("jointInfo[%d].m_jointName=%s\n",i,jointInfo.m_jointName);
104 }
105 }
106 #endif
107 {
108 b3SharedMemoryStatusHandle statusHandle;
109 b3SharedMemoryCommandHandle commandHandle;
110 double jointAngle = 0.f;
111 int jointIndex;
112 commandHandle = b3CreatePoseCommandInit(sm, bodyUniqueId);
113 for (jointIndex = 0; jointIndex < numJoints; jointIndex++)
114 {
115 b3CreatePoseCommandSetJointPosition(sm, commandHandle, jointIndex, jointAngle);
116 }
117
118 statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
119
120 ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED);
121 }
122 }
123
124 {
125 b3SharedMemoryStatusHandle statusHandle;
126 int statusType;
127 b3SharedMemoryCommandHandle command = b3LoadUrdfCommandInit(sm, urdfFileName);
128
129 //setting the initial position, orientation and other arguments are optional
130 startPosX = 2;
131 startPosY = 0;
132 startPosZ = 1;
133 ret = b3LoadUrdfCommandSetStartPosition(command, startPosX, startPosY, startPosZ);
134 statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
135 statusType = b3GetStatusType(statusHandle);
136 ASSERT_EQ(statusType, CMD_URDF_LOADING_COMPLETED);
137 bodyIndex = b3GetStatusBodyIndex(statusHandle);
138 }
139
140 if (bodyIndex >= 0)
141 {
142 numJoints = b3GetNumJoints(sm, bodyIndex);
143 for (i = 0; i < numJoints; i++)
144 {
145 struct b3JointInfo jointInfo;
146 b3GetJointInfo(sm, bodyIndex, i, &jointInfo);
147
148 // printf("jointInfo[%d].m_jointName=%s\n",i,jointInfo.m_jointName);
149 //pick the IMU link index based on torso name
150 if (strstr(jointInfo.m_linkName, "base_link"))
151 {
152 imuLinkIndex = i;
153 }
154
155 //pick the joint index based on joint name
156 if (strstr(jointInfo.m_jointName, "base_to_left_leg"))
157 {
158 sensorJointIndexLeft = i;
159 }
160 if (strstr(jointInfo.m_jointName, "base_to_right_leg"))
161 {
162 sensorJointIndexRight = i;
163 }
164 }
165
166 if ((sensorJointIndexLeft >= 0) || (sensorJointIndexRight >= 0))
167 {
168 b3SharedMemoryCommandHandle command = b3CreateSensorCommandInit(sm, bodyIndex);
169 b3SharedMemoryStatusHandle statusHandle;
170 if (imuLinkIndex >= 0)
171 {
172 ret = b3CreateSensorEnableIMUForLink(command, imuLinkIndex, 1);
173 }
174
175 if (sensorJointIndexLeft >= 0)
176 {
177 ret = b3CreateSensorEnable6DofJointForceTorqueSensor(command, sensorJointIndexLeft, 1);
178 }
179 if (sensorJointIndexRight >= 0)
180 {
181 ret = b3CreateSensorEnable6DofJointForceTorqueSensor(command, sensorJointIndexRight, 1);
182 }
183 statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
184 ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED);
185 }
186 }
187
188 {
189 b3SharedMemoryStatusHandle statusHandle;
190 b3SharedMemoryCommandHandle command = b3CreateBoxShapeCommandInit(sm);
191 ret = b3CreateBoxCommandSetStartPosition(command, 0, 0, -1);
192 ret = b3CreateBoxCommandSetStartOrientation(command, 0, 0, 0, 1);
193 ret = b3CreateBoxCommandSetHalfExtents(command, 10, 10, 1);
194 statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
195 ASSERT_EQ(b3GetStatusType(statusHandle), CMD_RIGID_BODY_CREATION_COMPLETED);
196 }
197
198 {
199 int statusType;
200 b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(sm, bodyIndex);
201 b3SharedMemoryStatusHandle statusHandle;
202 statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
203 statusType = b3GetStatusType(statusHandle);
204 ASSERT_EQ(statusType, CMD_ACTUAL_STATE_UPDATE_COMPLETED);
205
206 if (statusType == CMD_ACTUAL_STATE_UPDATE_COMPLETED)
207 {
208 b3GetStatusActualState(statusHandle,
209 0, &posVarCount, &dofCount,
210 0, 0, 0, 0);
211 ASSERT_EQ(posVarCount, 15);
212 ASSERT_EQ(dofCount, 14);
213 }
214 }
215
216 {
217 #if 0
218 b3SharedMemoryStatusHandle statusHandle;
219 b3SharedMemoryCommandHandle command = b3JointControlCommandInit( sm, CONTROL_MODE_VELOCITY);
220 for ( dofIndex=0;dofIndex<dofCount;dofIndex++)
221 {
222 b3JointControlSetDesiredVelocity(command,dofIndex,1);
223 b3JointControlSetMaximumForce(command,dofIndex,100);
224 }
225 statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
226 #endif
227 }
228 ///perform some simulation steps for testing
229 for (i = 0; i < 1000; i++)
230 {
231 b3SharedMemoryStatusHandle statusHandle;
232 int statusType;
233
234 if (b3CanSubmitCommand(sm))
235 {
236 statusHandle = b3SubmitClientCommandAndWaitStatus(sm, b3InitStepSimulationCommand(sm));
237 statusType = b3GetStatusType(statusHandle);
238 ASSERT_EQ(statusType, CMD_STEP_FORWARD_SIMULATION_COMPLETED);
239 }
240 else
241 {
242 break;
243 }
244 }
245
246 {
247 b3SharedMemoryCommandHandle command;
248 b3SharedMemoryStatusHandle statusHandle;
249 int width = 1024;
250 int height = 1024;
251
252 command = b3InitRequestCameraImage(sm);
253
254 b3RequestCameraImageSetPixelResolution(command, width, height);
255 b3RequestCameraImageSelectRenderer(command, ER_BULLET_HARDWARE_OPENGL);
256 statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
257 }
258
259 if (b3CanSubmitCommand(sm))
260 {
261 b3SharedMemoryStatusHandle state = b3SubmitClientCommandAndWaitStatus(sm, b3RequestActualStateCommandInit(sm, bodyIndex));
262
263 if (sensorJointIndexLeft >= 0)
264 {
265 struct b3JointSensorState sensorState;
266 b3GetJointState(sm, state, sensorJointIndexLeft, &sensorState);
267
268 b3Printf("Sensor for joint [%d] = %f,%f,%f\n", sensorJointIndexLeft,
269 sensorState.m_jointForceTorque[0],
270 sensorState.m_jointForceTorque[1],
271 sensorState.m_jointForceTorque[2]);
272 }
273
274 if (sensorJointIndexRight >= 0)
275 {
276 struct b3JointSensorState sensorState;
277 b3GetJointState(sm, state, sensorJointIndexRight, &sensorState);
278
279 b3Printf("Sensor for joint [%d] = %f,%f,%f\n", sensorJointIndexRight,
280 sensorState.m_jointForceTorque[0],
281 sensorState.m_jointForceTorque[1],
282 sensorState.m_jointForceTorque[2]);
283 }
284
285 {
286 b3SharedMemoryStatusHandle statusHandle;
287 statusHandle = b3SubmitClientCommandAndWaitStatus(sm, b3InitResetSimulationCommand(sm));
288 ASSERT_EQ(b3GetStatusType(statusHandle), CMD_RESET_SIMULATION_COMPLETED);
289 }
290 }
291 }
292 else
293 {
294 b3Warning("Cannot submit commands.\n");
295 }
296
297 b3DisconnectSharedMemory(sm);
298 }
299
300 #ifdef ENABLE_GTEST
301
TEST(BulletPhysicsClientServerTest,DirectConnection)302 TEST(BulletPhysicsClientServerTest, DirectConnection)
303 {
304 b3PhysicsClientHandle sm = b3ConnectPhysicsDirect();
305 testSharedMemory(sm);
306 }
307
TEST(BulletPhysicsClientServerTest,LoopBackSharedMemory)308 TEST(BulletPhysicsClientServerTest, LoopBackSharedMemory)
309 {
310 b3PhysicsClientHandle sm = b3ConnectPhysicsLoopback(SHARED_MEMORY_KEY);
311 testSharedMemory(sm);
312 }
313
314 #else
315
main(int argc,char * argv[])316 int main(int argc, char* argv[])
317 {
318 #ifdef PHYSICS_LOOP_BACK
319 b3PhysicsClientHandle sm = b3ConnectPhysicsLoopback(SHARED_MEMORY_KEY);
320 #endif
321
322 #ifdef PHYSICS_SERVER_DIRECT
323 b3PhysicsClientHandle sm = b3ConnectPhysicsDirect();
324 #endif
325
326 #ifdef PHYSICS_IN_PROCESS_EXAMPLE_BROWSER
327
328 #ifdef __APPLE__
329 b3PhysicsClientHandle sm = b3CreateInProcessPhysicsServerAndConnectMainThread(argc, argv);
330 #else
331 b3PhysicsClientHandle sm = b3CreateInProcessPhysicsServerAndConnect(argc, argv);
332 #endif //__APPLE__
333 #endif
334
335 #ifdef PHYSICS_SHARED_MEMORY
336 b3PhysicsClientHandle sm = b3ConnectSharedMemory(SHARED_MEMORY_KEY);
337 #endif //PHYSICS_SHARED_MEMORY
338
339 #ifdef PHYSICS_UDP
340 b3PhysicsClientHandle sm = b3ConnectPhysicsUDP("localhost", 1234);
341 #endif //PHYSICS_UDP
342
343 #ifdef PHYSICS_TCP
344 b3PhysicsClientHandle sm = b3ConnectPhysicsTCP("localhost", 6667);
345 #endif //PHYSICS_UDP
346
347 testSharedMemory(sm);
348 }
349 #endif
350