void SpecificWorker::moverpataPunto(QVec pfin)
{
QVec angles=QVec::zeros(3);
QVec p=inner->transform("arm1motor1T",pfin,"base");
float q1=atan(p.x()/p.z());
float x2=p.x()*p.x(),y2=p.y()*p.y(),z2=p.z()*p.z(),Femur2=Femur*Femur,Tibia2=Tibia*Tibia,
r=sqrt(x2+z2)-Coxa,
cosq3=((r*r)+y2-Tibia2-Femur2)/(2*Tibia*Femur),
senq3=-sqrt(1-(cosq3*cosq3));
qDebug()<<"cos (q3) = "<<r*r<<"+"<<y2<<"-"<<Tibia2<<"-"<<Femur2<<")/(2*"<<Tibia<<"*"<<Femur<<")"<<" = "<<cosq3;
qDebug()<<"sen (q3) = "<<senq3;
float q3=atan(senq3/cosq3);
float q2=atan(p.y()/r)-atan((Tibia*senq3)/(Femur+(Tibia*cosq3)));
angles(0)=q1;
angles(1)=q2+0.22113-0.5;
angles(2)=q3+0.578305+0.8;
moverangles(angles);
}
bool SpecificWorker::setParams(RoboCompCommonBehavior::ParameterList params)
{
try
{
string name = PROGRAM_NAME;
base=QString::fromStdString(params[name+".base"].value);
floor=QString::fromStdString(params[name+".floor"].value);
string s=params[name+".InnerModel"].value;
inner = new InnerModel(params[name+".InnerModel"].value);
motores<<QString::fromStdString(params[name+".m1"].value)<<QString::fromStdString(params[name+".m2"].value)<<QString::fromStdString(params[name+".m3"].value);
foot=QString::fromStdString(params[name+".foot"].value);
signleg=QString::fromStdString(params[name+".singleg"].value.data()).toInt();
QVec aux=inner->transform(motores.at(1),motores.at(0));
coxa=aux.norm2();
aux=inner->transform(motores.at(2),motores.at(1));
femur=aux.norm2();
aux=inner->transform(foot,motores.at(2));
tibia=aux.norm2();
pos_foot =inner->transform(floor,foot);
qDebug()<<"-----------------------------";
qDebug()<<" InnerModel ="<<QString::fromStdString(s);
qDebug()<<" coxa = "<<coxa;
qDebug()<<" femur = "<<femur;
qDebug()<<" tibia = "<<tibia;
qDebug()<<" signleg = "<<signleg;
qDebug()<<" foot = "<<foot;
qDebug()<<" base = "<<base;
qDebug()<<" floor = "<<floor;
qDebug()<<" m1 = "<<motores.at(0);
qDebug()<<" m2 = "<<motores.at(1);
qDebug()<<" m2 = "<<motores.at(2);
qDebug()<<" posfoot = "<<pos_foot;
qDebug()<<"-----------------------------";
moverangles(QVec::vec3(0.,0.3,-0.6),1);
// moverangles(QVec::vec3(0.,0.22113,0.578305),2);
}
catch(std::exception e)
{
qFatal("Error reading config params");
}
for(auto name:motores)
motorsparams[name.toStdString()]=jointmotor_proxy->getMotorParams(name.toStdString());
for(auto name:motores)
qDebug()<<motorsparams[name.toStdString()].offset;
timer.start(Period);
return true;
}
void SpecificWorker::setFKLeg(const AnglesLeg &al)
{
try
{
QVec angles=QVec::vec3(al.q1,al.q2,al.q3);
moverangles(angles, al.vel);
}
catch(const Ice::Exception &ex)
{
std::cout << ex << std::endl;
}
}
void SpecificWorker::sendData(const TData& data)
{
QVec angles=QVec::zeros(3);
for(auto m:data.axes)
{
if(m.name=="x")
angles(0)=(m.value/65537);
if(m.name=="y")
angles(1)=(m.value/65537);
if(m.name=="z")
angles(2)=(m.value/65537);
}
moverangles(angles);
}
bool SpecificWorker::setIKLeg(const PoseLeg &p, const bool &simu)
{
try
{
bool exito;
QVec posfoot=inner->transform(motores.at(0),QVec::vec3(p.x,p.y,p.z),QString::fromStdString(p.ref));
QVec angles=movFoottoPoint(posfoot, exito);
qDebug()<<exito<<"--------------------------------------------------";
if(!simu&&exito)
moverangles(angles, p.vel);
return exito;
}
catch(const Ice::Exception &ex)
{
std::cout << ex << std::endl;
return false;
}
}
void SpecificWorker::moverpatacd()
{
QVec angles=QVec::vec3((float)clavicula->value()/100,(float)hombro->value()/100,(float)codo->value()/100);
moverangles(angles);
bool moviendose=true;
while(moviendose){
moviendose=false;
MotorState m = jointmotor_proxy->getMotorState(m1);
if (m.isMoving) moviendose=true;
else {
m = jointmotor_proxy->getMotorState(m2);
if (m.isMoving) moviendose=true;
else {
m = jointmotor_proxy->getMotorState(m3);
if (m.isMoving) moviendose=true;
}
}
}
Posfin=inner->transform("base","axisA1T");
}
