void Solver::resume()
{
mutex.lock();
// read encoders
getFeedback(fbTorso,fbHead,drvTorso,drvHead,commData);
updateTorsoBlockedJoints(chainNeck,fbTorso);
updateTorsoBlockedJoints(chainEyeL,fbTorso);
updateTorsoBlockedJoints(chainEyeR,fbTorso);
// update kinematics
updateAngles();
updateNeckBlockedJoints(chainEyeL,fbHead);
updateNeckBlockedJoints(chainEyeR,fbHead);
chainNeck->setAng(neckPos);
chainEyeL->setAng(nJointsTorso+3,gazePos[0]); chainEyeR->setAng(nJointsTorso+3,gazePos[0]);
chainEyeL->setAng(nJointsTorso+4,gazePos[1]+gazePos[2]/2.0); chainEyeR->setAng(nJointsTorso+4,gazePos[1]-gazePos[2]/2.0);
// update latched quantities
fbTorsoOld=fbTorso;
mutex.unlock();
commData->port_xd->unlock();
RateThread::resume();
yInfo("Solver has been resumed!");
}
void Controller::resume()
{
getFeedback(fbTorso,fbHead,drvTorso,drvHead,commData);
fbNeck=fbHead.subVector(0,2);
fbEyes=fbHead.subVector(3,5);
RateThread::resume();
yInfo("Controller has been resumed!");
notifyEvent("resumed");
}
PUBLIC bool feedback(cchar *kind, cchar *fmt, ...)
{
va_list args;
va_start(args, fmt);
espSetFeedbackv(getStream(), kind, fmt, args);
va_end(args);
/*
Return true if there is not an error feedback message
*/
return getFeedback("error") == 0;
}
void Controller::resume()
{
if (Robotable)
{
getFeedback(fbTorso,fbHead,drvTorso,drvHead,commData);
fbNeck=fbHead.subVector(0,2);
fbEyes=fbHead.subVector(3,5);
}
RateThread::resume();
printf("Controller has been resumed!\n");
notifyEvent("resumed");
}
void jointStateCallback(const sensor_msgs::JointState::ConstPtr &msg) {
size_t msgSize = msg->name.size(), jointInfoSize = _jointInfo.name.size(), msgSizeP = msg->position.size(),
msgSizeV = msg->velocity.size(), msgSizeE = msg->effort.size();
for (int i = 0; i < msgSize; i++) {
for (int j = 0; j < jointInfoSize; j++) {
if (msg->name[i] == _jointInfo.name[j]) {
if (i < msgSizeP) _jointInfo.position[j] = msg->position[i];
if (i < msgSizeV) _jointInfo.velocity[j] = msg->velocity[i];
if (i < msgSizeE) _jointInfo.effort[j] = msg->effort[i];
}
}
}
if(_publishFeedback) {
size_t size = _feedback.joint_names.size();
_feedback.actual.positions.clear();
_feedback.actual.velocities.clear();
_feedback.actual.effort.clear();
_feedback.error.positions.clear();
_feedback.error.velocities.clear();
_feedback.error.effort.clear();
for (int i = 0; i < size; ++i) {
feedback_t singleJointFeedback = getFeedback(_feedback.joint_names[i], _feedback.desired.positions[i],
0.0,
_feedback.desired.velocities[i]);
_feedback.actual.positions.push_back(singleJointFeedback.actual.position);
_feedback.actual.velocities.push_back(singleJointFeedback.actual.velocity);
_feedback.actual.effort.push_back(singleJointFeedback.actual.effort);
_feedback.error.positions.push_back(singleJointFeedback.error.position);
_feedback.error.velocities.push_back(singleJointFeedback.error.velocity);
_feedback.error.effort.push_back(singleJointFeedback.error.effort);
}
_actionServer.publishFeedback(_feedback);
}
}
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);
}
Controller::Controller(PolyDriver *_drvTorso, PolyDriver *_drvHead, exchangeData *_commData,
const bool _neckPosCtrlOn, const double _neckTime, const double _eyesTime,
const double _minAbsVel, const unsigned int _period) :
RateThread(_period), drvTorso(_drvTorso), drvHead(_drvHead),
commData(_commData), neckPosCtrlOn(_neckPosCtrlOn), neckTime(_neckTime),
eyesTime(_eyesTime), minAbsVel(_minAbsVel), period(_period),
Ts(_period/1000.0), printAccTime(0.0)
{
// Instantiate objects
neck=new iCubHeadCenter(commData->head_version>1.0?"right_v2":"right");
eyeL=new iCubEye(commData->head_version>1.0?"left_v2":"left");
eyeR=new iCubEye(commData->head_version>1.0?"right_v2":"right");
// release links
neck->releaseLink(0); eyeL->releaseLink(0); eyeR->releaseLink(0);
neck->releaseLink(1); eyeL->releaseLink(1); eyeR->releaseLink(1);
neck->releaseLink(2); eyeL->releaseLink(2); eyeR->releaseLink(2);
// Get the chain objects
chainNeck=neck->asChain();
chainEyeL=eyeL->asChain();
chainEyeR=eyeR->asChain();
// add aligning matrices read from configuration file
getAlignHN(commData->rf_cameras,"ALIGN_KIN_LEFT",eyeL->asChain());
getAlignHN(commData->rf_cameras,"ALIGN_KIN_RIGHT",eyeR->asChain());
// overwrite aligning matrices iff specified through tweak values
if (commData->tweakOverwrite)
{
getAlignHN(commData->rf_tweak,"ALIGN_KIN_LEFT",eyeL->asChain());
getAlignHN(commData->rf_tweak,"ALIGN_KIN_RIGHT",eyeR->asChain());
}
// read number of joints
if (drvTorso!=NULL)
{
IEncoders *encTorso; drvTorso->view(encTorso);
encTorso->getAxes(&nJointsTorso);
}
else
nJointsTorso=3;
IEncoders *encHead; drvHead->view(encHead);
encHead->getAxes(&nJointsHead);
drvHead->view(modHead);
drvHead->view(posHead);
drvHead->view(velHead);
// if requested check if position control is available
if (neckPosCtrlOn)
{
neckPosCtrlOn=drvHead->view(posNeck);
yInfo("### neck control - requested POSITION mode: IPositionDirect [%s] => %s mode selected",
neckPosCtrlOn?"available":"not available",neckPosCtrlOn?"POSITION":"VELOCITY");
}
else
yInfo("### neck control - requested VELOCITY mode => VELOCITY mode selected");
// joints bounds alignment
lim=alignJointsBounds(chainNeck,drvTorso,drvHead,commData->eyeTiltMin,commData->eyeTiltMax);
// read starting position
fbTorso.resize(nJointsTorso,0.0);
fbHead.resize(nJointsHead,0.0);
// exclude acceleration constraints by fixing
// thresholds at high values
Vector a_robHead(nJointsHead,1e9);
velHead->setRefAccelerations(a_robHead.data());
copyJointsBounds(chainNeck,chainEyeL);
copyJointsBounds(chainEyeL,chainEyeR);
// find minimum allowed vergence
startupMinVer=lim(nJointsHead-1,0);
findMinimumAllowedVergence();
// reinforce vergence min bound
lim(nJointsHead-1,0)=commData->get_minAllowedVergence();
getFeedback(fbTorso,fbHead,drvTorso,drvHead,commData);
fbNeck=fbHead.subVector(0,2);
fbEyes=fbHead.subVector(3,5);
qdNeck.resize(3,0.0); qdEyes.resize(3,0.0);
vNeck.resize(3,0.0); vEyes.resize(3,0.0);
// Set the task execution time
setTeyes(eyesTime);
setTneck(neckTime);
mjCtrlNeck=new minJerkVelCtrlForIdealPlant(Ts,fbNeck.length());
mjCtrlEyes=new minJerkVelCtrlForIdealPlant(Ts,fbEyes.length());
IntState=new Integrator(Ts,fbHead,lim);
IntPlan=new Integrator(Ts,fbNeck,lim.submatrix(0,2,0,1));
v.resize(nJointsHead,0.0);
neckJoints.resize(3);
eyesJoints.resize(3);
neckJoints[0]=0;
neckJoints[1]=1;
neckJoints[2]=2;
eyesJoints[0]=3;
eyesJoints[1]=4;
eyesJoints[2]=5;
qd=fbHead;
q0deg=CTRL_RAD2DEG*qd;
qddeg=q0deg;
vdeg =CTRL_RAD2DEG*v;
port_xd=NULL;
ctrlActiveRisingEdgeTime=0.0;
saccadeStartTime=0.0;
unplugCtrlEyes=false;
ctrlInhibited=false;
}
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);
}
void Solver::run()
{
typedef enum { ctrl_off, ctrl_wait, ctrl_on } cstate;
static cstate state_=ctrl_off;
mutex.lock();
// get the current target
Vector xd=commData->port_xd->get_xdDelayed();
// update the target straightaway
commData->set_xd(xd);
// read encoders
getFeedback(fbTorso,fbHead,drvTorso,drvHead,commData);
updateTorsoBlockedJoints(chainNeck,fbTorso);
updateTorsoBlockedJoints(chainEyeL,fbTorso);
updateTorsoBlockedJoints(chainEyeR,fbTorso);
bool torsoChanged=(norm(fbTorso-fbTorsoOld)>NECKSOLVER_ACTIVATIONANGLE_JOINTS*CTRL_DEG2RAD);
// update kinematics
updateAngles();
updateNeckBlockedJoints(chainEyeL,fbHead);
updateNeckBlockedJoints(chainEyeR,fbHead);
chainNeck->setAng(neckPos);
// hereafter accumulate solving conditions: the order does matter
// 1) compute the angular distance
double theta=neckTargetRotAngle(xd);
bool doSolve=(theta>NECKSOLVER_ACTIVATIONANGLE);
// 2) skip if controller is active and no torso motion is detected
doSolve&=!(commData->ctrlActive && !torsoChanged);
// 3) skip if controller is inactive and we are not in tracking mode
doSolve&=commData->ctrlActive || commData->trackingModeOn;
// 4) solve straightaway if we are in tracking mode and torso motion is detected
doSolve|=commData->trackingModeOn && torsoChanged;
// 5) solve straightaway if the target has changed
doSolve|=commData->port_xd->get_newDelayed();
// 6) skip if the angle to target is lower than the user tolerance
doSolve&=(theta>neckAngleUserTolerance);
// clear triggers
commData->port_xd->get_newDelayed()=false;
// call the solver for neck
if (doSolve)
{
Vector gDir(4,0.0);
gDir[2]=gDir[3]=1.0;
if (commData->stabilizationOn)
{
Vector acc=commData->get_imu().subVector(3,5);
acc.push_back(1.0); // impose homogeneous coordinates
Matrix H=imu->getH(cat(fbTorso,fbHead.subVector(0,2)));
gDir=H*acc;
}
Vector xdUserTol=computeTargetUserTolerance(xd);
neckPos=invNeck->solve(neckPos,xdUserTol,gDir);
// update neck pitch,roll,yaw
commData->set_qd(0,neckPos[0]);
commData->set_qd(1,neckPos[1]);
commData->set_qd(2,neckPos[2]);
commData->neckSolveCnt++;
state_=ctrl_wait;
}
if (state_==ctrl_off)
{
// keep neck targets equal to current angles
// to avoid glitches in the control (especially
// during stabilization)
commData->set_qd(0,neckPos[0]);
commData->set_qd(1,neckPos[1]);
commData->set_qd(2,neckPos[2]);
}
else if (state_==ctrl_wait)
{
if (commData->ctrlActive)
state_=ctrl_on;
}
else if (state_==ctrl_on)
{
if (!commData->ctrlActive)
state_=ctrl_off;
}
// latch quantities
fbTorsoOld=fbTorso;
mutex.unlock();
}
Solver::Solver(PolyDriver *_drvTorso, PolyDriver *_drvHead, ExchangeData *_commData,
EyePinvRefGen *_eyesRefGen, Localizer *_loc, Controller *_ctrl,
const unsigned int _period) :
RateThread(_period), drvTorso(_drvTorso), drvHead(_drvHead),
commData(_commData), eyesRefGen(_eyesRefGen), loc(_loc),
ctrl(_ctrl), period(_period), Ts(_period/1000.0)
{
// Instantiate objects
neck=new iCubHeadCenter(commData->head_version>1.0?"right_v2":"right");
eyeL=new iCubEye(commData->head_version>1.0?"left_v2":"left");
eyeR=new iCubEye(commData->head_version>1.0?"right_v2":"right");
imu=new iCubInertialSensor(commData->head_version>1.0?"v2":"v1");
// remove constraints on the links: logging purpose
imu->setAllConstraints(false);
// block neck dofs
eyeL->blockLink(3,0.0); eyeR->blockLink(3,0.0);
eyeL->blockLink(4,0.0); eyeR->blockLink(4,0.0);
eyeL->blockLink(5,0.0); eyeR->blockLink(5,0.0);
// Get the chain objects
chainNeck=neck->asChain();
chainEyeL=eyeL->asChain();
chainEyeR=eyeR->asChain();
// add aligning matrices read from configuration file
getAlignHN(commData->rf_cameras,"ALIGN_KIN_LEFT",eyeL->asChain());
getAlignHN(commData->rf_cameras,"ALIGN_KIN_RIGHT",eyeR->asChain());
// overwrite aligning matrices iff specified through tweak values
if (commData->tweakOverwrite)
{
getAlignHN(commData->rf_tweak,"ALIGN_KIN_LEFT",eyeL->asChain());
getAlignHN(commData->rf_tweak,"ALIGN_KIN_RIGHT",eyeR->asChain());
}
// read number of joints
if (drvTorso!=NULL)
{
IEncoders *encTorso; drvTorso->view(encTorso);
encTorso->getAxes(&nJointsTorso);
}
else
nJointsTorso=3;
IEncoders *encHead; drvHead->view(encHead);
encHead->getAxes(&nJointsHead);
// joints bounds alignment
alignJointsBounds(chainNeck,drvTorso,drvHead,commData->eyeTiltLim);
// read starting position
fbTorso.resize(nJointsTorso,0.0);
fbHead.resize(nJointsHead,0.0);
getFeedback(fbTorso,fbHead,drvTorso,drvHead,commData);
copyJointsBounds(chainNeck,chainEyeL);
copyJointsBounds(chainEyeL,chainEyeR);
// store neck pitch/roll/yaw bounds
neckPitchMin=(*chainNeck)[3].getMin();
neckPitchMax=(*chainNeck)[3].getMax();
neckRollMin =(*chainNeck)[4].getMin();
neckRollMax =(*chainNeck)[4].getMax();
neckYawMin =(*chainNeck)[5].getMin();
neckYawMax =(*chainNeck)[5].getMax();
neckPos.resize(3);
gazePos.resize(3);
updateAngles();
updateTorsoBlockedJoints(chainNeck,fbTorso);
chainNeck->setAng(3,fbHead[0]);
chainNeck->setAng(4,fbHead[1]);
chainNeck->setAng(5,fbHead[2]);
updateTorsoBlockedJoints(chainEyeL,fbTorso);
updateTorsoBlockedJoints(chainEyeR,fbTorso);
updateNeckBlockedJoints(chainEyeL,fbHead);
updateNeckBlockedJoints(chainEyeR,fbHead);
Vector eyePos(2);
eyePos[0]=gazePos[0];
eyePos[1]=gazePos[1]+gazePos[2]/2.0;
chainEyeL->setAng(eyePos);
eyePos[1]=gazePos[1]-gazePos[2]/2.0;
chainEyeR->setAng(eyePos);
fbTorsoOld=fbTorso;
neckAngleUserTolerance=0.0;
}
void EyePinvRefGen::run()
{
if (genOn)
{
LockGuard guard(mutex);
double timeStamp;
// read encoders
getFeedback(fbTorso,fbHead,drvTorso,drvHead,commData,&timeStamp);
updateTorsoBlockedJoints(chainNeck,fbTorso);
updateTorsoBlockedJoints(chainEyeL,fbTorso);
updateTorsoBlockedJoints(chainEyeR,fbTorso);
// get current target
Vector xd=commData->port_xd->get_xd();
// update neck chain
chainNeck->setAng(nJointsTorso+0,fbHead[0]);
chainNeck->setAng(nJointsTorso+1,fbHead[1]);
chainNeck->setAng(nJointsTorso+2,fbHead[2]);
// ask for saccades (if possible)
if (commData->saccadesOn && (saccadesRxTargets!=commData->port_xd->get_rx()) &&
!commData->saccadeUnderway && (Time::now()-saccadesClock>commData->saccadesInhibitionPeriod))
{
Vector fph=xd; fph.push_back(1.0);
fph=SE3inv(chainNeck->getH())*fph; fph[3]=0.0;
double rot=CTRL_RAD2DEG*acos(fph[2]/norm(fph)); fph[3]=1.0;
// estimate geometrically the target tilt and pan of the eyes
Vector ang(3,0.0);
ang[0]=-atan2(fph[1],fabs(fph[2]));
ang[1]=atan2(fph[0],fph[2]);
// enforce joints bounds
ang[0]=sat(ang[0],lim(0,0),lim(0,1));
ang[1]=sat(ang[1],lim(1,0),lim(1,1));
// favor the smooth-pursuit in case saccades are small
if (rot>commData->saccadesActivationAngle)
{
// init vergence
ang[2]=fbHead[5];
// get rid of eyes tilt
Vector axis(4);
axis[0]=1.0; axis[1]=0.0; axis[2]=0.0; axis[3]=-ang[0];
fph=axis2dcm(axis)*fph;
// go on iff the point is in front of us
if (fph[2]>0.0)
{
double L,R;
// estimate geometrically the target vergence Vg=L-R
if (fph[0]>=eyesHalfBaseline)
{
L=M_PI/2.0-atan2(fph[2],fph[0]+eyesHalfBaseline);
R=M_PI/2.0-atan2(fph[2],fph[0]-eyesHalfBaseline);
}
else if (fph[0]>-eyesHalfBaseline)
{
L=M_PI/2.0-atan2(fph[2],fph[0]+eyesHalfBaseline);
R=-(M_PI/2.0-atan2(fph[2],eyesHalfBaseline-fph[0]));
}
else
{
L=-(M_PI/2.0-atan2(fph[2],-fph[0]-eyesHalfBaseline));
R=-(M_PI/2.0-atan2(fph[2],eyesHalfBaseline-fph[0]));
}
ang[2]=L-R;
}
// enforce joints bounds
ang[2]=sat(ang[2],lim(2,0),lim(2,1));
commData->set_qd(3,ang[0]);
commData->set_qd(4,ang[1]);
commData->set_qd(5,ang[2]);
Vector vel(3,SACCADES_VEL);
ctrl->doSaccade(ang,vel);
saccadesClock=Time::now();
}
}
// update eyes chains for convergence purpose
updateNeckBlockedJoints(chainEyeL,fbHead); updateNeckBlockedJoints(chainEyeR,fbHead);
chainEyeL->setAng(nJointsTorso+3,qd[0]); chainEyeR->setAng(nJointsTorso+3,qd[0]);
chainEyeL->setAng(nJointsTorso+4,qd[1]+qd[2]/2.0); chainEyeR->setAng(nJointsTorso+4,qd[1]-qd[2]/2.0);
// converge on target
if (CartesianHelper::computeFixationPointData(*chainEyeL,*chainEyeR,fp,eyesJ))
{
Vector v=EYEPINVREFGEN_GAIN*(pinv(eyesJ)*(xd-fp));
// update eyes chains in actual configuration for velocity compensation
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);
// compensate neck rotation at eyes level
if ((commData->eyesBoundVer>=0.0) || !CartesianHelper::computeFixationPointData(*chainEyeL,*chainEyeR,fp,eyesJ))
commData->set_counterv(zeros(qd.length()));
else
commData->set_counterv(getEyesCounterVelocity(eyesJ,fp));
// reset eyes controller and integral upon saccades transition on=>off
if (saccadeUnderWayOld && !commData->saccadeUnderway)
{
ctrl->resetCtrlEyes();
qd[0]=fbHead[3];
qd[1]=fbHead[4];
qd[2]=fbHead[5];
I->reset(qd);
}
// update reference
qd=I->integrate(v+commData->get_counterv());
}
else
commData->set_counterv(zeros(qd.length()));
// set a new target position
commData->set_xd(xd);
commData->set_x(fp,timeStamp);
commData->set_fpFrame(chainNeck->getH());
if (!commData->saccadeUnderway)
{
commData->set_qd(3,qd[0]);
commData->set_qd(4,qd[1]);
commData->set_qd(5,qd[2]);
}
// latch the saccades status
saccadeUnderWayOld=commData->saccadeUnderway;
saccadesRxTargets=commData->port_xd->get_rx();
}
}
EyePinvRefGen::EyePinvRefGen(PolyDriver *_drvTorso, PolyDriver *_drvHead,
ExchangeData *_commData, Controller *_ctrl,
const Vector &_counterRotGain, const unsigned int _period) :
RateThread(_period), drvTorso(_drvTorso), drvHead(_drvHead),
commData(_commData), ctrl(_ctrl), period(_period),
Ts(_period/1000.0), counterRotGain(_counterRotGain)
{
// Instantiate objects
neck=new iCubHeadCenter(commData->head_version>1.0?"right_v2":"right");
eyeL=new iCubEye(commData->head_version>1.0?"left_v2":"left");
eyeR=new iCubEye(commData->head_version>1.0?"right_v2":"right");
imu=new iCubInertialSensor(commData->head_version>1.0?"v2":"v1");
// remove constraints on the links: logging purpose
imu->setAllConstraints(false);
// block neck dofs
eyeL->blockLink(3,0.0); eyeR->blockLink(3,0.0);
eyeL->blockLink(4,0.0); eyeR->blockLink(4,0.0);
eyeL->blockLink(5,0.0); eyeR->blockLink(5,0.0);
// Get the chain objects
chainNeck=neck->asChain();
chainEyeL=eyeL->asChain();
chainEyeR=eyeR->asChain();
// add aligning matrices read from configuration file
getAlignHN(commData->rf_cameras,"ALIGN_KIN_LEFT",eyeL->asChain());
getAlignHN(commData->rf_cameras,"ALIGN_KIN_RIGHT",eyeR->asChain());
// overwrite aligning matrices iff specified through tweak values
if (commData->tweakOverwrite)
{
getAlignHN(commData->rf_tweak,"ALIGN_KIN_LEFT",eyeL->asChain());
getAlignHN(commData->rf_tweak,"ALIGN_KIN_RIGHT",eyeR->asChain());
}
if (commData->tweakOverwrite)
{
getAlignHN(commData->rf_tweak,"ALIGN_KIN_LEFT",eyeL->asChain());
getAlignHN(commData->rf_tweak,"ALIGN_KIN_RIGHT",eyeR->asChain());
}
// get the length of the half of the eyes baseline
eyesHalfBaseline=0.5*norm(eyeL->EndEffPose().subVector(0,2)-eyeR->EndEffPose().subVector(0,2));
// read number of joints
if (drvTorso!=NULL)
{
IEncoders *encTorso; drvTorso->view(encTorso);
encTorso->getAxes(&nJointsTorso);
}
else
nJointsTorso=3;
IEncoders *encHead; drvHead->view(encHead);
encHead->getAxes(&nJointsHead);
// joints bounds alignment
lim=alignJointsBounds(chainNeck,drvTorso,drvHead,commData->eyeTiltLim);
copyJointsBounds(chainNeck,chainEyeL);
copyJointsBounds(chainEyeL,chainEyeR);
// just eye part is required
lim=lim.submatrix(3,5,0,1);
// reinforce vergence min bound
lim(2,0)=commData->minAllowedVergence;
// read starting position
fbTorso.resize(nJointsTorso,0.0);
fbHead.resize(nJointsHead,0.0);
getFeedback(fbTorso,fbHead,drvTorso,drvHead,commData);
// save original eyes tilt and pan bounds
orig_eye_tilt_min=(*chainEyeL)[nJointsTorso+3].getMin();
orig_eye_tilt_max=(*chainEyeL)[nJointsTorso+3].getMax();
orig_eye_pan_min=(*chainEyeL)[nJointsTorso+4].getMin();
orig_eye_pan_max=(*chainEyeL)[nJointsTorso+4].getMax();
orig_lim=lim;
// Instantiate integrator
qd.resize(3);
qd[0]=fbHead[3];
qd[1]=fbHead[4];
qd[2]=fbHead[5];
I=new Integrator(Ts,qd,lim);
fp.resize(3,0.0);
eyesJ.resize(3,3);
eyesJ.zero();
genOn=false;
saccadeUnderWayOld=false;
}
int main()
{
// not recommended to do something with sizes :(
COORD size = {9, 8};
drawTable(size);
// task - aim, request - user input
i4 task(generateTask());
i4 request;
// index to watch which digit user is entered
// line - offset for the log output
int index = 0, line = 4;
bool bQuit = false;
while (!bQuit)
{
char ch[] = {readKey(), '\0'};
int code = (int)(ch[0]);
// if input is digit
if (code > 47 && code < 58 && index < 4)
{
int num = code - 48;
bool bDublicate = false;
for (int i = 0; i < index; ++i)
{
if (request[i] == num)
{
bDublicate = true;
break;
}
}
if (bDublicate)
continue;
request[index] = num;
++index;
print({2 + index, 1}, ch);
}
// if input is erase
if (code == KEY_BACK && index > 0)
print({2 + index--, 1}, "_");
// if input is confirm
if (code == KEY_ENTER && index == 4)
{
print({3, 1}, "____");
i2 fb = getFeedback(request, task);
// hard way to do simple thing
char c[] = {
request[0] + 48,
request[1] + 48,
request[2] + 48,
request[3] + 48,
' ',
fb[0] + 48,
':',
fb[1] + 48,
'\0'
};
print({1, line}, c);
++line;
index = 0;
// make the table longer if needed
if (line > size.Y-1)
{
print({0, size.Y}, "|");
print({size.X, size.Y}, "|");
++size.Y;
}
// win event
if (fb[0] == 4)
{
for (int i = 0; i < size.X-1; ++i)
print({1 + i, line}, "!");
readKey();
break;
}
}
// if input is exit
if (code == KEY_ESC)
return 0;
//printf("%d", code);
}
clearScreen();
system("pause");
return 0;
}
