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