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); } } } }