void CBubbleRob::handleSimulation() { // called when the main script calls: simHandleModule _initialize(); if (_movementDuration>0.0f) { // movement mode if (simReadProximitySensor(_handleOfProximitySensor,NULL,NULL,NULL)>0) _backMovementDuration=3.0f; // we detected an obstacle, we move backward for 3 seconds if (_backMovementDuration>0.0f) { // We move backward simSetJointTargetVelocity(_handleOfLeftJoint,-_maxVelocity*_backRelativeVelocities[0]); simSetJointTargetVelocity(_handleOfRightJoint,-_maxVelocity*_backRelativeVelocities[1]); _backMovementDuration-=simGetSimulationTimeStep(); } else { // We move forward simSetJointTargetVelocity(_handleOfLeftJoint,_maxVelocity); simSetJointTargetVelocity(_handleOfRightJoint,_maxVelocity); } _movementDuration-=simGetSimulationTimeStep(); if ((_movementDuration<=0.0f)&&(_setToZeroAtMovementEnd!=NULL)) { _setToZeroAtMovementEnd[0]=0; // This will unblock the thread that called simExtBubbleMoveAndAvoid(x,y,false) // Above memory location can only be manipulated if we are sure the simulation is still running!! _setToZeroAtMovementEnd=NULL; } } else { // stopped mode simSetJointTargetVelocity(_handleOfLeftJoint,0.0f); simSetJointTargetVelocity(_handleOfRightJoint,0.0f); } }
float CK3::getFingerProxSensor(int index) { _initialize(); if ( (index<0)||(index>=2) ) return(-1.0f); // error float ptAndDist[4]; if (((simGetExplicitHandling(_k3GripperDistanceSensorHandles[index])&1)==0)&&(simReadProximitySensor(_k3GripperDistanceSensorHandles[index],ptAndDist,NULL,NULL)>0)) return(ptAndDist[3]); else return(NO_DETECTION_VALUE_DISTANCE); }
void CK3::_updateStateVisualization() { _initialize(); // IR sensors on the base: float ptAndDist[4]; std::string s; for (int i=0;i<9;i++) { if (((simGetExplicitHandling(_k3IrSensorHandles[i])&1)==0)&&(simReadProximitySensor(_k3IrSensorHandles[i],ptAndDist,NULL,NULL)>0)) { simSetUIButtonLabel(_k3DisplayHandle,110+i,"&&Box",NULL); std::stringstream out; out << int(ptAndDist[3]*1000.0f); s=out.str(); simSetUIButtonLabel(_k3DisplayHandle,210+i,s.c_str(),NULL); } else { simSetUIButtonLabel(_k3DisplayHandle,110+i,"",NULL); simSetUIButtonLabel(_k3DisplayHandle,210+i,"",NULL); } } // UI title: char* objName=simGetObjectName(_associatedObjectID); if (objName!=NULL) { std::string nm(objName); simReleaseBuffer(objName); nm+=" state visualization"; simSetUIButtonLabel(_k3DisplayHandle,0,nm.c_str(),NULL); } // US sensors on the base: for (int i=0;i<5;i++) { if (((simGetExplicitHandling(_k3UsSensorHandles[i])&1)==0)&&(simReadProximitySensor(_k3UsSensorHandles[i],ptAndDist,NULL,NULL)>0)) { simSetUIButtonLabel(_k3DisplayHandle,100+i,"&&Box",NULL); std::stringstream out; out << int(ptAndDist[3]*1000.0f); s=out.str(); simSetUIButtonLabel(_k3DisplayHandle,200+i,s.c_str(),NULL); } else { simSetUIButtonLabel(_k3DisplayHandle,100+i,"",NULL); simSetUIButtonLabel(_k3DisplayHandle,200+i,"",NULL); } } // Color sensors on base: for (int i=0;i<2;i++) { float* auxValues=NULL; int* auxValuesCount=NULL; float col[3]={0.0f,0.0f,0.0f}; if (simReadVisionSensor(_k3ColorSensorHandles[i],&auxValues,&auxValuesCount)>=0) { if ((auxValuesCount[0]>0)||(auxValuesCount[1]>=15)) { col[0]=auxValues[11]; col[1]=auxValues[12]; col[2]=auxValues[13]; } simReleaseBuffer((char*)auxValues); simReleaseBuffer((char*)auxValuesCount); } simSetUIButtonColor(_k3DisplayHandle,300+i,col,NULL,NULL); } // Base motor velocities and encoders: for (int i=0;i<2;i++) { std::stringstream out1,out2; out1 << std::fixed << std::setprecision(1) << _currentVelocities[i]*180.0f/piValue; s=out1.str(); simSetUIButtonLabel(_k3DisplayHandle,320+i,s.c_str(),NULL); out2 << int(float(ENCODER_IMPULSES_PER_TURN)*_cumulativeMotorAngles[i]/(2.0f*piValue)); s=out2.str(); simSetUIButtonLabel(_k3DisplayHandle,310+i,s.c_str(),NULL); } // Arm position: std::stringstream out; out << int((1.0f-(_currentArmPosition*180.0f/(195.0f*piValue)))*600.0f+300.0f); s=out.str(); simSetUIButtonLabel(_k3DisplayHandle,120,s.c_str(),NULL); simSetUISlider(_k3DisplayHandle,121,int(1000.0f*_currentArmPosition*180.0f/(195.0f*piValue))); // Finger motors: out.str(""); out << int(_currentGripperGap_unscaled*1000.0f); s=out.str(); simSetUIButtonLabel(_k3DisplayHandle,130,s.c_str(),NULL); simSetUISlider(_k3DisplayHandle,131,int(0.5f+(1.0f-_currentGripperGap_unscaled/0.055f)*1000.0f)); simSetUISlider(_k3DisplayHandle,132,int(0.5f+(_currentGripperGap_unscaled/0.055f)*1000.0f)); // Gripper proximity sensors: for (int i=0;i<2;i++) { if (((simGetExplicitHandling(_k3GripperDistanceSensorHandles[i])&1)==0)&&(simReadProximitySensor(_k3GripperDistanceSensorHandles[i],ptAndDist,NULL,NULL)>0)) { out.str(""); out << int(ptAndDist[3]*1000.0f); s=out.str(); simSetUIButtonLabel(_k3DisplayHandle,133+i,s.c_str(),NULL); } else simSetUIButtonLabel(_k3DisplayHandle,133+i,"",NULL); } // Gripper color sensors: for (int i=0;i<2;i++) { float* auxValues=NULL; int* auxValuesCount=NULL; float col[3]={0.0f,0.0f,0.0f}; if (simReadVisionSensor(_k3GripperColorSensorHandles[i],&auxValues,&auxValuesCount)>=0) { if ((auxValuesCount[0]>0)||(auxValuesCount[1]>=15)) { col[0]=auxValues[11]; col[1]=auxValues[12]; col[2]=auxValues[13]; } simReleaseBuffer((char*)auxValues); simReleaseBuffer((char*)auxValuesCount); } simSetUIButtonColor(_k3DisplayHandle,135+i,col,NULL,NULL); } }
VREP_DLLEXPORT void* v_repMessage(int message,int* auxiliaryData,void* customData,int* replyData) { // This is called quite often. Just watch out for messages/events you want to handle // This function should not generate any error messages: int errorModeSaved; simGetIntegerParameter(sim_intparam_error_report_mode,&errorModeSaved); simSetIntegerParameter(sim_intparam_error_report_mode,sim_api_errormessage_ignore); void* retVal=NULL; if (message==sim_message_eventcallback_modulehandle) { if ( (customData==NULL)||(std::string("K3").compare((char*)customData)==0) ) // is the command also meant for Khepera3? { float dt=simGetSimulationTimeStep(); for (unsigned int k3Index=0;k3Index<allK3s.size();k3Index++) { // 1. Run the robot control: for (int i=0;i<2;i++) { if (allK3s[k3Index].targetVelocities[i]>allK3s[k3Index].currentVelocities[i]) { allK3s[k3Index].currentVelocities[i]=allK3s[k3Index].currentVelocities[i]+allK3s[k3Index].maxAcceleration*dt; if (allK3s[k3Index].currentVelocities[i]>allK3s[k3Index].targetVelocities[i]) allK3s[k3Index].currentVelocities[i]=allK3s[k3Index].targetVelocities[i]; } else { allK3s[k3Index].currentVelocities[i]=allK3s[k3Index].currentVelocities[i]-allK3s[k3Index].maxAcceleration*dt; if (allK3s[k3Index].currentVelocities[i]<allK3s[k3Index].targetVelocities[i]) allK3s[k3Index].currentVelocities[i]=allK3s[k3Index].targetVelocities[i]; } simSetJointTargetVelocity(allK3s[k3Index].wheelMotorHandles[i],allK3s[k3Index].currentVelocities[i]); float jp; simGetJointPosition(allK3s[k3Index].wheelMotorHandles[i],&jp); float dp=jp-allK3s[k3Index].previousMotorAngles[i]; if (fabs(dp)>3.1415f) dp-=(2.0f*3.1415f*fabs(dp)/dp); allK3s[k3Index].cumulativeMotorAngles[i]+=dp; // corrected on 5/3/2012 thanks to Felix Fischer allK3s[k3Index].previousMotorAngles[i]=jp; } float adp=allK3s[k3Index].targetArmPosition-allK3s[k3Index].currentArmPosition; if (adp!=0.0f) { if (adp*allK3s[k3Index].currentArmVelocity>=0.0f) { float decelToZeroTime=(fabs(allK3s[k3Index].currentArmVelocity)+allK3s[k3Index].maxArmAcceleration*dt*1.0f)/allK3s[k3Index].maxArmAcceleration; float distToZero=0.5f*allK3s[k3Index].maxArmAcceleration*decelToZeroTime*decelToZeroTime; float dir=1.0f; if (allK3s[k3Index].currentArmVelocity!=0.0f) dir=allK3s[k3Index].currentArmVelocity/fabs(allK3s[k3Index].currentArmVelocity); else dir=adp/fabs(adp); if (fabs(adp)>distToZero) allK3s[k3Index].currentArmVelocity+=dir*allK3s[k3Index].maxArmAcceleration*dt; else allK3s[k3Index].currentArmVelocity-=dir*allK3s[k3Index].maxArmAcceleration*dt; } else allK3s[k3Index].currentArmVelocity*=(1-allK3s[k3Index].maxArmAcceleration*dt/fabs(allK3s[k3Index].currentArmVelocity)); } else { if (allK3s[k3Index].currentArmVelocity!=0.0f) { float dv=-allK3s[k3Index].currentArmVelocity*allK3s[k3Index].maxArmAcceleration*dt/fabs(allK3s[k3Index].currentArmVelocity); if ((allK3s[k3Index].currentArmVelocity+dv)*allK3s[k3Index].currentArmVelocity<0.0f) allK3s[k3Index].currentArmVelocity=0.0f; else allK3s[k3Index].currentArmVelocity+=dv; } } allK3s[k3Index].currentArmPosition+=allK3s[k3Index].currentArmVelocity*dt; simSetJointTargetPosition(allK3s[k3Index].armMotorHandles[0],allK3s[k3Index].currentArmPosition); simSetJointTargetPosition(allK3s[k3Index].armMotorHandles[1],-allK3s[k3Index].currentArmPosition); simSetJointTargetPosition(allK3s[k3Index].armMotorHandles[2],allK3s[k3Index].currentArmPosition); simSetJointTargetPosition(allK3s[k3Index].armMotorHandles[3],allK3s[k3Index].currentArmPosition); simSetJointTargetPosition(allK3s[k3Index].armMotorHandles[4],allK3s[k3Index].currentArmPosition); simSetJointTargetPosition(allK3s[k3Index].armMotorHandles[5],allK3s[k3Index].currentArmPosition); float jp; simGetJointPosition(allK3s[k3Index].fingerMotorHandles[0],&jp); allK3s[k3Index].currentGripperGap=(jp)+0.04f; float dp=allK3s[k3Index].targetGripperGap-allK3s[k3Index].currentGripperGap; float velToRegulate=0.0f; if (fabs(dp)<0.005f) { if (dp!=0.0f) velToRegulate=(fabs(dp)/0.005f)*0.045f*dp/fabs(dp); } else velToRegulate=0.045f*dp/fabs(dp); simSetJointTargetVelocity(allK3s[k3Index].fingerMotorHandles[0],velToRegulate); simSetJointTargetPosition(allK3s[k3Index].fingerMotorHandles[1],-jp*0.5f); simSetJointTargetPosition(allK3s[k3Index].fingerMotorHandles[2],-jp*0.5f); // 2. Update the robot's UI: // IR sensors on the base: float ptAndDist[4]; std::string s; for (int i=0;i<9;i++) { if (((simGetExplicitHandling(allK3s[k3Index].irSensorHandles[i])&1)==0)&&(simReadProximitySensor(allK3s[k3Index].irSensorHandles[i],ptAndDist,NULL,NULL)>0)) { simSetUIButtonLabel(allK3s[k3Index].uiHandle,110+i,"&&Box",NULL); std::stringstream out; out << int(ptAndDist[3]*1000.0f); s=out.str(); simSetUIButtonLabel(allK3s[k3Index].uiHandle,210+i,s.c_str(),NULL); } else { simSetUIButtonLabel(allK3s[k3Index].uiHandle,110+i,"",NULL); simSetUIButtonLabel(allK3s[k3Index].uiHandle,210+i,"",NULL); } } // UI title: char* objName=simGetObjectName(allK3s[k3Index].k3BaseHandle); if (objName!=NULL) { std::string nm(objName); simReleaseBuffer(objName); nm+=" state visualization"; simSetUIButtonLabel(allK3s[k3Index].uiHandle,0,nm.c_str(),NULL); } // US sensors on the base: for (int i=0;i<5;i++) { if (((simGetExplicitHandling(allK3s[k3Index].usSensorHandles[i])&1)==0)&&(simReadProximitySensor(allK3s[k3Index].usSensorHandles[i],ptAndDist,NULL,NULL)>0)) { simSetUIButtonLabel(allK3s[k3Index].uiHandle,100+i,"&&Box",NULL); std::stringstream out; out << int(ptAndDist[3]*1000.0f); s=out.str(); simSetUIButtonLabel(allK3s[k3Index].uiHandle,200+i,s.c_str(),NULL); } else { simSetUIButtonLabel(allK3s[k3Index].uiHandle,100+i,"",NULL); simSetUIButtonLabel(allK3s[k3Index].uiHandle,200+i,"",NULL); } } // Color sensors on base: for (int i=0;i<2;i++) { float* auxValues=NULL; int* auxValuesCount=NULL; float col[3]={0.0f,0.0f,0.0f}; if (simReadVisionSensor(allK3s[k3Index].colorSensorHandles[i],&auxValues,&auxValuesCount)>=0) { if ((auxValuesCount[0]>0)||(auxValuesCount[1]>=15)) { col[0]=auxValues[11]; col[1]=auxValues[12]; col[2]=auxValues[13]; } simReleaseBuffer((char*)auxValues); simReleaseBuffer((char*)auxValuesCount); } simSetUIButtonColor(allK3s[k3Index].uiHandle,300+i,col,NULL,NULL); } // Base motor velocities and encoders: for (int i=0;i<2;i++) { std::stringstream out1,out2; out1 << std::fixed << std::setprecision(1) << allK3s[k3Index].currentVelocities[i]*180.0f/3.1415f; s=out1.str(); simSetUIButtonLabel(allK3s[k3Index].uiHandle,320+i,s.c_str(),NULL); out2 << int(float(2764)*allK3s[k3Index].cumulativeMotorAngles[i]/(2.0f*3.1415f)); s=out2.str(); simSetUIButtonLabel(allK3s[k3Index].uiHandle,310+i,s.c_str(),NULL); } // Arm position: std::stringstream out; out << int((1.0f-(allK3s[k3Index].currentArmPosition*180.0f/(195.0f*3.1415f)))*600.0f+300.0f); s=out.str(); simSetUIButtonLabel(allK3s[k3Index].uiHandle,120,s.c_str(),NULL); simSetUISlider(allK3s[k3Index].uiHandle,121,int(1000.0f*allK3s[k3Index].currentArmPosition*180.0f/(195.0f*3.1415f))); // Finger motors: out.str(""); out << int(allK3s[k3Index].currentGripperGap*1000.0f); s=out.str(); simSetUIButtonLabel(allK3s[k3Index].uiHandle,130,s.c_str(),NULL); simSetUISlider(allK3s[k3Index].uiHandle,131,int(0.5f+(1.0f-allK3s[k3Index].currentGripperGap/0.055f)*1000.0f)); simSetUISlider(allK3s[k3Index].uiHandle,132,int(0.5f+(allK3s[k3Index].currentGripperGap/0.055f)*1000.0f)); // Gripper proximity sensors: for (int i=0;i<2;i++) { if (((simGetExplicitHandling(allK3s[k3Index].gripperDistanceSensorHandles[i])&1)==0)&&(simReadProximitySensor(allK3s[k3Index].gripperDistanceSensorHandles[i],ptAndDist,NULL,NULL)>0)) { out.str(""); out << int(ptAndDist[3]*1000.0f); s=out.str(); simSetUIButtonLabel(allK3s[k3Index].uiHandle,133+i,s.c_str(),NULL); } else simSetUIButtonLabel(allK3s[k3Index].uiHandle,133+i,"",NULL); } // Gripper color sensors: for (int i=0;i<2;i++) { float* auxValues=NULL; int* auxValuesCount=NULL; float col[3]={0.0f,0.0f,0.0f}; if (simReadVisionSensor(allK3s[k3Index].gripperColorSensorHandles[i],&auxValues,&auxValuesCount)>=0) { if ((auxValuesCount[0]>0)||(auxValuesCount[1]>=15)) { col[0]=auxValues[11]; col[1]=auxValues[12]; col[2]=auxValues[13]; } simReleaseBuffer((char*)auxValues); simReleaseBuffer((char*)auxValuesCount); } simSetUIButtonColor(allK3s[k3Index].uiHandle,135+i,col,NULL,NULL); } } } } if (message==sim_message_eventcallback_simulationended) { // simulation ended. Destroy all Khepera3 instances: allK3s.clear(); } simSetIntegerParameter(sim_intparam_error_report_mode,errorModeSaved); // restore previous settings return(retVal); }
void LUA_GETGRIPPERPROXSENSOR_CALLBACK(SLuaCallBack* p) { p->outputArgCount=0; CLuaFunctionData D; bool success=false; float distance=100.0f; if (D.readDataFromLua(p,inArgs_GETGRIPPERPROXSENSOR,inArgs_GETGRIPPERPROXSENSOR[0],LUA_GETGRIPPERPROXSENSOR_COMMAND)) { std::vector<CLuaFunctionDataItem>* inData=D.getInDataPtr(); int k3Index=getK3IndexFromHandle(inData->at(0).intData[0]); int sensorIndex=inData->at(1).intData[0]; if (k3Index!=-1) { if ( (sensorIndex>=0)&&(sensorIndex<2) ) { float ptAndDist[4]; if (((simGetExplicitHandling(allK3s[k3Index].gripperDistanceSensorHandles[sensorIndex])&1)==0)&&(simReadProximitySensor(allK3s[k3Index].gripperDistanceSensorHandles[sensorIndex],ptAndDist,NULL,NULL)>0)) distance=ptAndDist[3]; success=true; } else simSetLastError(LUA_GETGRIPPERPROXSENSOR_COMMAND,"Invalid index."); } else simSetLastError(LUA_GETGRIPPERPROXSENSOR_COMMAND,"Invalid Khepera3 handle."); } if (success) D.pushOutData(CLuaFunctionDataItem(distance)); D.writeDataToLua(p); }