Commit 88a5e414 authored by simalpha's avatar simalpha
Browse files

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!
Please register or to comment