void CSubscriberData::setJointStateCallback(const vrep_common::JointSetStateData::ConstPtr& data)
{
if ( (data->handles.data.size()>0)&&(data->handles.data.size()==data->setModes.data.size())&&(data->handles.data.size()==data->values.data.size()) )
{
for (unsigned int i=0;i<data->handles.data.size();i++)
{
int handle=data->handles.data[i];
unsigned char setMode=data->setModes.data[i];
float val=data->values.data[i];
if (setMode==0)
simSetJointPosition(handle,val);
if (setMode==1)
simSetJointTargetPosition(handle,val);
if (setMode==2)
simSetJointTargetVelocity(handle,val);
if (setMode==3)
simSetJointForce(handle,val);
}
}
}
void CSubscriberData::setJointTargetVelocityCallback(const std_msgs::Float64::ConstPtr& vel)
{
if (simSetJointTargetVelocity(auxInt1,(float)vel->data)==-1)
shutDownGeneralSubscriber();
}
void CK3::handleSimulation()
{ // called when the main script calls: simHandleModule
_initialize();
float dt=simGetSimulationTimeStep();
for (int i=0;i<2;i++)
{
if (_targetVelocities[i]>_currentVelocities[i])
{
_currentVelocities[i]=_currentVelocities[i]+_maxAcceleration*dt;
if (_currentVelocities[i]>_targetVelocities[i])
_currentVelocities[i]=_targetVelocities[i];
}
else
{
_currentVelocities[i]=_currentVelocities[i]-_maxAcceleration*dt;
if (_currentVelocities[i]<_targetVelocities[i])
_currentVelocities[i]=_targetVelocities[i];
}
simSetJointTargetVelocity(_k3MotorHandles[i],_currentVelocities[i]);
float jp;
simGetJointPosition(_k3MotorHandles[i],&jp);
float dp=jp-_previousMotorAngles[i];
if (fabs(dp)>piValue)
dp-=(2.0f*piValue*fabs(dp)/dp);
// _cumulativeMotorAngles[i]+=dp/(2.0f*piValue);
_cumulativeMotorAngles[i]+=dp; // corrected on 5/3/2012 thanks to Felix Fischer
_previousMotorAngles[i]=jp;
}
float adp=_targetArmPosition-_currentArmPosition;
if (adp!=0.0f)
{
if (adp*_currentArmVelocity>=0.0f)
{
float decelToZeroTime=(fabs(_currentArmVelocity)+_maxArmAcceleration*dt*1.0f)/_maxArmAcceleration;
float distToZero=0.5f*_maxArmAcceleration*decelToZeroTime*decelToZeroTime;
float dir=1.0f;
if (_currentArmVelocity!=0.0f)
dir=_currentArmVelocity/fabs(_currentArmVelocity);
else
dir=adp/fabs(adp);
if (fabs(adp)>distToZero)
_currentArmVelocity+=dir*_maxArmAcceleration*dt;
else
_currentArmVelocity-=dir*_maxArmAcceleration*dt;
}
else
_currentArmVelocity*=(1-_maxArmAcceleration*dt/fabs(_currentArmVelocity));
}
else
{
if (_currentArmVelocity!=0.0f)
{
float dv=-_currentArmVelocity*_maxArmAcceleration*dt/fabs(_currentArmVelocity);
if ((_currentArmVelocity+dv)*_currentArmVelocity<0.0f)
_currentArmVelocity=0.0f;
else
_currentArmVelocity+=dv;
}
}
_currentArmPosition+=_currentArmVelocity*dt;
simSetJointTargetPosition(_k3GripperArmMotorHandles[0],_currentArmPosition);
simSetJointTargetPosition(_k3GripperArmMotorHandles[1],-_currentArmPosition);
simSetJointTargetPosition(_k3GripperArmMotorHandles[2],_currentArmPosition);
simSetJointTargetPosition(_k3GripperArmMotorHandles[3],_currentArmPosition);
simSetJointTargetPosition(_k3GripperArmMotorHandles[4],_currentArmPosition);
simSetJointTargetPosition(_k3GripperArmMotorHandles[5],_currentArmPosition);
float jp;
simGetJointPosition(_k3GripperFingerMotorHandles[0],&jp);
_currentGripperGap_unscaled=(jp/_scaling)+0.04f; // Don't forget scaling!!
float dp=_targetGripperGap_unscaled-_currentGripperGap_unscaled;
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(_k3GripperFingerMotorHandles[0],velToRegulate*_scaling); // Don't forget scaling!!
simSetJointTargetPosition(_k3GripperFingerMotorHandles[1],-jp*0.5f/_scaling); // Don't forget scaling!!
simSetJointTargetPosition(_k3GripperFingerMotorHandles[2],-jp*0.5f/_scaling); // Don't forget scaling!!
_updateStateVisualization();
}
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);
}