void Planner3D::testControls()
{
initTree(m_rrttree, m_npose);
Matrix<3,1> u;
bool flip = false;
Matrix<3> pt;
pt[0] = 5;
pt[1] = 0;
pt[2] = -5;
double d = dist(m_npose, pt);
std::cout << "dist: " << d << std::endl;
localPlanner(m_rrttree, 0, pt, u, flip, false);
std::cout << "u: " << u[0] << " " << u[1] << " " << u[2] << std::endl;
std::list<Control> clist;
u[0] = 3*M_PI*0.5;
u[1] = -M_PI*0.1;
u[2] = 5;
Matrix<4,4> nT = stepTDiscrete(m_npose, u, flip, 1.0);
std::cout << "Projected transform matrix:\n" << nT << std::endl;
Control c;
c.m_v = 0.0;
c.m_w = -M_PI*0.1;
c.m_dt = 1.0;
clist.push_back(c);
c.m_v = 3.0*M_PI*0.5;
c.m_w = 0.0;
c.m_dt = 1.0;
clist.push_back(c);
m_npose = executeControls(m_npose, clist);
/*
getControls(u, flip, clist);
//m_npose = executeControlsWithNoise(m_npose, clist);
m_npose = executeControls(m_npose, clist);
for(std::list<Control>::iterator cit = clist.begin(); cit != clist.end(); ++cit) {
std::cout << "v: " << cit->m_v << " w: " << cit->m_w << " dt: " << cit->m_dt << std::endl;
}
*/
std::cout << "New transform matrix:\n" << m_npose << std::endl;
updateNeedleTipPose(m_npose);
}
void PathShortener::shortenPath(list<VectorXd> &path)
{
printf("--> Start Brute Force Shortener \n");
srand(time(NULL));
VectorXd savedDofs = robot->getConfig(dofs);
const int numShortcuts = path.size() * 5;
// Number of checks
for( int count = 0; count < numShortcuts; count++ ) {
if( path.size() < 3 ) { //-- No way we can reduce something leaving out the extremes
return;
}
int node1Index;
int node2Index;
do {
node1Index = (int) RAND12(0, path.size());
node2Index = (int) RAND12(0, path.size());
} while(node2Index <= node1Index + 1);
list<VectorXd>::iterator node1Iter = path.begin();
advance(node1Iter, node1Index);
list<VectorXd>::iterator node2Iter = node1Iter;
advance(node2Iter, node2Index - node1Index);
list<VectorXd> intermediatePoints;
if(localPlanner(intermediatePoints, node1Iter, node2Iter)) {
list<VectorXd>::iterator node1NextIter = node1Iter;
node1NextIter++;
path.erase(node1NextIter, node2Iter);
path.splice(node2Iter, intermediatePoints);
}
}
robot->setConfig(dofs, savedDofs);
printf("End Brute Force Shortener \n");
}