Commit 88a5e414 by simalpha

### Set orientation to go back along trajectory line.

parent 4e3d98f9
 ... ... @@ -25,14 +25,7 @@ Eigen::Isometry3d fromMatrix4fToIsometry3d(Eigen::Matrix4f matrix){ 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; return atan2(v1(0)*v2(1) - v1(1)*v2(0), v1(0)*v2(0) + v1(1)*v2(1)) * 180.0 / M_PI; } //extract integers from a string. ... ...
 ... ... @@ -42,29 +42,22 @@ 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){ double angle = 0.0; double angle = 180.0; // Compute orientation along trajectory line if (i != path.size()-1) { // angle between x-axis of current frame Eigen::Vector2d v1; v1 = path[i].matrix().block<2,1>(0,0); // and trajectory line Eigen::Vector2d v2; v2 = path[i+1].matrix().block<2,1>(0,0) - v1; v2 << path[i+1].translation()(0, 0) - path[i].translation()(0, 0), path[i+1].translation()(1, 0) - path[i].translation()(1, 0); angle = angleBetweenVectors2d(v1, v2); } ... ...
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!