search

gtrll / gpmp2

Gaussian Process Motion Planner 2
Other
310 stars 97 forks source link

Updating MATLAB Toolbox Wrapping + Integration #54

Open mattking-smith opened 2 years ago

mattking-smith commented 2 years ago

Description

I have been working on using the current GTSAM wrap repository for integrating this toolbox into MATLAB for research purposes. I was able to successfully do this some time ago on another development computer, but the wrap repository has changed since then, and I am now facing a segmentation fault errors in the MATLAB environment, when running a few of the examples, post toolbox wrapping.

Additionally, I hope this issue provides information for new documentation on an up-to-date MATLAB + GPMP2 toolbox wrapping procedure on the README.md.

Steps to reproduce current wrapping method

Wrapping GTSAM to MATLAB Toolbox

  1. Build GTSAM from source, clone or download the latest release from the GTSAM Github repo. Then follow the build & install instructions posted on GTSAM build page.
  2. To flag the MATLAB toolbox for to build in the \home\user\ directory in the folder \toolbox, before the build process, in the terminal, run the command 
    cmake -DGTSAM_TOOLBOX_INSTALL_PATH:PATH= $HOME/toolbox ..`
  3. Install GTSAM by running the command make install
  4. Follow the step given in the MATLAB Wrapper documentation.

Wrapping GPMP2 to MATLAB Toolbox

  1. In the command prompt run 
    git clone https://github.com/gtrll/gpmp2.git
  2. Download and install the current matlab.h from the GTSAM wrap repository into a wrap folder within the gpmp2 folder. 
    sudo mkdir wrap
  3. Noting the MATLAB wrapper vdocumentation in the current GTSAM wrap repository , and after some discussion with the developers of the wrap repository I needed to make edits to the CMakeLists.txt in the main gpmp2 repository. Specifically, I changes the following lines: https://github.com/gtrll/gpmp2/blob/48c41c142df062832ffbe9792f0cbecdb36485cc/CMakeLists.txt#L65-L74 to 
    
# Find current wrap repository
find_package(gtwrap)

Wrapping to MATLAB

if(GPMP2_BUILD_MATLAB_TOOLBOX)

include(MatlabWrap) matlab_wrap(gpmp2.h ${PROJECT_NAME} "gtsam;${PROJECT_NAME}" "" "" "")

install matlab functions and scripts

add_subdirectory(matlab) endif()

4. To flag for installing the MATLAB toolbox and a path for the toolbox to be installed (I have chosen to install in the /home/user/toolbox directory), in the command prompt run

cd gpmp2 && mkdir build && cd build sudo cmake -DGPMP2_BUILD_MATLAB_TOOLBOX:=ON -DWRAP_TOOLBOX_INSTALL_PATH=$HOME/gpmp2_toolbox ..


5. Now executing `sudo make install` in the command prompt resulted initially in the following errors:

In file included from /usr/local/gpmp2/gpmp2/kinematics/GaussianPriorWorkspacePositionArm.h:12, from /usr/local/gpmp2/build/wrap/gpmp2/gpmp2_wrapper.cpp:20: /usr/local/gpmp2/gpmp2/kinematics/GaussianPriorWorkspacePosition.h: In member function ‘gtsam::Vector gpmp2::GaussianPriorWorkspacePosition::evaluateError(const Pose&, boost::optional<Eigen::Matrix<double, -1, -1>&>) const’: /usr/local/gpmp2/gpmp2/kinematics/GaussianPriorWorkspacePosition.h:66:28: error: ‘gtsam::Point3’ {aka ‘class Eigen::Matrix<double, 3, 1>’} has no member named ‘vector’ 66 | return curr_position.vector() - desposition.vector(); | ^~ /usr/local/gpmp2/gpmp2/kinematics/GaussianPriorWorkspacePosition.h:66:53: error: ‘const Point3’ {aka ‘const class Eigen::Matrix<double, 3, 1>’} has no member named ‘vector’ 66 | return curr_position.vector() - desposition.vector(); | ^~ /usr/local/gpmp2/gpmp2/kinematics/GaussianPriorWorkspacePosition.h:69:46: error: ‘const Point3’ {aka ‘const class Eigen::Matrix<double, 3, 1>’} has no member named ‘vector’ 69 | return jointpos[joint].translation().vector() - desposition.vector(); | ^~ /usr/local/gpmp2/gpmp2/kinematics/GaussianPriorWorkspacePosition.h:69:71: error: ‘const Point3’ {aka ‘const class Eigen::Matrix<double, 3, 1>’} has no member named ‘vector’ 69 | return jointpos[joint].translation().vector() - desposition.vector(); | ^~ . . .

make[2]: [CMakeFiles/gpmp2_matlab_wrapper.dir/build.make:79: CMakeFiles/gpmp2_matlab_wrapper.dir/wrap/gpmp2/gpmp2_wrapper.cpp.o] Error 1 make[1]: [CMakeFiles/Makefile2:401: CMakeFiles/gpmp2_matlab_wrapper.dir/all] Error 2 make: *** [Makefile:141: all] Error 2

which I believe is an error related to an open [pull request](https://github.com/gtrll/gpmp2/pull/44). 

Specifically, it appears that Eigen and Pose Tranform have changed in GTSAM and those changes need to be updated in a few key header files. Without specifically without naming all of the header files I changed here in this issues, as I will open a [pull request](https://github.com/gtrll/gpmp2/issues/54) after this issue submission, note that I had to remove the functions `.vector()` and `.print()` in order to successfully compile and wrap.

6. After removing `.vector()` and `.print()` from key header files, again run `sudo make install` in the command prompt. 
7. Note that for my system I was originally having a mex linker problem as discussed on the [GTSAM MATLAB section](https://github.com/borglab/gtsam/blob/develop/matlab/README.md), so in order to satisfy MATLAB when using GTSAM + GPMP2 once wrapped, I run the following at the command line to open MATLAB

export LD_PRELOAD=/usr/lib/x86_64-linux-gnu/libstdc++.so.6; matlab

8. Once in MATLAB make sure to add the file paths to the toolboxes

addpath('/home/user/toolbox'); addpath('/home/user/gpmp2_toolbox');

which should be sufficient for running the toolbox with both GTSAM + GPMP2. 

## Outstanding Issues
Currently I am able to successfully run a handful of examples without issue, like PointRobot2DFactorGraphExample.m, but I am getting a segmentation fault in one crucial function, `arm.forwardKinematicsPosition()` (which is called in many plotting functions likes [PlotPlanarArm.m](https://github.com/gtrll/gpmp2/blob/main/matlab/%2Bgpmp2/plotPlanarArm.m)), is possibly related to the altering of the header files necessary for building, as I was mentioning previously. 

For instance, if I run  `>> Arm3FactorGraphExample` in the MATLAB command prompt, I get the following segmentation error:

MATLAB Log File

      Segmentation violation detected at 2021-12-12 11:30:43 -0500

Configuration: Crash Decoding : Disabled - No sandbox or build area path Crash Mode : continue (default) Default Encoding : UTF-8 Deployed : false Desktop Environment : ubuntu:GNOME GNU C Library : 2.31 stable Graphics Driver : Intel Mesa Intel(R) HD Graphics 5500 (BDW GT2) Version 4.6 (Compatibility Profile) Mesa 21.0.3 Graphics card 1 : Not Started 0x10de ( 0x10de ) 0x1346 Version 0.0.0.0 (0-0-0) Graphics card 2 : 0x8086 ( 0x8086 ) 0x1616 Version 0.0.0.0 (0-0-0) Java Version : Java 1.8.0_202-b08 with Oracle Corporation Java HotSpot(TM) 64-Bit Server VM mixed mode MATLAB Architecture : glnxa64 MATLAB Entitlement ID : 2075860 MATLAB Root : /usr/local/MATLAB/R2021a MATLAB Version : 9.10.0.1684407 (R2021a) Update 3 OpenGL : hardware Operating System : Ubuntu 20.04.3 LTS Process ID : 91269 Processor ID : x86 Family 6 Model 61 Stepping 4, GenuineIntel Session Key : f4ed3dee-2797-450d-91e9-c762785e1bfc Static TLS mitigation : Enabled: Full Window System : The X.Org Foundation (12011000), display :0

Fault Count: 1

Abnormal termination: Segmentation violation

Current Thread: 'MCR 0 interpret' id 140577137805056

Register State (from fault): RAX = 00007fda2bb51ac0 RBX = 00007fdaaa5c6440 RCX = 0000000000000000 RDX = 0000000000000018 RSP = 00007fdaaa5c6078 RBP = 00007fdaaa5c6590 RSI = 00007fda29c821d0 RDI = 00007fda2bb51ac0

R8 = 00007fdaaa5c64a0 R9 = 00007fdaaa5c6480 R10 = 00007fdaaa5c6020 R11 = 0000000000000000 R12 = 0000000000000000 R13 = 00007fdaaa5c6690 R14 = 00007fd9d7d2da70 R15 = 00007fdaaa5c65f0

RIP = 00007fd9cf34a15a EFL = 0000000000010206

CS = 0033 FS = 0000 GS = 0000

Stack Trace (from fault): [ 0] 0x00007fd9cf34a15a /usr/local/lib/libgpmp2.so.0+00528730 _ZN5Eigen8internal21compute_inverse_size4ILi1EdNS_6MatrixIdLi4ELi4ELi0ELi4ELi4EEES3_E3runERKS3RS3+00001354 [ 1] 0x00007fd9cf346586 /usr/local/lib/libgpmp2.so.0+00513414 . . .


The `forwardKinematicsPosition()` is defined in the [ForwardKinematics-inl.h](https://github.com/gtrll/gpmp2/blob/main/gpmp2/kinematics/ForwardKinematics-inl.h), where we can see `.vector()` is used as follows:
https://github.com/gtrll/gpmp2/blob/48c41c142df062832ffbe9792f0cbecdb36485cc/gpmp2/kinematics/ForwardKinematics-inl.h#L25
https://github.com/gtrll/gpmp2/blob/48c41c142df062832ffbe9792f0cbecdb36485cc/gpmp2/kinematics/ForwardKinematics-inl.h#L39
So in my code, I explicitly removed `.vector()` from each of these lines in order to successfully compile and wrap. However now whenever MATLAB tries to execute `arm.forwardKinematicsPosition()` I get a segmentation faults when I get the program gets to a custom wrapper definition generated in the toolbox:
function varargout = forwardKinematicsPosition(this, varargin)
  % FORWARDKINEMATICSPOSITION usage: forwardKinematicsPosition(Pose2Vector jp) : returns Matrix
  % Doxygen can be found at https://gtsam.org/doxygen/
  if length(varargin) == 1 && isa(varargin{1},'gpmp2.Pose2Vector')
    varargout{1} = gpmp2_wrapper(56, this, varargin{:});
    return
  end
  error('Arguments do not match any overload of function gpmp2.Pose2MobileArm.forwardKinematicsPosition');
end

I am sure this is result of editing ForwardKinematics-inl.h](https://github.com/gtrll/gpmp2/blob/main/gpmp2/kinematics/ForwardKinematics-inl.h), but I am not sure what edits need to made to allow me to compile + wrap and work in MATLAB. 

Any suggestions or edits on this matter would be greatly appreciated.

Also if someone could verify this install method, that would be beneficial too.
## Environment
Linux OS: Ubuntu 20.04
MATLAB version: 2021a 
Standard GTSAM + GPMP2 + MATLAB Toolbox build procedure.

## Additional information
I do current have a fully working (i.e. no segmentation faults on any GPMP2 example) development computer which has successfully wrapped and working [GTSAM](https://github.com/borglab/gtsam) + GPMP2 for the MATLAB environment, but that was done some time ago and that code is out of sync with GPMP2. Hence I have not pulled the current repositories onto the development computer and have been testing this MATLAB toolbox integration on a different computer.
mattking-smith commented 2 years ago

Segmentation Fault Update:

After looking at some previously closed issues and comments, (https://github.com/gtrll/gpmp2/issues/29#issuecomment-764590323) I ended up needing to add Eigen::aligned_allocator<Eigen::Matrix4d to keys lines in the Arm.cpp code to get over working on the current forwardKinematics segmentation fault. These changes can be see in the most recent PR push: https://github.com/gtrll/gpmp2/pull/55/commits/9f10f84058c50c3bdb92e7d2ec5b7441a1a0c07d.

Although these changes seemed to have fixed running commands like arm.forwardKinematicsPosition(), I am now finding segmentation faults when calling optimizer.optimize();:

MATLAB Log File: /home/matt/matlab_crash_dump.128916-1

------------------------------------------------
MATLAB Log File
------------------------------------------------ 

--------------------------------------------------------------------------------
                Assertion detected at 2021-12-13 16:38:23 -0500
--------------------------------------------------------------------------------

Configuration:
  Crash Decoding           : Disabled - No sandbox or build area path
  Crash Mode               : continue (default)
  Default Encoding         : UTF-8
  Deployed                 : false
  Desktop Environment      : ubuntu:GNOME
  GNU C Library            : 2.31 stable
  Graphics Driver          : Intel Mesa Intel(R) HD Graphics 5500 (BDW GT2) Version 4.6 (Compatibility Profile) Mesa 21.0.3
  Graphics card 1          : Not Started 0x10de ( 0x10de ) 0x1346 Version 0.0.0.0 (0-0-0)
  Graphics card 2          : 0x8086 ( 0x8086 ) 0x1616 Version 0.0.0.0 (0-0-0)
  Java Version             : Java 1.8.0_202-b08 with Oracle Corporation Java HotSpot(TM) 64-Bit Server VM mixed mode
  MATLAB Architecture      : glnxa64
  MATLAB Entitlement ID    : 2075860
  MATLAB Root              : /usr/local/MATLAB/R2021a
  MATLAB Version           : 9.10.0.1684407 (R2021a) Update 3
  OpenGL                   : hardware
  Operating System         : Ubuntu 20.04.3 LTS
  Process ID               : 128916
  Processor ID             : x86 Family 6 Model 61 Stepping 4, GenuineIntel
  Session Key              : 3e4bec99-ac1d-440c-9c41-4de0e5307aee
  Static TLS mitigation    : Enabled: Full
  Window System            : The X.Org Foundation (12011000), display :0

Fault Count: 1

Assertion in find at management.cpp line 761:
find: no active context for type '(anonymous namespace)::AlreadyReportedFailure'

Current Thread: 'MCR 0 interpret' id 140450581174016

Register State (captured):
  RAX = 00007fbd32ffac90  RBX = 00007fbda020cb68
  RCX = 000000000000000a  RDX = 00007fbd32ffb330
  RSP = 00007fbd32ffab00  RBP = 00007fbd32ffaee0
  RSI = 00007fbda01d452a  RDI = 00007fbd32ffab10

   R8 = 0000000000000000   R9 = 00007fbd32ffade0
  R10 = 000000000000000a  R11 = 00007fbd32ffb330
  R12 = 00007fbda01fcae8  R13 = 00007fbd9f452c63
  R14 = 00007fbd9f453d63  R15 = 00007fbd32ffb7f0

  RIP = 00007fbda017580e  EFL = 0000000000000002

   CS = d139   FS = a0ba   GS = 7fbd

Stack Trace (captured):
[  0] 0x00007fbda016a923    /usr/local/MATLAB/R2021a/bin/glnxa64/libmwfl.so+00387363
[  1] 0x00007fbda016ab1c    /usr/local/MATLAB/R2021a/bin/glnxa64/libmwfl.so+00387868 _ZN10foundation4core4diag15stacktrace_base7captureEm+00000028
[  2] 0x00007fbda016ec45    /usr/local/MATLAB/R2021a/bin/glnxa64/libmwfl.so+00404549
[  3] 0x00007fbda016ed80    /usr/local/MATLAB/R2021a/bin/glnxa64/libmwfl.so+00404864
[  4] 0x00007fbd9f44252e /usr/local/MATLAB/R2021a/bin/glnxa64/libmwfoundation_usm.so+00087342
[  5] 0x00007fbd9f450a48 /usr/local/MATLAB/R2021a/bin/glnxa64/libmwfoundation_usm.so+00145992 _ZN10foundation3usm6DetailINS0_5scope3MvmEE4findEmRKSt9type_info+00000136
[  6] 0x00007fbd8c0b25db /usr/local/MATLAB/R2021a/bin/glnxa64/libmwbridge.so+00275931
[  7] 0x00007fbda0281b69      /usr/local/MATLAB/R2021a/bin/glnxa64/libut.so+00473961 utVprintf+00000185
[  8] 0x00007fbd685829eb     /usr/local/MATLAB/R2021a/bin/glnxa64/libmex.so+00723435 mexPrintf+00000139
[  9] 0x00007fbc6fa5f04d            /home/matt/toolbox/gtsam_wrapper.mexa64+20148301 _ZN7mstream6xsputnEPKcl+00000029
[ 10] 0x00007fbda1704824               /lib/x86_64-linux-gnu/libstdc++.so.6+01259556 _ZSt16__ostream_insertIcSt11char_traitsIcEERSt13basic_ostreamIT_T0_ES6_PKS3_l+00000404
[ 11] 0x00007fbc728b57b3                       /usr/local/lib/libgpmp2.so.0+00518067 _ZNK5gpmp23Arm17forwardKinematicsERKN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEEN5boost8optionalIS5_EERSt6vectorIN5gtsam5Pose3ESaISB_EENS7_IRS9_INS2_IdLi3ELi1ELi0ELi3ELi1EEESaISF_EEEENS7_IRS9_INS2_IdLin1ELin1ELi0ELin1ELin1EEESaISK_EEEESO_SO_+00000147
[ 12] 0x00007fbc72b0dd31      /home/matt/gpmp2_toolbox/gpmp2_wrapper.mexa64+01473841 _ZNK5gpmp210RobotModelINS_3ArmEE13sphereCentersERKN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEERSt6vectorINS4_IdLi3ELi1ELi0ELi3ELi1EEESaIS9_EEN5boost8optionalIRS8_INS4_IdLin1ELin1ELi0ELin1ELin1EEESaISF_EEEE+00000225
[ 13] 0x00007fbc72b1c11b      /home/matt/gpmp2_toolbox/gpmp2_wrapper.mexa64+01532187 _ZNK5gpmp223ObstaclePlanarSDFFactorINS_10RobotModelINS_3ArmEEEE13evaluateErrorERKN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEEN5boost8optionalIRNS6_IdLin1ELin1ELi0ELin1ELin1EEEEE+00000235
[ 14] 0x00007fbc72b4342c      /home/matt/gpmp2_toolbox/gpmp2_wrapper.mexa64+01692716 _ZNK5gtsam17NoiseModelFactor1IN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEEE15unwhitenedErrorERKNS_6ValuesEN5boost8optionalIRSt6vectorINS2_IdLin1ELin1ELi0ELin1ELin1EEESaISB_EEEE+00000124
[ 15] 0x00007fbc726924c3                       /usr/local/lib/libgtsam.so.4+04072643 _ZNK5gtsam16NoiseModelFactor9linearizeERKNS_6ValuesE+00000419
[ 16] 0x00007fbc72693bf4                       /usr/local/lib/libgtsam.so.4+04078580
[ 17] 0x00007fbc72694178                       /usr/local/lib/libgtsam.so.4+04079992
[ 18] 0x00007fbd9e1c55f9   /usr/local/MATLAB/R2021a/bin/glnxa64/libtbb.so.2+00173561
[ 19] 0x00007fbd9e1bf69e   /usr/local/MATLAB/R2021a/bin/glnxa64/libtbb.so.2+00149150
[ 20] 0x00007fbd9e1bd67c   /usr/local/MATLAB/R2021a/bin/glnxa64/libtbb.so.2+00140924
[ 21] 0x00007fbd9e1b8a46   /usr/local/MATLAB/R2021a/bin/glnxa64/libtbb.so.2+00121414
[ 22] 0x00007fbd9e1b89d6   /usr/local/MATLAB/R2021a/bin/glnxa64/libtbb.so.2+00121302
[ 23] 0x00007fbda17be609              /lib/x86_64-linux-gnu/libpthread.so.0+00038409
[ 24] 0x00007fbda14e6293                    /lib/x86_64-linux-gnu/libc.so.6+01188499 clone+00000067

Note that adding print statements into the forwardKinematics() shows that GPMP2 fails at different points when running optimizer.optimize(); in the function forwardKinematics().

mattking-smith commented 2 years ago

I have successfully wrapped GPMP2 into the MATLAB environment.

I was able achieve this by looking at the tag number of the repository which I had originally successfully wrapped the GPMP2 library into on my currently working development computer. I found that the gpmp2.h file had changed between tags 0.3.0 and 0.2.1. So by reverting to the gpmp2.h from tag 0.2.1 to tage 0.2.0 and updating some .m and .h files (which are given in the most recent PR) I am running GPMP2 segmentation free.

mattking-smith commented 2 years ago

I know that reverting to gpmp2.h from tag 0.2.0 is undesirable, so perhaps we could work together to figure out why gpmp2.h from 0.3.0 is causing the segmentation faults @mhmukadam @dongjing3309 @kalyanvasudev.

wbthomason commented 2 years ago

This may be more suitable for a new issue, but I do want to note that the Python wrapper procedure seems to be broken in a similar way, after updates on GTSAM's end.

mattking-smith commented 2 years ago

@wbthomason It is difficult to say if the issue is related or not without opening a new issue.