void Controller::run()
{
LockGuard guard(mutexRun);
mutexCtrl.lock();
bool jointsHealthy=areJointsHealthyAndSet();
mutexCtrl.unlock();
if (!jointsHealthy)
{
stopControlHelper();
port_xd->get_new()=false;
}
string event="none";
// verify if any saccade is still underway
mutexCtrl.lock();
if (commData->get_isSaccadeUnderway() && (Time::now()-saccadeStartTime>=Ts))
{
bool done;
posHead->checkMotionDone(eyesJoints.size(),eyesJoints.getFirst(),&done);
commData->get_isSaccadeUnderway()=!done;
if (!commData->get_isSaccadeUnderway())
notifyEvent("saccade-done");
}
mutexCtrl.unlock();
// get data
double x_stamp;
Vector xd=commData->get_xd();
Vector x=commData->get_x(x_stamp);
Vector new_qd=commData->get_qd();
// read feedbacks
q_stamp=Time::now();
if (!getFeedback(fbTorso,fbHead,drvTorso,drvHead,commData,&q_stamp))
{
yWarning("Communication timeout detected!");
notifyEvent("comm-timeout");
suspend();
return;
}
IntState->reset(fbHead);
fbNeck=fbHead.subVector(0,2);
fbEyes=fbHead.subVector(3,5);
double errNeck=norm(new_qd.subVector(0,2)-fbNeck);
double errEyes=norm(new_qd.subVector(3,new_qd.length()-1)-fbEyes);
bool swOffCond=(Time::now()-ctrlActiveRisingEdgeTime<GAZECTRL_SWOFFCOND_DISABLETIME) ? false :
(!commData->get_isSaccadeUnderway() &&
(errNeck<GAZECTRL_MOTIONDONE_NECK_QTHRES*CTRL_DEG2RAD) &&
(errEyes<GAZECTRL_MOTIONDONE_EYES_QTHRES*CTRL_DEG2RAD));
// verify control switching conditions
if (commData->get_isCtrlActive())
{
// switch-off condition
if (swOffCond)
{
event="motion-done";
mutexCtrl.lock();
stopLimb(false);
mutexCtrl.unlock();
}
// manage new target while controller is active
else if (port_xd->get_new())
{
event="motion-onset";
mutexData.lock();
motionOngoingEventsCurrent=motionOngoingEvents;
mutexData.unlock();
}
port_xd->get_new()=false;
}
else if (jointsHealthy)
{
// inhibition is cleared upon new target arrival
if (ctrlInhibited)
ctrlInhibited=!port_xd->get_new();
// switch-on condition
commData->get_isCtrlActive()=port_xd->get_new() ||
(!ctrlInhibited &&
(new_qd[0]!=qd[0]) || (new_qd[1]!=qd[1]) || (new_qd[2]!=qd[2]) ||
(!commData->get_canCtrlBeDisabled() && (norm(port_xd->get_xd()-x)>GAZECTRL_MOTIONSTART_XTHRES)));
// reset controllers
if (commData->get_isCtrlActive())
{
ctrlActiveRisingEdgeTime=Time::now();
port_xd->get_new()=false;
Vector zeros(3,0.0);
mjCtrlNeck->reset(zeros);
mjCtrlEyes->reset(zeros);
IntPlan->reset(fbNeck);
event="motion-onset";
mutexData.lock();
motionOngoingEventsCurrent=motionOngoingEvents;
mutexData.unlock();
}
}
mutexCtrl.lock();
if (event=="motion-onset")
{
setJointsCtrlMode();
q0deg=CTRL_RAD2DEG*fbHead;
}
mutexCtrl.unlock();
qd=new_qd;
qdNeck=qd.subVector(0,2);
qdEyes=qd.subVector(3,5);
if (commData->get_isCtrlActive())
{
// control loop
vNeck=mjCtrlNeck->computeCmd(neckTime,qdNeck-fbNeck);
IntPlan->integrate(vNeck);
if (unplugCtrlEyes)
{
if (Time::now()-saccadeStartTime>=Ts)
vEyes=commData->get_counterv();
}
else
vEyes=mjCtrlEyes->computeCmd(eyesTime,qdEyes-fbEyes)+commData->get_counterv();
}
else
{
vNeck=0.0;
vEyes=0.0;
}
v.setSubvector(0,vNeck);
v.setSubvector(3,vEyes);
// apply bang-bang just in case to compensate
// for unachievable low velocities
for (size_t i=0; i<v.length(); i++)
if ((v[i]!=0.0) && (v[i]>-minAbsVel) && (v[i]<minAbsVel))
v[i]=yarp::math::sign(qd[i]-fbHead[i])*minAbsVel;
// convert to degrees
mutexData.lock();
qddeg=CTRL_RAD2DEG*qd;
qdeg =CTRL_RAD2DEG*fbHead;
vdeg =CTRL_RAD2DEG*v;
mutexData.unlock();
// send commands to the robot
if (commData->get_isCtrlActive())
{
mutexCtrl.lock();
Vector posdeg;
if (neckPosCtrlOn)
{
posdeg=(CTRL_RAD2DEG)*IntPlan->get();
posNeck->setPositions(neckJoints.size(),neckJoints.getFirst(),posdeg.data());
velHead->velocityMove(eyesJoints.size(),eyesJoints.getFirst(),vdeg.subVector(3,5).data());
}
else
velHead->velocityMove(vdeg.data());
if (commData->debugInfoEnabled && (port_debug.getOutputCount()>0))
{
Bottle info;
int j=0;
if (neckPosCtrlOn)
{
for (size_t i=0; i<posdeg.length(); i++)
{
ostringstream ss;
ss<<"pos_"<<i;
info.addString(ss.str().c_str());
info.addDouble(posdeg[i]);
}
j=eyesJoints[0];
}
for (size_t i=j; i<vdeg.length(); i++)
{
ostringstream ss;
ss<<"vel_"<<i;
info.addString(ss.str().c_str());
info.addDouble(vdeg[i]);
}
port_debug.prepare()=info;
txInfo_debug.update(q_stamp);
port_debug.setEnvelope(txInfo_debug);
port_debug.writeStrict();
}
mutexCtrl.unlock();
}
// print info
if (commData->verbose)
printIter(xd,x,qddeg,qdeg,vdeg,1.0);
// send x,q through YARP ports
Vector q(nJointsTorso+nJointsHead);
int j;
for (j=0; j<nJointsTorso; j++)
q[j]=CTRL_RAD2DEG*fbTorso[j];
for (; (size_t)j<q.length(); j++)
q[j]=qdeg[j-nJointsTorso];
txInfo_x.update(x_stamp);
if (port_x.getOutputCount()>0)
{
port_x.prepare()=x;
port_x.setEnvelope(txInfo_x);
port_x.write();
}
txInfo_q.update(q_stamp);
if (port_q.getOutputCount()>0)
{
port_q.prepare()=q;
port_q.setEnvelope(txInfo_q);
port_q.write();
}
// update pose information
mutexChain.lock();
for (int i=0; i<nJointsTorso; i++)
{
chainNeck->setAng(i,fbTorso[i]);
chainEyeL->setAng(i,fbTorso[i]);
chainEyeR->setAng(i,fbTorso[i]);
}
for (int i=0; i<3; i++)
{
chainNeck->setAng(nJointsTorso+i,fbHead[i]);
chainEyeL->setAng(nJointsTorso+i,fbHead[i]);
chainEyeR->setAng(nJointsTorso+i,fbHead[i]);
}
chainEyeL->setAng(nJointsTorso+3,fbHead[3]); chainEyeR->setAng(nJointsTorso+3,fbHead[3]);
chainEyeL->setAng(nJointsTorso+4,fbHead[4]+fbHead[5]/2.0); chainEyeR->setAng(nJointsTorso+4,fbHead[4]-fbHead[5]/2.0);
txInfo_pose.update(q_stamp);
mutexChain.unlock();
if (event=="motion-onset")
notifyEvent(event);
motionOngoingEventsHandling();
if (event=="motion-done")
{
motionOngoingEventsFlush();
notifyEvent(event);
}
// update joints angles
fbHead=IntState->integrate(v);
commData->set_q(fbHead);
commData->set_torso(fbTorso);
commData->set_v(v);
}
void Controller::run()
{
LockGuard guard(mutexRun);
mutexCtrl.lock();
bool jointsHealthy=areJointsHealthyAndSet();
mutexCtrl.unlock();
if (!jointsHealthy)
{
stopControlHelper();
commData->port_xd->get_new()=false;
}
string event="none";
// verify if any saccade is still underway
mutexCtrl.lock();
if (commData->saccadeUnderway && (Time::now()-saccadeStartTime>=Ts))
{
bool done;
posHead->checkMotionDone(eyesJoints.size(),eyesJoints.getFirst(),&done);
commData->saccadeUnderway=!done;
if (!commData->saccadeUnderway)
notifyEvent("saccade-done");
}
mutexCtrl.unlock();
// get data
double x_stamp;
Vector xd=commData->get_xd();
Vector x=commData->get_x(x_stamp);
qd=commData->get_qd();
// read feedbacks
q_stamp=Time::now();
if (!getFeedback(fbTorso,fbHead,drvTorso,drvHead,commData,&q_stamp))
{
yError("Communication timeout detected!");
notifyEvent("comm-timeout");
suspend();
return;
}
// update pose information
{
mutexChain.lock();
for (int i=0; i<nJointsTorso; i++)
{
chainNeck->setAng(i,fbTorso[i]);
chainEyeL->setAng(i,fbTorso[i]);
chainEyeR->setAng(i,fbTorso[i]);
}
for (int i=0; i<3; i++)
{
chainNeck->setAng(nJointsTorso+i,fbHead[i]);
chainEyeL->setAng(nJointsTorso+i,fbHead[i]);
chainEyeR->setAng(nJointsTorso+i,fbHead[i]);
}
chainEyeL->setAng(nJointsTorso+3,fbHead[3]);
chainEyeL->setAng(nJointsTorso+4,fbHead[4]+fbHead[5]/2.0);
chainEyeR->setAng(nJointsTorso+3,fbHead[3]);
chainEyeR->setAng(nJointsTorso+4,fbHead[4]-fbHead[5]/2.0);
txInfo_pose.update(q_stamp);
mutexChain.unlock();
}
IntState->reset(fbHead);
fbNeck=fbHead.subVector(0,2);
fbEyes=fbHead.subVector(3,5);
double errNeck=norm(qd.subVector(0,2)-fbNeck);
double errEyes=norm(qd.subVector(3,qd.length()-1)-fbEyes);
bool swOffCond=(Time::now()-ctrlActiveRisingEdgeTime<GAZECTRL_SWOFFCOND_DISABLESLOT*(0.001*getRate())) ? false :
(!commData->saccadeUnderway &&
(errNeck<GAZECTRL_MOTIONDONE_NECK_QTHRES*CTRL_DEG2RAD) &&
(errEyes<GAZECTRL_MOTIONDONE_EYES_QTHRES*CTRL_DEG2RAD));
// verify control switching conditions
if (commData->ctrlActive)
{
// manage new target while controller is active
if (commData->port_xd->get_new())
{
reliableGyro=true;
event="motion-onset";
mutexData.lock();
motionOngoingEventsCurrent=motionOngoingEvents;
mutexData.unlock();
ctrlActiveRisingEdgeTime=Time::now();
commData->port_xd->get_new()=false;
}
// switch-off condition
else if (swOffCond)
{
if (commData->trackingModeOn)
{
if (!motionDone)
event="motion-done";
motionDone=true;
}
else
{
event="motion-done";
mutexCtrl.lock();
stopLimb(false);
mutexCtrl.unlock();
}
}
}
else if (jointsHealthy)
{
// inhibition is cleared upon new target arrival
if (ctrlInhibited)
ctrlInhibited=!commData->port_xd->get_new();
// switch-on condition
commData->ctrlActive=commData->port_xd->get_new() || (!ctrlInhibited && (commData->neckSolveCnt>0));
// reset controllers
if (commData->ctrlActive)
{
ctrlActiveRisingEdgeTime=Time::now();
commData->port_xd->get_new()=false;
commData->neckSolveCnt=0;
if (stabilizeGaze)
{
mjCtrlNeck->reset(vNeck);
mjCtrlEyes->reset(vEyes);
}
else
{
mjCtrlNeck->reset(zeros(fbNeck.length()));
mjCtrlEyes->reset(zeros(fbEyes.length()));
IntStabilizer->reset(zeros(vNeck.length()));
IntPlan->reset(fbNeck);
}
reliableGyro=true;
event="motion-onset";
mutexData.lock();
motionOngoingEventsCurrent=motionOngoingEvents;
mutexData.unlock();
}
}
mutexCtrl.lock();
if (event=="motion-onset")
{
setJointsCtrlMode();
jointsToSet.clear();
motionDone=false;
q0=fbHead;
}
mutexCtrl.unlock();
if (commData->trackingModeOn || stabilizeGaze)
{
mutexCtrl.lock();
setJointsCtrlMode();
mutexCtrl.unlock();
}
qdNeck=qd.subVector(0,2);
qdEyes=qd.subVector(3,5);
// compute current point [%] in the path
double dist=norm(qd-q0);
pathPerc=(dist>IKIN_ALMOST_ZERO)?norm(fbHead-q0)/dist:1.0;
pathPerc=sat(pathPerc,0.0,1.0);
if (commData->ctrlActive)
{
// control
vNeck=mjCtrlNeck->computeCmd(neckTime,qdNeck-fbNeck);
if (unplugCtrlEyes)
{
if (Time::now()-saccadeStartTime>=Ts)
vEyes=commData->get_counterv();
}
else
vEyes=mjCtrlEyes->computeCmd(eyesTime,qdEyes-fbEyes)+commData->get_counterv();
// stabilization
if (commData->stabilizationOn)
{
Vector gyro=CTRL_DEG2RAD*commData->get_imu().subVector(6,8);
Vector dx=computedxFP(imu->getH(cat(fbTorso,fbNeck)),zeros(fbNeck.length()),gyro,x);
Vector imuNeck=computeNeckVelFromdxFP(x,dx);
if (reliableGyro)
{
vNeck=commData->stabilizationGain*IntStabilizer->integrate(vNeck-imuNeck);
// only if the speed is low and we are close to the target
if ((norm(vNeck)<commData->gyro_noise_threshold) && (pathPerc>0.9))
reliableGyro=false;
}
// hysteresis
else if ((norm(imuNeck)>1.5*commData->gyro_noise_threshold) || (pathPerc<0.9))
{
IntStabilizer->reset(zeros(vNeck.length()));
reliableGyro=true;
}
}
IntPlan->integrate(vNeck);
}
else if (stabilizeGaze)
{
Vector gyro=CTRL_DEG2RAD*commData->get_imu().subVector(6,8);
Vector dx=computedxFP(imu->getH(cat(fbTorso,fbNeck)),zeros(fbNeck.length()),gyro,x);
Vector imuNeck=computeNeckVelFromdxFP(x,dx);
vNeck=commData->stabilizationGain*IntStabilizer->integrate(-1.0*imuNeck);
vEyes=computeEyesVelFromdxFP(dx);
IntPlan->integrate(vNeck);
}
else
{
vNeck=0.0;
vEyes=0.0;
}
v.setSubvector(0,vNeck);
v.setSubvector(3,vEyes);
// apply bang-bang just in case to compensate
// for unachievable low velocities
for (size_t i=0; i<v.length(); i++)
if ((v[i]!=0.0) && (v[i]>-min_abs_vel) && (v[i]<min_abs_vel))
v[i]=yarp::math::sign(qd[i]-fbHead[i])*min_abs_vel;
// convert to degrees
mutexData.lock();
qddeg=CTRL_RAD2DEG*qd;
qdeg =CTRL_RAD2DEG*fbHead;
vdeg =CTRL_RAD2DEG*v;
mutexData.unlock();
// send commands to the robot
if (commData->ctrlActive || stabilizeGaze)
{
mutexCtrl.lock();
Vector posdeg;
if (commData->neckPosCtrlOn)
{
posdeg=(CTRL_RAD2DEG)*IntPlan->get();
posNeck->setPositions(neckJoints.size(),neckJoints.getFirst(),posdeg.data());
velHead->velocityMove(eyesJoints.size(),eyesJoints.getFirst(),vdeg.subVector(3,5).data());
}
else
velHead->velocityMove(vdeg.data());
if (commData->debugInfoEnabled && (port_debug.getOutputCount()>0))
{
Bottle info;
int j=0;
if (commData->neckPosCtrlOn)
{
for (size_t i=0; i<posdeg.length(); i++)
{
ostringstream ss;
ss<<"pos_"<<i;
info.addString(ss.str().c_str());
info.addDouble(posdeg[i]);
}
j=eyesJoints[0];
}
for (size_t i=j; i<vdeg.length(); i++)
{
ostringstream ss;
ss<<"vel_"<<i;
info.addString(ss.str().c_str());
info.addDouble(vdeg[i]);
}
port_debug.prepare()=info;
txInfo_debug.update(q_stamp);
port_debug.setEnvelope(txInfo_debug);
port_debug.writeStrict();
}
mutexCtrl.unlock();
}
// print info
if (commData->verbose)
printIter(xd,x,qddeg,qdeg,vdeg,1.0);
// send x,q through YARP ports
Vector q(nJointsTorso+nJointsHead);
int j;
for (j=0; j<nJointsTorso; j++)
q[j]=CTRL_RAD2DEG*fbTorso[j];
for (; (size_t)j<q.length(); j++)
q[j]=qdeg[j-nJointsTorso];
txInfo_x.update(x_stamp);
if (port_x.getOutputCount()>0)
{
port_x.prepare()=x;
port_x.setEnvelope(txInfo_x);
port_x.write();
}
txInfo_q.update(q_stamp);
if (port_q.getOutputCount()>0)
{
port_q.prepare()=q;
port_q.setEnvelope(txInfo_q);
port_q.write();
}
if (event=="motion-onset")
notifyEvent(event);
motionOngoingEventsHandling();
if (event=="motion-done")
{
motionOngoingEventsFlush();
notifyEvent(event);
}
// update joints angles
fbHead=IntState->integrate(v);
commData->set_q(fbHead);
commData->set_torso(fbTorso);
commData->set_v(v);
}