Open tstiger opened 6 years ago
bool Bezier::harmonizeLineOrientation(EigenSTL::vector_Affine3d &poses_on_line,
const Eigen::Vector3d &direction_ref)
{
if (poses_on_line.size() <= 1)
return false;
// Compare orientation of lines with reference
Eigen::Vector3d current_line_orientation(poses_on_line.back().translation() - poses_on_line.front().translation());
// If dot product > 0 we don't invert the line
if (direction_ref.dot(current_line_orientation) > 0)
return false;
std::reverse(poses_on_line.begin(), poses_on_line.end());
// We reversed the line order so we need to reverse the axis X/Y of each pose as well
invertXAxisOfPoses(poses_on_line);
return true;
}
beizier_library.cpp line 914
// If dot product > 0 we don't invert the line
if (direction_ref.dot(current_line_orientation) > 0)
return false;
Eigen::Vector3d dot function ,cause data is not aligend.
Beizier_library.hpp
#define EIGEN_DONT_ALIGN
I added this macro, in beizier_library.hpp.
it's work, but
in surfacing.cpp
line 150
// Bezier trajectory
if (group.computeCartesianPath(way_points_msg, 0.05, 0.0, srv.request.trajectory) < 0.95)
{
ROS_ERROR_STREAM(
"Bezier application: A solution could not be found to move the robot along the trajectory, aborting.");
break;
}
case break , ros_error.
dpkg -l |grep libeigen*
ii libeigen3-dev 3.3~beta1-2 all lightweight C++ template library for linear algebra
I'm not sure if std::pair
with Eigen structures requires a specific alignment:
$ grep -rn "std::pair<Eigen"
include/bezier/bezier_library.hpp:79: typedef std::pair<Eigen::Vector3d, Eigen::Vector3d> PointNormal;
include/bezier/params_observer.hpp:117: std::pair<Eigen::Vector3d, Eigen::Vector3d> slicing_orientation_;
This might be the cause of the crash