void PhysicsClientExample::stepSimulation(float deltaTime)
{
if (m_physicsClient.isConnected())
{
SharedMemoryStatus status;
bool hasStatus = m_physicsClient.processServerStatus(status);
if (hasStatus && status.m_type == CMD_URDF_LOADING_COMPLETED)
{
for (int i=0;i<m_physicsClient.getNumJoints();i++)
{
b3JointInfo info;
m_physicsClient.getJointInfo(i,info);
b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex);
}
if (hasStatus && status.m_type == CMD_URDF_LOADING_COMPLETED)
{
for (int i=0;i<m_physicsClient.getNumJoints();i++)
{
b3JointInfo info;
m_physicsClient.getJointInfo(i,info);
b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex);
if (info.m_flags & JOINT_HAS_MOTORIZED_POWER)
{
if (m_numMotors<MAX_NUM_MOTORS)
{
char motorName[1024];
sprintf(motorName,"%s q'", info.m_jointName);
MyMotorInfo2* motorInfo = &m_motorTargetVelocities[m_numMotors];
motorInfo->m_velTarget = 0.f;
motorInfo->m_uIndex = info.m_uIndex;
SliderParams slider(motorName,&motorInfo->m_velTarget);
slider.m_minVal=-4;
slider.m_maxVal=4;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
m_numMotors++;
}
}
}
}
}
if (hasStatus && status.m_type ==CMD_DEBUG_LINES_COMPLETED)
{
SharedMemoryCommand command;
if (status.m_sendDebugLinesArgs.m_numRemainingDebugLines>0)
{
//continue requesting debug lines for drawing
command.m_type =CMD_REQUEST_DEBUG_LINES;
command.m_requestDebugLinesArguments.m_debugMode = btIDebugDraw::DBG_DrawWireframe;//DBG_DrawConstraints;
command.m_requestDebugLinesArguments.m_startingLineIndex = status.m_sendDebugLinesArgs.m_numDebugLines+status.m_sendDebugLinesArgs.m_startingLineIndex;
enqueueCommand(command);
}
}
if (m_physicsClient.canSubmitCommand())
{
if (m_userCommandRequests.size())
{
//b3Printf("Outstanding user command requests: %d\n", m_userCommandRequests.size());
SharedMemoryCommand command = m_userCommandRequests[0];
//a manual 'pop_front', we don't use 'remove' because it will re-order the commands
for (int i=1;i<m_userCommandRequests.size();i++)
{
m_userCommandRequests[i-1] = m_userCommandRequests[i];
}
m_userCommandRequests.pop_back();
//for the CMD_RESET_SIMULATION we need to do something special: clear the GUI sliders
if (command.m_type==CMD_RESET_SIMULATION)
{
m_guiHelper->getParameterInterface()->removeAllParameters();
m_numMotors=0;
createButtons();
}
m_physicsClient.submitClientCommand(command);
} else
{
if (m_numMotors)
{
SharedMemoryCommand command;
command.m_type =CMD_SEND_DESIRED_STATE;
prepareControlCommand(command);
enqueueCommand(command);
command.m_type =CMD_STEP_FORWARD_SIMULATION;
enqueueCommand(command);
command.m_type = CMD_REQUEST_ACTUAL_STATE;
enqueueCommand(command);
}
}
}
}
}
void RobotControlExample::stepSimulation(float deltaTime)
{
m_physicsServer.processClientCommands();
if (m_physicsClient.isConnected())
{
SharedMemoryStatus status;
bool hasStatus = m_physicsClient.processServerStatus(status);
if (hasStatus && status.m_type == CMD_URDF_LOADING_COMPLETED)
{
for (int i=0;i<m_physicsClient.getNumJoints();i++)
{
b3JointInfo info;
m_physicsClient.getJointInfo(i,info);
if (m_verboseOutput)
{
b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex);
}
if (info.m_flags & JOINT_HAS_MOTORIZED_POWER)
{
if (m_numMotors<MAX_NUM_MOTORS)
{
switch (m_option)
{
case ROBOT_VELOCITY_CONTROL:
{
char motorName[1024];
sprintf(motorName,"%s q'", info.m_jointName);
MyMotorInfo* motorInfo = &m_motorTargetState[m_numMotors];
motorInfo->m_velTarget = 0.f;
motorInfo->m_posTarget = 0.f;
motorInfo->m_uIndex = info.m_uIndex;
SliderParams slider(motorName,&motorInfo->m_velTarget);
slider.m_minVal=-4;
slider.m_maxVal=4;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
m_numMotors++;
break;
}
case ROBOT_PD_CONTROL:
{
char motorName[1024];
MyMotorInfo* motorInfo = &m_motorTargetState[m_numMotors];
motorInfo->m_velTarget = 0.f;
motorInfo->m_posTarget = 0.f;
motorInfo->m_uIndex = info.m_uIndex;
motorInfo->m_posIndex = info.m_qIndex;
motorInfo->m_kp = 1;
motorInfo->m_kd = 0;
{
sprintf(motorName,"%s kp", info.m_jointName);
SliderParams slider(motorName,&motorInfo->m_kp);
slider.m_minVal=0;
slider.m_maxVal=1;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
{
sprintf(motorName,"%s q", info.m_jointName);
SliderParams slider(motorName,&motorInfo->m_posTarget);
slider.m_minVal=-SIMD_PI;
slider.m_maxVal=SIMD_PI;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
{
sprintf(motorName,"%s kd", info.m_jointName);
SliderParams slider(motorName,&motorInfo->m_kd);
slider.m_minVal=0;
slider.m_maxVal=1;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
{
sprintf(motorName,"%s q'", info.m_jointName);
SliderParams slider(motorName,&motorInfo->m_velTarget);
slider.m_minVal=-10;
slider.m_maxVal=10;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
m_numMotors++;
break;
}
default:
{
b3Warning("Unknown control mode in RobotControlExample::stepSimulation");
}
};
}
}
}
}
if (m_physicsClient.canSubmitCommand())
{
if (m_userCommandRequests.size())
{
if (m_verboseOutput)
{
b3Printf("Outstanding user command requests: %d\n", m_userCommandRequests.size());
}
SharedMemoryCommand cmd = m_userCommandRequests[0];
//a manual 'pop_front', we don't use 'remove' because it will re-order the commands
for (int i=1;i<m_userCommandRequests.size();i++)
{
m_userCommandRequests[i-1] = m_userCommandRequests[i];
}
m_userCommandRequests.pop_back();
if (cmd.m_type == CMD_SEND_BULLET_DATA_STREAM)
{
char relativeFileName[1024];
bool fileFound = b3ResourcePath::findResourcePath(cmd.m_dataStreamArguments.m_bulletFileName,relativeFileName,1024);
if (fileFound)
{
FILE *fp = fopen(relativeFileName, "rb");
if (fp)
{
fseek(fp, 0L, SEEK_END);
int mFileLen = ftell(fp);
fseek(fp, 0L, SEEK_SET);
if (mFileLen<SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE)
{
char* data = (char*)malloc(mFileLen);
fread(data, mFileLen, 1, fp);
fclose(fp);
cmd.m_dataStreamArguments.m_streamChunkLength = mFileLen;
m_physicsClient.uploadBulletFileToSharedMemory(data,mFileLen);
if (m_verboseOutput)
{
b3Printf("Loaded bullet data chunks into shared memory\n");
}
free(data);
} else
{
b3Warning("Bullet file size (%d) exceeds of streaming memory chunk size (%d)\n", mFileLen,SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
}
} else
{
b3Warning("Cannot open file %s\n", relativeFileName);
}
} else
{
b3Warning("Cannot find file %s\n", cmd.m_dataStreamArguments.m_bulletFileName);
}
}
m_physicsClient.submitClientCommand(cmd);
} else
{
if (m_numMotors)
{
SharedMemoryCommand command;
command.m_type =CMD_SEND_DESIRED_STATE;
prepareControlCommand(command);
enqueueCommand(command);
command.m_type =CMD_STEP_FORWARD_SIMULATION;
enqueueCommand(command);
}
}
}
}
}
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_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);
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 RobotControlExample::stepSimulation(float deltaTime)
{
m_physicsServer.processClientCommands();
if (m_physicsClient.isConnected())
{
SharedMemoryStatus status;
bool hasStatus = m_physicsClient.processServerStatus(status);
if (hasStatus && status.m_type == CMD_ACTUAL_STATE_UPDATE_COMPLETED)
{
//update sensor feedback: joint force/torque data and measured joint positions
for (int i=0;i<m_numMotors;i++)
{
int jointIndex = m_motorTargetState[i].m_jointIndex;
int positionIndex = m_motorTargetState[i].m_posIndex;
int velocityIndex = m_motorTargetState[i].m_uIndex;
m_motorTargetState[i].m_measuredJointPosition = status.m_sendActualStateArgs.m_actualStateQ[positionIndex];
m_motorTargetState[i].m_measuredJointVelocity = status.m_sendActualStateArgs.m_actualStateQdot[velocityIndex];
m_motorTargetState[i].m_measuredJointForce.setValue(status.m_sendActualStateArgs.m_jointReactionForces[6*jointIndex],
status.m_sendActualStateArgs.m_jointReactionForces[6*jointIndex+1],
status.m_sendActualStateArgs.m_jointReactionForces[6*jointIndex+2]);
m_motorTargetState[i].m_measuredJointTorque.setValue(status.m_sendActualStateArgs.m_jointReactionForces[6*jointIndex+3],
status.m_sendActualStateArgs.m_jointReactionForces[6*jointIndex+4],
status.m_sendActualStateArgs.m_jointReactionForces[6*jointIndex+5]);
if (m_motorTargetState[i].m_measuredJointPosition>0.1)
{
m_motorTargetState[i].m_velTarget = -1.5;
} else
{
m_motorTargetState[i].m_velTarget = 1.5;
}
b3Printf("Joint Force (Linear) [%s]=(%f,%f,%f)\n",m_motorTargetState[i].m_jointName.c_str(),m_motorTargetState[i].m_measuredJointForce.x(),m_motorTargetState[i].m_measuredJointForce.y(),m_motorTargetState[i].m_measuredJointForce.z());
b3Printf("Joint Torque (Angular) [%s]=(%f,%f,%f)\n",m_motorTargetState[i].m_jointName.c_str(),m_motorTargetState[i].m_measuredJointTorque.x(),m_motorTargetState[i].m_measuredJointTorque.y(),m_motorTargetState[i].m_measuredJointTorque.z());
}
}
if (hasStatus && status.m_type == CMD_URDF_LOADING_COMPLETED)
{
SharedMemoryCommand sensorCommand;
sensorCommand.m_type = CMD_CREATE_SENSOR;
sensorCommand.m_createSensorArguments.m_numJointSensorChanges = 0;
for (int jointIndex=0;jointIndex<m_physicsClient.getNumJoints();jointIndex++)
{
b3JointInfo info;
m_physicsClient.getJointInfo(jointIndex,info);
if (m_verboseOutput)
{
b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex);
}
if (info.m_flags & JOINT_HAS_MOTORIZED_POWER)
{
if (m_numMotors<MAX_NUM_MOTORS)
{
switch (m_option)
{
case ROBOT_VELOCITY_CONTROL:
{
char motorName[1024];
sprintf(motorName,"%s q'", info.m_jointName);
MyMotorInfo* motorInfo = &m_motorTargetState[m_numMotors];
motorInfo->m_jointName = info.m_jointName;
motorInfo->m_velTarget = 0.f;
motorInfo->m_posTarget = 0.f;
motorInfo->m_uIndex = info.m_uIndex;
SliderParams slider(motorName,&motorInfo->m_velTarget);
slider.m_minVal=-4;
slider.m_maxVal=4;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
m_numMotors++;
break;
}
case ROBOT_PD_CONTROL:
{
char motorName[1024];
MyMotorInfo* motorInfo = &m_motorTargetState[m_numMotors];
motorInfo->m_jointName = info.m_jointName;
motorInfo->m_velTarget = 0.f;
motorInfo->m_posTarget = 0.f;
motorInfo->m_uIndex = info.m_uIndex;
motorInfo->m_posIndex = info.m_qIndex;
motorInfo->m_kp = 1;
motorInfo->m_kd = 0;
{
sprintf(motorName,"%s kp", info.m_jointName);
SliderParams slider(motorName,&motorInfo->m_kp);
slider.m_minVal=0;
slider.m_maxVal=1;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
{
sprintf(motorName,"%s q", info.m_jointName);
SliderParams slider(motorName,&motorInfo->m_posTarget);
slider.m_minVal=-SIMD_PI;
slider.m_maxVal=SIMD_PI;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
{
sprintf(motorName,"%s kd", info.m_jointName);
SliderParams slider(motorName,&motorInfo->m_kd);
slider.m_minVal=0;
slider.m_maxVal=1;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
{
sprintf(motorName,"%s q'", info.m_jointName);
SliderParams slider(motorName,&motorInfo->m_velTarget);
slider.m_minVal=-10;
slider.m_maxVal=10;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
m_numMotors++;
break;
}
case ROBOT_PING_PONG_JOINT_FEEDBACK:
{
if (info.m_flags & JOINT_HAS_MOTORIZED_POWER)
{
if (m_numMotors<MAX_NUM_MOTORS)
{
MyMotorInfo* motorInfo = &m_motorTargetState[m_numMotors];
motorInfo->m_jointName = info.m_jointName;
motorInfo->m_velTarget = 0.f;
motorInfo->m_posTarget = 0.f;
motorInfo->m_uIndex = info.m_uIndex;
motorInfo->m_posIndex = info.m_qIndex;
motorInfo->m_jointIndex = jointIndex;
sensorCommand.m_createSensorArguments.m_jointIndex[sensorCommand.m_createSensorArguments.m_numJointSensorChanges] = jointIndex;
sensorCommand.m_createSensorArguments.m_enableJointForceSensor[sensorCommand.m_createSensorArguments.m_numJointSensorChanges] = true;
sensorCommand.m_createSensorArguments.m_numJointSensorChanges++;
m_numMotors++;
}
}
break;
}
default:
{
b3Warning("Unknown control mode in RobotControlExample::stepSimulation");
}
};
}
}
}
if (sensorCommand.m_createSensorArguments.m_numJointSensorChanges)
{
enqueueCommand(sensorCommand);
}
}
if (m_physicsClient.canSubmitCommand())
{
if (m_userCommandRequests.size())
{
if (m_verboseOutput)
{
b3Printf("Outstanding user command requests: %d\n", m_userCommandRequests.size());
}
SharedMemoryCommand cmd = m_userCommandRequests[0];
//a manual 'pop_front', we don't use 'remove' because it will re-order the commands
for (int i=1;i<m_userCommandRequests.size();i++)
{
m_userCommandRequests[i-1] = m_userCommandRequests[i];
}
m_userCommandRequests.pop_back();
if (cmd.m_type == CMD_CREATE_SENSOR)
{
b3Printf("CMD_CREATE_SENSOR!\n");
}
if (cmd.m_type == CMD_SEND_BULLET_DATA_STREAM)
{
char relativeFileName[1024];
bool fileFound = b3ResourcePath::findResourcePath(cmd.m_dataStreamArguments.m_bulletFileName,relativeFileName,1024);
if (fileFound)
{
FILE *fp = fopen(relativeFileName, "rb");
if (fp)
{
fseek(fp, 0L, SEEK_END);
int mFileLen = ftell(fp);
fseek(fp, 0L, SEEK_SET);
if (mFileLen<SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE)
{
char* data = (char*)malloc(mFileLen);
fread(data, mFileLen, 1, fp);
fclose(fp);
cmd.m_dataStreamArguments.m_streamChunkLength = mFileLen;
m_physicsClient.uploadBulletFileToSharedMemory(data,mFileLen);
if (m_verboseOutput)
{
b3Printf("Loaded bullet data chunks into shared memory\n");
}
free(data);
} else
{
b3Warning("Bullet file size (%d) exceeds of streaming memory chunk size (%d)\n", mFileLen,SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
}
} else
{
b3Warning("Cannot open file %s\n", relativeFileName);
}
} else
{
b3Warning("Cannot find file %s\n", cmd.m_dataStreamArguments.m_bulletFileName);
}
}
m_physicsClient.submitClientCommand(cmd);
} else
{
if (m_numMotors)
{
SharedMemoryCommand command;
command.m_type =CMD_SEND_DESIRED_STATE;
prepareControlCommand(command);
enqueueCommand(command);
command.m_type =CMD_STEP_FORWARD_SIMULATION;
enqueueCommand(command);
command.m_type = CMD_REQUEST_ACTUAL_STATE;
enqueueCommand(command);
}
}
}
}
}
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);
}
};
}
