Added a demo reading a simulated trajectory and building a WOLF problem using...

Added a demo reading a simulated trajectory and building a WOLF problem using LAAS multicontact-api and pinocchio libs. Some wrong formulas to fix.

CMakeLists.txt

@@ -33,6 +33,7 @@ CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
 if(COMPILER_SUPPORTS_CXX11)
 		message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has C++11 support.")
     set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
+    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fext-numeric-literals")  # necessary for files using boost 
 elseif(COMPILER_SUPPORTS_CXX0X)
 		message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has C++0x support.")
     set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
@@ -47,8 +48,8 @@ if(UNIX)
 endif(UNIX)
 
 
-#OPTION(BUILD_DOC "Build Documentation" OFF)
 OPTION(BUILD_TESTS "Build Unit tests" ON)
+OPTION(BUILD_DEMOS "Build demos in demos folder, requires multicontact-api and pinocchio packages" OFF)
 #############
 ## Testing ##
 #############
@@ -112,8 +113,6 @@ IF(EXISTS "${WOLF_CONFIG_DIR}" AND NOT IS_DIRECTORY "${WOLF_CONFIG_DIR}")
 ENDIF()
 # Configure config.h
 configure_file(${CMAKE_CURRENT_SOURCE_DIR}/internal/config.h.in "${WOLF_CONFIG_DIR}/config.h")
-message("WOLF CONFIG ${WOLF_CONFIG_DIR}/config.h")
-message("CONFIG DIRECTORY ${PROJECT_BINARY_DIR}")
 include_directories("${PROJECT_BINARY_DIR}/conf")
 
 include_directories("include")

demos/CMakeLists.txt

+
+find_package(PkgConfig REQUIRED) 
+
+pkg_check_modules(MCAPI REQUIRED multicontact-api)
+pkg_check_modules(PINOCCHIO REQUIRED pinocchio)
+
+# Variables to be used
+# message("\n\nMCAPI")
+# message("\n${MCAPI_INCLUDE_DIRS}")
+# message("\n${MCAPI_LIBRARY_DIRS}")
+# message("\n${MCAPI_LIBRARIES}")
+# message("\n\nPINOCCHIO")
+message("\n${PINOCCHIO_INCLUDE_DIRS}")
+# message("\n${PINOCCHIO_LIBRARY_DIRS}")
+# message("\n${PINOCCHIO_LIBRARIES}")
+message("\n${PINOCCHIO_CFLAGS_OTHER}")
+
+# message("${wolf_LIBRARIES}")
+# message("${wolfimu_LIBRARIES}")
+# message("${PLUGIN_NAME}")
+
+# message("\n\n${CMAKE_CXX_FLAGS}")
+
+# SYSTEM disables warnings from library headers
+include_directories(
+    SYSTEM ${MCAPI_INCLUDE_DIRS}
+    SYSTEM ${PINOCCHIO_INCLUDE_DIRS}
+)
+
+link_directories(
+    ${MCAPI_LIBRARY_DIRS}
+    ${PINOCCHIO_LIBRARY_DIRS} 
+)
+
+add_library(mcapi_utils mcapi_utils.cpp)
+
+add_executable(mcapi_povcdl_estimation mcapi_povcdl_estimation.cpp)
+target_link_libraries(mcapi_povcdl_estimation mcapi_utils)
+target_compile_definitions(mcapi_povcdl_estimation PRIVATE ${PINOCCHIO_CFLAGS_OTHER})  # ??
+
+
+
+target_link_libraries(mcapi_povcdl_estimation
+    ${wolf_LIBRARIES}
+    ${wolfimu_LIBRARIES}
+    ${PLUGIN_NAME}
+    ${MCAPI_LIBRARIES}
+    ${PINOCCHIO_LIBRARIES}
+)

demos/mcapi_utils.cpp

+
+
+#include "mcapi_utils.h"
+
+Vector3d acc_from_spatial(const Vector3d& ddr, const Vector3d& w, const Vector3d& dr)
+{
+    return ddr + w.cross(dr);
+}
+
+
+std::vector<Vector3d> contacts_from_footrect_center()
+{
+    double lx = 0.1;
+    double ly = 0.65;
+    std::vector<Vector3d> contacts; contacts.resize(4);
+    contacts[0] = {-lx, -ly, 0};
+    contacts[1] = {-lx,  ly, 0};
+    contacts[2] = { lx, -ly, 0};
+    contacts[3] = { lx,  ly, 0};
+    return contacts;
+}
+
+
+Matrix<double, 6, 1> contact_force_to_wrench(const std::vector<Vector3d>& contacts, 
+                                             const Matrix<double, 12, 1>& w_cforces,
+                                             const Matrix3d& wRl)
+{
+    Vector3d wf = w_cforces.segment<3>(0) + w_cforces.segment<3>(3) + w_cforces.segment<3>(6) + w_cforces.segment<3>(9);
+    Vector3d lf = wRl.inverse() * wf;
+    // torque at point l (center of the foot) in world frame
+    Vector3d w_tau_l = contacts[0].cross(w_cforces.segment<3>(0))
+                     + contacts[1].cross(w_cforces.segment<3>(3))
+                     + contacts[2].cross(w_cforces.segment<3>(6))
+                     + contacts[3].cross(w_cforces.segment<3>(9));
+    Vector3d l_tau_l = wRl.inverse() * w_tau_l;
+    Matrix<double, 6, 1> l_wrench; l_wrench << lf, l_tau_l;
+    return l_wrench;
+}

demos/mcapi_utils.h

+#include "Eigen/Dense"
+
+using namespace Eigen; 
+
+
+/**
+ * \brief Compute a 3D acceleration from 6D spatial acceleration
+ * 
+ * \param ddr spatial acc linear part
+ * \param w spatial acc rotational part
+ * \param dr spatial velocity linear part
+**/
+Vector3d acc_from_spatial(const Vector3d& ddr, const Vector3d& w, const Vector3d& dr);
+
+
+/**
+* \brief Compute the relative contact points from foot center of a Talos foot.
+* 
+* Order is clockwise, starting from bottom left (looking at a forward pointing foot from above).
+* Expressed in local foot coordinates. 
+**/
+std::vector<Vector3d> contacts_from_footrect_center();
+
+
+/**
+* \brief Compute the wrench at the end effector center expressed in world coordinates.
+
+* Each contact force (at a foot for instance) is represented in MAPI as a set of 
+* 4 forces at the corners of the contact polygone (foot -> rectangle). These forces,
+* as the other quantities, are expressed in world coordinates. From this 4 3D forces, 
+* we can compute the 6D wrench at the center of the origin point of the contacts vectors.
+
+* \param contacts std vector of 3D contact forces
+* \param cforces 12D corner forces vector ordered as [fx1, fy1, fz1, fx2, fy2... fz4], same order as contacts
+* \param wRl rotation matrix, orientation from world frame to foot frame
+**/
+Matrix<double, 6, 1> contact_force_to_wrench(const std::vector<Vector3d>& contacts, 
+                                             const Matrix<double, 12, 1>& w_cforces,
+                                             const Matrix3d& wRl);

demos/processor_imu_mcapi_demo.yaml

+type: "ProcessorImu"         # This must match the KEY used in the SensorFactory. Otherwise it is an error.
+name: "Main imu"             # This is ignored. The name provided to the SensorFactory prevails
+time_tolerance: 0.0025         # Time tolerance for joining KFs
+unmeasured_perturbation_std: 0.00000001
+keyframe_vote:
+    max_time_span:      1   # seconds
+    max_buff_length:    500   # motion deltas
+    dist_traveled:      20000.0   # meters
+    angle_turned:       1000   # radians (1 rad approx 57 deg, approx 60 deg)
+    voting_active:      true
+    voting_aux_active:  false
\ No newline at end of file