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!");
}
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)));
}
void WalkBalance::walkBalanceCheck()
{
updateAngles();
if(angleX > angleXmax || angleX <angleXmin ||angleY >angleYmax ||angleY<angleYmin)
{
}
}
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();
}
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);
}
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());
}
}
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();
}
/*!
Initialized the preshape type to \a p and updates the DOF values.
*/
preshape::preshape(preshapeType p) {
pType = p;
updateAngles();
}
/*!
Sets the preshape to PR_None and sets the initial angle values.
*/
preshape::preshape() {
pType = PR_None;
updateAngles();
}
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 EulerWidget::setValue(const Eigen::Quaterniond &q) {
if (_q.isApprox(q)) return;
_q = q;
updateAngles();
emit valueChanged(q);
}
