static int
processInput (void) {
int command = readBrailleCommand(&brl, getCurrentCommandContext());
if (command != EOF) {
switch (command & BRL_MSK_CMD) {
case BRL_CMD_OFFLINE:
if (!isOffline) {
logMessage(LOG_DEBUG, "braille display offline");
isOffline = 1;
}
return 0;
default:
break;
}
}
if (isOffline) {
logMessage(LOG_DEBUG, "braille display online");
isOffline = 0;
}
if (command == EOF) return 0;
enqueueCommand(command);
return 1;
}
void
eventHelper_suspendThread(jbyte sessionID, jthread thread)
{
JNIEnv *env = getEnv();
HelperCommand *command = jvmtiAllocate(sizeof(*command));
if (command == NULL) {
EXIT_ERROR(AGENT_ERROR_OUT_OF_MEMORY,"HelperCommmand");
}
(void)memset(command, 0, sizeof(*command));
command->commandKind = COMMAND_SUSPEND_THREAD;
command->sessionID = sessionID;
saveGlobalRef(env, thread, &(command->u.suspendThread.thread));
enqueueCommand(command, JNI_TRUE, JNI_FALSE);
}
/*
* This, currently, cannot go through the normal event handling code
* because the JVMTI event does not contain a thread.
*/
void
eventHelper_reportVMInit(JNIEnv *env, jbyte sessionID, jthread thread, jbyte suspendPolicy)
{
HelperCommand *command = jvmtiAllocate(sizeof(*command));
if (command == NULL) {
EXIT_ERROR(AGENT_ERROR_OUT_OF_MEMORY,"HelperCommmand");
}
(void)memset(command, 0, sizeof(*command));
command->commandKind = COMMAND_REPORT_VM_INIT;
command->sessionID = sessionID;
saveGlobalRef(env, thread, &(command->u.reportVMInit.thread));
command->u.reportVMInit.suspendPolicy = suspendPolicy;
enqueueCommand(command, JNI_TRUE, JNI_FALSE);
}
void
eventHelper_reportInvokeDone(jbyte sessionID, jthread thread)
{
JNIEnv *env = getEnv();
HelperCommand *command = jvmtiAllocate(sizeof(*command));
if (command == NULL) {
EXIT_ERROR(AGENT_ERROR_OUT_OF_MEMORY,"HelperCommand");
}
(void)memset(command, 0, sizeof(*command));
command->commandKind = COMMAND_REPORT_INVOKE_DONE;
command->sessionID = sessionID;
saveGlobalRef(env, thread, &(command->u.reportInvokeDone.thread));
enqueueCommand(command, JNI_TRUE, JNI_FALSE);
}
static int
brl_readCommand (BrailleDisplay *brl, KeyTableCommandContext context) {
InputPacket packet;
int length;
if (context == KTB_CTX_WAITING) return BRL_CMD_NOOP;
if (writeFunction) return EOF;
while (*initialCommand != EOF) enqueueCommand(*initialCommand++);
while ((length = readPacket(brl, &packet))) {
if ((packet.data.type >= IPT_MINIMUM_LINE) &&
(packet.data.type <= IPT_MAXIMUM_LINE)) {
enqueueCommand(BRL_CMD_BLK(GOTOLINE) | BRL_FLG_MOTION_TOLEFT | (packet.data.type - IPT_MINIMUM_LINE));
writeFunction = writeLine;
return EOF;
}
switch (packet.data.type) {
case IPT_SEARCH_ATTRIBUTE:
case IPT_CURRENT_LINE:
enqueueCommand(BRL_CMD_HOME);
enqueueCommand(BRL_CMD_LNBEG);
writeFunction = writeLine;
return EOF;
case IPT_CURRENT_LOCATION:
writeFunction = writeLocation;
return EOF;
default:
logUnexpectedPacket(&packet, length);
break;
}
}
return (errno == EAGAIN)? EOF: BRL_CMD_RESTARTBRL;
}
jbyte
eventHelper_reportEvents(jbyte sessionID, struct bag *eventBag)
{
int size = bagSize(eventBag);
jbyte suspendPolicy = JDWP_SUSPEND_POLICY(NONE);
jboolean reportingVMDeath = JNI_FALSE;
jboolean wait;
int command_size;
HelperCommand *command;
ReportEventCompositeCommand *recc;
struct singleTracker tracker;
if (size == 0) {
return suspendPolicy;
}
(void)bagEnumerateOver(eventBag, enumForCombinedSuspendPolicy, &suspendPolicy);
(void)bagEnumerateOver(eventBag, enumForVMDeath, &reportingVMDeath);
/*LINTED*/
command_size = (int)(sizeof(HelperCommand) +
sizeof(CommandSingle)*(size-1));
command = jvmtiAllocate(command_size);
(void)memset(command, 0, command_size);
command->commandKind = COMMAND_REPORT_EVENT_COMPOSITE;
command->sessionID = sessionID;
recc = &command->u.reportEventComposite;
recc->suspendPolicy = suspendPolicy;
recc->eventCount = size;
tracker.recc = recc;
tracker.index = 0;
(void)bagEnumerateOver(eventBag, enumForCopyingSingles, &tracker);
/*
* We must wait if this thread (the event thread) is to be
* suspended or if the VM is about to die. (Waiting in the latter
* case ensures that we get the event out before the process dies.)
*/
wait = (jboolean)((suspendPolicy != JDWP_SUSPEND_POLICY(NONE)) ||
reportingVMDeath);
enqueueCommand(command, wait, reportingVMDeath);
return suspendPolicy;
}
static int
processInput (void) {
int command = readBrailleCommand(&brl, getCurrentCommandContext());
if (command != EOF) {
switch (command & BRL_MSK_CMD) {
case BRL_CMD_OFFLINE:
setBrailleOffline(&brl);
return 0;
default:
break;
}
}
setBrailleOnline(&brl);
if (command == EOF) return 0;
enqueueCommand(command);
return 1;
}
void com_ximeta_driver_NDASDiskArrayController::processSettingCommand(com_ximeta_driver_NDASCommand *command)
{
PPNPCommand pnpCommand = command->pnpCommand();
int result;
// Discovery.
result = fProvider->discovery();
/*
if ( kNDASStatusFailure != fProvider->status() ) {
result = fProvider->discovery();
} else {
return;
}
*/
// Check Data connection
for (int count = 0; count < MAX_NR_OF_TARGETS_PER_DEVICE; count++) {
fProvider->checkDataConnection(count);
}
if ( kNDASStatusOffline == result ) {
for (int count = 0; count < MAX_NR_OF_TARGETS_PER_DEVICE; count++) {
if ( kNDASUnitStatusMountRO == fProvider->targetStatus(count)
|| kNDASUnitStatusMountRW == fProvider->targetStatus(count) ) {
result = kNDASStatusOnline;
goto out;
}
}
}
switch(fProvider->status()) {
case kNDASStatusOffline:
{
// Get Unit disk info.
if( kNDASStatusOnline == result ) {
result = fProvider->getUnitDiskInfo();
}
DbgIOLog(DEBUG_MASK_NDAS_INFO, ("processSettingCommand: The Status of this NDAS changed to %d", fProvider->getState()));
}
break;
case kNDASStatusOnline:
{
// Check every Targets.
for(int count = 0; count < MAX_NR_OF_TARGETS_PER_DEVICE; count++) {
bool bNeedMount = false;
bool bNeedUnmount = false;
bool bNeedCreateDevice = false;
bool bNeedDeleteDevice = false;
// Check BSD Name.
if(fTargetDevices[count] != NULL) {
fProvider->setTargetBSDName(count, fTargetDevices[count]->bsdName());
}
if ( fProvider->targetNeedRefresh(count) ) {
result = fProvider->getUnitDiskInfo();
fProvider->setTargetNeedRefresh(count, false);
}
switch(fProvider->targetStatus(count)) {
case kNDASUnitStatusNotPresent:
break;
case kNDASUnitStatusUnmount:
{
// UInt32 status;
// Need Mount?
switch(fProvider->targetConfiguration(count)) {
case kNDASUnitConfigurationMountRW:
{
bNeedMount = true;
if(fTargetDevices[count] == NULL) {
bNeedCreateDevice = true;
}
}
break;
case kNDASUnitConfigurationMountRO:
{
bNeedMount = true;
if(fTargetDevices[count] == NULL) {
bNeedCreateDevice = true;
}
}
break;
default:
break;
}
}
break;
case kNDASUnitStatusMountRO:
{
// UInt32 status;
switch(fProvider->targetConfiguration(count)) {
case kNDASUnitConfigurationMountRW:
case kNDASUnitConfigurationUnmount:
{
// Need Unmount.
bNeedUnmount = true;
bNeedDeleteDevice = true;
}
break;
case kNDASUnitConfigurationMountRO:
{
// Interface Change.
if(pnpCommand->fInterfaceChanged) {
DbgIOLog(DEBUG_MASK_NDAS_INFO, ("processSettingCommand: Interface Changed.\n"));
bNeedUnmount = true;
//fProvider->unmount(count);
} else {
// Check Connection.
if(!fProvider->targetConnected(count)) {
DbgIOLog(DEBUG_MASK_NDAS_WARING, ("processSettingCommand: Connection Disconnected!!!\n"));
// Need Reconnect.
bNeedMount = true;
}
}
}
break;
default:
break;
}
}
break;
case kNDASUnitStatusMountRW:
{
// UInt32 status;
// Need Unmount?
switch(fProvider->targetConfiguration(count)) {
case kNDASUnitConfigurationMountRO:
case kNDASUnitConfigurationUnmount:
{
bNeedUnmount = true;
bNeedDeleteDevice = true;
}
break;
case kNDASUnitConfigurationMountRW:
{
// Interface Change.
if(pnpCommand->fInterfaceChanged) {
DbgIOLog(DEBUG_MASK_NDAS_WARING, ("processSettingCommand: Interface Changed.\n"));
bNeedUnmount = true;
//fProvider->unmount(count);
} else {
// Check Connection.
if(!fProvider->targetConnected(count)) {
DbgIOLog(DEBUG_MASK_NDAS_WARING, ("processSettingCommand: Connection Disconnected!!!\n"));
// Need Reconnect.
bNeedMount = true;
}
}
}
break;
default:
break;
}
}
default:
break;
}
//
// Mount, Unmount...
//
if(bNeedMount) {
UInt32 status, prevStatus;
prevStatus = fProvider->targetStatus(count);
status = fProvider->mount(count, pnpCommand);
if((status == kNDASUnitStatusMountRW
|| status == kNDASUnitStatusMountRO))
{
//
// Create Logical Device.
//
if(bNeedCreateDevice)
{
// Create Logical Device.
if(createLogicalDevice(count) != kIOReturnSuccess) {
fProvider->unmount(count);
}
}
//
// Process pending command caused by disconnection.
//
com_ximeta_driver_NDASCommand* command = fProvider->targetPendingCommand(count);
if(NULL != command) {
DbgIOLog(DEBUG_MASK_NDAS_ERROR, ("processSettingCommand: Processing Pending Command\n"));
fProvider->setTargetPendingCommand(count, NULL);
if(fTargetDevices[count] != NULL) {
enqueueCommand(command);
/*
PSCSICommand pScsiCommand;
pScsiCommand = command->scsiCommand();
if(kIOReturnSuccess != pScsiCommand->CmdData.results) {
pScsiCommand->CmdData.results = kIOReturnBusy;
pScsiCommand->CmdData.taskStatus = kSCSITaskStatus_ProtocolTimeoutOccurred;
}
fTargetDevices[count]->completeCommand(command);
*/
} else {
DbgIOLog(DEBUG_MASK_NDAS_ERROR, ("processSettingCommand: fTargetDevice is NULL!!!\n"));
}
}
} else {
if(kNDASUnitStatusMountRW == prevStatus) {
bNeedUnmount = true;
bNeedDeleteDevice = true;
}
}
}
if(bNeedUnmount) {
if(bNeedDeleteDevice) {
if(deleteLogicalDevice(count) == kIOReturnSuccess
&& bNeedUnmount) {
fProvider->unmount(count);
}
} else {
fProvider->unmount(count);
}
}
}
}
break;
case kNDASStatusNoPermission:
case kNDASStatusFailure:
{
if( kNDASStatusOnline == result ) {
result = fProvider->getUnitDiskInfo();
for(int count = 0; count < MAX_NR_OF_TARGETS_PER_DEVICE; count++) {
fProvider->setTargetNeedRefresh(count, false);
}
}
}
break;
default:
DbgIOLog(DEBUG_MASK_NDAS_ERROR, ("processSettingCommand: Bad Status %d", fProvider->status()));
break;
}
out:
// Change Status.
fProvider->setStatus(result);
fProvider->completePnPMessage(command);
return;
}
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 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::stepSimulation(float deltaTime)
{
if (m_options == eCLIENTEXAMPLE_SERVER)
{
for (int i=0;i<100;i++)
{
m_physicsServer.processClientCommands();
}
}
if (m_prevSelectedBody != m_selectedBody)
{
createButtons();
m_prevSelectedBody = m_selectedBody;
}
//while (!b3CanSubmitCommand(m_physicsClientHandle))
{
b3SharedMemoryStatusHandle status = b3ProcessServerStatus(m_physicsClientHandle);
bool hasStatus = (status != 0);
if (hasStatus)
{
int statusType = b3GetStatusType(status);
if (statusType == CMD_ACTUAL_STATE_UPDATE_COMPLETED)
{
//b3Printf("bla\n");
}
if (statusType ==CMD_CAMERA_IMAGE_COMPLETED)
{
static int counter=0;
char msg[1024];
sprintf(msg,"Camera image %d OK\n",counter++);
b3CameraImageData imageData;
b3GetCameraImageData(m_physicsClientHandle,&imageData);
if (m_canvas && m_canvasIndex >=0)
{
for (int i=0;i<imageData.m_pixelWidth;i++)
{
for (int j=0;j<imageData.m_pixelHeight;j++)
{
int xIndex = int(float(i)*(float(camVisualizerWidth)/float(imageData.m_pixelWidth)));
int yIndex = int(float(j)*(float(camVisualizerHeight)/float(imageData.m_pixelHeight)));
btClamp(yIndex,0,imageData.m_pixelHeight);
btClamp(xIndex,0,imageData.m_pixelWidth);
int bytesPerPixel = 4;
int pixelIndex = (i+j*imageData.m_pixelWidth)*bytesPerPixel;
m_canvas->setPixel(m_canvasIndex,xIndex,camVisualizerHeight-1-yIndex,
imageData.m_rgbColorData[pixelIndex],
imageData.m_rgbColorData[pixelIndex+1],
imageData.m_rgbColorData[pixelIndex+2],
255);
// imageData.m_rgbColorData[pixelIndex+3]);
}
}
m_canvas->refreshImageData(m_canvasIndex);
}
b3Printf(msg);
}
if (statusType == CMD_CAMERA_IMAGE_FAILED)
{
b3Printf("Camera image FAILED\n");
}
if (statusType == CMD_URDF_LOADING_COMPLETED)
{
int bodyIndex = b3GetStatusBodyIndex(status);
if (bodyIndex>=0)
{
int numJoints = b3GetNumJoints(m_physicsClientHandle,bodyIndex);
for (int i=0;i<numJoints;i++)
{
b3JointInfo info;
b3GetJointInfo(m_physicsClientHandle,bodyIndex,i,&info);
b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex);
}
ComboBoxParams comboParams;
comboParams.m_comboboxId = bodyIndex;
comboParams.m_numItems = 1;
comboParams.m_startItem = 0;
comboParams.m_callback = MyComboBoxCallback;
comboParams.m_userPointer = this;
const char* bla = "bla";
const char* blarray[1];
blarray[0] = bla;
comboParams.m_items=blarray;//{&bla};
m_guiHelper->getParameterInterface()->registerComboBox(comboParams);
}
}
}
}
if (b3CanSubmitCommand(m_physicsClientHandle))
{
if (m_userCommandRequests.size())
{
//b3Printf("Outstanding user command requests: %d\n", m_userCommandRequests.size());
int commandId = 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 (commandId ==CMD_RESET_SIMULATION)
{
m_selectedBody = -1;
m_numMotors=0;
createButtons();
b3SharedMemoryCommandHandle commandHandle = b3InitResetSimulationCommand(m_physicsClientHandle);
if (m_options == eCLIENTEXAMPLE_SERVER)
{
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
while (!b3CanSubmitCommand(m_physicsClientHandle))
{
m_physicsServer.processClientCommands();
b3SharedMemoryStatusHandle status = b3ProcessServerStatus(m_physicsClientHandle);
bool hasStatus = (status != 0);
if (hasStatus)
{
int statusType = b3GetStatusType(status);
b3Printf("Status after reset: %d",statusType);
}
}
} else
{
prepareAndSubmitCommand(commandId);
}
} else
{
prepareAndSubmitCommand(commandId);
}
} else
{
if (m_numMotors)
{
enqueueCommand(CMD_SEND_DESIRED_STATE);
enqueueCommand(CMD_STEP_FORWARD_SIMULATION);
if (m_options != eCLIENTEXAMPLE_SERVER)
{
enqueueCommand(CMD_REQUEST_DEBUG_LINES);
}
//enqueueCommand(CMD_REQUEST_ACTUAL_STATE);
}
}
}
}
void PhysicsClientExample::stepSimulation(float deltaTime)
{
if (m_prevSelectedBody != m_selectedBody)
{
createButtons();
m_prevSelectedBody = m_selectedBody;
}
//while (!b3CanSubmitCommand(m_physicsClientHandle))
{
b3SharedMemoryStatusHandle status = b3ProcessServerStatus(m_physicsClientHandle);
bool hasStatus = (status != 0);
if (hasStatus)
{
int statusType = b3GetStatusType(status);
if (statusType == CMD_ACTUAL_STATE_UPDATE_COMPLETED)
{
//b3Printf("bla\n");
}
if (statusType == CMD_URDF_LOADING_COMPLETED)
{
int bodyIndex = b3GetStatusBodyIndex(status);
if (bodyIndex>=0)
{
int numJoints = b3GetNumJoints(m_physicsClientHandle,bodyIndex);
for (int i=0;i<numJoints;i++)
{
b3JointInfo info;
b3GetJointInfo(m_physicsClientHandle,bodyIndex,i,&info);
b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex);
}
ComboBoxParams comboParams;
comboParams.m_comboboxId = bodyIndex;
comboParams.m_numItems = 1;
comboParams.m_startItem = 0;
comboParams.m_callback = MyComboBoxCallback;
comboParams.m_userPointer = this;
const char* bla = "bla";
const char* blarray[1];
blarray[0] = bla;
comboParams.m_items=blarray;//{&bla};
m_guiHelper->getParameterInterface()->registerComboBox(comboParams);
}
}
}
}
if (b3CanSubmitCommand(m_physicsClientHandle))
{
if (m_userCommandRequests.size())
{
//b3Printf("Outstanding user command requests: %d\n", m_userCommandRequests.size());
int commandId = 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 (commandId ==CMD_RESET_SIMULATION)
{
m_selectedBody = -1;
m_numMotors=0;
createButtons();
}
prepareAndSubmitCommand(commandId);
} else
{
if (m_numMotors)
{
enqueueCommand(CMD_SEND_DESIRED_STATE);
enqueueCommand(CMD_STEP_FORWARD_SIMULATION);
enqueueCommand(CMD_REQUEST_DEBUG_LINES);
//enqueueCommand(CMD_REQUEST_ACTUAL_STATE);
}
}
}
}
