/* * converts cartesian via points to joint space via points */ vector<ColumnVector> TrajectoryController::getJSPoints( vector<ColumnVector>& cartesianList) { vector<ColumnVector> jsPoints; ColumnVector cartesian(6), currentQ(6), previousQ(6), joint(6); Generator* gen = getGenerator(cartesianList); if (gen != NULL) { currentQ = 0; joint = 0; // - convert ith cartesian to thetas // - if conditions satisfied, add point in path. for (int i = 0; i <= PNT_SAMPLES; i++) { double k = ((i * 1.0) / PNT_SAMPLES); // cout << k << endl; //get xyz and orientation from generator for time k. cartesian = gen->getPosition(k); // cout << "cart c " << cartesian.as_row(); Quaternion quat = gen->getOrientation(k); cartesian.resize_keep(6); Matrix R = quat.R(); cartesian.Rows(4, 6) = irpy(R); // cout << cartesian.AsRow() << endl; // cout << quat << endl; //add the joint solution if it exists if (kineSolver->getBestSolution(cartesian, currentQ, joint)) { // cout << "cartesian " << cartesian.AsRow() << endl; // cout << "joint " << joint.AsRow() << endl; jsPoints.push_back(joint); currentQ = joint; } else { //path has to be rejected cout << cartesian.AsRow() << " : Via point out of workspace. Path rejected." << endl; jsPoints.clear(); break; } } } return jsPoints; }