Пример #1
0
void FamousoPlugin::simExtSubscribeLaserData(SLuaCallBack* p)
{
  const char* obstackleName=simGetObjectName(p->inputInt[0]);
  if(obstackleName)
  {
    Log::out() << "subscribing for laser scanner data of " << simGetObjectName(p->objectID) << " detecting " << obstackleName
               << " on subject \"" << p->inputChar << "\"" << std::endl;
    std::shared_ptr<Subscriber> sub(new Subscriber(p->inputChar));
    plugin.laserSubs[sub->subject()]={sub, p->objectID, p->inputInt[0], 0.0f, 0.0f};
    sub->subscribe();
    sub->callback.bind<FamousoPlugin, &FamousoPlugin::laserCallBack>(&plugin);
  }
}
Пример #2
0
void DrawMeshHandler::synchronize() {
	// We update DrawMeshHandler's data from its associated scene object custom data:
	// 1. Get all the developer data attached to the associated scene object (this is custom data added by the developer):
	int buffSize = simGetObjectCustomDataLength(_associatedObjectID,
			CustomDataHeaders::DEVELOPER_DATA_HEADER);
	char* datBuff = new char[buffSize];
	simGetObjectCustomData(_associatedObjectID,
			CustomDataHeaders::DEVELOPER_DATA_HEADER, datBuff);
	std::vector<unsigned char> developerCustomData(datBuff, datBuff + buffSize);
	delete[] datBuff;
	// 2. From that retrieved data, try to extract sub-data with the DATA_MAIN tag:
	std::vector<unsigned char> tempMainData;

	if (CAccess::extractSerializationData(developerCustomData, DATA_MAIN,
			tempMainData)) { // Yes, the tag is there. For now we only have to synchronize _maxVelocity:

		// Remove # chars for compatibility with ROS
		_associatedObjectName = simGetObjectName(_associatedObjectID);
		std::stringstream streamtemp;
		streamtemp << "- [Draw Mesh] in '" << _associatedObjectName << "'."
				<< std::endl;

	}

}
Пример #3
0
void SetTwistHandler::synchronize(){
    // We update SetTwistHandler's data from its associated scene object custom data:
    // 1. Get all the developer data attached to the associated scene object (this is custom data added by the developer):
    int buffSize = simGetObjectCustomDataLength(_associatedObjectID, CustomDataHeaders::DEVELOPER_DATA_HEADER);
    char* datBuff=new char[buffSize];
    simGetObjectCustomData(_associatedObjectID, CustomDataHeaders::DEVELOPER_DATA_HEADER,datBuff);
    std::vector<unsigned char> developerCustomData(datBuff,datBuff+buffSize);
    delete[] datBuff;
    // 2. From that retrieved data, try to extract sub-data with the SET_OBJ_TWIST_DATA_MAIN tag:
    std::vector<unsigned char> tempMainData;

    if (CAccess::extractSerializationData(developerCustomData,CustomDataHeaders::SET_OBJ_TWIST_DATA_MAIN,tempMainData)){ // Yes, the tag is there. For now we only have to synchronize _maxVelocity:

       	// Remove # chars for compatibility with ROS
		_associatedObjectName = simGetObjectName(_associatedObjectID);
		std::string objectName(_associatedObjectName);
		std::replace( objectName.begin(), objectName.end(), '#', '_');
		//_pub = _nh.advertise<geometry_msgs::TwistStamped>(objectName+"/twist", 1);
		std::stringstream streamtemp;


		streamtemp << "- [Set Twist Target] in '" << _associatedObjectName << "'. Subscribed on the topic "<< objectName<<"/SetTwist"<< std::endl;
		ConsoleHandler::printInConsole(streamtemp);

    }



}
Пример #4
0
void FamousoPlugin::simExtSubscribeMotorPosition(SLuaCallBack* p)
{
  const char* actuatorName=simGetObjectName(p->inputInt[0]);
  if(actuatorName)
  {
    const char* actuatorTopic  = p->inputChar;
    simInt      actuatorObject = p->inputInt[0];

    plugin.mActuators.emplace_back(new PositionMotor(actuatorObject, actuatorTopic));

    Log::out() << "Registering " << *plugin.mActuators.back() << std::endl;
  }
}
Пример #5
0
void FamousoPlugin::simExtPublishProximityData(SLuaCallBack* p)
{
  const char* sensorName=simGetObjectName(p->inputInt[0]);
  if(sensorName)
  {
    const char* sensorTopic  = p->inputChar;
    simInt      sensorObject = p->inputInt[0];
    
    plugin.mSensors.emplace_back(new ProximitySensor(sensorObject, sensorTopic));

    Log::out() << "Registering " << *plugin.mSensors.back() << std::endl;
  }
}
Пример #6
0
void CBubbleRobDialog::refresh()
{ // This refreshed the dialog's items:

	int lastSel=simGetObjectLastSelection();

	m_bubbleRobList.ResetContent();
	for (int i=0;i<CAccess::bubbleRobContainer->getCount();i++)
	{
		CBubbleRob* bubbleRob=CAccess::bubbleRobContainer->getFromIndex(i);
		int sceneObjectHandle=bubbleRob->getAssociatedObject();
		std::string txt="Object: ";
		char* name=NULL;
		if (sceneObjectHandle!=-1)
			name=simGetObjectName(sceneObjectHandle);
		if (name==NULL)
			txt+="Unassociated";
		else
		{
			txt+="Associated with ";
			txt+=name;
			simReleaseBuffer(name);
		}
		int index=m_bubbleRobList.AddString(txt.c_str());
		m_bubbleRobList.SetItemData(index,(DWORD)bubbleRob->getID());
	}
	CBubbleRob* taggedObj=NULL;
	if (lastSel!=-1)
	{
		taggedObj=CAccess::bubbleRobContainer->getFromAssociatedObject(lastSel);
		if (taggedObj==NULL)
			selectObjectInList(-1);
		else
			selectObjectInList(taggedObj->getID());
	}
	else
		selectObjectInList(-1);
	taggedObj=CAccess::bubbleRobContainer->getFromID(getSelectedObjectInList());
	m_maxVelocity.EnableWindow(taggedObj!=NULL);
	if (taggedObj!=NULL)
	{
		CString tmp;
		tmp.Format("%0.2f",120.0f*taggedObj->getMaxVelocity()/3.1415f); // display it in RPM
		m_maxVelocity.SetWindowText(tmp);
	}
	else
		m_maxVelocity.SetWindowText("");
}
Пример #7
0
void GetTwistHandler::synchronize(){
    // We update GetTwistHandler's data from its associated scene object custom data:
    // 1. Get all the developer data attached to the associated scene object (this is custom data added by the developer):
    int buffSize = simGetObjectCustomDataLength(_associatedObjectID, CustomDataHeaders::DEVELOPER_DATA_HEADER);
    char* datBuff=new char[buffSize];
    simGetObjectCustomData(_associatedObjectID, CustomDataHeaders::DEVELOPER_DATA_HEADER,datBuff);
    std::vector<unsigned char> developerCustomData(datBuff,datBuff+buffSize);
    delete[] datBuff;
    // 2. From that retrieved data, try to extract sub-data with the OBJ_TWIST_DATA_MAIN tag:
    std::vector<unsigned char> tempMainData;

    if (CAccess::extractSerializationData(developerCustomData,CustomDataHeaders::OBJ_TWIST_DATA_MAIN,tempMainData)){ // Yes, the tag is there. For now we only have to synchronize _maxVelocity:

       	// Remove # chars for compatibility with ROS
		_associatedObjectName = simGetObjectName(_associatedObjectID);
		std::string objectName(_associatedObjectName);
		std::replace( objectName.begin(), objectName.end(), '#', '_');
		_pub = _nh.advertise<geometry_msgs::TwistStamped>(objectName+"/twist", 1);
		std::stringstream streamtemp;

    	int temp_freq = CAccess::pop_int(tempMainData);
    	if (temp_freq > 0){
    		_ObjTwistFrequency = temp_freq;
    		//std::cout << "Found Twist target in " << _associatedObjectName << ". Frequency publisher: " << _ObjTwistFrequency << std::endl;

    		streamtemp << "- [Twist Target] in '" << _associatedObjectName << "'. Frequency publisher: " << _ObjTwistFrequency << "." << std::endl;
    		ConsoleHandler::printInConsole(streamtemp);
      	} else {
      		//std::cout << "Found Twist target in " << _associatedObjectName << " at the simulation frequency. " << std::endl;

    		streamtemp << "- [Twist Target] in '" << _associatedObjectName << "'. Frequency publisher: Simulation frequency. " << std::endl;
    	    ConsoleHandler::printInConsole(streamtemp);
      	}
    }


}
Пример #8
0
void CMtbRobotDialog::updateObjectsInList()
{
	ui->qqRobotList->clear();
	for (int i=0;i<CAccess::mtbRobotContainer->getCount();i++)
	{
		CMtbRobot* mtbRobot=CAccess::mtbRobotContainer->getFromIndex(i);
		int sceneObjectHandle=mtbRobot->getAssociatedObject();
		std::string txt;
		char* name=NULL;
		if (sceneObjectHandle!=-1)
			name=simGetObjectName(sceneObjectHandle);
		if (name==NULL)
			txt="Error";
		else
		{
			txt=name;
			simReleaseBuffer(name);
		}
		QListWidgetItem* itm=new QListWidgetItem(txt.c_str());
		itm->setData(Qt::UserRole,QVariant(mtbRobot->getID()));
		itm->setFlags(Qt::ItemIsSelectable|Qt::ItemIsEnabled);
		ui->qqRobotList->addItem(itm);
	}
}
Пример #9
0
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);
	}
	
}
Пример #10
0
void CK3::_initialize()
{
	if (_initialized)
		return;
	checkScale();
	// We need to find the handle of K3's motors and sensors according to data tags attached to objects.
	// Since there might be several K3s in the scene, we should only explore this K3's tree hierarchy
	// to find those tags:
	for (int i=0;i<2;i++)
		_k3MotorHandles[i]=-1;
	for (int i=0;i<2;i++)
		_k3ColorSensorHandles[i]=-1;
	for (int i=0;i<9;i++)
		_k3IrSensorHandles[i]=-1;
	for (int i=0;i<5;i++)
		_k3UsSensorHandles[i]=-1;
	for (int i=0;i<6;i++)
		_k3GripperArmMotorHandles[i]=-1;
	for (int i=0;i<3;i++)
		_k3GripperFingerMotorHandles[i]=-1;
	for (int i=0;i<2;i++)
		_k3GripperDistanceSensorHandles[i]=-1;
	for (int i=0;i<2;i++)
		_k3GripperColorSensorHandles[i]=-1;

	std::vector<int> toExplore;
	toExplore.push_back(_associatedObjectID); // We start exploration with the base of the K3-tree
	while (toExplore.size()!=0)
	{
		int objHandle=toExplore[toExplore.size()-1];
		toExplore.pop_back();
		// 1. Add this object's children to the list to explore:
		int index=0;
		int childHandle=simGetObjectChild(objHandle,index++);
		while (childHandle!=-1)
		{
			toExplore.push_back(childHandle);
			childHandle=simGetObjectChild(objHandle,index++);
		}
		// 2. Now check if this object has one of the tags we are looking for:
		// a. Get all the developer data attached to this scene object (this is custom data added by the developer):
		int buffSize=simGetObjectCustomDataLength(objHandle,DEVELOPER_DATA_HEADER);
		if (buffSize!=0)
		{ // Yes there is some custom data written by us (the developer with the DEVELOPER_DATA_HEADER header)
			char* datBuff=new char[buffSize];
			simGetObjectCustomData(objHandle,DEVELOPER_DATA_HEADER,datBuff);
			std::vector<unsigned char> developerCustomData(datBuff,datBuff+buffSize);
			delete[] datBuff;
			// b. From that retrieved data, try to extract sub-data with the searched tags:
			std::vector<unsigned char> k3TagData;
			if (CAccess::extractSerializationData(developerCustomData,K3_LEFTMOTOR,k3TagData))
				_k3MotorHandles[0]=objHandle;
			if (CAccess::extractSerializationData(developerCustomData,K3_RIGHTMOTOR,k3TagData))
				_k3MotorHandles[1]=objHandle;
			if (CAccess::extractSerializationData(developerCustomData,K3_COLORSENSORLEFT,k3TagData))
				_k3ColorSensorHandles[0]=objHandle;
			if (CAccess::extractSerializationData(developerCustomData,K3_COLORSENSORRIGHT,k3TagData))
				_k3ColorSensorHandles[1]=objHandle;
			if (CAccess::extractSerializationData(developerCustomData,K3_IRSENSOR1,k3TagData))
				_k3IrSensorHandles[0]=objHandle;
			if (CAccess::extractSerializationData(developerCustomData,K3_IRSENSOR2,k3TagData))
				_k3IrSensorHandles[1]=objHandle;
			if (CAccess::extractSerializationData(developerCustomData,K3_IRSENSOR3,k3TagData))
				_k3IrSensorHandles[2]=objHandle;
			if (CAccess::extractSerializationData(developerCustomData,K3_IRSENSOR4,k3TagData))
				_k3IrSensorHandles[3]=objHandle;
			if (CAccess::extractSerializationData(developerCustomData,K3_IRSENSOR5,k3TagData))
				_k3IrSensorHandles[4]=objHandle;
			if (CAccess::extractSerializationData(developerCustomData,K3_IRSENSOR6,k3TagData))
				_k3IrSensorHandles[5]=objHandle;
			if (CAccess::extractSerializationData(developerCustomData,K3_IRSENSOR7,k3TagData))
				_k3IrSensorHandles[6]=objHandle;
			if (CAccess::extractSerializationData(developerCustomData,K3_IRSENSOR8,k3TagData))
				_k3IrSensorHandles[7]=objHandle;
			if (CAccess::extractSerializationData(developerCustomData,K3_IRSENSOR9,k3TagData))
				_k3IrSensorHandles[8]=objHandle;
			if (CAccess::extractSerializationData(developerCustomData,K3_USSENSOR1,k3TagData))
				_k3UsSensorHandles[0]=objHandle;
			if (CAccess::extractSerializationData(developerCustomData,K3_USSENSOR2,k3TagData))
				_k3UsSensorHandles[1]=objHandle;
			if (CAccess::extractSerializationData(developerCustomData,K3_USSENSOR3,k3TagData))
				_k3UsSensorHandles[2]=objHandle;
			if (CAccess::extractSerializationData(developerCustomData,K3_USSENSOR4,k3TagData))
				_k3UsSensorHandles[3]=objHandle;
			if (CAccess::extractSerializationData(developerCustomData,K3_USSENSOR5,k3TagData))
				_k3UsSensorHandles[4]=objHandle;
			if (CAccess::extractSerializationData(developerCustomData,K3_GRIPPER_ARMMOTOR1,k3TagData))
				_k3GripperArmMotorHandles[0]=objHandle;
			if (CAccess::extractSerializationData(developerCustomData,K3_GRIPPER_ARMMOTOR2,k3TagData))
				_k3GripperArmMotorHandles[1]=objHandle;
			if (CAccess::extractSerializationData(developerCustomData,K3_GRIPPER_ARMMOTOR3,k3TagData))
				_k3GripperArmMotorHandles[2]=objHandle;
			if (CAccess::extractSerializationData(developerCustomData,K3_GRIPPER_ARMMOTOR4,k3TagData))
				_k3GripperArmMotorHandles[3]=objHandle;
			if (CAccess::extractSerializationData(developerCustomData,K3_GRIPPER_ARMMOTOR5,k3TagData))
				_k3GripperArmMotorHandles[4]=objHandle;
			if (CAccess::extractSerializationData(developerCustomData,K3_GRIPPER_ARMMOTOR6,k3TagData))
				_k3GripperArmMotorHandles[5]=objHandle;
			if (CAccess::extractSerializationData(developerCustomData,K3_GRIPPER_GRIPPERMOTOR1,k3TagData))
				_k3GripperFingerMotorHandles[0]=objHandle;
			if (CAccess::extractSerializationData(developerCustomData,K3_GRIPPER_GRIPPERMOTOR2,k3TagData))
				_k3GripperFingerMotorHandles[1]=objHandle;
			if (CAccess::extractSerializationData(developerCustomData,K3_GRIPPER_GRIPPERMOTOR3,k3TagData))
				_k3GripperFingerMotorHandles[2]=objHandle;
			if (CAccess::extractSerializationData(developerCustomData,K3_GRIPPER_LEFTDISTSENSOR,k3TagData))
				_k3GripperDistanceSensorHandles[0]=objHandle;
			if (CAccess::extractSerializationData(developerCustomData,K3_GRIPPER_RIGHTDISTSENSOR,k3TagData))
				_k3GripperDistanceSensorHandles[1]=objHandle;
			if (CAccess::extractSerializationData(developerCustomData,K3_GRIPPER_LEFTCOLSENSOR,k3TagData))
				_k3GripperColorSensorHandles[0]=objHandle;
			if (CAccess::extractSerializationData(developerCustomData,K3_GRIPPER_RIGHTCOLSENSOR,k3TagData))
				_k3GripperColorSensorHandles[1]=objHandle;
		}
	}
	// Now get the display handle (Custom UI):
	char* baseName=simGetObjectName(_associatedObjectID);
	int suffixNumber=simGetNameSuffix(baseName);
	simReleaseBuffer(baseName);
	int globalSuffixSave=simGetNameSuffix(NULL);
	simSetNameSuffix(suffixNumber);
	_k3DisplayHandle=simGetUIHandle("K3_stateVisualization");
	simSetNameSuffix(globalSuffixSave); // reset to previous value
	
	_targetVelocities[0]=0.0f;
	_targetVelocities[1]=0.0f;
	_currentVelocities[0]=0.0f;
	_currentVelocities[1]=0.0f;
	_cumulativeMotorAngles[0]=0.0f;
	_cumulativeMotorAngles[1]=0.0f;
	_previousMotorAngles[0]=0.0f;
	_previousMotorAngles[1]=0.0f;
	_currentArmPosition=0.0f;
	_targetArmPosition=0.0f;
	_currentArmVelocity=0.0f;
	_targetGripperGap_unscaled=0.055f;

	_initialized=true;
}
Пример #11
0
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);
}