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); }