bool RobotSim::moveTo(const vector<double> & _q)
{
int i;
if(_q.size()!=joints.size())return false;
if(!checkJointValues(_q))return false; //limits checking
for(i=0;i<(int)joints.size();i++)joints[i]->setValue(_q[i]);
return true;
}
bool AdeptOneSim::getConfigurationOf(vector<double> _q, unsigned char &conf)
{
//Compruebo rangos de coordenadas
if(checkJointValues(_q)==false)return false;
//Calculo de la configuracion actual correspondiente
//Se anidan dos if para que la variable *conf tenga una valor de retorno aunque la configuracion sea no valida
if(_q[1]<0)
conf=ELBOW_LEFT;
else
conf=ELBOW_RIGHT;
if(fabs(_q[1])<EPS)
return false;
return true;
}
bool RobotSim::forwardKinematicsAbs(vector<double> _q, Transformation3D& t)
{
int i;
if(tcp==0)return false;
if(links.size()==0)return false;
if(!checkJointValues(_q))return false; //limits checking
vector<double> qold;
//save current joint values and update the joint values
for(i=0;i<(int)joints.size();i++){
qold.push_back(joints[i]->getValue());
joints[i]->setValue(_q[i]);
}
// Compute the relative transformation between the tcp and the robot base
t=tcp->getAbsoluteT3D();
// Move the robot to the qold values
for(i=0;i<(int)joints.size();i++)joints[i]->setValue(qold[i]);
return true;
}
bool EUITIbotSim::getConfigurationOf(const vector<double> &_q, unsigned char &_conf)
{
//Compruebo tamaño y rangos de coordenadas
if(!checkJointValues(_q))return false;
double _s,_e;
//HOMBRO
if(_q[1]>PI/2)//Hombro arriba
_s=1.0;
else//Hombro abajo
_s=-1.0;
//CODO
if (_s == -1){
if (_q[2] < PI/2) _e = -1;
else{ _e = 1;}
}else{
if (_q[2] < PI/2) _e = 1;
else{ _e = -1;}
}
return configuration(_s,_e,_conf);
}
