int main(int argc, char* argv[])
{
#ifdef PHYSICS_LOOP_BACK
b3PhysicsClientHandle sm = b3ConnectPhysicsLoopback(SHARED_MEMORY_KEY);
#endif
#ifdef PHYSICS_SERVER_DIRECT
b3PhysicsClientHandle sm = b3ConnectPhysicsDirect();
#endif
#ifdef PHYSICS_IN_PROCESS_EXAMPLE_BROWSER
#ifdef __APPLE__
b3PhysicsClientHandle sm = b3CreateInProcessPhysicsServerAndConnectMainThread(argc,argv);
#else
b3PhysicsClientHandle sm = b3CreateInProcessPhysicsServerAndConnect(argc,argv);
#endif //__APPLE__
#endif
#ifdef PHYSICS_SHARED_MEMORY
b3PhysicsClientHandle sm = b3ConnectSharedMemory(SHARED_MEMORY_KEY);
#endif //PHYSICS_SHARED_MEMORY
#ifdef PHYSICS_UDP
b3PhysicsClientHandle sm = b3ConnectPhysicsUDP("localhost",1234);
#endif //PHYSICS_UDP
testSharedMemory(sm);
}
static PyObject *
pybullet_connectPhysicsServer(PyObject *self, PyObject *args)
{
if (0!=sm)
{
PyErr_SetString(SpamError, "Already connected to physics server, disconnect first.");
return NULL;
}
{
int method=eCONNECT_GUI;
if (!PyArg_ParseTuple(args, "i", &method))
{
PyErr_SetString(SpamError, "connectPhysicsServer expected argument eCONNECT_GUI, eCONNECT_DIRECT or eCONNECT_SHARED_MEMORY");
return NULL;
}
switch (method)
{
case eCONNECT_GUI:
{
int argc=0;
char* argv[1]={0};
#ifdef __APPLE__
sm = b3CreateInProcessPhysicsServerAndConnectMainThread(argc, argv);
#else
sm = b3CreateInProcessPhysicsServerAndConnect(argc, argv);
#endif
break;
}
case eCONNECT_DIRECT:
{
sm = b3ConnectPhysicsDirect();
break;
}
case eCONNECT_SHARED_MEMORY:
{
sm = b3ConnectSharedMemory(SHARED_MEMORY_KEY);
break;
}
default:
{
PyErr_SetString(SpamError, "connectPhysicsServer unexpected argument");
return NULL;
}
};
}
Py_INCREF(Py_None);
return Py_None;
}
int main(int argc, char* argv[])
{
kPhysClient = b3CreateInProcessPhysicsServerAndConnect(argc, argv);
if (!kPhysClient)
return -1;
// visualizer
command = b3InitConfigureOpenGLVisualizer(kPhysClient);
b3ConfigureOpenGLVisualizerSetVisualizationFlags(command, COV_ENABLE_GUI, 0);
b3SubmitClientCommandAndWaitStatus(kPhysClient, command);
b3ConfigureOpenGLVisualizerSetVisualizationFlags(command, COV_ENABLE_SHADOWS, 0);
b3SubmitClientCommandAndWaitStatus(kPhysClient, command);
b3SetTimeOut(kPhysClient, 10);
//syncBodies is only needed when connecting to an existing physics server that has already some bodies
command = b3InitSyncBodyInfoCommand(kPhysClient);
statusHandle = b3SubmitClientCommandAndWaitStatus(kPhysClient, command);
statusType = b3GetStatusType(statusHandle);
// set fixed time step
command = b3InitPhysicsParamCommand(kPhysClient);
ret = b3PhysicsParamSetTimeStep(command, FIXED_TIMESTEP);
statusHandle = b3SubmitClientCommandAndWaitStatus(kPhysClient, command);
ret = b3PhysicsParamSetRealTimeSimulation(command, false);
statusHandle = b3SubmitClientCommandAndWaitStatus(kPhysClient, command);
b3Assert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED);
// load test
command = b3LoadUrdfCommandInit(kPhysClient, "TwoJointRobot_wo_fixedJoints.urdf");
int flags = URDF_USE_INERTIA_FROM_FILE;
b3LoadUrdfCommandSetFlags(command, flags);
b3LoadUrdfCommandSetUseFixedBase(command, true);
// q.setEulerZYX(0, 0, 0);
// b3LoadUrdfCommandSetStartOrientation(command, q[0], q[1], q[2], q[3]);
b3LoadUrdfCommandSetUseMultiBody(command, true);
statusHandle = b3SubmitClientCommandAndWaitStatus(kPhysClient, command);
statusType = b3GetStatusType(statusHandle);
b3Assert(statusType == CMD_URDF_LOADING_COMPLETED);
if (statusType == CMD_URDF_LOADING_COMPLETED)
{
twojoint = b3GetStatusBodyIndex(statusHandle);
}
//disable default linear/angular damping
b3SharedMemoryCommandHandle command = b3InitChangeDynamicsInfo(kPhysClient);
double linearDamping = 0;
double angularDamping = 0;
b3ChangeDynamicsInfoSetLinearDamping(command, twojoint, linearDamping);
b3ChangeDynamicsInfoSetAngularDamping(command, twojoint, angularDamping);
statusHandle = b3SubmitClientCommandAndWaitStatus(kPhysClient, command);
int numJoints = b3GetNumJoints(kPhysClient, twojoint);
printf("twojoint numjoints = %d\n", numJoints);
// Loop through all joints
for (int i = 0; i < numJoints; ++i)
{
b3GetJointInfo(kPhysClient, twojoint, i, &jointInfo);
if (jointInfo.m_jointName[0])
{
jointNameToId[std::string(jointInfo.m_jointName)] = i;
}
else
{
continue;
}
// Reset before torque control - see #1459
command = b3JointControlCommandInit2(kPhysClient, twojoint, CONTROL_MODE_VELOCITY);
b3JointControlSetDesiredVelocity(command, jointInfo.m_uIndex, 0);
b3JointControlSetMaximumForce(command, jointInfo.m_uIndex, 0);
statusHandle = b3SubmitClientCommandAndWaitStatus(kPhysClient, command);
}
// loop
unsigned long dtus1 = (unsigned long)1000000.0 * FIXED_TIMESTEP;
double simTimeS = 0;
double q[2], v[2];
while (b3CanSubmitCommand(kPhysClient))
{
simTimeS += 0.000001 * dtus1;
// apply some torque
b3GetJointInfo(kPhysClient, twojoint, jointNameToId["joint_2"], &jointInfo);
command = b3JointControlCommandInit2(kPhysClient, twojoint, CONTROL_MODE_TORQUE);
b3JointControlSetDesiredForceTorque(command, jointInfo.m_uIndex, 0.5 * sin(10 * simTimeS));
statusHandle = b3SubmitClientCommandAndWaitStatus(kPhysClient, command);
// get joint values
command = b3RequestActualStateCommandInit(kPhysClient, twojoint);
statusHandle = b3SubmitClientCommandAndWaitStatus(kPhysClient, command);
b3GetJointState(kPhysClient, statusHandle, jointNameToId["joint_1"], &state);
q[0] = state.m_jointPosition;
v[0] = state.m_jointVelocity;
b3GetJointState(kPhysClient, statusHandle, jointNameToId["joint_2"], &state);
q[1] = state.m_jointPosition;
v[1] = state.m_jointVelocity;
statusHandle = b3SubmitClientCommandAndWaitStatus(kPhysClient, b3InitStepSimulationCommand(kPhysClient));
// debugging output
printf("%.3f\t%.3f\t%.3f\t%.3f\t%.3f\n", simTimeS, q[0], q[1], v[0], v[1]);
b3Clock::usleep(1000. * 1000. * FIXED_TIMESTEP);
}
b3DisconnectSharedMemory(kPhysClient);
return 0;
}
