Commit 4e3d98f9 authored by simalpha's avatar simalpha
Browse files

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<typename T>
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<char> const& facet)
{
......
#include "aicp_ros/talker_ros.hpp"
//#include <eigen_conversions/eigen_msg.h>
#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!
Please register or to comment