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