Mnaveau/use shared pointer

CMakeLists.txt

@@ -4,76 +4,72 @@
 # License BSD-3 clause
 #
 
-
-CMAKE_MINIMUM_REQUIRED(VERSION 3.1)
+cmake_minimum_required(VERSION 3.1)
 
 # ----------------------------------------------------
 # --- CXX FLAGS --------------------------------------
 # ----------------------------------------------------
 
-SET(CXX_DISABLE_WERROR True)
-SET(CMAKE_VERBOSE_MAKEFILE True)
+set(CXX_DISABLE_WERROR True)
+set(CMAKE_VERBOSE_MAKEFILE True)
 
 # These variables have to be defined before running SETUP_PROJECT
-SET(PROJECT_NAME odri_control_interface)
-SET(PROJECT_DESCRIPTION "Common interface for controlling robots build with the odri master board.")
-SET(PROJECT_URL https://github.com/open-dynamic-robot-initiative/odri_control_interface)
+set(PROJECT_NAME odri_control_interface)
+set(PROJECT_DESCRIPTION
+    "Common interface for controlling robots build with the odri master board.")
+set(PROJECT_URL
+    https://github.com/open-dynamic-robot-initiative/odri_control_interface)
 set(PROJECT_USE_CMAKE_EXPORT TRUE)
 
-
 # Check if the submodule cmake have been initialized
-IF(NOT EXISTS "${CMAKE_SOURCE_DIR}/cmake/base.cmake")
-  MESSAGE(FATAL_ERROR "\nPlease run the following command first:\ngit submodule update --init\n")
-ENDIF()
+if(NOT EXISTS "${CMAKE_SOURCE_DIR}/cmake/base.cmake")
+  message(
+    FATAL_ERROR
+      "\nPlease run the following command first:\ngit submodule update --init\n"
+  )
+endif()
 
 # --- OPTIONS ----------------------------------------
-OPTION(BUILD_PYTHON_INTERFACE "Build the python binding" ON)
-OPTION(PYTHON_STANDARD_LAYOUT "Enable standard Python package layout" ON)
-OPTION(PYTHON_DEB_LAYOUT "Enable Debian-style Python package layout" OFF)
+option(BUILD_PYTHON_INTERFACE "Build the python binding" ON)
+option(PYTHON_STANDARD_LAYOUT "Enable standard Python package layout" ON)
+option(PYTHON_DEB_LAYOUT "Enable Debian-style Python package layout" OFF)
 
-INCLUDE(cmake/base.cmake)
-INCLUDE(cmake/boost.cmake)
-INCLUDE(cmake/python.cmake)
-INCLUDE(cmake/ide.cmake)
+include(cmake/base.cmake)
+include(cmake/boost.cmake)
+include(cmake/python.cmake)
+include(cmake/ide.cmake)
 
-COMPUTE_PROJECT_ARGS(PROJECT_ARGS LANGUAGES CXX)
-PROJECT(${PROJECT_NAME} ${PROJECT_ARGS})
-CHECK_MINIMAL_CXX_STANDARD(11 ENFORCE)
+compute_project_args(PROJECT_ARGS LANGUAGES CXX)
+project(${PROJECT_NAME} ${PROJECT_ARGS})
+check_minimal_cxx_standard(11 ENFORCE)
 
 # ----------------------------------------------------
 # --- DEPENDENCIES -----------------------------------
 # ----------------------------------------------------
 
-ADD_PROJECT_DEPENDENCY(yaml-cpp CONFIG REQUIRED)
-ADD_PROJECT_DEPENDENCY(Eigen3 REQUIRED PKG_CONFIG_REQUIRES "eigen3 >= 3.0.5")
-ADD_PROJECT_DEPENDENCY(master_board_sdk REQUIRED)
+add_project_dependency(yaml-cpp CONFIG REQUIRED)
+add_project_dependency(Eigen3 REQUIRED PKG_CONFIG_REQUIRES "eigen3 >= 3.0.5")
+add_project_dependency(master_board_sdk REQUIRED)
 
-# Set component to fetch from boost
-# Get the python interface for the bindings
-IF(BUILD_PYTHON_INTERFACE)
-  FINDPYTHON(REQUIRED)
-  SEARCH_FOR_BOOST_PYTHON(REQUIRED)
-  ADD_PROJECT_DEPENDENCY(eigenpy 2.5.0 REQUIRED PKG_CONFIG_REQUIRES "eigenpy >= 2.5.0")
-ENDIF(BUILD_PYTHON_INTERFACE)
+# Set component to fetch from boost Get the python interface for the bindings
+if(BUILD_PYTHON_INTERFACE)
+  findpython(REQUIRED)
+  search_for_boost_python(REQUIRED)
+  add_project_dependency(eigenpy 2.5.0 REQUIRED PKG_CONFIG_REQUIRES
+                         "eigenpy >= 2.5.0")
+endif(BUILD_PYTHON_INTERFACE)
 
 # ----------------------------------------------------
 # --- INCLUDE ----------------------------------------
 # ----------------------------------------------------
 
 # --- MAIN LIBRARY -------------------------------------------------------------
-SET(ODRI_CONTROL_INTERFACE_SRC
-    src/joint_modules.cpp
-    src/imu.cpp
-    src/robot.cpp
-    src/calibration.cpp
-    src/utils.cpp
-)
-ADD_LIBRARY(${PROJECT_NAME} SHARED
-  ${ODRI_CONTROL_INTERFACE_SRC}
-)
-target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${EIGEN3_INCLUDE_DIR})
-target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${MASTER_BOARD_SDK_INCLUDE_DIRS})
-TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${YAML_CPP_LIBRARIES})
+set(ODRI_CONTROL_INTERFACE_SRC src/joint_modules.cpp src/imu.cpp src/robot.cpp
+                               src/calibration.cpp src/utils.cpp)
+add_library(${PROJECT_NAME} SHARED ${ODRI_CONTROL_INTERFACE_SRC})
+target_link_libraries(${PROJECT_NAME} ${YAML_CPP_LIBRARIES})
+target_link_libraries(${PROJECT_NAME} master_board_sdk::master_board_sdk)
+target_link_libraries(${PROJECT_NAME} Eigen3::Eigen)
 target_include_directories(
   ${PROJECT_NAME} PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
                          $<INSTALL_INTERFACE:include>)
@@ -85,36 +81,41 @@ install(DIRECTORY include/ DESTINATION include)
 
 # --- BINDINGS -----------------------------------------------------------------
 
-IF(BUILD_PYTHON_INTERFACE)
+if(BUILD_PYTHON_INTERFACE)
   # --- Setup the wrapper name and source files --- #
-  SET(PYWRAP ${PROJECT_NAME}_pywrap)
-  SET(${PYWRAP}_HEADERS
-    srcpy/bindings.h
-  )
-  SET(${PYWRAP}_SOURCES
-    srcpy/bindings.cpp
-  )
+  set(PYWRAP ${PROJECT_NAME}_pywrap)
+  set(${PYWRAP}_HEADERS srcpy/bindings.h)
+  set(${PYWRAP}_SOURCES srcpy/bindings.cpp)
 
   # --- Build the wrapper library --- #
-  SET(${PYWRAP}_INSTALL_DIR ${PYTHON_SITELIB})
-  ADD_LIBRARY(${PYWRAP} SHARED ${${PYWRAP}_SOURCES} ${${PYWRAP}_HEADERS})
-  TARGET_INCLUDE_DIRECTORIES(${PYWRAP} SYSTEM PRIVATE ${PYTHON_INCLUDE_DIR})
-  TARGET_LINK_LIBRARIES(${PYWRAP} ${PROJECT_NAME}
-      eigenpy::eigenpy master_board_sdk::master_board_sdk)
-  TARGET_LINK_BOOST_PYTHON(${PYWRAP})
-  SET_TARGET_PROPERTIES(${PYWRAP} PROPERTIES
-    INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib")
-  INSTALL(TARGETS ${PYWRAP} DESTINATION ${${PYWRAP}_INSTALL_DIR})
+  set(${PYWRAP}_INSTALL_DIR ${PYTHON_SITELIB})
+  add_library(${PYWRAP} SHARED ${${PYWRAP}_SOURCES} ${${PYWRAP}_HEADERS})
+  target_include_directories(${PYWRAP} SYSTEM PRIVATE ${PYTHON_INCLUDE_DIR})
+  target_link_libraries(${PYWRAP} ${PROJECT_NAME} eigenpy::eigenpy)
+  target_link_boost_python(${PYWRAP})
+  set_target_properties(${PYWRAP} PROPERTIES INSTALL_RPATH
+                                             "${CMAKE_INSTALL_PREFIX}/lib")
+  install(TARGETS ${PYWRAP} DESTINATION ${${PYWRAP}_INSTALL_DIR})
 
   # --- Allow to do: make python --- #
-  ADD_CUSTOM_TARGET(python)
-  SET_TARGET_PROPERTIES(python PROPERTIES EXCLUDE_FROM_DEFAULT_BUILD True)
-  ADD_DEPENDENCIES(python ${PYWRAP})
-
-ENDIF(BUILD_PYTHON_INTERFACE)
+  add_custom_target(python)
+  set_target_properties(python PROPERTIES EXCLUDE_FROM_DEFAULT_BUILD True)
+  add_dependencies(python ${PYWRAP})
 
+endif(BUILD_PYTHON_INTERFACE)
 
 # --- CHECK DEMOS -----------------------------------------------------------
-ADD_EXECUTABLE(demo_solo12 demos/demo_solo12.cpp)
-TARGET_LINK_LIBRARIES(demo_solo12 ${PROJECT_NAME} master_board_sdk::master_board_sdk)
-INSTALL(TARGETS demo_solo12 DESTINATION bin)
+macro(create_solo12_demo source)
+
+  set(demo_name ${PROJECT_NAME}_${source})
+  add_executable(${demo_name} demos/${source}.cpp)
+  target_link_libraries(${demo_name} ${PROJECT_NAME})
+  target_compile_definitions(
+    ${demo_name}
+    PUBLIC CONFIG_SOLO12_YAML="${PROJECT_SOURCE_DIR}/demos/config_solo12.yaml")
+  install(TARGETS ${demo_name} DESTINATION bin)
+
+endmacro(create_solo12_demo source)
+
+create_solo12_demo(demo_create_solo12_robot)
+create_solo12_demo(demo_solo12_from_yaml)

demos/config_solo12.yaml

@@ -33,7 +33,7 @@ joint_calibrator:
         0.000,  0.310,  0.165, 0.150,
         0.365, 0.202, -0.175, -0.175
     ]
-    Kp: 5.
-    Kd: 0.1
+    Kp: 3.
+    Kd: 0.05
     T: 1.
     dt: 0.001

demos/demo_create_solo12_robot.cpp

@@ -0,0 +1,142 @@
+#include <odri_control_interface/calibration.hpp>
+#include <odri_control_interface/robot.hpp>
+
+#include <odri_control_interface/utils.hpp>
+
+using namespace odri_control_interface;
+
+#include <iostream>
+#include <stdexcept>
+
+typedef Eigen::Matrix<double, 12, 1> Vector12d;
+typedef Eigen::Matrix<bool, 12, 1> Vector12b;
+
+typedef Eigen::Matrix<long, 3, 1> Vector3l;
+typedef Eigen::Matrix<long, 4, 1> Vector4l;
+typedef Eigen::Matrix<long, 12, 1> Vector12l;
+typedef Eigen::Matrix<int, 12, 1> Vector12i;
+
+int main(int argc, char **argv)
+{
+    if (argc != 2)
+    {
+        throw std::runtime_error(
+            "Please provide the interface name "
+            "(i.e. using 'ifconfig' on linux");
+    }
+
+    nice(-20);  // Give the process a high priority.
+
+    auto main_board_ptr_ = std::make_shared<MasterBoardInterface>(argv[1]);
+
+    Vector12i motor_numbers;
+    motor_numbers << 0, 3, 2, 1, 5, 4, 6, 9, 8, 7, 11, 10;
+    Vector12b motor_reversed;
+    motor_reversed << true, false, true, true, false, false, true, false, true,
+        true, false, false;
+
+    Vector12d joint_lower_limits;
+    joint_lower_limits << -1.2, -1.7, -3.4, -1.2, -1.7, -3.4, -1.2, -1.7, -3.4,
+        -1.2, -1.7, -3.4;
+    Vector12d joint_upper_limits;
+    joint_upper_limits << 1.2, 1.7, +3.4, +1.2, +1.7, +3.4, 1.2, 1.7, +3.4,
+        +1.2, +1.7, +3.4;
+
+    // Define the joint module.
+    auto joints = std::make_shared<JointModules>(main_board_ptr_,
+                                                 motor_numbers,
+                                                 0.025,
+                                                 9.,
+                                                 1.,
+                                                 motor_reversed,
+                                                 joint_lower_limits,
+                                                 joint_upper_limits,
+                                                 80.,
+                                                 0.5);
+
+    // Define the IMU.
+    Vector3l rotate_vector;
+    Vector4l orientation_vector;
+    rotate_vector << 1, 2, 3;
+    orientation_vector << 1, 2, 3, 4;
+    auto imu = std::make_shared<IMU>(
+        main_board_ptr_, rotate_vector, orientation_vector);
+
+    // Define the robot.
+    auto robot = std::make_shared<Robot>(main_board_ptr_, joints, imu);
+
+    // Start the robot.
+    robot->Start();
+
+    // Define controller to calibrate the joints.
+    std::vector<CalibrationMethod> directions = {
+        AUTO, AUTO, AUTO, AUTO, AUTO, AUTO, AUTO, AUTO, AUTO, AUTO, AUTO, AUTO};
+    Vector12d position_offsets;
+    position_offsets << 0.184, -0.370, -0.005, -.150, 0., 0.310, 0.165, 0.150,
+        0.365, 0.202, -0.175, -0.175;
+
+    auto calib_ctrl = std::make_shared<JointCalibrator>(
+        joints, directions, position_offsets, 5., 0.05, 2., 0.001);
+
+    Vector12d torques;
+
+    double kp = 3.;
+    double kd = 0.05;
+    int c = 0;
+    std::chrono::time_point<std::chrono::system_clock> last =
+        std::chrono::system_clock::now();
+    bool is_calibrated = false;
+    while (!robot->IsTimeout())
+    {
+        if (((std::chrono::duration<double>)(std::chrono::system_clock::now() -
+                                             last))
+                .count() > 0.001)
+        {
+            last = std::chrono::system_clock::now();  // last+dt would be better
+            robot->ParseSensorData();
+
+            if (robot->IsReady())
+            {
+                if (!is_calibrated)
+                {
+                    is_calibrated = calib_ctrl->Run();
+                    if (is_calibrated)
+                    {
+                        std::cout << "Calibration is done." << std::endl;
+                    }
+                }
+                else
+                {
+                    // Run the main controller.
+                    auto pos = robot->joints->GetPositions();
+                    auto vel = robot->joints->GetVelocities();
+                    // Reverse the positions;
+                    for (int i = 0; i < 12; i++)
+                    {
+                        torques[i] = -kp * pos[i] - kd * vel[i];
+                    }
+                    robot->joints->SetTorques(torques);
+                }
+            }
+
+            // Checks if the robot is in error state (that is, if any component
+            // returns an error). If there is an error, the commands to send
+            // are changed to send the safety control.
+            robot->SendCommand();
+
+            c++;
+            if (c % 1000 == 0)
+            {
+                std::cout << "Joints: ";
+                joints->PrintVector(joints->GetPositions());
+                std::cout << std::endl;
+            }
+        }
+        else
+        {
+            std::this_thread::yield();
+        }
+    }
+    std::cout << "Normal program shutdown." << std::endl;
+    return 0;
+}