static PyObject* pybullet_renderImage(PyObject* self, PyObject* args)
{
if (0==sm)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
///request an image from a simulated camera, using a software renderer.
struct b3CameraImageData imageData;
PyObject* objViewMat,* objProjMat;
int width, height;
if (PyArg_ParseTuple(args, "iiOO", &width, &height, &objViewMat, &objProjMat))
{
PyObject* seq;
int i, len;
PyObject* item;
float viewMatrix[16];
float projectionMatrix[16];
int valid = 1;
{
seq = PySequence_Fast(objViewMat, "expected a sequence");
len = PySequence_Size(objViewMat);
//printf("objViewMat size = %d\n", len);
if (len==16)
{
if (PyList_Check(seq))
{
for (i = 0; i < len; i++)
{
item = PyList_GET_ITEM(seq, i);
viewMatrix[i] = PyFloat_AsDouble(item);
float v = viewMatrix[i];
//printf("view %d to %f\n", i,v);
}
}
else
{
for (i = 0; i < len; i++)
{
item = PyTuple_GET_ITEM(seq,i);
viewMatrix[i] = PyFloat_AsDouble(item);
}
}
} else
{
valid = 0;
}
}
{
seq = PySequence_Fast(objProjMat, "expected a sequence");
len = PySequence_Size(objProjMat);
//printf("projMat len = %d\n", len);
if (len==16)
{
if (PyList_Check(seq))
{
for (i = 0; i < len; i++)
{
item = PyList_GET_ITEM(seq, i);
projectionMatrix[i] = PyFloat_AsDouble(item);
}
}
else
{
for (i = 0; i < len; i++)
{
item = PyTuple_GET_ITEM(seq,i);
projectionMatrix[i] = PyFloat_AsDouble(item);
}
}
} else
{
valid = 0;
}
}
Py_DECREF(seq);
{
b3SharedMemoryCommandHandle command;
command = b3InitRequestCameraImage(sm);
if (valid)
{
//printf("set b3RequestCameraImageSetCameraMatrices\n");
b3RequestCameraImageSetCameraMatrices(command, viewMatrix, projectionMatrix);
}
b3RequestCameraImageSetPixelResolution(command,width,height);
if (b3CanSubmitCommand(sm))
{
b3SharedMemoryStatusHandle statusHandle;
int statusType;
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
statusType = b3GetStatusType(statusHandle);
if (statusType==CMD_CAMERA_IMAGE_COMPLETED)
{
PyObject *item2;
PyObject* pyResultList;//store 4 elements in this result: width, height, rgbData, depth
b3GetCameraImageData(sm, &imageData);
//todo: error handling if image size is 0
pyResultList = PyTuple_New(4);
PyTuple_SetItem(pyResultList, 0, PyInt_FromLong(imageData.m_pixelWidth));
PyTuple_SetItem(pyResultList, 1, PyInt_FromLong(imageData.m_pixelHeight));
PyObject *pylistPos;
PyObject* pylistDep;
int i,j,p;
//printf("image width = %d, height = %d\n", imageData.m_pixelWidth, imageData.m_pixelHeight);
{
PyObject *item;
int bytesPerPixel = 4;//Red, Green, Blue, each 8 bit values
int num=bytesPerPixel*imageData.m_pixelWidth*imageData.m_pixelHeight;
pylistPos = PyTuple_New(num);
pylistDep = PyTuple_New(imageData.m_pixelWidth*imageData.m_pixelHeight);
for (i=0;i<imageData.m_pixelWidth;i++)
{
for (j=0;j<imageData.m_pixelHeight;j++)
{
int depIndex = i+j*imageData.m_pixelWidth;
item = PyFloat_FromDouble(imageData.m_depthValues[depIndex]);
PyTuple_SetItem(pylistDep, depIndex, item);
for (p=0;p<bytesPerPixel;p++)
{
int pixelIndex = bytesPerPixel*(i+j*imageData.m_pixelWidth)+p;
item = PyInt_FromLong(imageData.m_rgbColorData[pixelIndex]);
PyTuple_SetItem(pylistPos, pixelIndex, item);
}
}
}
}
PyTuple_SetItem(pyResultList, 2,pylistPos);
PyTuple_SetItem(pyResultList, 3,pylistDep);
return pyResultList;
}
}
}
}
Py_INCREF(Py_None);
return Py_None;
}
void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
{
switch (commandId)
{
case CMD_LOAD_URDF:
{
b3SharedMemoryCommandHandle commandHandle = b3LoadUrdfCommandInit(m_physicsClientHandle, "kuka_iiwa/model.urdf");
//setting the initial position, orientation and other arguments are optional
double startPosX = 0;
static double startPosY = 0;
double startPosZ = 0;
b3LoadUrdfCommandSetStartPosition(commandHandle, startPosX,startPosY,startPosZ);
startPosY += 2.f;
// ret = b3LoadUrdfCommandSetUseFixedBase(commandHandle, 1);
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
break;
}
case CMD_LOAD_SDF:
{
b3SharedMemoryCommandHandle commandHandle = b3LoadSdfCommandInit(m_physicsClientHandle, "two_cubes.sdf");
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
break;
}
case CMD_REQUEST_CAMERA_IMAGE_DATA:
{
///request an image from a simulated camera, using a software renderer.
b3SharedMemoryCommandHandle commandHandle = b3InitRequestCameraImage(m_physicsClientHandle);
float viewMatrix[16];
float projectionMatrix[16];
this->m_guiHelper->getRenderInterface()->getActiveCamera()->getCameraProjectionMatrix(projectionMatrix);
this->m_guiHelper->getRenderInterface()->getActiveCamera()->getCameraViewMatrix(viewMatrix);
b3RequestCameraImageSetCameraMatrices(commandHandle, viewMatrix,projectionMatrix);
b3RequestCameraImageSetPixelResolution(commandHandle, camVisualizerWidth,camVisualizerHeight);
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
break;
}
case CMD_CREATE_BOX_COLLISION_SHAPE:
{
b3SharedMemoryCommandHandle commandHandle = b3CreateBoxShapeCommandInit(m_physicsClientHandle);
b3CreateBoxCommandSetStartPosition(commandHandle,0,0,-3);
b3CreateBoxCommandSetColorRGBA(commandHandle,0,0,1,1);
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
break;
}
case CMD_CREATE_RIGID_BODY:
{
b3SharedMemoryCommandHandle commandHandle = b3CreateBoxShapeCommandInit(m_physicsClientHandle);
b3CreateBoxCommandSetStartPosition(commandHandle,0,0,0);
b3CreateBoxCommandSetMass(commandHandle,1);
b3CreateBoxCommandSetCollisionShapeType(commandHandle,COLLISION_SHAPE_TYPE_CYLINDER_Y);
b3CreateBoxCommandSetColorRGBA(commandHandle,1,1,0,1);
double radius = 0.2;
double halfHeight = 0.5;
b3CreateBoxCommandSetHalfExtents(commandHandle,radius,halfHeight,radius);
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
break;
}
case CMD_REQUEST_ACTUAL_STATE:
{
if (m_selectedBody>=0)
{
b3SharedMemoryCommandHandle commandHandle = b3RequestActualStateCommandInit(m_physicsClientHandle,m_selectedBody);
b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_physicsClientHandle, commandHandle);
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
int numJoints = b3GetNumJoints(m_physicsClientHandle, m_selectedBody);
for (int i = 0; i < numJoints; ++i) {
struct b3JointSensorState sensorState;
b3GetJointState(m_physicsClientHandle, statusHandle, i, &sensorState);
b3Printf("Joint %d: %f", i, sensorState.m_jointMotorTorque);
}
}
break;
};
case CMD_INIT_POSE:
{
if (m_selectedBody>=0)
{
b3SharedMemoryCommandHandle commandHandle = b3CreatePoseCommandInit(m_physicsClientHandle,m_selectedBody);
static int toggle = 0;
double pos[3] = {0,0,0};
pos[toggle] = 2;
toggle++;
if (toggle>2)
toggle=0;
btQuaternion orn;
orn.setValue(0,0,0,1);
switch (toggle)
{
case 0:
orn = btQuaternion(btVector3(1,0,0),SIMD_HALF_PI);
break;
case 1:
orn = btQuaternion(btVector3(0,1,0),SIMD_HALF_PI);
break;
case 2:
orn = btQuaternion(btVector3(0,0,1),SIMD_HALF_PI);
break;
default:
orn.setValue(0,0,0,1);
};
b3CreatePoseCommandSetBaseOrientation(commandHandle,orn[0],orn[1],orn[2],orn[3]);
b3CreatePoseCommandSetBasePosition(commandHandle, pos[0],pos[1],pos[2]);
int numJoints = b3GetNumJoints(m_physicsClientHandle,m_selectedBody);
static double jointPos = SIMD_PI/2.f;
for (int i=0;i<numJoints;i++)
{
b3JointInfo info;
b3GetJointInfo(m_physicsClientHandle, m_selectedBody,i, &info);
if ((info.m_jointType == 0) || (info.m_jointType == 1)) //revolute or prismatic
{
b3CreatePoseCommandSetJointPosition(m_physicsClientHandle,commandHandle,i,jointPos);
}
}
jointPos += SIMD_PI/8.0;
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
}
break;
}
case CMD_STEP_FORWARD_SIMULATION:
{
b3SharedMemoryCommandHandle commandHandle = b3InitStepSimulationCommand(m_physicsClientHandle);
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
break;
}
case CMD_REQUEST_DEBUG_LINES:
{
b3SharedMemoryCommandHandle commandHandle = b3InitRequestDebugLinesCommand(m_physicsClientHandle, btIDebugDraw::DBG_DrawWireframe);
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
break;
}
case CMD_SEND_DESIRED_STATE:
{
// b3SharedMemoryCommandHandle command = b3JointControlCommandInit( m_physicsClientHandle, CONTROL_MODE_VELOCITY);
b3SharedMemoryCommandHandle command = b3JointControlCommandInit( m_physicsClientHandle, CONTROL_MODE_POSITION_VELOCITY_PD);
prepareControlCommand(command);
b3SubmitClientCommand(m_physicsClientHandle, command);
break;
}
case CMD_RESET_SIMULATION:
{
b3SharedMemoryCommandHandle commandHandle = b3InitResetSimulationCommand(m_physicsClientHandle);
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
break;
}
case CMD_SEND_BULLET_DATA_STREAM:
{
#if 0
//this worked, but needs C-API and a streaming options, similar to debug lines
command.m_type = buttonId;
cl->enqueueCommand(command);
#endif
break;
}
case CMD_SEND_PHYSICS_SIMULATION_PARAMETERS: {
b3SharedMemoryCommandHandle commandHandle = b3InitPhysicsParamCommand(m_physicsClientHandle);
b3PhysicsParamSetGravity(commandHandle, 0.0, 0.0, -9.8);
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
break;
}
default:
{
b3Error("Unknown buttonId");
btAssert(0);
}
};
}
void testSharedMemory(b3PhysicsClientHandle sm)
{
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;
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);
}
{
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);
statusType = b3GetStatusType(statusHandle);
ASSERT_EQ(statusType, CMD_SDF_LOADING_COMPLETED);
numBodies = b3GetStatusBodyIndices(statusHandle, bodyIndicesOut, 10);
ASSERT_EQ(numBodies,1);
bodyUniqueId = bodyIndicesOut[0];
{
{
b3SharedMemoryStatusHandle statusHandle;
int statusType;
b3SharedMemoryCommandHandle command = b3InitRequestVisualShapeInformation(sm, bodyUniqueId);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
statusType = b3GetStatusType(statusHandle);
if (statusType == CMD_VISUAL_SHAPE_INFO_COMPLETED)
{
struct b3VisualShapeInformation vi;
b3GetVisualShapeInformation(sm, &vi);
}
}
}
numJoints = b3GetNumJoints(sm,bodyUniqueId);
ASSERT_EQ(numJoints,7);
#if 0
b3Printf("numJoints: %d\n", numJoints);
for (i=0;i<numJoints;i++)
{
struct b3JointInfo jointInfo;
if (b3GetJointInfo(sm,bodyUniqueId, i,&jointInfo))
{
b3Printf("jointInfo[%d].m_jointName=%s\n",i,jointInfo.m_jointName);
}
}
#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);
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);
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++)
{
struct b3JointInfo jointInfo;
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"))
{
imuLinkIndex = i;
}
//pick the joint index based on joint name
if (strstr(jointInfo.m_jointName,"base_to_left_leg"))
{
sensorJointIndexLeft = i;
}
if (strstr(jointInfo.m_jointName,"base_to_right_leg"))
{
sensorJointIndexRight = i;
}
}
if ((sensorJointIndexLeft>=0) || (sensorJointIndexRight>=0))
{
b3SharedMemoryCommandHandle command = b3CreateSensorCommandInit(sm, bodyIndex);
b3SharedMemoryStatusHandle statusHandle;
if (imuLinkIndex>=0)
{
ret = b3CreateSensorEnableIMUForLink(command, imuLinkIndex, 1);
}
if (sensorJointIndexLeft>=0)
{
ret = b3CreateSensorEnable6DofJointForceTorqueSensor(command, sensorJointIndexLeft, 1);
}
if(sensorJointIndexRight>=0)
{
ret = b3CreateSensorEnable6DofJointForceTorqueSensor(command, sensorJointIndexRight, 1);
}
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
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);
}
{
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);
for ( dofIndex=0;dofIndex<dofCount;dofIndex++)
{
b3JointControlSetDesiredVelocity(command,dofIndex,1);
b3JointControlSetMaximumForce(command,dofIndex,100);
}
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
#endif
}
///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;
}
}
{
b3SharedMemoryCommandHandle command;
b3SharedMemoryStatusHandle statusHandle;
int width = 1024;
int height = 1024;
command = b3InitRequestCameraImage(sm);
b3RequestCameraImageSetPixelResolution(command, width, height);
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);
b3Printf("Sensor for joint [%d] = %f,%f,%f\n", sensorJointIndexLeft,
sensorState.m_jointForceTorque[0],
sensorState.m_jointForceTorque[1],
sensorState.m_jointForceTorque[2]);
}
if (sensorJointIndexRight>=0)
{
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);
}
}
}
b3DisconnectSharedMemory(sm);
}
void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
{
switch (commandId)
{
case CMD_LOAD_URDF:
{
b3SharedMemoryCommandHandle commandHandle = b3LoadUrdfCommandInit(m_physicsClientHandle, "kuka_iiwa/model.urdf");
//setting the initial position, orientation and other arguments are optional
double startPosX = 0;
static double startPosY = 0;
double startPosZ = 0;
b3LoadUrdfCommandSetStartPosition(commandHandle, startPosX, startPosY, startPosZ);
startPosY += 2.f;
// ret = b3LoadUrdfCommandSetUseFixedBase(commandHandle, 1);
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
break;
}
case CMD_LOAD_SDF:
{
#ifdef BT_DEBUG
b3SharedMemoryCommandHandle commandHandle = b3LoadSdfCommandInit(m_physicsClientHandle, "two_cubes.sdf");
#else
b3SharedMemoryCommandHandle commandHandle = b3LoadSdfCommandInit(m_physicsClientHandle, "kitchens/1.sdf"); //two_cubes.sdf");//kitchens/1.sdf");//kuka_iiwa/model.sdf");
#endif
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
break;
}
case CMD_REQUEST_CAMERA_IMAGE_DATA:
{
///request an image from a simulated camera, using a software renderer.
b3SharedMemoryCommandHandle commandHandle = b3InitRequestCameraImage(m_physicsClientHandle);
//b3RequestCameraImageSelectRenderer(commandHandle,ER_BULLET_HARDWARE_OPENGL);
float viewMatrix[16];
float projectionMatrix[16];
m_guiHelper->getRenderInterface()->getActiveCamera()->getCameraProjectionMatrix(projectionMatrix);
m_guiHelper->getRenderInterface()->getActiveCamera()->getCameraViewMatrix(viewMatrix);
b3RequestCameraImageSetCameraMatrices(commandHandle, viewMatrix, projectionMatrix);
b3RequestCameraImageSetPixelResolution(commandHandle, camVisualizerWidth, camVisualizerHeight);
float lightPos[3];
lightPos[0] = m_lightPos[0];
lightPos[1] = m_lightPos[1];
lightPos[2] = m_lightPos[2];
b3RequestCameraImageSetLightDirection(commandHandle, lightPos);
b3RequestCameraImageSetLightSpecularCoeff(commandHandle, m_specularCoeff);
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
break;
}
case CMD_CREATE_BOX_COLLISION_SHAPE:
{
b3SharedMemoryCommandHandle commandHandle = b3CreateBoxShapeCommandInit(m_physicsClientHandle);
b3CreateBoxCommandSetStartPosition(commandHandle, 0, 0, -1.5);
b3CreateBoxCommandSetColorRGBA(commandHandle, 0, 0, 1, 1);
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
break;
}
case CMD_CREATE_RIGID_BODY:
{
b3SharedMemoryCommandHandle commandHandle = b3CreateBoxShapeCommandInit(m_physicsClientHandle);
b3CreateBoxCommandSetStartPosition(commandHandle, 0, 0, 0);
b3CreateBoxCommandSetMass(commandHandle, 1);
b3CreateBoxCommandSetCollisionShapeType(commandHandle, COLLISION_SHAPE_TYPE_CYLINDER_Y);
b3CreateBoxCommandSetColorRGBA(commandHandle, 1, 1, 0, 1);
double radius = 0.2;
double halfHeight = 0.5;
b3CreateBoxCommandSetHalfExtents(commandHandle, radius, halfHeight, radius);
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
break;
}
case CMD_REQUEST_ACTUAL_STATE:
{
if (m_selectedBody >= 0)
{
b3SharedMemoryCommandHandle commandHandle = b3RequestActualStateCommandInit(m_physicsClientHandle, m_selectedBody);
b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_physicsClientHandle, commandHandle);
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
int numJoints = b3GetNumJoints(m_physicsClientHandle, m_selectedBody);
for (int i = 0; i < numJoints; ++i)
{
struct b3JointSensorState sensorState;
b3GetJointState(m_physicsClientHandle, statusHandle, i, &sensorState);
//b3Printf("Joint %d: %f", i, sensorState.m_jointMotorTorque);
}
}
break;
};
case CMD_INIT_POSE:
{
if (m_selectedBody >= 0)
{
b3SharedMemoryCommandHandle commandHandle = b3CreatePoseCommandInit(m_physicsClientHandle, m_selectedBody);
static int toggle = 0;
double pos[3] = {0, 0, 0};
pos[toggle] = 2;
toggle++;
if (toggle > 2)
toggle = 0;
btQuaternion orn;
orn.setValue(0, 0, 0, 1);
switch (toggle)
{
case 0:
orn = btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI);
break;
case 1:
orn = btQuaternion(btVector3(0, 1, 0), SIMD_HALF_PI);
break;
case 2:
orn = btQuaternion(btVector3(0, 0, 1), SIMD_HALF_PI);
break;
default:
orn.setValue(0, 0, 0, 1);
};
b3CreatePoseCommandSetBaseOrientation(commandHandle, orn[0], orn[1], orn[2], orn[3]);
b3CreatePoseCommandSetBasePosition(commandHandle, pos[0], pos[1], pos[2]);
int numJoints = b3GetNumJoints(m_physicsClientHandle, m_selectedBody);
static double jointPos = SIMD_PI / 2.f;
for (int i = 0; i < numJoints; i++)
{
b3JointInfo info;
b3GetJointInfo(m_physicsClientHandle, m_selectedBody, i, &info);
if ((info.m_jointType == 0) || (info.m_jointType == 1)) //revolute or prismatic
{
b3CreatePoseCommandSetJointPosition(m_physicsClientHandle, commandHandle, i, jointPos);
}
}
jointPos += SIMD_PI / 8.0;
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
}
break;
}
case CMD_STEP_FORWARD_SIMULATION:
{
b3SharedMemoryCommandHandle commandHandle = b3InitStepSimulationCommand(m_physicsClientHandle);
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
break;
}
case CMD_REQUEST_DEBUG_LINES:
{
b3SharedMemoryCommandHandle commandHandle = b3InitRequestDebugLinesCommand(m_physicsClientHandle, btIDebugDraw::DBG_DrawWireframe);
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
break;
}
case CMD_SEND_DESIRED_STATE:
{
if (m_selectedBody >= 0)
{
// b3SharedMemoryCommandHandle command = b3JointControlCommandInit( m_physicsClientHandle, m_selectedBody, CONTROL_MODE_VELOCITY);
b3SharedMemoryCommandHandle command = b3JointControlCommandInit2(m_physicsClientHandle, m_selectedBody, CONTROL_MODE_POSITION_VELOCITY_PD);
// b3Printf("prepare control command for body %d", m_selectedBody);
prepareControlCommand(command);
b3SubmitClientCommand(m_physicsClientHandle, command);
}
break;
}
case CMD_RESET_SIMULATION:
{
b3SharedMemoryCommandHandle commandHandle = b3InitResetSimulationCommand(m_physicsClientHandle);
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
break;
}
case CMD_SEND_BULLET_DATA_STREAM:
{
#if 0
//this worked, but needs C-API and a streaming options, similar to debug lines
command.m_type = buttonId;
cl->enqueueCommand(command);
#endif
break;
}
case CMD_CUSTOM_SET_GRAVITY:
{
b3SharedMemoryCommandHandle commandHandle = b3InitPhysicsParamCommand(m_physicsClientHandle);
b3PhysicsParamSetGravity(commandHandle, 0.0, 0.0, -9.8);
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
break;
}
case CMD_CUSTOM_SET_REALTIME_SIMULATION:
{
b3SharedMemoryCommandHandle commandHandle = b3InitPhysicsParamCommand(m_physicsClientHandle);
b3PhysicsParamSetRealTimeSimulation(commandHandle, 1);
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
break;
}
case CMD_CALCULATE_INVERSE_DYNAMICS:
{
if (m_selectedBody >= 0)
{
btAlignedObjectArray<double> jointPositionsQ;
btAlignedObjectArray<double> jointVelocitiesQdot;
btAlignedObjectArray<double> jointAccelerations;
int numJoints = b3GetNumJoints(m_physicsClientHandle, m_selectedBody);
if (numJoints)
{
b3Printf("Compute inverse dynamics for joint accelerations:");
jointPositionsQ.resize(numJoints);
jointVelocitiesQdot.resize(numJoints);
jointAccelerations.resize(numJoints);
for (int i = 0; i < numJoints; i++)
{
jointAccelerations[i] = 100;
b3Printf("Desired joint acceleration[%d]=%f", i, jointAccelerations[i]);
}
b3SharedMemoryCommandHandle commandHandle = b3CalculateInverseDynamicsCommandInit(m_physicsClientHandle,
m_selectedBody, &jointPositionsQ[0], &jointVelocitiesQdot[0], &jointAccelerations[0]);
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
}
}
break;
}
case CMD_REQUEST_CONTACT_POINT_INFORMATION:
{
b3SharedMemoryCommandHandle commandHandle = b3InitRequestContactPointInformation(m_physicsClientHandle);
b3SetContactFilterBodyA(commandHandle, 0);
b3SetContactFilterBodyB(commandHandle, 1);
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
break;
}
case CMD_SAVE_WORLD:
{
b3SharedMemoryCommandHandle commandHandle = b3SaveWorldCommandInit(m_physicsClientHandle, "saveWorld.py");
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
break;
}
case CMD_REQUEST_VISUAL_SHAPE_INFO:
{
if (m_selectedBody >= 0)
{
//request visual shape information
b3SharedMemoryCommandHandle commandHandle = b3InitRequestVisualShapeInformation(m_physicsClientHandle, m_selectedBody);
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
}
break;
}
case CMD_SET_SHADOW:
{
b3SharedMemoryCommandHandle commandHandle = b3InitRequestCameraImage(m_physicsClientHandle);
float viewMatrix[16];
float projectionMatrix[16];
m_guiHelper->getRenderInterface()->getActiveCamera()->getCameraProjectionMatrix(projectionMatrix);
m_guiHelper->getRenderInterface()->getActiveCamera()->getCameraViewMatrix(viewMatrix);
b3RequestCameraImageSetCameraMatrices(commandHandle, viewMatrix, projectionMatrix);
b3RequestCameraImageSetPixelResolution(commandHandle, camVisualizerWidth, camVisualizerHeight);
bool hasShadow = true;
b3RequestCameraImageSetShadow(commandHandle, hasShadow);
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
break;
}
case CMD_UPDATE_VISUAL_SHAPE:
{
int objectUniqueId = 0;
int linkIndex = -1;
int shapeIndex = -1;
int textureIndex = -2;
double rgbaColor[4] = {0.0, 1.0, 0.0, 1.0};
b3SharedMemoryCommandHandle commandHandle = b3InitUpdateVisualShape2(m_physicsClientHandle, objectUniqueId, linkIndex, shapeIndex);
b3UpdateVisualShapeRGBAColor(commandHandle, rgbaColor);
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
break;
}
default:
{
b3Error("Unknown buttonId");
btAssert(0);
}
};
}
