Rewrite state machine (#62)

Merge of the rewrite_StateMachine development branch including the following changes:
- A rewrite of the state machine system and associated upgrade of all demo apps. See #51 .
- Some changes on the main app timing (CAN and RT thread timing management and overtime error)
- Slight simplification of the CMakeFile: no more ROS1 cross-compilation option and allow out-of-folder apps
- Support for Fourier ArmMotus EMU version
- Some documentation rewrite

Co-authored-by: Emek Barış Küçüktabak <32764586+emekBaris@users.noreply.github.com>
Co-authored-by: Ben <75413535+benvonh@users.noreply.github.com>
Co-authored-by: justinfong-unimelb <49131879+justinfong-unimelb@users.noreply.github.com>

Author: 4 people

CMakeLists.txt

@@ -3,9 +3,6 @@ project(CORC C CXX)
 
 #To cross-compile for Arm (BeagleBone) use the armhf toolchain:
 # cmake -DCMAKE_TOOLCHAIN_FILE=../armhf.cmake ..
-#To cross-compile with ROS support prepare a sysroot and pass it to cmake:
-# cmake -DCMAKE_SYSROOT=/path/to/sysroot/ -DCMAKE_TOOLCHAIN_FILE=../armhf.cmake ..
-# See doc for more details.
 ################################## USER FLAGS ##################################
 
 ## Select the application by setting the state machine to use
@@ -14,12 +11,16 @@ project(CORC C CXX)
 
 set (STATE_MACHINE_NAME "ExoTestMachine")
 #set (STATE_MACHINE_NAME "M1DemoMachine")
-#set (STATE_MACHINE_NAME "M1DemoMachineROS") # uncomment other cfg files under generate_dynamic_reconfigure_options
+#set (STATE_MACHINE_NAME "M1DemoMachineROS")
 #set (STATE_MACHINE_NAME "M2DemoMachine")
 #set (STATE_MACHINE_NAME "M3DemoMachine")
-#set (STATE_MACHINE_NAME "X2DemoMachine")  # uncomment other cfg files under generate_dynamic_reconfigure_options
+#set (STATE_MACHINE_NAME "X2DemoMachine")
 #set (STATE_MACHINE_NAME "LoggingDevice")
 
+#Use this if your state machine code folder is not in CORC 'src/apps/' folder.
+#Can be a relative or absolute path.
+#set (STATE_MACHINE_PATH "../")
+
 # Comment to use actual hardware, uncomment for a nor robot (virtual) app
 set(NO_ROBOT ON)
 
@@ -30,6 +31,12 @@ set(USE_ROS OFF)
 set(CORC_LOGGING_LEVEL INFO)
 
 ################################################################################
+#Default path if not set
+if(NOT STATE_MACHINE_PATH)
+    set (STATE_MACHINE_PATH "src/apps")
+endif()
+
+#ROS internal flags
 if(USE_ROS)
     add_definitions(-DUSEROS)
 endif()
@@ -49,8 +56,8 @@ add_definitions(-DFP_STARTTPDO=0x3E1)
 
 #######################
 
-## Compile as C++11
-set(CMAKE_CXX_STANDARD 11)
+## Compile as C++14
+set(CMAKE_CXX_STANDARD 14)
 set(CMAKE_CXX_STANDARD_REQUIRED ON)
 if(USE_ROS)
     set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wno-int-in-bool-context" )
@@ -69,7 +76,7 @@ set(THREADS_PREFER_PTHREAD_FLAG TRUE)
 if(NOT CMAKE_BUILD_TYPE)
     set(CMAKE_BUILD_TYPE Release)
 endif()
-set(CMAKE_CXX_FLAGS_DEBUG "-g")
+set(CMAKE_CXX_FLAGS_DEBUG "-g -O0")
 set(CMAKE_CXX_FLAGS_RELEASE "-O3")
 
 ## Current state machine (APP) to be build
@@ -84,8 +91,8 @@ if(NO_ROBOT)
 endif()
 
 ## Get all source and header files (only the target app folder is included)
-file(GLOB_RECURSE SOURCES "src/core/*.cpp" "src/core/*.c" "src/hardware/*.cpp" "src/hardware/*.c" "src/apps/${STATE_MACHINE_NAME}/*.c" "src/apps/${STATE_MACHINE_NAME}/*.cpp" "lib/FLNL/src/*.cpp")
-file(GLOB_RECURSE HEADERS "src/core/*.h" "src/hardware/*.h" "src/apps/${STATE_MACHINE_NAME}/*.h" )
+file(GLOB_RECURSE SOURCES "src/core/*.cpp" "src/core/*.c" "src/hardware/*.cpp" "src/hardware/*.c" "${STATE_MACHINE_PATH}/${STATE_MACHINE_NAME}/*.c" "${STATE_MACHINE_PATH}/${STATE_MACHINE_NAME}/*.cpp" "lib/FLNL/src/*.cpp")
+file(GLOB_RECURSE HEADERS "src/core/*.h" "src/hardware/*.h" "${STATE_MACHINE_PATH}/${STATE_MACHINE_NAME}/*.h" )
 
 ## Set every folder containing .h file as include directory
 set (INCLUDE_DIRS "")
@@ -110,95 +117,64 @@ else()
     add_definitions(-DBASE_DIRECTORY=${CMAKE_SOURCE_DIR})
 endif()
 
-## Add ROS dependencies
+## Add ROS 1 dependencies
 if(USE_ROS)
-    #ROS local compile: use catkin
-    if(NOT CMAKE_CROSSCOMPILING)
-        message("--catkin--")
-        # Required ROS packages
+    #ROS 1 local compile: use catkin
+    message("--catkin--")
+    # Required ROS packages
+    find_package(catkin REQUIRED COMPONENTS
+        roscpp
+        rospy
+        std_msgs
+        std_srvs
+        sensor_msgs
+        geometry_msgs
+        dynamic_reconfigure
+        message_generation
+    )
+    if(SIM)
         find_package(catkin REQUIRED COMPONENTS
-                roscpp
-                rospy
-                std_msgs
-                std_srvs
-                sensor_msgs
-                geometry_msgs
-                dynamic_reconfigure
-                message_generation
-        )
-        if(SIM)
-            find_package(catkin REQUIRED COMPONENTS
-            controller_manager_msgs
-            cob_gazebo_ros_control
-            x2_description
-            )
-        endif()
-
-        generate_dynamic_reconfigure_options(
-                config/m1_dynamic_params.cfg
-#                config/x2_dynamic_params.cfg
-        )
-
-        add_message_files(
-                FILES
-                X2Array.msg
-                X2Acceleration.msg
-                X2AccelerationMerge.msg
-        )
-
-        generate_messages(
-                DEPENDENCIES
-                std_msgs
+        controller_manager_msgs
+        cob_gazebo_ros_control
+        x2_description
         )
-
-
-
-        catkin_package(
-                #  INCLUDE_DIRS include
-                #  LIBRARIES x2
-              CATKIN_DEPENDS
-                roscpp
-                rospy
-                std_msgs
-                std_srvs
-                sensor_msgs
-                geometry_msgs
-                dynamic_reconfigure
-                message_runtime
-                #  DEPENDS system_lib
-        )
-
-        #include CATKIN
-        include_directories(${catkin_INCLUDE_DIRS})
-        set(ROS_LIBRARIES ${catkin_LIBRARIES})
-    #Cross-compile with ROS (w/o catkin)
-    else()
-        # Required ROS packages
-        find_package(roscpp REQUIRED CONFIG ONLY_CMAKE_FIND_ROOT_PATH)
-        find_package(std_msgs REQUIRED CONFIG ONLY_CMAKE_FIND_ROOT_PATH)
-        find_package(sensor_msgs REQUIRED CONFIG ONLY_CMAKE_FIND_ROOT_PATH)
-        find_package(geometry_msgs REQUIRED CONFIG ONLY_CMAKE_FIND_ROOT_PATH)
-        # If cross-compiling force search in sysroot
-        if(IS_DIRECTORY ${CMAKE_SYSROOT})
-            message("-- Using sysroot folder: ${CMAKE_SYSROOT}")
-            #Reset include dirs towards sysroot only
-            foreach(inc_dir ${roscpp_INCLUDE_DIRS} ${std_msgs_INCLUDE_DIRS})
-                string(REGEX REPLACE "^/usr/" "${CMAKE_SYSROOT}usr/" inc_dir_cross ${inc_dir})
-                include_directories(${inc_dir_cross})
-            endforeach()
-            #Reset libraries towards sysroot only
-            foreach(lib_path ${roscpp_LIBRARIES} ${std_msgs_LIBRARIES})
-                string(REPLACE "/" ";" lib_path_list ${lib_path}) #Breakdown path in list
-                list(REVERSE lib_path_list)
-                list(GET lib_path_list 0 lib) #Get last element (library name)
-                find_library(new_path ${lib} PATHS ${CMAKE_SYSROOT}) #Force to look for it in actual sysroot path
-                list(APPEND ROS_LIBRARIES ${new_path}) #Add it
-                unset(new_path CACHE) #Clear variable to allow new search
-            endforeach()
-        else()
-            message(FATAL_ERROR "No sysroot (CMAKE_SYSROOT) defined or not an accessible path")
-        endif()
     endif()
+
+    generate_dynamic_reconfigure_options(
+            config/m1_dynamic_params.cfg
+            config/x2_dynamic_params.cfg
+    )
+
+    add_message_files(
+        FILES
+        X2Array.msg
+        X2Acceleration.msg
+        X2AccelerationMerge.msg
+    )
+
+    generate_messages(
+       DEPENDENCIES
+       std_msgs
+    )
+
+    catkin_package(
+        #  INCLUDE_DIRS include
+        #  LIBRARIES x2
+        CATKIN_DEPENDS
+        roscpp
+        rospy
+        std_msgs
+        std_srvs
+        sensor_msgs
+        geometry_msgs
+        dynamic_reconfigure
+        message_runtime
+        #  DEPENDS system_lib
+    )
+
+    #include CATKIN
+    include_directories(${catkin_INCLUDE_DIRS})
+    set(ROS_LIBRARIES ${catkin_LIBRARIES})
 endif()
 
 
@@ -231,4 +207,4 @@ if(USE_ROS)
 endif()
 
 
-message("-----------------------------------------------\nBuilding application ${APP_NAME}\n-----------------------------------------------")
+message("-----------------------------------------------\nBuilding application ${APP_NAME}\n-----------------------------------------------")

README.md

@@ -2,15 +2,15 @@
 
 CORC is a free and open source robotic development software stack, written in C++.
 
-The project was initiated at the University of Melbourne in partnership with Fourier Intelligence, however has welcomed (and continues to welcome) collaborators from all institutions. The project was initially developed to run on an ExoMotus X2 Exoskeleton powered by a Beaglebone Black, however, the software is designed to be extensible to any embedded Linux and CANopen enabled Robotic platform. The repository currently also includes code which has been run on the ArmMotus M1 and M3 rehabilitation devices, and using desktop or laptop Ubuntu installations.
+The project was initiated at the University of Melbourne in partnership with Fourier Intelligence, however has welcomed (and continues to welcome) collaborators from all institutions. The project was initially developed to run on an ExoMotus X2 Exoskeleton powered by a Beaglebone Black, however, the software is designed to be extensible to any embedded Linux and CANopen enabled Robotic platform. The repository currently also includes code which has been run on the ArmMotus M1, M2 and M3 (EMU) rehabilitation devices, and using desktop or laptop Ubuntu installations.
 
 ## The CANOpen Robot Controller project includes:
 
 - An extensible framework to represent multibody rigid robotic systems.
-- An event driven state machine to develop custom applications (see [here](doc/3.Software/StateMachine.md)).
+- An event driven state machine to develop custom applications (see [here](doc/3.Software/CustomApplication.md)).
 - An implementation of [CANopen Socket](https://github.com/CANopenNode/CANopenSocket) to provide an interface between CAN enabled embedded Linux system and CANopen-based motor drivers/sensors.
 - Documentation (this page and associated ones and code Doxygen).
-- Functional application examples.
+- Functional application examples. 
 
 ### Project Overview
 
@@ -34,7 +34,7 @@ CORC relies on [spdlog](https://github.com/gabime/spdlog) for both general loggi
 See [here](doc/3.Software/Logging.md) for more info on using the logging system.
 
 ### ROS Support
-See [here](doc/1.GettingStarted/Simulation.md) for instructions on how to build and run a CORC app with ROS support.
+See [here](doc/1.GettingStarted/AdvancedSimulationAndHardwareTesting.md) for instructions on how to build and run a CORC app with ROS support.
 
 ### Network communication
 See [here](doc/3.Software/NetworkCommunication.md) for instructions on using libFLNL for communication.
@@ -47,10 +47,8 @@ To generate the Doxygen documentation of the code you can simply run `doxygen Do
 
 ## Developer Information
 
-- Detailed documentation: https://exoembedded.readthedocs.io/en/latest/
-- Source code documentation: https://capstonealex.github.io/exo/index.html
-- Project Repository: https://github.com/capstonealex/exo
 - CANopen Socket: https://github.com/CANopenNode/CANopenSocket
+- CANopen CiA 402 (motor drive standard) ressources: https://www.can-cia.org/can-knowledge/canopen/cia402/ and https://doc.synapticon.com/software/40/object_dict/all_objects/index.html#all-objects
 
 
 ## Contributors

config/M3_params.yaml

@@ -26,10 +26,13 @@
 EMU_MELB:
   dqMax: 360 # Max joint speed (deg.s-1) Set to 1 rot/s
   tauMax: 42 # Max joint torque (Nm) Set to max motor torque w/ 1:22 reduction (yes, this is the actual value!)
+  iPeakDrives: [45.0, 45.0, 45.0] #Drive peak current (I peak)
+  motorCstt: [1., 1., 1.] #Motors constants
+  qSigns: [1, 1, 1] # Joints direction
 
   linkLengths: [0.056, 0.135, 0.5, 0.475]   # Link lengths used for kinematic models (in m), excluding tool
   linkMasses: [0., 0.450, 0.400, 0.100, .0] # Link masses used for gravity compensation (in kg), excluding tool
-
+  
   qLimits: [-45, 45, -15, 70, 0, 95] # Joints limits (in deg) {q1_min, q1_max, q2_min, q2_max, q3_min, q3_max}
   qCalibration: [-38, 70, 95] # Calibration configuration (in deg): posture in which the robot is when using the calibration procedure
 
@@ -44,20 +47,24 @@ EMU_MELB:
 
 EMU_FOURIER:
   dqMax: 360 # Max joint speed (deg.s-1) Set to 1 rot/s
-  tauMax: 42 # Max joint torque (Nm) Set to max motor torque w/ 1:22 reduction (yes, this is the actual value!)
+  tauMax: 80 # Max joint torque (Nm) Set to max motor torque w/ 1:22 reduction (yes, this is the actual value!)
+  iPeakDrives: [45.0, 23.0, 23.0] #Drive peak current (I peak): Values look wrong but work on real system in combination w/ below motor constants.
+  motorCstt: [0.132, 0.132, 0.132] #Motors constants
   qSigns: [-1, 1, -1] # Joints direction
 
-  linkLengths: [0.056, 0.135, 0.5, 0.615]   # Link lengths used for kinematic models (in m), excluding tool
-  linkMasses: [0, 0.450, 0.400, 0.100, .270] # Link masses used for gravity compensation (in kg), excluding tool
+  linkLengths: [0.0, 0.135, 0.5, 0.615]   # Link lengths used for kinematic models (in m), excluding tool
+  massCoeff: [-1.30, -1.06] # Mass coefficient (identified) used for gravity compensation (in kg), excluding tool. Model is tau[1]=M[0] * sin(q[1])*g and tau[2]=M[1] * cos(q[2])*g ;
+  qSpringK: [0, 2.42, 4.19] #Joint spring compensation: tau=Ko+K*q
+  qSpringKo: [0, 0.47, 0]
 
-  qLimits: [-43, 43, -15, 70, 0, 95] # Joints limits (in deg) {q1_min, q1_max, q2_min, q2_max, q3_min, q3_max}
+  qLimits: [-42, 42, -15, 70, 0, 95] # Joints limits (in deg) {q1_min, q1_max, q2_min, q2_max, q3_min, q3_max}
   qCalibration: [+44, 76, 90] # Calibration configuration (in deg): posture in which the robot is when using the calibration procedure
 
-  frictionVis: [0.2, 0.2, 0.2]  # Joint viscous friction compensation coefficients
-  frictionCoul: [0., 0., 0.] # Joint Coulomb (static) friction compensation coefficients
+  frictionVis: [0.33, 0.76, 0.69]  # Joint viscous friction compensation coefficients
+  frictionCoul: [0.35, 0.19, 0.24] # Joint Coulomb (static) friction compensation coefficients
 
   #End-effector description
   tool:
     name: "Fourier Handle"
     length: 0.0 #Tool length from attachment
-    mass:   0.345  #Tool mass in kg
+    mass:   0.1  #Tool mass in kg

doc/1.GettingStarted/GettingStarted.md

@@ -10,6 +10,8 @@ CORC is flexible in that it can run on any computer with a Controller Area Netwo
 
 Examples of use of a dedicated embedded computer are when the robotic device is mobile (such as an assistive exoskeleton) or when separation between the robot controller and the user interface is desired (such as on deployment-ready rehabilitation devices). Alternatively, you may choose to use a desktop computer if you wish to run more powerful control algorithms on your device, or if your development is more experimental in nature. With appropriate choice of software packages, it is also possible to transition deployment from a desktop computer to an embedded computer. 
 
+> Note that cross-compiling a ROS app is theoretically possible but not described in this documentation (contibutions are welcome!).
+
 ## Choice of Development Environment
 Independent of the deployment environment, a choice may also be made regarding the development environment. If the deployment environment is Linux-based, this can be the same device as your deployment computer (although, development will typically require more computational power than deployment). Alternatively, you may develop on a different computer to the deployment computer. In this case, you have the choice of using either Windows or Linux based environments*, and you will need to cross compile for your deployment environment (i.e. compile on a system which is not used to run the executable). This requires an appropriate toolchain to be installed.