Repository: stack-of-tasks/tsid (language: python)

Issue:
sync submodule
To remove some ROS warnings,
ref https://github.com/jrl-umi3218/jrl-cmakemodules/pull/559
Alignment error with -march=native, fixed with c++17
Hi

I am trying to get more performance by compiling with `-march=native` to get Eigen vectorization.

I am compiling with clang for now:
` CC=clang CXX=clang++ CXXFLAGS="-march=native" cmake .. -DCMAKE_PREFIX_PATH=/home/pal/install -DCMAKE_BUILD_TYPE=Debug -DCMAKE_INSTALL_PREFIX=/home/pal/install -DBUILD_PYTHON_INTERFACE=OFF`

=> the test tsid-formulation fails

```

~/tsid/build > tests/tsid-formulation  
Running 4 test cases...

*** test_invdyn_formulation_acc_force_remove_contact ***
q: 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
tsid-formulation: /usr/include/eigen3/Eigen/src/Core/DenseStorage.h:128: Eigen::internal::plain_array<double, 36, 0, 32>::plain_array() [T = double, Size = 36, MatrixOrArrayOptions = 0, Alignment = 32]: Assertion `(internal::UIntPtr(array) & (31)) == 0 && "this assertion is explained here: " "http://eigen.tuxfamily.org/dox-devel/group__TopicUnalignedArrayAssert.html" " **** READ THIS WEB PAGE !!! ****"' failed.
unknown location(0): fatal error: in "tsid_formulationTest/test_invdyn_formulation_acc_force_remove_contact": signal: SIGABRT (application abort requested)
/home/pal/tsid/tests/tsid-formulation.cpp(128): last checkpoint

*** test_invdyn_formulation_acc_force ***
q: 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
tsid-formulation: /usr/include/eigen3/Eigen/src/Core/DenseStorage.h:128: Eigen::internal::plain_array<double, 36, 0, 32>::plain_array() [T = double, Size = 36, MatrixOrArrayOptions = 0, Alignment = 32]: Assertion `(internal::UIntPtr(array) & (31)) == 0 && "this assertion is explained here: " "http://eigen.tuxfamily.org/dox-devel/group__TopicUnalignedArrayAssert.html" " **** READ THIS WEB PAGE !!! ****"' failed.
unknown location(0): fatal error: in "tsid_formulationTest/test_invdyn_formulation_acc_force": signal: SIGABRT (application abort requested)
/home/pal/tsid/tests/tsid-formulation.cpp(128): last checkpoint

*** test_contact_point_invdyn_formulation_acc_force ***
tsid-formulation: /usr/include/eigen3/Eigen/src/Core/DenseStorage.h:128: Eigen::internal::plain_array<double, 36, 0, 32>::plain_array() [T = double, Size = 36, MatrixOrArrayOptions = 0, Alignment = 32]: Assertion `(internal::UIntPtr(array) & (31)) == 0 && "this assertion is explained here: " "http://eigen.tuxfamily.org/dox-devel/group__TopicUnalignedArrayAssert.html" " **** READ THIS WEB PAGE !!! ****"' failed.
unknown location(0): fatal error: in "tsid_formulationTest/test_contact_point_invdyn_formulation_acc_force": signal: SIGABRT (application abort requested)
/home/pal/tsid/tests/tsid-formulation.cpp(517): last checkpoint
```

If we follow the link (http://eigen.tuxfamily.org/dox-devel/group__TopicUnalignedArrayAssert.html), one solution is to switch to c++-17, which seems to work. In CMakeList.txt: 
`CHECK_MINIMAL_CXX_STANDARD(17 ENFORCE)`

```
Level 1
 - contact_rfoot_force_reg_task: w=1e-05, equality, 6x61
 - contact_lfoot_force_reg_task: w=1e-05, equality, 6x61
 - task-com: w=1, equality, 3x61
 - task-posture: w=0.01, equality, 31x61

Time 0
  contact_rfoot err: 0  contact_lfoot err: 0    task-com err: 0.1       task-posture err: 0     v=0      dv=0
dv(37x1): [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]

### TEST FINISHED ###
v(37x1): [ 0.0001,    0.03,  -6e-06,   1e-09,  0.0002,   0.004,  -0.004,   -0.05, -0.0006, -0.0001,  0.0005,    0.05,  -0.004,   -0.05,  0.0005, -0.0001, -0.0006,    0.05,   0.007, -0.0009,   0.003,   1e-05,  0.0009,   4e-06,  0.0001,  -1e-05,  0.0002,   1e-05,  -8e-06,  -3e-05,  0.0006,   0.003,   8e-06,  0.0009,   4e-06,  0.0001,   4e-06]
Final   CoM position:   0.0219545 0.000104108    0.665915
Desired CoM position: 0.0219541       0.1  0.665915
[...]
```

Otherwise, maybe there is a `EIGEN_MAKE_ALIGNED_OPERATOR_NEW` that is missing, but I did not find it (yet).

=> should TSID switch to c++-17?





Discussion / hints:

Fix patch:
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 5c6737c9..8a213163 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -69,7 +69,7 @@ if(EIGEN_NO_AUTOMATIC_RESIZING)
   add_definitions(-DEIGEN_NO_AUTOMATIC_RESIZING)
 endif(EIGEN_NO_AUTOMATIC_RESIZING)
 
-check_minimal_cxx_standard(11 ENFORCE)
+check_minimal_cxx_standard(17 ENFORCE)
 
 # Project dependencies
 if(BUILD_PYTHON_INTERFACE)
diff --git a/cmake b/cmake
index a558ff94..b5763ed9 160000
--- a/cmake
+++ b/cmake
@@ -1,1 +1,1 @@
-Subproject commit a558ff948ce65384b3ccd9e5cd9a295a2c0016bc
+Subproject commit b5763ed91fd58343c001e26720253cd79c008d49
diff --git a/demo/demo_quadruped.py b/demo/demo_quadruped.py
index b9618d9b..be79cdcb 100644
--- a/demo/demo_quadruped.py
+++ b/demo/demo_quadruped.py
@@ -1,6 +1,4 @@
-import pinocchio as se3
 import pinocchio as pin
-#from pinocchio import libpinocchio_pywrap as pin
 import tsid
 import numpy as np
 from numpy import nan
@@ -43,12 +41,12 @@
 filename = str(os.path.dirname(os.path.abspath(__file__)))
 path = filename + '/../models'
 urdf = path + '/quadruped/urdf/quadruped.urdf'
-vector = se3.StdVec_StdString()
+vector = pin.StdVec_StdString()
 vector.extend(item for item in path)
-robot = tsid.RobotWrapper(urdf, vector, se3.JointModelFreeFlyer(), False)
+robot = tsid.RobotWrapper(urdf, vector, pin.JointModelFreeFlyer(), False)
 
 # for gepetto viewer
-robot_display = se3.RobotWrapper.BuildFromURDF(urdf, [path, ], se3.JointModelFreeFlyer())
+robot_display = pin.RobotWrapper.BuildFromURDF(urdf, [path, ], pin.JointModelFreeFlyer())
 l = subprocess.getstatusoutput("ps aux |grep 'gepetto-gui'|grep -v 'grep'|wc -l")
 if int(l[1]) == 0:
     os.system('gepetto-gui &')
@@ -60,7 +58,7 @@
 #model = robot.model()
 #pin.loadReferenceConfigurations(model, srdf, False)
 #q = model.referenceConfigurations["half_sitting"]
-#q = se3.getNeutralConfigurationFromSrdf(robot.model(), srdf, False)
+#q = pin.getNeutralConfigurationFromSrdf(robot.model(), srdf, False)
 #q = robot_display.model.neutralConfiguration #np.zeros(robot.nq)
 q = np.zeros(robot.nq)
 q[6] = 1.0
@@ -113,7 +111,7 @@
 q_ref = q[7:]
 trajPosture = tsid.TrajectoryEuclidianConstant("traj_joint", q_ref)
 
-print("Create QP solver with ", invdyn.nVar, " variables, ", invdyn.nEq, 
+print("Create QP solver with ", invdyn.nVar, " variables, ", invdyn.nEq,
       " equality and ", invdyn.nIn, " inequality constraints")
 solver = tsid.SolverHQuadProgFast("qp solver")
 solver.resize(invdyn.nVar, invdyn.nEq, invdyn.nIn)
@@ -176,7 +174,7 @@
 
     v_mean = v + 0.5*dt*dv
     v += dt*dv
-    q = se3.integrate(robot.model(), q, dt*v_mean)
+    q = pin.integrate(robot.model(), q, dt*v_mean)
     t += dt
     
     if i%DISPLAY_N == 0: robot_display.display(q)
diff --git a/package.xml b/package.xml
index 8d67fdae..4f442ae0 100644
--- a/package.xml
+++ b/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>tsid</name>
-  <version>1.6.2</version>
+  <version>1.6.3</version>
   <description>
     Efficient Task Space Inverse Dynamics (TSID) based on Pinocchio
   </description>


Test patch:
diff --git a/models/arm/generator.py b/tests/python/generator.py
similarity index 100%
rename from models/arm/generator.py
rename to tests/python/generator.py
diff --git a/tests/python/test_Tasks.py b/tests/python/test_Tasks.py
index 590895a6..dd94157f 100644
--- a/tests/python/test_Tasks.py
+++ b/tests/python/test_Tasks.py
@@ -262,10 +262,7 @@
 print("")
 
 # Get robot model generator module
-import sys
-filepath = os.path.normpath(os.path.join(os.path.dirname(os.path.abspath(__file__)), '../../'))
-sys.path.append(str(filepath))
-from models.arm.generator import create_7dof_arm
+from generator import create_7dof_arm
 
 # Get robot model
 model, geom_model = create_7dof_arm() # A robot containing sperical joints where nq != nv