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