/**
* @function planPath
* @brief Main function
*/
std::vector< Eigen::VectorXd > JTFollower::PlanPath( const Eigen::VectorXd &_start,
const std::vector<Eigen::VectorXd> &_workspacePath ) {
//-- Follow the path
std::vector< Eigen::VectorXd > configPath;
Eigen::VectorXd q;
int numPoints = _workspacePath.size();
//-- Initialize
q = _start;
for( size_t i = 1; i < numPoints; ++i ) { // start from 1 since 0 is the current start position
if( GoToXYZ( q, _workspacePath[i], configPath ) == false ) {
printf(" --(x) An error here, stop following path \n"); break;
}
}
printf("End of Plan Path \n");
return configPath;
}
/**
* @function planPath
* @brief Main function
*/
std::vector< Eigen::VectorXd > JTFollower::PlanPath( int _robotId,
const Eigen::VectorXi &_links,
const Eigen::VectorXd &_start,
std::string _EEName,
int _EEId,
double _res,
const std::vector<Eigen::VectorXd> &_workspacePath ) {
mRobotId = _robotId;
mLinks = _links;
mMaxIter = 100;
mWorkspaceThresh = _res; // An error of half the resolution
mEENode = mWorld->getRobot(mRobotId)->getNode( _EEName.c_str() );
mEEId = _EEId;
//-- Follow the path
std::vector< Eigen::VectorXd > configPath;
Eigen::VectorXd q;
int numPoints = _workspacePath.size();
//-- Initialize
q = _start;
for( size_t i = 1; i < numPoints; ++i ) { // start from 1 since 0 is the current start position
if( GoToXYZ( q, _workspacePath[i], configPath ) == false ) {
printf(" --(x) An error here, stop following path \n"); break;
}
}
printf("End of Plan Path \n");
return configPath;
}
