search

dqrobotics / cpp

The DQ Robotics library in C++
https://dqrobotics.github.io
GNU Lesser General Public License v3.0
39 stars 13 forks source link

[QUESTION] Eigen UnalignedArrayAssert because of DQ_SerialManipulator during runtime on development PPA #22

Closed marcos-pereira closed 5 years ago

marcos-pereira commented 5 years ago

Bug description

To Reproduce

Code

robot_interface_node.cpp

...
#include <dqrobotics/DQ.h>
#include <dqrobotics/robot_modeling/DQ_Kinematics.h>
#include <dqrobotics/robots/KukaLw4Robot.h>
...
ROS_INFO_STREAM("TEST");
DQ_SerialManipulator robot_kinematics = KukaLw4Robot::kinematics();
ROS_INFO_STREAM("TEST");

CMakeLists.txt

cmake_minimum_required(VERSION 2.8.3)
project(robot_interface)

## Compile as C++11, supported in ROS Kinetic and newer
add_compile_options(-std=c++14)
...
## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
# include
  ${catkin_INCLUDE_DIRS}
)
...
## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
add_executable(${PROJECT_NAME}_node src/robot_interface_node.cpp)

## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
## target back to the shorter version for ease of user use
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")

## Add cmake target dependencies of the executable
## same as for the library above
 add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

## Specify libraries to link a library or executable target against
# target_link_libraries(${PROJECT_NAME}_node
#   ${catkin_LIBRARIES}
# )
target_link_libraries(${PROJECT_NAME}_node
                     ${catkin_LIBRARIES}
                     -ldqrobotics
)

Output rosrun robot_interface robot_interface_node

[ INFO] [1563287024.186883840]: TEST
robot_interface_node: /usr/local/include/Eigen/src/Core/DenseStorage.h:109: Eigen::internal::plain_array<T, Size, MatrixOrArrayOptions, 16>::plain_array() [with T = double; int Size = 8; int MatrixOrArrayOptions = 0]: Assertion `(internal::UIntPtr(eigen_unaligned_array_assert_workaround_gcc47(array)) & (15)) == 0 && "this assertion is explained here: " "http://eigen.tuxfamily.org/dox-devel/group__TopicUnalignedArrayAssert.html" " **** READ THIS WEB PAGE !!! ****"' failed.
Aborted (core dumped)

Expected behavior

Expected output

[ INFO] [1563287024.186883840]: TEST
[ INFO] [1563287024.186883840]: TEST

Environment:

Additional context

Reading symbols from ./devel/lib/robot_interface/robot_interface_node...(no debugging symbols found)...done.

(gdb) run

Starting program: /home/marcos/git/mastermsp/manipulation-planning-v2/catkin_ws/devel/lib/robot_interface/robot_interface_node 
[Thread debugging using libthread_db enabled]
Using host libthread_db library "/lib/x86_64-linux-gnu/libthread_db.so.1".
[New Thread 0x7ffff0019700 (LWP 25634)]
[New Thread 0x7fffef818700 (LWP 25635)]
[New Thread 0x7fffef017700 (LWP 25636)]
[New Thread 0x7fffee816700 (LWP 25637)]
[ WARN] [1563292402.495467043]: Robot interface initializing...
[ INFO] [1563292402.500789607]: TESTE
robot_interface_node: /usr/local/include/Eigen/src/Core/DenseStorage.h:109: Eigen::internal::plain_array<T, Size, MatrixOrArrayOptions, 16>::plain_array() [with T = double; int Size = 8; int MatrixOrArrayOptions = 0]: Assertion `(internal::UIntPtr(eigen_unaligned_array_assert_workaround_gcc47(array)) & (15)) == 0 && "this assertion is explained here: " "http://eigen.tuxfamily.org/dox-devel/group__TopicUnalignedArrayAssert.html" " **** READ THIS WEB PAGE !!! ****"' failed.

Thread 1 "robot_interface" received signal SIGABRT, Aborted.
__GI_raise (sig=sig@entry=6) at ../sysdeps/unix/sysv/linux/raise.c:51
51  ../sysdeps/unix/sysv/linux/raise.c: No such file or directory.

(gdb) bt

#0  __GI_raise (sig=sig@entry=6) at ../sysdeps/unix/sysv/linux/raise.c:51
#1  0x00007ffff5c6d801 in __GI_abort () at abort.c:79
#2  0x00007ffff5c5d39a in __assert_fail_base (fmt=0x7ffff5de47d8 "%s%s%s:%u: %s%sAssertion `%s' failed.\n%n", 
    assertion=assertion@entry=0x7ffff7aa5910 "(internal::UIntPtr(eigen_unaligned_array_assert_workaround_gcc47(array)) & (15)) == 0 && \"this assertion is explained here: \" \"http://eigen.tuxfamily.org/dox-devel/group__TopicUnalignedArrayAssert.htm"..., 
    file=file@entry=0x7ffff7aa58d8 "/usr/local/include/Eigen/src/Core/DenseStorage.h", line=line@entry=109, 
    function=function@entry=0x7ffff7aa5b20 <Eigen::internal::plain_array<double, 8, 0, 16>::plain_array()::__PRETTY_FUNCTION__> "Eigen::internal::plain_array<T, Size, MatrixOrArrayOptions, 16>::plain_array() [with T = double; int Size = 8; int MatrixOrArrayOptions = 0]")
    at assert.c:92
#3  0x00007ffff5c5d412 in __GI___assert_fail (
    assertion=0x7ffff7aa5910 "(internal::UIntPtr(eigen_unaligned_array_assert_workaround_gcc47(array)) & (15)) == 0 && \"this assertion is explained here: \" \"http://eigen.tuxfamily.org/dox-devel/group__TopicUnalignedArrayAssert.htm"..., 
    file=0x7ffff7aa58d8 "/usr/local/include/Eigen/src/Core/DenseStorage.h", line=109, 
    function=0x7ffff7aa5b20 <Eigen::internal::plain_array<double, 8, 0, 16>::plain_array()::__PRETTY_FUNCTION__> "Eigen::internal::plain_array<T, Size, MatrixOrArrayOptions, 16>::plain_array() [with T = double; int Size = 8; int MatrixOrArrayOptions = 0]") at assert.c:101
#4  0x00007ffff79cf0b9 in Eigen::internal::plain_array<double, 8, 0, 16>::plain_array() ()
   from /home/marcos/git/mastermsp/manipulation-planning-v2/catkin_ws/devel/lib/libdq_robotics.so
#5  0x00007ffff79ce790 in Eigen::DenseStorage<double, 8, 8, 1, 0>::DenseStorage() ()
   from /home/marcos/git/mastermsp/manipulation-planning-v2/catkin_ws/devel/lib/libdq_robotics.so
#6  0x00007ffff79cd9da in Eigen::PlainObjectBase<Eigen::Matrix<double, 8, 1, 0, 8, 1> >::PlainObjectBase() ()
   from /home/marcos/git/mastermsp/manipulation-planning-v2/catkin_ws/devel/lib/libdq_robotics.so
#7  0x00007ffff79cd240 in Eigen::Matrix<double, 8, 1, 0, 8, 1>::Matrix() ()
   from /home/marcos/git/mastermsp/manipulation-planning-v2/catkin_ws/devel/lib/libdq_robotics.so
#8  0x00007ffff79c0dfa in DQ_robotics::DQ::DQ(double const&, double const&, double const&, double const&, double const&, double const&, double const&, double const&) () from /home/marcos/git/mastermsp/manipulation-planning-v2/catkin_ws/devel/lib/libdq_robotics.so
---Type <return> to continue, or q <return> to quit---
#9  0x00007ffff6aed704 in DQ_robotics::DQ_Kinematics::DQ_Kinematics (this=0x7fffffffcf50)
    at /usr/src/dqrobotics/robot_modeling/DQ_Kinematics.cpp:32
#10 0x00007ffff6af4439 in DQ_robotics::DQ_SerialManipulator::DQ_SerialManipulator (this=0x7fffffffcf50, dh_matrix=..., 
    convention="standard") at /usr/src/dqrobotics/robot_modeling/DQ_SerialManipulator.cpp:51
#11 0x00007ffff6afe909 in DQ_robotics::KukaLw4Robot::kinematics () at /usr/src/dqrobotics/robots/KukaLw4Robot.cpp:41
#12 0x00005555555a6b58 in ControllingNode::ROSControlLoop(int) ()
#13 0x00005555555ae4f0 in main ()

I followed the instructions on how to run the cpp-examples and the performance evaluation example worked without errors. It also uses the DQ_SerialManipulator. However, as you know, it includes the DQ header from the source code and not from the PPA.

mmmarinho commented 5 years ago

Hello, @marcos-pereira.

Thanks for the report.

I couldn't replicate the issue on my computer so I think that maybe it is some ROS Package configuration issue on your side.

Can you post the package.xml?

PS: Also give me some info on your computer if you can. Is it a regular 64bit architecture or something else?

PPS: Please add the whole code, I also need to see all the includes you're using

...
#include <dqrobotics/DQ.h>
#include <dqrobotics/robot_modeling/DQ_Kinematics.h>
#include <dqrobotics/robots/KukaLw4Robot.h>
...
ROS_INFO_STREAM("TEST");
DQ_SerialManipulator robot_kinematics = KukaLw4Robot::kinematics();
ROS_INFO_STREAM("TEST");

For example, at least you have

#include<ros/ros.h>

That you didn't mention in the initial report.

marcos-pereira commented 5 years ago

Hello @mmmarinho .

Thanks for the answer.

The following package.xml is being used:

<?xml version="1.0"?>
<package format="2">
  <name>robot_interface</name>
  <version>0.0.0</version>
  <description>The robot_interface package</description>

  <!-- One maintainer tag required, multiple allowed, one person per tag -->
  <!-- Example:  -->
  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
  <maintainer email="marcos@todo.todo">marcos</maintainer>

  <!-- One license tag required, multiple allowed, one license per tag -->
  <!-- Commonly used license strings: -->
  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
  <license>TODO</license>

  <!-- Url tags are optional, but multiple are allowed, one per tag -->
  <!-- Optional attribute type can be: website, bugtracker, or repository -->
  <!-- Example: -->
  <!-- <url type="website">http://wiki.ros.org/robot_interface</url> -->

  <!-- Author tags are optional, multiple are allowed, one per tag -->
  <!-- Authors do not have to be maintainers, but could be -->
  <!-- Example: -->
  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->

  <!-- The *depend tags are used to specify dependencies -->
  <!-- Dependencies can be catkin packages or system dependencies -->
  <!-- Examples: -->
  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
  <!--   <depend>roscpp</depend> -->
  <!--   Note that this is equivalent to the following: -->
  <!--   <build_depend>roscpp</build_depend> -->
  <!--   <exec_depend>roscpp</exec_depend> -->
  <!-- Use build_depend for packages you need at compile time: -->
  <!--   <build_depend>message_generation</build_depend> -->
  <!-- Use build_export_depend for packages you need in order to build against this package: -->
  <!--   <build_export_depend>message_generation</build_export_depend> -->
  <!-- Use buildtool_depend for build tool packages: -->
  <!--   <buildtool_depend>catkin</buildtool_depend> -->
  <!-- Use exec_depend for packages you need at runtime: -->
  <!--   <exec_depend>message_runtime</exec_depend> -->
  <!-- Use test_depend for packages you need only for testing: -->
  <!--   <test_depend>gtest</test_depend> -->
  <!-- Use doc_depend for packages you need only for building documentation: -->
  <!--   <doc_depend>doxygen</doc_depend> -->
  <buildtool_depend>catkin</buildtool_depend>
  <build_depend>dq_robotics</build_depend>
  <build_depend>geometry_msgs</build_depend>
  <build_depend>ompl</build_depend>
  <build_depend>roscpp</build_depend>
  <build_depend>rospy</build_depend>
  <build_depend>sensor_msgs</build_depend>
  <build_depend>std_msgs</build_depend>
  <build_depend>vrep_objects_pose_msgs</build_depend>
  <build_export_depend>dq_robotics</build_export_depend>
  <build_export_depend>geometry_msgs</build_export_depend>
  <build_export_depend>ompl</build_export_depend>
  <build_export_depend>roscpp</build_export_depend>
  <build_export_depend>rospy</build_export_depend>
  <build_export_depend>sensor_msgs</build_export_depend>
  <build_export_depend>std_msgs</build_export_depend>
  <build_export_depend>vrep_objects_pose_msgs</build_export_depend>
  <exec_depend>dq_robotics</exec_depend>
  <exec_depend>geometry_msgs</exec_depend>
  <exec_depend>ompl</exec_depend>
  <exec_depend>roscpp</exec_depend>
  <exec_depend>rospy</exec_depend>
  <exec_depend>sensor_msgs</exec_depend>
  <exec_depend>std_msgs</exec_depend>
  <exec_depend>vrep_objects_pose_msgs</exec_depend>

  <!-- The export tag contains other, unspecified, tags -->
  <export>
    <!-- Other tools can request additional information be placed here -->

  </export>
</package>

I am using some DH parameters from an older version of DQ robotics. That's why dq_robotics is included. I have already tried removing them, but the unaligned array assert still happens.

Code robot_node_interface.hpp

// Random number generator
#include <random>
#include <iostream>

// Print data to file
#include <sstream>
#include <fstream>

// Eigen
// #include <Eigen/Dense>
// #include <eigen3/Eigen/Dense>
// #include<eigen3/Eigen/StdVector>

//ROS
#include <ros/ros.h>

//Messages
#include <sensor_msgs/JointState.h>
#include <geometry_msgs/Wrench.h> // Meka robot force sensor
#include <geometry_msgs/Point.h> // Object position
#include <geometry_msgs/Quaternion.h> // Object orientation
#include <std_msgs/Int32.h> // to hold collision_count and gripper command
#include <vrep_objects_pose_msgs/ObjectsPose.h> // to hold objects

#include <dqrobotics/DQ.h>
#include <dqrobotics/robot_modeling/DQ_Kinematics.h>
#include <dqrobotics/robots/KukaLw4Robot.h>
#include <dqrobotics/robot_modeling/DQ_SerialManipulator.h>

// I already tried to comment or uncomment this, but the Eigen unaligned array assert still happens
// #include<dq_robotics/robot_dh/Kuka_LBR4p_VREP.h>

//C++ Libs
#include<cmath>
#include<signal.h>

// Convert string to double: atof
#include <stdlib.h>

#include "ProportionalController.hpp"
#include "Robot.hpp"
#include "Planner.hpp"

using namespace DQ_robotics;

ProportionalController.hpp

#include <dqrobotics/DQ.h>
#include <dqrobotics/robot_modeling/DQ_Kinematics.h>
#include <dqrobotics/robot_modeling/DQ_SerialManipulator.h>

Robot.hpp

//Eigen
#include <eigen3/Eigen/Dense>
using namespace Eigen;

Planner.hpp

// Standard libraries
#include <iostream>

// Boost Graph Library
#include <utility>                   // for std::pair
#include <algorithm>                 // for std::for_each
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/copy.hpp>

// Cartesian product graph
#include <boost/graph/graph_utility.hpp>
#include <boost/graph/connected_components.hpp>
#include <boost/config.hpp>

using namespace boost;

If you need more information, I can also provide the .cpp code, but they are longer. Nonetheless, these are all the includes I am using.

mmmarinho commented 5 years ago

Hello, @marcos-pereira,

Thanks for the report. This is most likely a conflict with the dq_robotics ROS package that you are adding to your manifest. It might be also a conflict between two different Eigen versions. A big indication of this is that the assertion error is coming from the dq_robotics being built in your code

/home/marcos/git/mastermsp/manipulation-planning-v2/catkin_ws/devel/lib/libdq_robotics.so

and not the one you get from the PPA.

To test if my hypothesis is correct, run the following script in a terminal (you can copy and paste it)

mkdir issue22
cd issue22
cat <<EOT >> issue22.cpp
#include<iostream>
#include<dqrobotics/robot_modeling/DQ_SerialManipulator.h>
#include<dqrobotics/robots/KukaLw4Robot.h>

using namespace DQ_robotics;

int main(void)
{

DQ_SerialManipulator robot;
DQ_SerialManipulator kuka = KukaLw4Robot::kinematics();
std::cout << "Number of links = " << kuka.get_dim_configuration_space() << std::endl;

return 0;
}
EOT
g++ -o issue22 issue22.cpp -ldqrobotics
./issue22

This will create a minimal example of the DQ_SerialManipulator in the folder issue22, cd to it, compile, link with the dqrobotics library, and execute the compiled program.

If the program compiles correctly, you'll get the following output:

Number of links = 7

Supposing that everything goes well with this test, the way to solve your problem is unrelated to the DQ_Robotics library and, instead, related to the way your ROS code is being managed. You have to be sure that only one version of DQ_Robotics and only one version of Eigen3 are being compiled in the same scope.

Note that catkin_make will include the package even if you remove the #include<> in your code, and that can mess up the whole compilation pipeline. The only way you can be sure there aren't any conflicts stemming from the dq_robotics ROS package is to remove it from the package.xml AND fully clean your workspace (by deleting the devel folder).

Kind regards, Murilo

marcos-pereira commented 5 years ago

Hello @mmmarinho.

Thank you for the instructions.

I ran the script you sent. It compiled correctly and the result was the expected output.

I did what you said and removed everything related to the old dq_robotics from the CMakeLists.txt and package.xml. Now it works!

Many thanks, Marcos

mmmarinho commented 5 years ago

Great!