Commit 4e3d98f9 by simalpha

### Towards fixing orientation on way back (about z-axis).

parent 365f3547
 ... ... @@ -22,6 +22,10 @@ //convert formats in Eigen Eigen::Isometry3d fromMatrix4fToIsometry3d(Eigen::Matrix4f matrix); //compute angle between two vecotrs (returns degrees) double angleBetweenVectors2d(Eigen::Vector2d& v1, Eigen::Vector2d& v2); //swapping two values. template inline bool swap_if_gt(T& a, T& b) { ... ...
 ... ... @@ -22,6 +22,19 @@ Eigen::Isometry3d fromMatrix4fToIsometry3d(Eigen::Matrix4f matrix){ return isometry; } double angleBetweenVectors2d(Eigen::Vector2d& v1, Eigen::Vector2d& v2){ double numerator = (v1.transpose() * v2); std::cout << "numerator: " << numerator << std::endl; double denominator = (v1.norm() * v2.norm()); std::cout << "denominator: " << denominator << std::endl; double result = acos(numerator / denominator) * 180.0 / M_PI; std::cout << "result: " << result << std::endl; return result; } //extract integers from a string. std::string extract_ints(std::ctype_base::mask category, std::string str, std::ctype const& facet) { ... ...
 #include "aicp_ros/talker_ros.hpp" //#include #include "aicp_utils/common.hpp" namespace aicp { ... ... @@ -43,14 +42,37 @@ void ROSTalker::publishFootstepPlan(PathPoses& path, void ROSTalker::reversePath(PathPoses& path){ // Eigen::Vector3d v1; // v1 << 1, 0, 0; // Eigen::Vector3d v2; // v2 << 1, 0, 0; // Eigen::Matrix3d rotx; // rotx = Eigen::AngleAxisd(0*M_PI/180, Eigen::Vector3d::UnitX()) // * Eigen::AngleAxisd(0*M_PI/180, Eigen::Vector3d::UnitY()) // * Eigen::AngleAxisd(30*M_PI/180, Eigen::Vector3d::UnitZ()); // v2 = rotx * v2; // double res = angleBetweenVectors2d(v1, v2); // std::cout << "res: " << res << std::endl; std::reverse(path.begin(),path.end()); for (size_t i = 0; i < path.size(); ++i){ // Turn 90 degrees about z-axis double angle = 0.0; if (i != path.size()-1) { Eigen::Vector2d v1; v1 = path[i].matrix().block<2,1>(0,0); Eigen::Vector2d v2; v2 = path[i+1].matrix().block<2,1>(0,0) - v1; angle = angleBetweenVectors2d(v1, v2); } // Turn about z-axis Eigen::Matrix3d rot; rot = Eigen::AngleAxisd(0*M_PI/180, Eigen::Vector3d::UnitX()) * Eigen::AngleAxisd(0*M_PI/180, Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(180*M_PI/180, Eigen::Vector3d::UnitZ()); * Eigen::AngleAxisd(angle*M_PI/180, Eigen::Vector3d::UnitZ()); path[i].linear() = rot * path[i].rotation(); } ... ...
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!