bool KinematicsSolver::checkTheta123(double t1, double t2, double t3) {
double x = pos(1);
double y = pos(2);
double z = pos(3);
Matrix T = getT(DH_a0, DH_d1, DH_alfa0, t1) * getT(DH_a1, DH_d2, DH_alfa1,
t2) * getT(DH_a2, DH_d3, DH_alfa2, t3) * getT(DH_a3, DH_d4,
DH_alfa3, 0);
//TODO: 0.27 0.12 0.024 is not being solved!!
//cout << T << endl;
//cout << x << " " << y << " " << z << endl;
double _X = T(1, 4);
double _Y = T(2, 4);
double _Z = T(3, 4) - DH_d1;
//cout << endl << "CHECKING 123 [ " << t1 << ", " << t2 << ", " << t3 << "] for ";
//cout << "Loc: " << "[ " << x << ", " << y << ", " << z << "]..... ";
bool _check = DBL_EQ(_X, x) && DBL_EQ(_Y, y) && DBL_EQ(_Z, z);
//if (_check) {
// cout << "OK.." << _Z << endl;
//} else {
// cout << "wrong!" << endl;
//}
return _check;
}
bool KinematicsSolver::solveTranscendal(double a, double b, double c,
double* theta) {
/*
* Solution for Transcendal Eqn
* aCos(x) + bSin(x) = c;
*/
if (DBL_EQ(a, 0) && !DBL_EQ(b, 0) ) {
// then Sin(x) = c/b;
theta[0] = asin(c/b);
theta[1] = M_PI - theta[0];
return true; // solved!
}
if (DBL_EQ(b, 0) && !DBL_EQ(a, 0) ) {
// then Cos(x) = c/a;
theta[0] = acos(c/a);
theta[1] = -theta[0];
return true; // solved!
}
double denom = roundTo(a + c, 0.0);
double rootTerm = roundTo(pow(b, 2) + pow(a, 2) - pow(c, 2) , 0);
// TODO: Check for denom == 0 or rootTerm < 0 first???
// check for a + c = 0;
if (DBL_EQ(denom, 0) ) {
theta[0] = M_PI; //u1 and u2 will be +180. -180 resp
theta[1] = -M_PI;
return true; // solved! :)
}
if (rootTerm < 0) {
//cout<<"Error!: Solution did not converge" << endl;
return false; // solution is imaginary and hence does not converge :(
}
rootTerm = sqrt(rootTerm);
//distinct solutions possible
theta[0] = 2 * atan( (b + rootTerm ) / denom);
theta[1] = 2 * atan( (b - rootTerm ) / denom);
return true; // solved!
}
inline double roundTo(double x, double y) {
if (DBL_EQ(x, y)) {
return y;
}
return x;
}
void LLCCEP_exec::softcore::emulated_cmp(LLCCEP::instruction data)
{
SOFTCORE_OK;
double n0 = get(data.args[0]), n1 = get(data.args[1]);
cmp = 0b1000;
if (DBL_LESS(n0, n1))
cmp |= 0b0010;
if (DBL_ABOVE(n0, n1))
cmp |= 0b0001;
if (DBL_EQ(n0, n1))
cmp |= 0b0100;
SOFTCORE_OK;
}
double
xround(double a)
{
double c;
c = ceil(a);
if (DBL_EQ(c - a, 0.5)) { /* round to even */
if (((long) c) % 2 == 0)
return c;
else
return c - 1;
} else if (DBL_LT(c, 0.5)) /* a is closer to his upper integer */
return c;
else
return c - 1;
}
bool KinematicsSolver::solveTheta56(double t3, double t2, double t1) {
Matrix T = getT(DH_a0, DH_d1, DH_alfa0, t1) * getT(DH_a1, DH_d2, DH_alfa1,
t2) * getT(DH_a2, DH_d3, DH_alfa2, t3);
// cout << "===================" << endl;
// cout << T.i() << endl;
// cout << "===================" << endl;
T = T.i() * target;
// cout << "===================" << endl;
// cout << T << endl;
// cout << "===================" << endl;
double* t5 = new double[2];
t5[0] = -acos(T(2, 3));
if (DBL_EQ(t5[0], 0)) {
t5[0] = 0;
return solveWristSingularity(currentQ4, t3, t2, t1);
}
// We proceed if t5 != 0;
t5[1] = -t5[0];
//cout << t5[0] << endl;
bool solved1 = false;
bool solved2 = false;
if (checkJointValidity(t5[0], JL5)) {
//if(checkTheta5(t5[0])){
solved1 = solveTheta6(t5[0], t3, t2, t1, T);
//}
}
if (!sameAngles(t5)) {
if (checkJointValidity(t5[1], JL5)) {
//if (checkTheta5(t5[1])){
solved2 = solveTheta6(t5[1], t3, t2, t1, T);
//}
}
}
return solved1 || solved2;
}
bool KinematicsSolver::solveTheta1(double t3, double t2, double F1, double F2) {
//cout << "T2: " << t2 << endl;
try {
double G1 = roundTo( (cos(t2) * F1 - sin(t2) * F2) + DH_a1, 0);
//cout << endl << "G1 is " << G1;
if(DBL_EQ(G1, 0)) {
return solveTheta56(t3, t2, 0);
}
Matrix G(2, 2);
G.Row(1) << G1 << 0;
G.Row(2) << 0 << G1;
ColumnVector p(2);
p(1) = pos(1);
p(2) = pos(2);
ColumnVector tmp = G.i() * p;
double __tmp1 = roundTo(tmp(1), 0);
double __tmp2 = roundTo(tmp(2), 0);
double t1 = atan2(__tmp2, __tmp1);
//cout << "T1: " << t1 << endl;
bool solved = false;
if (checkJointValidity(t1, JL1)) {
if(checkTheta123(t1, t2, t3)) {
//cout<< "Thetas 123: " << t1 << " " << t2 << " " << t3 << endl;
solved = solved || solveTheta56(t3, t2, t1);
}
}
return solved;
} catch (Exception e) {
cout << e.what() << endl;
}
return false;
}
inline bool sameAngles(double t[2]) {
return DBL_EQ(t[0], t[1]);
}
