/*
* 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;
}
