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; }
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; }
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; }