/**
* \brief This method stored the motors's names, the initial position of the motors and the goal
* position of them. When all is prepared, it raises up the flag READY.
* @param newAnglesOfMotors an structure with the name of the motors and the value os them.
*/
void SpecificWorker::setJointPosition(const MotorAngleList &newAnglesOfMotors)
{
qDebug()<<"YEAH";
QMutexLocker ml(mutex);
if(INITIALIZED == true)
{
// 1) SACAMOS VALORES ACTUALES DE LOS MOTORES: (aprovechamos y sacamos motores disponibles)
QVec firstAngles;
for(auto motor : newAnglesOfMotors)
{
float angle = innerModel->getJoint(QString::fromStdString(motor.name))->getAngle();
firstAngles.push_back(angle);
selectedMotors<<QString::fromStdString(motor.name);
}
// 2) SACAMOS VALORES FINALES DE LOS MOTORES
QVec finalAngles;
for(auto motor : newAnglesOfMotors)
{
float angle = motor.angle;
finalAngles.push_back(angle);
}
jointValues.append(firstAngles);
jointValues.append(finalAngles);
COMPUTE_READY = true;
qDebug()<<"||------------------------------------------------";
qDebug()<<"|| setJointPosition: jointValues-->"<<jointValues;
qDebug()<<"||------------------------------------------------";
}
}
//-----------------------------------------------------------------------------
// MÉTODO EN PRUEBAS
//-----------------------------------------------------------------------------
// Devuelve los ángulos de rotación sacados de la matriz de rotación en un vector
// de 6 ELEMENTOS: x1, y1, z1, x2, y2, z2 (ángulos y sus opuestos: signos comabiados) En el primer caso
// X, Y, Z, X, Y, Z (ángulos repetidos) segundo caso.
// DOCUMENTACIÓN: http://www.soi.city.ac.uk/~sbbh653/publications/euler.pdf
QVec RMat::QMat::extractAnglesR() const
{
// Ten en cuenta: matriz transpuesta y con signos cambiados.
QVec angulos;
float x, y, z;
float x1, x2, y1, y2, z1, z2;
if (fabs(operator()(0,2)) > 1 || fabs(operator()(0,2)) < 1)
{
// ROTACION EN Y
y1 = asin(operator()(0,2));
y2 = M_PI-y1;
// ROTACION EN X
x1 = atan2((-operator()(1,2)/cos(y1)), (operator()(2,2)/cos(y1)));
x2 = atan2((-operator()(1,2)/cos(y2)), (operator()(2,2)/cos(y2)));
//ROTACION EN Z
z1 = atan2((-operator()(0,1)/cos(y1)), (operator()(0,0)/cos(y1)));
z2 = atan2((-operator()(0,1)/cos(y2)), (operator()(0,0)/cos(y2)));
angulos.push_back(x1); angulos.push_back(y1); angulos.push_back(z1); // ir por el camino 1
angulos.push_back(x2); angulos.push_back(y2); angulos.push_back(z2); // ir por el camino 2
}
else
{
// REVISAR LOS SIGNOS!!!
z = 0;
if (operator()(0,2) == 1)//Original if -sin(y)==-1 en el nuestro: if sin(y)==1
{
y = M_PI/2;
x = z + atan2( operator()(1,0), -operator()(2,0));// al final queda atan2(sin x, -(-cosx))-->atan2(sin x, cos x)
}
else //si sin(y)==-1 --> y = -pi/2
{
y = -M_PI/2;
x = -z +atan2(-operator()(1,0), operator()(2,0));
}
angulos.push_back(x); angulos.push_back(y); angulos.push_back(z);
angulos.push_back(x); angulos.push_back(y); angulos.push_back(z); //Repetimos los ángulos, por ser coherentes....
}
return angulos;
}