void Thread_RRT::getNextGoal(RRT_Node& goal, RRT_Node& goalThisIter)
{
//first, see if we are sampling randomly, or around goal
double randNum = ((double)rand()) / ((double)RAND_MAX);
if (randNum < RANDOM_SAMPLE_THRESH)
{
//goalThisIter.copyNode(goal);
//return;
setThisGoalNearGoal(goal, goalThisIter);
return;
}
//sample random goal
Matrix4d startTrans;
goal.thread->getStartTransform(startTrans);
Vector3d startPos = startTrans.corner(Eigen::TopRight,3,1);
Vector3d startTan = startTrans.corner(Eigen::TopLeft,3,1);
goalThisIter.thread = new Thread(goal.thread->length(), startPos, startTan);
goalThisIter.thread->minimize_energy_fixedPieces();
goalThisIter.setPoints();
}
void Thread::printThreadInfo()
{
//print out some information
Eigen::Transform3d first_rotation_transform(Eigen::AngleAxisd(_angle_first_rot, _tangents[0]));
int numPieces = 0;
double energy = 0.0;
double twist = 0.0;
double length = 0.0;
ThreadPiece* currPiece = threadList;
Matrix4d trans = _translate_to_start*first_rotation_transform*_transform_to_start;
trans.corner(Eigen::TopLeft,3,3) *= _length;
while (currPiece != NULL)
{
numPieces++;
energy += currPiece->_length*(currPiece->_torsion*currPiece->_torsion + currPiece->_curvature*currPiece->_curvature);
//twist += currPiece->_length*currPiece->_torsion;
length += currPiece->_length;
std::cout << "length:" << currPiece->_length << std::endl;
std::cout << "curvature: " << currPiece->_curvature/_length << " torsion: " << currPiece->_torsion/_length << std::endl;
trans *= currPiece->_transform;
currPiece = currPiece->_next_segment;
}
std::cout << "Optimal thread has " << numPieces << " pieces, and a calculated energy of " << energy*_length << std::endl;
std::cout << "Final Position: \n" << trans.corner(Eigen::TopRight, 3,1) << std::endl;
std::cout << "Final Position Wanted: \n" << _positions[1] << std::endl;
std::cout << "Final Tangent: \n" << trans.corner(Eigen::TopLeft, 3, 1)/_length << std::endl;
}
void Thread::getActualEndTangent(Vector3d& endTan)
{
Matrix4d currTrans;
getStartTransform(currTrans);
currTrans.corner(Eigen::TopLeft,3,3) *= _length;
ThreadPiece* currPiece = threadList;
while (currPiece != NULL)
{
currTrans *= currPiece->_transform;
currPiece = currPiece->_next_segment;
}
endTan = currTrans.corner(Eigen::TopLeft,3,1);
}
void Thread::getPoints(MatrixXd& points)
{
Eigen::Transform3d first_rotation_transform(Eigen::AngleAxisd(_angle_first_rot, _tangents[0]));
Matrix4d trans = _translate_to_start*first_rotation_transform*_transform_to_start;
trans.corner(Eigen::TopLeft,3,3) *= _length;
threadList->getPoints(points, 0.0, 1.0/((double)(points.rows()-1)), 0, trans);
}
Matrix4d rotateZ(double _angle)
{
Matrix4d m = Matrix4d::Identity();
Matrix3d r;
r = AngleAxisd(_angle, Vector3d::UnitZ());
m.corner(TopLeft, 3, 3) = r;
return m;
}
void Thread::optimizeManyPoints(MatrixXd& constraints, int num_pts_between, vector<double>&curvatures_resampled, vector<double>& torsions_resampled, Matrix4d& start_transform)
{
int num_constraints = constraints.rows()-1;
int num_pieces_reopt = num_pts_between*num_constraints;
double length_per_piece = 1.0/( num_pieces_reopt);
//opt_params_many_points.transform_back = _transform_to_start;
opt_params_many_points.num_segments = num_pieces_reopt;
opt_params_many_points.length_per_segment = length_per_piece;
opt_params_many_points.points.resize(num_constraints);
opt_params_many_points.orig_params_each_piece = new NEWMAT::ColumnVector(2*num_pieces_reopt+3);
opt_params_many_points.num_pts_between = num_pts_between;
opt_params_many_points.gravity_multipler = (GRAVITY_CONSTANT)*_length;
opt_params_many_points.total_length = _length;
for (int i=0; i < num_constraints; i++)
{
for (int j=0; j < num_pts_between; j++)
{
int piece_num = i*num_pts_between+j;
opt_params_many_points.orig_params_each_piece->element(2*piece_num) = curvatures_resampled[i];
opt_params_many_points.orig_params_each_piece->element(2*piece_num+1) = torsions_resampled[i];
}
}
double eulerZ, eulerY, eulerX;
eulerAnglesFramTransform(start_transform.corner(Eigen::TopLeft,3,3), eulerZ, eulerY, eulerX);
opt_params_many_points.orig_params_each_piece->element(2*num_pieces_reopt) = eulerZ;
opt_params_many_points.orig_params_each_piece->element(2*num_pieces_reopt+1) = eulerY;
opt_params_many_points.orig_params_each_piece->element(2*num_pieces_reopt+2) = eulerX;
for (int i=0; i < num_constraints; i++)
{
opt_params_many_points.points[i] = ((constraints.block((i+1),0,1,3) - constraints.block(0,0,1,3))/_length).transpose();
}
NEWMAT::ColumnVector x_sol;
optimize_FDNLF(2*opt_params_many_points.num_segments+3, energyEvalFunctionManyPoints, energyEvalFunctionManyPoints_init, x_sol);
std::cout << "done re-optimizing" << std::endl;
delete opt_params_many_points.orig_params_each_piece;
//set thread params
_positions[0] = constraints.block(0,0,1,3).transpose();
transformFromEulerAngles(_transform_to_start, x_sol(2*num_pieces_reopt+1), x_sol(2*num_pieces_reopt+2));
_translate_to_start = Matrix4d::Identity();
_translate_to_start.corner(Eigen::TopRight,3,1) = _positions[0];
_tangents[0] = _transform_to_start.corner(Eigen::TopLeft,3,1);
//_tangents[0] = orig_thread->_tangents[0];
_angle_first_rot = x_sol(2*num_pieces_reopt+3);
inverseTransform(_transform_to_start, _transform_to_unit);
Matrix4d transform_back;
getStartTransform(transform_back);
transform_back.corner(Eigen::TopLeft,3,3) *= _length;
threadList = new ThreadPiece(x_sol(1), x_sol(2), length_per_piece);
ThreadPiece* currPiece = threadList;
for (int i=1; i < num_pieces_reopt; i++)
{
currPiece->addSegmentAfter(new ThreadPiece(x_sol(2*i+1), x_sol(2*i+2), length_per_piece));
transform_back *= currPiece->_transform;
currPiece = currPiece->_next_segment;
}
//_positions[1] = transform_back.corner(Eigen::TopRight,3,1);
// _positions[1] = orig_thread->_positions[1];
// _tangents[1] = orig_thread->_tangents[1];
// _tangents[1].normalize();
// _tangents[0].normalize();
_positions[1] = transform_back.corner(Eigen::TopRight,3,1);
_tangents[1] = transform_back.corner(Eigen::TopLeft,3,1);
_tangents[1].normalize();
//printThreadInfo();
}
Matrix4d rotationFromMatrix3d(const Matrix3d &_mat)
{
Matrix4d m = Matrix4d::Identity();
m.corner(TopLeft, 3, 3) = _mat;
return m;
}
void Thread_RRT::setThisGoalNearGoal(RRT_Node& goal, RRT_Node& goalThisIter)
{
//sample something in sphere between current point and goal
//first, set the things we know
Matrix4d startTrans;
goal.thread->getStartTransform(startTrans);
Vector3d posNewThread[2];
Vector3d tanNewThread[2];
double lengthNewThread[2];
double length_thread = goal.thread->length();
lengthNewThread[0] = lengthNewThread[1] = length_thread/2.0;
posNewThread[0] = startTrans.corner(Eigen::TopRight,3,1);
tanNewThread[0] = startTrans.corner(Eigen::TopLeft,3,1);
Vector3d endPos;
goal.thread->getWantedEndPosition(endPos);
Vector3d endTan;
goal.thread->getWantedEndTangent(endTan);
//random params for initializing
double curvatureNewThread[2] = {randomNumUnit()*RANDOM_THREAD_MAX_CURVATURE_INIT, randomNumUnit()*RANDOM_THREAD_MAX_CURVATURE_INIT};
double torsionNewThread[2] = {randomNumUnit()*RANDOM_THREAD_MAX_TORSION_INIT, randomNumUnit()*RANDOM_THREAD_MAX_TORSION_INIT};
//find closest thread to goal
int indClosest;
findClosestNode(goal, indClosest);
Vector3d endPosClosestToGoal;
currNodes[indClosest]->this_node.thread->getWantedEndPosition(endPosClosestToGoal);
Vector3d endTanClosestToGoal;
currNodes[indClosest]->this_node.thread->getWantedEndTangent(endTanClosestToGoal);
//calculate the distance to this end
//sample from this radius, add to end point
double randX, randY, randZ, sampledRadius;
double radius_pos = (endPos - endPosClosestToGoal).norm() + ADD_TO_GOAL_DIST_RADIUS;
double dist_new_thread;
do
{
//find point in sphere with this radius, add to goal endpoint
//std::cout << "sampling end point" << std::endl;
double sampledRadius;
do
{
//std::cout << "sampling circle" << std::endl;
randX = randomMaxAbsValue(radius_pos);
randY = randomMaxAbsValue(radius_pos);
randZ = randomMaxAbsValue(radius_pos);
sampledRadius = randX*randX + randY*randY + randZ*randZ;
} while (sampledRadius >= (radius_pos*radius_pos));
posNewThread[1](0) = randX+endPos(0);
posNewThread[1](1) = randY+endPos(1);
posNewThread[1](2) = randZ+endPos(2);
dist_new_thread = (posNewThread[1]-posNewThread[0]).norm();
} while (dist_new_thread >= length_thread);
//sample from a tan around the goal tan
//sample from outside of sphere with radius equal to length between goal tan and closest tan
//normalize the length of that new tan
double radius_tan = (endTan - endTanClosestToGoal).norm() + ADD_TO_GOAL_TAN_RADIUS;
double randTheta = M_PI*randomNumUnit();
double randPhi = 2.0*M_PI*randomNumUnit();
tanNewThread[1](0) = endTan(0)+radius_tan*cos(randTheta)*sin(randPhi);
tanNewThread[1](1) = endTan(1)+radius_tan*sin(randTheta)*sin(randPhi);
tanNewThread[1](2) = endTan(2)+radius_tan*cos(randPhi);
tanNewThread[1].normalize();
// std::cout << "goal end pos: " << endPos << std::endl;
// std::cout << "goal end tan: " << endTan << std::endl;
// std::cout << "new end pos: " << posNewThread[1] << std::endl;
// std::cout << "new end tan: " << tanNewThread[1] << std::endl;
goalThisIter.thread = new Thread(goal.thread);
goalThisIter.thread->setEndConstraint(posNewThread[1], tanNewThread[1]);
// goalThisIter.thread->upsampleAndOptimize_minLength(0.065);
goalThisIter.thread->minimize_energy_fixedPieces();
//goalThisIter.thread = new Thread(curvatureNewThread, torsionNewThread, lengthNewThread, 2, posNewThread, tanNewThread);
//goalThisIter.thread = new Thread(length_thread, posNewThread[0], tanNewThread[0]);
//goalThisIter.thread->minimize_energy_fixedPieces();
goalThisIter.setPoints();
double distToGoal = goal.distanceToNode(goalThisIter);
//std::cout << "distance to actual goal: " << distToGoal << std::endl;
}
