/**
* \brief Construct a diagonal matrix
*
* Use vector values to create a new diagonal matrix
* @param v Vector to get diagonal values
* @return QMat new diagonal matrix
*/
QMat RMat::QMat::makeDiagonal ( const QVec &v )
{
QMat R ( v.size(),v.size(), (T)0 );
int f = v.size();
for ( int i=0; i<f; i++ )
R ( i,i ) = v ( i );
return R;
}
/**
* \brief Computes the coefficients of the implicit line equation: y = mx + n passing by two points
* from two points (x1,y1) e (x2,y2) satisfying (y-y1)/(y2-y1) = (x-x1)/(x2-x1),
* that solving for y gives: y = x( (y2-y1)/(x2-x1) ) - x1( (y2-y1)/(x2-x1) ) + y1
* @param p1 containing x1,y1
* @param p2 containing x2,y2
* @return a QVec vector of 2 dimensions with m and n
*/
QVec RMat::QVec::line2DImplicitCoefsFrom2Points(const QVec & p1, const QVec & p2)
{
Q_ASSERT( p1.size() == 2 and p2.size() == 2 and p2(0)-p1(0) != 0 );
QVec res(2);
res(0) = (p2(1)-p1(1))/(p2(0)-p1(0)) ;
res(1) = p1(1) - p1(0) * ((p2(1)-p1(1))/(p2(0)-p1(0))) ;
return res;
}
/**
* \brief Computes the coefficients of the explicit line equation: Ax+By+C=0 passing by two points
* from two points (x1,x2) e (y1,y2) satisfying (x-x1)/v1 = (y-y1)/v2, being v1 and v2 the direction vectors of the line
* that after some algebra gives: A=v2, B= -v1, C= v1y1-v2x2
* @param p1 containing x1,y1
* @param p2 containing x2,y2
* @return a QVec vector of 3 dimensions with A, B and C
*/
QVec RMat::QVec::line2DExplicitCoefsFrom2Points(const QVec & p1, const QVec & p2)
{
Q_ASSERT( p1.size() == 2 and p2.size() == 2);
QVec res(3);
//A
res(0) = p2(1)-p1(1) ;
//B
res(1) = -(p2(0)-p1(0)) ;
//C
res(2) = -res(1)*p1(1) - res(0)*p1(0);
return res;
}
/**
* \brief Vector to matrix constructor
*
* Creates a row or column matrix from vector
* @param vector Vector to convert to matrix type
* @param rowVector if true, creates a row matrix from the vector, else creates a column matrix from it
*/
QMat::QMat(const QVec &vector, const bool rowVector)
{
cols = rowVector?vector.size():1;
rows = rowVector?1:vector.size();
data = new DataBuffer(cols*rows);
if (rowVector)
setRow(0, vector);
else
setCol(0, vector);
}
/**
* \brief Multiplies a n-column vector times a m-row vector to generate a nxm matrix
* @param vector vector to play the rol of row vector
* @return resulting QMat matrix
*/
QMat QVec::externProduct(const QVec & vector) const
{
Q_ASSERT(size() == vector.size());
const int s = size();
QMat C ( size(), vector.size() );
#ifdef COMPILE_IPP
ippmMul_mm_32f ( getReadData(), s * sizeof ( T ), sizeof ( T ), s , s , vector.getReadData(), vector.size() * sizeof ( T ), sizeof ( T ), vector.size() , vector.size() , C.getWriteData(), vector.size() * sizeof ( T ), sizeof ( T ) );
#else
for(int r=0;r<s;r++)
for(int c=0;c<vector.size();c++)
C(r,c) = this->operator()(r) * vector(c);
#endif
return C;
}
RMat::T RMat::QVec::distanceTo2DLine( const QVec & line ) const
{
Q_ASSERT_X( size() == 2 and line.size() == 3, "QVec::distanceTo2DLine", "incorrect size of parameters");
return fabs(line(0)*operator()(0) + line(1)*operator()(1) + line(2)) / sqrt(line(0)*line(0) + line(1)*line(1));
}
/**
* \brief Matrix to vector product operator; \f$ C = this * vector \f$
* @param A vector factor for operation
* @return QVec New vector result
*/
QVec RMat::QMat::operator *(const QVec & vector) const
{
Q_ASSERT ( cols == vector.size());
QMat aux = vector.toColumnMatrix();
QMat result = operator*(aux);
return result.toVector();
}
/**
* \brief Dot product of operand with current vector
* @param vector
* @return dot product vector
*/
T QVec::dotProduct(const QVec &vector) const
{
Q_ASSERT(size() == vector.size());
double accum = 0;
for (int i = 0; i < size(); i++)
accum += at(i) * vector[i];
return accum;
}
/**
* \brief operator that subtracts a vector from the current one
* @param vector
* @return the difference vector
*/
QVec QVec::operator -(const QVec & vector) const
{
Q_ASSERT( size() == vector.size() );
QVec result(size());
for (int i = 0; i < size(); i++)
result[i] = at(i) - vector[i];
return result;
}
void RMat::QMat::setRow(const int row, QVec vector)
{
Q_ASSERT(row < nRows());
Q_ASSERT(nCols() == vector.size());
const int cols = nCols();
for (int col= 0; col < cols; col++)
operator()(row,col) = vector[col];
}
/**
* \brief Element to element product
* @param vector
* @return product vector
*/
QVec QVec::pointProduct(const QVec & vector) const
{
Q_ASSERT(size() == vector.size());
QVec result = *this;
for (int i = 0; i < size(); i++)
result[i] *= vector[i];
return result;
}
//Do it with inject
void RMat::QMat::setCol(const int col, QVec vector)
{
Q_ASSERT(col < nCols());
Q_ASSERT(nRows() == vector.size());
const int rows = nRows();
for (int row= 0; row < rows; row++)
operator()(row,col) = vector[row];
}
/**
* \brief Compares two vector for equal values element to element
* @param vector
* @return true if both are equal, false if not
*/
bool RMat::QVec::equals(const QVec & vector ) const
{
if (size() != vector.size())
return false;
for (int i = 0; i < size(); i++)
if (at(i) != vector[i])
return false;
return true;
}
/**
* \brief Computes de crossproduct of current 3-vector and a 3-vector parameter
* @param vector 3-vector that operates on current vector
* @return crossproduct
*/
QVec RMat::QVec::crossProduct(const QVec & vector) const
{
Q_ASSERT(size() == vector.size());
Q_ASSERT(size() == 3);
const double x1 = at(0), y1 = at(1), z1 = at(2), x2 = vector[0], y2 = vector[1], z2 = vector[2];
QVec v(3);
v[0] = -y2*z1 + y1*z2;
v[1] = x2*z1 - x1*z2;
v[2] = -x2*y1 + x1*y2;
return v;
}
QMat RMat::QMat::eigenValsVectors ( QVec & vals )
{
if ( isSquare() and ( vals.size() ==cols ) )
{
QMat R ( rows,cols,0.);
#ifdef COMPILE_IPP
T valsIm[cols];
int SizeBytes;
ippmEigenValuesVectorsGetBufSize_32f(cols, &SizeBytes);
Ipp8u pBuffer[SizeBytes];
IppStatus status = ippmEigenValuesVectorsRight_m_32f (toData(), cols*sizeof(T), sizeof(T), R.toData(), cols*sizeof(T), sizeof(T), vals.getWriteData(),valsIm, cols, pBuffer);
if ( status == ippStsNoErr )
return R;
else
{
printf ( "EigenValsVectors error: %d, %s\n", status, ippGetStatusString ( status ) );
( *this ).print ( "this" );
QString s="QMat::eigenVectors() - Error returned by IPP"; throw s;
}
#else
// qFatal("eigen values using Eigen has not been properly tested yet");
double data[this->getDataSize()];
const T *dataBuffer = this->getReadData();
for(int i = 0; i < this->getDataSize(); i++)
data[i] = (double)dataBuffer[i];
gsl_matrix_view m = gsl_matrix_view_array (data, rows,rows);
gsl_vector *eval = gsl_vector_alloc (rows);
gsl_matrix *evec = gsl_matrix_alloc (rows, rows);
gsl_eigen_symmv_workspace * w = gsl_eigen_symmv_alloc (rows);
gsl_eigen_symmv (&m.matrix, eval, evec, w);
gsl_eigen_symmv_free (w);
gsl_eigen_symmv_sort (eval, evec, GSL_EIGEN_SORT_ABS_DESC);
for(int i=0;i<rows;i++)
vals(i) = gsl_vector_get(eval,i);
QMat P(evec);
gsl_vector_free (eval);
gsl_matrix_free (evec);
return P;
#endif
}
QString s="QMat::EigenValsVectors() - Not Square Matrix"; throw s;
return QMat();
}
/** REVISARRRR
* @brief Sends the robot bakcwards on a straight line until targetT is reached.
*
* @param innerModel ...
* @param target position in World Reference System
* @return bool
*/
bool SpecificWorker::goBackwardsCommand(InnerModel *innerModel, CurrentTarget ¤t, CurrentTarget ¤tT, TrajectoryState &state, WayPoints &myRoad)
{
const float MAX_ADV_SPEED = 400.f;
const float MAX_POSITIONING_ERROR = 40; //mm
static float errorAnt = std::numeric_limits<float>::max();
///////////////////
//CHECK PARAMETERS
///////////////////
QVec target = current.getTranslation();
if (target.size() < 3 or std::isnan(target.x()) or std::isnan(target.y()) or std::isnan(target.z()))
{
qDebug() << __FUNCTION__ << "Returning. Invalid target";
RoboCompTrajectoryRobot2D::RoboCompException ex;
ex.text = "Returning. Invalid target";
throw ex;
return false;
}
state.setState("BACKWARDS");
QVec rPose = innerModel->transform("world", "robot");
float error = (rPose - target).norm2();
bool errorIncreasing = false;
if (error > errorAnt)
errorIncreasing = true;
qDebug() << __FUNCTION__ << "doing backwards" << error;
if ((error < MAX_POSITIONING_ERROR) or (errorIncreasing == true)) //TASK IS FINISHED
{
controller->stopTheRobot(omnirobot_proxy);
myRoad.requiresReplanning = true;
currentT.setWithoutPlan(true);
currentT.setState(CurrentTarget::State::GOTO);
errorAnt = std::numeric_limits<float>::max();
}
else
{
float vadv = -0.5 * error; //Proportional controller
if (vadv < -MAX_ADV_SPEED) vadv = -MAX_ADV_SPEED;
try
{ omnirobot_proxy->setSpeedBase(0, vadv, 0); }
catch (const Ice::Exception &ex)
{ std::cout << ex << std::endl; }
}
errorAnt = error;
return true;
}
std::tuple< bool, QString > Sampler::checkRobotValidStateAtTarget(const QVec& target) const
{
if( target.size() != 6 )
return std::make_tuple(false, QString("Invalid target vector. A 6D vector is required"));
return checkRobotValidStateAtTarget(target.subVector(0,2), target.subVector(3,5));
}
/**
* @brief Computes de angle (-PI, PI) between this and endpoint p wrt the Y axis. If both points are the same, the method returns 0.
*
* @param p end point of segment
* @return angle
**/
RMat::T RMat::QVec::angleOf2DSegment( const QVec & p) const
{
Q_ASSERT_X( size() == 2 and p.size()==2 , "QVec::angleOf2DSegment", "incorrect size of parameters");
return atan2( p.x()-operator[](0), p.y()-operator[](1) );
}