Esempio n. 1
0
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!");
}
Esempio n. 2
0
EulerWidget::EulerWidget(QWidget *parent) :
   QWidget(parent), _ui(new Ui::EulerWidget)
{
	qRegisterMetaType<Eigen::Quaterniond>("Eigen::Quaterniond");

	_ui->setupUi(this);
	_ui->a1->setCurrentIndex(0);
	_ui->a2->setCurrentIndex(1); disableAxis(_ui->a2, 0);
	_ui->a3->setCurrentIndex(2); disableAxis(_ui->a3, 1);

	_q = Eigen::Quaterniond::Identity();
	updateAngles();

	// react to axis changes
	connect(_ui->a1, SIGNAL(currentIndexChanged(int)),
	        this, SLOT(axisChanged(int)));
	connect(_ui->a2, SIGNAL(currentIndexChanged(int)),
	        this, SLOT(axisChanged(int)));
	connect(_ui->a3, SIGNAL(currentIndexChanged(int)),
	        this, SLOT(axisChanged(int)));

	// react to angle changes
	connect(_ui->e1, SIGNAL(valueChanged(double)),
	        this, SLOT(angleChanged(double)));
	connect(_ui->e2, SIGNAL(valueChanged(double)),
	        this, SLOT(angleChanged(double)));
	connect(_ui->e3, SIGNAL(valueChanged(double)),
	        this, SLOT(angleChanged(double)));
}
Esempio n. 3
0
void WalkBalance::walkBalanceCheck()
{
	updateAngles();
	if(angleX > angleXmax || angleX <angleXmin ||angleY >angleYmax ||angleY<angleYmin)
	{

	}

}
Esempio n. 4
0
    void Camera::rotate(Radians angle)
    {
        Quat rot = glm::angleAxis(angle.value(), mUpReal);

        mFront = Vec3(rot * mFront).normalized();
        mRight = Vec3(rot * mRight).normalized();

        mMatrixUpdateFlags |= MatrixRotationUpdated;
        updateAngles();
    }
Esempio n. 5
0
void EulerWidget::setEulerAxes(uint a1, uint a2, uint a3)
{
	if (a1 > 2 || a2 > 2 || a3 > 2) return;
	if (a1 == _ui->a1->currentIndex() &&
	    a2 == _ui->a2->currentIndex() &&
	    a3 == _ui->a3->currentIndex()) return;

	this->blockSignals(true);
	_ui->a3->setCurrentIndex(a3);
	_ui->a2->setCurrentIndex(a2);
	_ui->a1->setCurrentIndex(a1);
	this->blockSignals(false);
	updateAngles();

	emit axesChanged(a1, a2, a3);
}
Esempio n. 6
0
void EulerWidget::axisChanged(int axis) {
	bool bFirstCall = !this->signalsBlocked();
	this->blockSignals(true);

	// ensure different axes for consecutive operations
	QComboBox* origin = dynamic_cast<QComboBox*>(sender());
	if (origin == _ui->a1) disableAxis(_ui->a2, axis);
	if (origin == _ui->a2) disableAxis(_ui->a3, axis);

	if (bFirstCall) {
		updateAngles();
		this->blockSignals(false);

		emit axesChanged(_ui->a1->currentIndex(),
		                 _ui->a2->currentIndex(),
		                 _ui->a3->currentIndex());
	}
}
Esempio n. 7
0
    void Camera::lookAt(Vec3 pos, Vec3 at, Vec3 up)
    {
        mEye = pos;
        mAt = at;
        mUp = up.normalized();

        mFront = (mAt - mEye).normalized();

        Vec3 oldRight = mRight;
        mRight = mFront.cross(mUp);

        if (mRight.isZero()) {
            oldRight.y = 0;
            mRight = oldRight;
            gLog.trace("right was zero, reverted to %s\n", utils::toString(mRight).c_str());
        }

        mRight = mRight.normalized();
        mUpReal = mRight.cross(mFront).normalized();

        sbAssert(mFront.dot(mFront) > 0.0f, "zero front vector");
        sbAssert(mRight.dot(mRight) > 0.0f, "zero right vector");
        sbAssert(mUpReal.dot(mUpReal) > 0.0f, "zero upReal vector");

        sbAssert(std::abs(mRight.length() - 1.0f) < 0.001f,
                 "mRight (%f, %f, %f; length = %f) not normalized",
                 mRight.x, mRight.y, mRight.z, mRight.length());
        sbAssert(std::abs(mUpReal.length() - 1.0f) < 0.001f,
                 "mUpReal (%f, %f, %f; length = %f) not normalized",
                 mUpReal.x, mUpReal.y, mUpReal.z, mUpReal.length());
        sbAssert(std::abs(mFront.length() - 1.0f) < 0.001f,
                 "mFront (%f, %f, %f; length = %f) not normalized",
                 mFront.x, mFront.y, mFront.z, mFront.length());

        mMatrixUpdateFlags |= MatrixTranslationUpdated | MatrixRotationUpdated;
        updateAngles();
    }
/*!
  Sets the preshape type to \a p and updates the DOF values.
*/
void
preshape::set_preshapeType(preshapeType p) {
  pType = p;
  updateAngles();
}
/*!
  Copies preshape \a p .
*/
preshape::preshape(const preshape &p) {
  pType = p.get_preshapeType();
  p.get_preshape(a, f1, f2, f3);
  updateAngles();
}
Esempio n. 10
0
/*!
  Initialized the preshape type to \a p and updates the DOF values.
*/
preshape::preshape(preshapeType p) {
  pType = p;
  updateAngles();
}
Esempio n. 11
0
/*!
  Sets the preshape to PR_None and sets the initial angle values.
*/
preshape::preshape() {
  pType = PR_None;
  updateAngles();
}
Esempio n. 12
0
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();
}
Esempio n. 13
0
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;
}
Esempio n. 14
0
void EulerWidget::setValue(const Eigen::Quaterniond &q) {
	if (_q.isApprox(q)) return;
	_q = q;
	updateAngles();
	emit valueChanged(q);
}