// Input x, y ,z and the angle vector bool ikine(vector<double> *coords, vector<double> *angles, int grip) { //cout << endl << "Andy Ikine says, hello world" << endl << endl; double x = coords->at(0); double y = coords->at(1); double z = coords->at(2) - L1; // calculate the closes reach double reach_limit = L1*cosd(90) + L2*cosd(90 + (asin((-L1*sind(90))/L2) * 180 / M_PI) ); //cout << "Min reach: " << reach_limit << endl; // Note that the dynamixel and rotate 150(degree) from origin double magnitude = sqrt( pow(x,2) + pow(y,2) + pow(z,2) ); #if DEBUG cerr << "Resultant Vector: " << magnitude << endl; #endif if ( magnitude > (L2 + L3) ) { cerr << "Out of reach" << endl; //return false; } // (horizontal, vertical) theta A angles->at(0) = atan2(x,y); // converted hypotenuse as the new y value y = y / cos(angles->at(0)); // equation from the online source double temp1 = (pow(y,2) + pow(z,2) - pow(L2,2) - pow(L3,2)) / (2 * L2 * L3); double temp2 = -sqrt( 1 - pow(temp1, 2) ); // theta C angles->at(2) = atan2( temp2, temp1); double k1 = L2 + L3 * cos(angles->at(2)); double k2 = L3 * sin(angles->at(2)); // theta B angles->at(1) = atan2( z, y ) - atan2( k2, k1 ); angles->at(2) += M_PI / 2 - M_PI / 6; angles->at(1) -= M_PI / 2; // open : close angles->at(3) = grip ? 1.3 : -1.4;//(-GRIP_ANGLE / 180.0 * M_PI) ; //print_values(angles); return check_angle_range(angles); }
void SickTimDatagramTest::update_config (sick_tim::SickTimConfig& new_config, uint32_t level) { check_angle_range (new_config); config_ = new_config; }