void ExperimentalTrajectory::removeWaypoint(int index)
{
std::cout << "Removing waypoint index: " << index << "..." << std::endl;
if ( (index>=0) && (index<nWaypoints) )
{
Eigen::MatrixXd newWaypointsMat = Eigen::MatrixXd::Zero(nDoF, nWaypoints-1);
newWaypointsMat.leftCols(index) = waypoints.leftCols(index);
newWaypointsMat.rightCols(nWaypoints-1-index) = waypoints.rightCols(nWaypoints-1-index);
waypoints.resize(nDoF, nWaypoints-1);
waypoints = newWaypointsMat;
Eigen::VectorXd durationVectorTmp = Eigen::VectorXd::Zero(nWaypoints-2);
int durVecIndex = index;
durationVectorTmp.head(durVecIndex) = pointToPointDurationVector.head(durVecIndex);
durationVectorTmp.tail(nWaypoints -1 -durVecIndex) = pointToPointDurationVector.tail(nWaypoints -1 -durVecIndex);
pointToPointDurationVector.resize(durationVectorTmp.size());
pointToPointDurationVector = durationVectorTmp;
reinitialize();
}
else{
std::cout << "[ERROR] (ExperimentalTrajectory::addNewWaypoint): New waypoint time is out of index bounds. Should have fall between 0 and " << nWaypoints-1 << "." << std::endl;
}
}
void TaskSolverDmpArm2D::performRollout(const Eigen::VectorXd& sample, const Eigen::VectorXd& task_parameters, Eigen::MatrixXd& cost_vars) const
{
TaskSolverDmp::performRollout(sample, task_parameters, cost_vars);
// cost_vars_angles structure is (without the link positions!)
//
// time joint angles (e.g. n_dofs = 3) forcing term link positions (e.g. 3+1)
// ____ __________________________________ __________ _________________________
// t | a a a | ad ad ad | add add add | f f f | x y | x y | x y | x y |
//
// We now compute the link positions and add them to the end
int n_dofs = dmp_->dim_orig();
int n_time_steps = cost_vars.rows();
assert(cost_vars.cols() == 1+4*n_dofs);
// Make room for link positions, i.e. 2 * (n_links+1)
cost_vars.conservativeResize(n_time_steps, 1 + 4*n_dofs + 2*(n_dofs+1));
MatrixXd angles = cost_vars.block(0,1,n_time_steps,n_dofs);
MatrixXd link_positions;
anglesToLinkPositions(angles,link_positions);
// Add the link positions to the right side of the cost_vars matrix
cost_vars.rightCols(2*(n_dofs+1)) = link_positions;
}
Eigen::MatrixXd ExperimentalTrajectory::getWaypointData()
{
Eigen::MatrixXd dataMat = Eigen::MatrixXd::Zero(nWaypoints, nDoF+1);
dataMat.col(0) = kernelCenters;
dataMat.rightCols(nDoF) = waypoints.transpose();
return dataMat;
}
void ExperimentalTrajectory::addNewWaypoint(Eigen::VectorXd newWaypoint, double waypointTime)
{
std::cout << "Adding waypoint at: " << waypointTime << " seconds..." << std::endl;
if ( (newWaypoint.rows() == nDoF) && (waypointTime>=0.0) && (waypointTime<=kernelCenters.maxCoeff()) )
{
//Find out where the new T falls...
for (int i=0; i<kernelCenters.size(); i++)
{
std::cout << "i = " << i << std::endl;
if (kernelCenters(i)>waypointTime) {
youngestWaypointIndex=i;
std::cout << "youngestWaypointIndex" << youngestWaypointIndex << std::endl;
i = kernelCenters.size();
}
}
Eigen::MatrixXd newWaypointsMat = Eigen::MatrixXd::Zero(nDoF, nWaypoints+1);
newWaypointsMat.leftCols(youngestWaypointIndex) = waypoints.leftCols(youngestWaypointIndex);
newWaypointsMat.col(youngestWaypointIndex) = newWaypoint;
newWaypointsMat.rightCols(nWaypoints - youngestWaypointIndex) = waypoints.rightCols(nWaypoints - youngestWaypointIndex);
waypoints.resize(nDoF, nWaypoints+1);
waypoints = newWaypointsMat;
Eigen::VectorXd durationVectorTmp = Eigen::VectorXd::Zero(nWaypoints);
std::cout << "\npointToPointDurationVector\n " << pointToPointDurationVector << std::endl;
durationVectorTmp.head(youngestWaypointIndex-1) = pointToPointDurationVector.head(youngestWaypointIndex-1);
durationVectorTmp.row(youngestWaypointIndex-1) << waypointTime - kernelCenters(youngestWaypointIndex-1);
durationVectorTmp.tail(nWaypoints - youngestWaypointIndex) = pointToPointDurationVector.tail(nWaypoints - youngestWaypointIndex);
pointToPointDurationVector.resize(durationVectorTmp.size());
pointToPointDurationVector = durationVectorTmp;
std::cout << "\npointToPointDurationVector\n " << pointToPointDurationVector << std::endl;
reinitialize();
}
else{
if (newWaypoint.rows() != nDoF){
std::cout << "[ERROR] (ExperimentalTrajectory::addNewWaypoint): New waypoint is not the right size. Should have dim = " <<nDoF << "x1." << std::endl;
}
if ((waypointTime<=0.0) || (waypointTime>=kernelCenters.maxCoeff())){
std::cout << "[ERROR] (ExperimentalTrajectory::addNewWaypoint): New waypoint time is out of time bounds. Should have fall between 0.0 and " << kernelCenters.maxCoeff() << " seconds." << std::endl;
}
}
}
int main(int argc, char * argv[])
{
using namespace Eigen;
using namespace std;
using namespace igl;
VectorXd D;
if(!read_triangle_mesh("../shared/beetle.off",V,F))
{
cout<<"failed to load mesh"<<endl;
}
twod = V.col(2).minCoeff()==V.col(2).maxCoeff();
bbd = (V.colwise().maxCoeff()-V.colwise().minCoeff()).norm();
SparseMatrix<double> L,M;
cotmatrix(V,F,L);
L = (-L).eval();
massmatrix(V,F,MASSMATRIX_TYPE_DEFAULT,M);
const size_t k = 5;
if(!eigs(L,M,k+1,EIGS_TYPE_SM,U,D))
{
cout<<"failed."<<endl;
}
U = ((U.array()-U.minCoeff())/(U.maxCoeff()-U.minCoeff())).eval();
igl::viewer::Viewer viewer;
viewer.callback_key_down = [&](igl::viewer::Viewer & viewer,unsigned char key,int)->bool
{
switch(key)
{
default:
return false;
case ' ':
{
U = U.rightCols(k).eval();
// Rescale eigen vectors for visualization
VectorXd Z =
bbd*0.5*U.col(c);
Eigen::MatrixXd C;
igl::parula(U.col(c).eval(),false,C);
c = (c+1)%U.cols();
if(twod)
{
V.col(2) = Z;
}
viewer.data.set_mesh(V,F);
viewer.data.compute_normals();
viewer.data.set_colors(C);
return true;
}
}
};
viewer.callback_key_down(viewer,' ',0);
viewer.core.show_lines = false;
viewer.launch();
}
void BranchList::calculateOrientations()
{
for (int i = 0; i < mBranches.size(); i++)
{
Eigen::MatrixXd positions = mBranches[i]->getPositions();
Eigen::MatrixXd diff = positions.rightCols(positions.cols() - 1) - positions.leftCols(positions.cols() - 1);
Eigen::MatrixXd orientations(positions.rows(),positions.cols());
orientations.leftCols(orientations.cols() - 1) = diff / diff.norm();
orientations.rightCols(1) = orientations.col(orientations.cols() - 1);
mBranches[i]->setOrientations(orientations);
}
}
void ExperimentalTrajectory::initializeTrajectory()
{
originalWaypoints = waypoints;
kernelLengthParameter = pointToPointDurationVector.mean();
// Add a waypoint to the end.
int extraWaypoints = 1;
int nStartWp = extraWaypoints;
int nEndWp = extraWaypoints;
int nWpNew = nWaypoints + nStartWp + nEndWp;
Eigen::MatrixXd waypointsTmp = Eigen::MatrixXd::Zero(nDoF, nWpNew);
waypointsTmp.leftCols(nStartWp) = waypoints.leftCols(1).replicate(1,nStartWp);
waypointsTmp.middleCols(nStartWp, nWaypoints) = waypoints;
waypointsTmp.rightCols(nEndWp) = waypoints.rightCols(1).replicate(1,nEndWp);
waypoints.resize(nDoF, nWaypoints);
waypoints = waypointsTmp;
// std::cout << pointToPointDurationVector << std::endl;
Eigen::VectorXd durationVectorTmp = Eigen::VectorXd::Zero(nWpNew-1);
double extraWpDt = 0.01 * kernelLengthParameter;
// double extraWpDt = 0.01 * pointToPointDurationVector.sum();
// std::cout << "extraWpDt: " << extraWpDt << std::endl;
durationVectorTmp.head(nStartWp) = Eigen::VectorXd::Constant(nStartWp, extraWpDt);
durationVectorTmp.segment(nStartWp, nWaypoints-1) = pointToPointDurationVector;
durationVectorTmp.tail(nEndWp) = Eigen::VectorXd::Constant(nEndWp, extraWpDt);
pointToPointDurationVector.resize(durationVectorTmp.size());
pointToPointDurationVector = durationVectorTmp;
nWaypoints = nWpNew;
std::cout << pointToPointDurationVector << std::endl;
calculateVarianceParameters();
}
Eigen::MatrixXd eraseCol(int removeIndex, Eigen::MatrixXd positions)
{
positions.block(0 , removeIndex , positions.rows() , positions.cols() - removeIndex - 1) = positions.rightCols(positions.cols() - removeIndex - 1);
positions.conservativeResize(Eigen::NoChange, positions.cols() - 1);
return positions;
}
void BranchList::findBranchesInCenterline(Eigen::MatrixXd positions)
{
positions = sortMatrix(2,positions);
Eigen::MatrixXd positionsNotUsed = positions;
// int minIndex;
int index;
int splitIndex;
Eigen::MatrixXd::Index startIndex;
BranchPtr branchToSplit;
while (positionsNotUsed.cols() > 0)
{
if (!mBranches.empty())
{
double minDistance = 1000;
for (int i = 0; i < mBranches.size(); i++)
{
std::pair<std::vector<Eigen::MatrixXd::Index>, Eigen::VectorXd> distances;
distances = dsearchn(positionsNotUsed, mBranches[i]->getPositions());
double d = distances.second.minCoeff(&index);
if (d < minDistance)
{
minDistance = d;
branchToSplit = mBranches[i];
startIndex = index;
if (minDistance < 2)
break;
}
}
std::pair<Eigen::MatrixXd::Index, double> dsearchResult = dsearch(positionsNotUsed.col(startIndex) , branchToSplit->getPositions());
splitIndex = dsearchResult.first;
}
else //if this is the first branch. Select the top position (Trachea).
startIndex = positionsNotUsed.cols() - 1;
std::pair<Eigen::MatrixXd,Eigen::MatrixXd > connectedPointsResult = findConnectedPointsInCT(startIndex , positionsNotUsed);
Eigen::MatrixXd branchPositions = connectedPointsResult.first;
positionsNotUsed = connectedPointsResult.second;
if (branchPositions.cols() >= 5) //only include brances of length >= 5 points
{
BranchPtr newBranch = BranchPtr(new Branch());
newBranch->setPositions(branchPositions);
mBranches.push_back(newBranch);
if (mBranches.size() > 1) // do not try to split another branch when the first branch is processed
{
if ((splitIndex + 1 >= 5) && (branchToSplit->getPositions().cols() - splitIndex - 1 >= 5))
//do not split branch if the new branch is close to the edge of the branch
//if the new branch is not close to one of the edges of the
//connected existing branch: Split the existing branch
{
BranchPtr newBranchFromSplit = BranchPtr(new Branch());
Eigen::MatrixXd branchToSplitPositions = branchToSplit->getPositions();
newBranchFromSplit->setPositions(branchToSplitPositions.rightCols(branchToSplitPositions.cols() - splitIndex - 1));
branchToSplit->setPositions(branchToSplitPositions.leftCols(splitIndex + 1));
mBranches.push_back(newBranchFromSplit);
newBranchFromSplit->setParentBranch(branchToSplit);
newBranch->setParentBranch(branchToSplit);
newBranchFromSplit->setChildBranches(branchToSplit->getChildBranches());
branchVector branchToSplitChildren = branchToSplit->getChildBranches();
for (int i = 0; i < branchToSplitChildren.size(); i++)
branchToSplitChildren[i]->setParentBranch(newBranchFromSplit);
branchToSplit->deleteChildBranches();
branchToSplit->addChildBranch(newBranchFromSplit);
branchToSplit->addChildBranch(newBranch);
}
else if (splitIndex + 1 < 5)
// If the new branch is close to the start of the existing
// branch: Connect it to the same position start as the
// existing branch
{
newBranch->setParentBranch(branchToSplit->getParentBranch());
branchToSplit->getParentBranch()->addChildBranch(newBranch);
}
else if (branchToSplit->getPositions().cols() - splitIndex - 1 < 5)
// If the new branch is close to the end of the existing
// branch: Connect it to the end of the existing branch
{
newBranch->setParentBranch(branchToSplit);
branchToSplit->addChildBranch(newBranch);
}
}
}
}
}
