From: Walter Fetter Lages Date: Sun, 6 May 2018 03:53:00 +0000 (-0300) Subject: 2018-05-03 X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=refs%2Fheads%2Ftiago;p=twil.git 2018-05-03 --- diff --git a/twil/package.xml b/twil/package.xml index 9724f87..8d77827 100644 --- a/twil/package.xml +++ b/twil/package.xml @@ -41,10 +41,11 @@ catkin - twil_description twil_bringup twil_controllers - twil_hw + twil_msgs + twil_description + twil_hardware diff --git a/twil_bringup/CMakeLists.txt b/twil_bringup/CMakeLists.txt index 2a7e313..099845c 100644 --- a/twil_bringup/CMakeLists.txt +++ b/twil_bringup/CMakeLists.txt @@ -5,10 +5,6 @@ add_definitions(-std=c++11) ## Find catkin macros and libraries find_package(catkin REQUIRED COMPONENTS - controller_interface - controller_manager - hardware_interface - joint_state_controller roscpp rospy std_msgs @@ -16,7 +12,6 @@ find_package(catkin REQUIRED COMPONENTS trajectory_msgs geometry_msgs ) -find_package(twil_hw REQUIRED) ## System dependencies are found with CMake's conventions find_package(Boost REQUIRED COMPONENTS system) @@ -27,7 +22,7 @@ find_package(Boost REQUIRED COMPONENTS system) catkin_package( INCLUDE_DIRS include LIBRARIES twil_bringup - CATKIN_DEPENDS controller_interface controller_manager hardware_interface joint_state_controller roscpp rospy std_msgs + CATKIN_DEPENDS roscpp rospy std_msgs DEPENDS system_lib ) @@ -38,8 +33,6 @@ catkin_package( include_directories( include ${catkin_INCLUDE_DIRS} - ${twil_hw_INCLUDE_DIRS} - ${AIC_LIB_ROOT_DIR}/include ) add_executable(twil_joystick_node src/joy.cpp) @@ -67,8 +60,8 @@ install(TARGETS twil_joystick_node ) ## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) + install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE + ) diff --git a/twil_bringup/config/controllers/backup_16_04_2018/mini_twil_dynamics_mrac_backstepping_control.yaml b/twil_bringup/config/controllers/backup_16_04_2018/mini_twil_dynamics_mrac_backstepping_control.yaml new file mode 100644 index 0000000..17eb69b --- /dev/null +++ b/twil_bringup/config/controllers/backup_16_04_2018/mini_twil_dynamics_mrac_backstepping_control.yaml @@ -0,0 +1,46 @@ +mini_twil: + + joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 100 + + dynamics_mrac_backstepping_controller: + type: twil_controllers/DynamicsMRACBacksteppingController + joints: + - left_wheel_joint + - right_wheel_joint + + feedback_linearization: + A: -9.13781 + B: -0.01999 + C: 0.36874 + D: -8.20998 + E: -1.98363 + F: 3.62234 + + Alpha: [12.0, 10.0] + + take: 5 + + nonsmooth_controller: + lambda1: 1.0 + lambda2: 1.0 + lambda3: 1.0 + gamma1: 2.0 + gamma2: 0.01 + #gamma1: 0.2826 + #gamma2: 0.4995 + #gamma1: 0.6297 + #gamma2: 1.2124 + + update_ratio: 5 + + #x_ini: 1.0 + #y_ini: 1.0 + #th_ini: 1.57 + x_ini: 0.0 + y_ini: 0.0 + th_ini: 0.0 + + #tolerances: [0.005, 0.01745] + tolerances: [0.0, 0.0] diff --git a/twil_bringup/config/controllers/backup_16_04_2018/mini_twil_dynamics_pid_backstepping_control.yaml b/twil_bringup/config/controllers/backup_16_04_2018/mini_twil_dynamics_pid_backstepping_control.yaml new file mode 100644 index 0000000..4649d19 --- /dev/null +++ b/twil_bringup/config/controllers/backup_16_04_2018/mini_twil_dynamics_pid_backstepping_control.yaml @@ -0,0 +1,61 @@ +mini_twil: + + joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 25 + + dynamics_pid_backstepping_controller: + type: twil_controllers/DynamicsPIDBacksteppingController + joints: + - left_wheel_joint + - right_wheel_joint + + feedback_linearization: + A: -9.13781 + B: -0.01999 + C: 0.36874 + D: -8.20998 + E: -1.98363 + F: 3.62234 + + #A: -8.75680 + #B: -0.01814 + #C: 0.34788 + #D: -7.92150 + #E: -1.48242 + #F: 3.43286 + + linear_velocity_pid_parameters: + p: 16.0 + i: 64.0 + d: 0.0 + i_clamp: 12000 + antiwindup: false + + angular_velocity_pid_parameters: + p: 16.0 + i: 64.0 + d: 0.0 + i_clamp: 12000 + antiwindup: false + + take: 5 + + nonsmooth_controller: + lambda1: 1 + lambda2: 1 + lambda3: 1 + gamma1: 2 + gamma2: 0.01 + #gamma1: 0.5577 + #gamma2: 0.5661 + #gamma1: 0.2826 + #gamma2: 0.4995 + + update_ratio: 1 + + x_ini: 1.0 + y_ini: 1.0 + th_ini: 1.57 + #tolerances: [0.005, 0.04] + tolerances: [0.0, 0.0] diff --git a/twil_bringup/config/controllers/backup_16_04_2018/mini_twil_dynamics_pid_control.yaml b/twil_bringup/config/controllers/backup_16_04_2018/mini_twil_dynamics_pid_control.yaml new file mode 100644 index 0000000..a27f5fd --- /dev/null +++ b/twil_bringup/config/controllers/backup_16_04_2018/mini_twil_dynamics_pid_control.yaml @@ -0,0 +1,44 @@ +mini_twil: + joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 25 + + dynamics_pid_controller: + type: twil_controllers/DynamicsPIDController + joints: + - left_wheel_joint + - right_wheel_joint + + feedback_linearization: + A: -9.13781 + B: -0.01999 + C: 0.36874 + D: -8.20998 + E: -1.98363 + F: 3.62234 + + #A: 1 + #B: 1 + #C: 1 + #D: 1 + #E: 1 + #F: 1 + + take: 0 + + linear_velocity_pid_parameters: + p: 16 + i: 64 + d: 0.0 + i_clamp: 12000 + antiwindup: false + + angular_velocity_pid_parameters: + p: 16 + i: 64 + d: 0.0 + i_clamp: 12000 + antiwindup: false + + + diff --git a/twil_bringup/config/controllers/backup_16_04_2018/mini_twil_joint_velocity_control.yaml b/twil_bringup/config/controllers/backup_16_04_2018/mini_twil_joint_velocity_control.yaml new file mode 100644 index 0000000..b3e3ddb --- /dev/null +++ b/twil_bringup/config/controllers/backup_16_04_2018/mini_twil_joint_velocity_control.yaml @@ -0,0 +1,27 @@ +mini_twil: + + joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 25 + + left_wheel_joint_velocity_controller: + type: effort_controllers/JointVelocityController + joint: left_wheel_joint + pid: + p: 2.455561e-01 + i: 1.814526e+00 + d: 0 + i_clamp: 12000 + antiwindup: False + + right_wheel_joint_velocity_controller: + type: effort_controllers/JointVelocityController + joint: right_wheel_joint + pid: + p: 0.5147369 + i: 4.295548 + d: 0.0 + i_clamp: 12000 + antiwindup: False + + diff --git a/twil_bringup/config/controllers/backup_16_04_2018/mini_twil_linearizing_control.yaml b/twil_bringup/config/controllers/backup_16_04_2018/mini_twil_linearizing_control.yaml new file mode 100644 index 0000000..589170f --- /dev/null +++ b/twil_bringup/config/controllers/backup_16_04_2018/mini_twil_linearizing_control.yaml @@ -0,0 +1,21 @@ +mini_twil: + joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 25 + + cart_linearizing_controller: + type: twil_controllers/CartLinearizingController + joints: + - left_wheel_joint + - right_wheel_joint + + take: 0 + + feedback_linearization: + A: -8.75680 + B: -0.01814 + C: 0.34788 + D: -7.92150 + E: -1.48242 + F: 3.43286 + diff --git a/twil_bringup/config/controllers/backup_16_04_2018/mini_twil_nonlinear_backstepping_control.yaml b/twil_bringup/config/controllers/backup_16_04_2018/mini_twil_nonlinear_backstepping_control.yaml new file mode 100644 index 0000000..20e877d --- /dev/null +++ b/twil_bringup/config/controllers/backup_16_04_2018/mini_twil_nonlinear_backstepping_control.yaml @@ -0,0 +1,53 @@ +mini_twil: + + joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 25 + + nonlinear_backstepping_controller: + type: twil_controllers/NonlinearBacksteppingController + joints: + - left_wheel_joint + - right_wheel_joint + + feedback_linearization: + #A: -7.969445e+00 + #B: -1.876616e-02 + #C: 3.454858e-01 + #D: -9.405481e+00 + #E: -5.987630e+00 + #F: 3.472006e+00 + A: -8.030481e+00 + B: -2.555340e-02 + C: 3.787045e-01 + D: -1.041838e+01 + E: -7.303194e+00 + F: 3.864439e+00 + + take: 0 + + nonsmooth_controller: + lambda1: 0.1 + lambda2: 0.1 + lambda3: 0.1 + lambda4: 0.1 + lambda5: 0.1 + gamma1: 1.0 + gamma2: 1.0 + gamma3: 1.0 + gamma4: 1.0 + update_ratio: 5 + + x_ini: 0.0 + y_ini: 0.0 + th_ini: 0.0 + +#lambda1: 10.0 +#lambda2: 0.1 +#lambda3: 0.1 +#lambda4: 50.0 +#lambda5: 100.0 +#gamma1: 10.0 +#gamma2: 1.0 +#gamma3: 10.0 +#gamma4: 10.0 diff --git a/twil_bringup/config/controllers/backup_16_04_2018/mini_twil_twist_pid_backstepping_control.yaml b/twil_bringup/config/controllers/backup_16_04_2018/mini_twil_twist_pid_backstepping_control.yaml new file mode 100644 index 0000000..e6c0d11 --- /dev/null +++ b/twil_bringup/config/controllers/backup_16_04_2018/mini_twil_twist_pid_backstepping_control.yaml @@ -0,0 +1,43 @@ +mini_twil: + + joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 25 + + twist_pid_backstepping_controller: + type: twil_controllers/TwistPIDBacksteppingController + joints: + - left_wheel_joint + - right_wheel_joint + + left_wheel_joint_pid_parameters: + p: 1.44697 + i: 14.52408 + d: 0.00000 + i_clamp: 12000 + antiwindup: true + + right_wheel_joint_pid_parameters: + p: 1.07365 + i: 11.34182 + d: 0.00000 + i_clamp: 12000 + antiwindup: False + + take: 4 + + nonsmooth_controller: + lambda1: 1 + lambda2: 1 + lambda3: 1 + gamma1: 2 + gamma2: 0.01 + #gamma1: 0.5577 + #gamma2: 0.5661 + + update_ratio: 1 + + x_ini: 0.0 + y_ini: 0.0 + th_ini: 0.0 + tolerances: [0.005, 0.05] diff --git a/twil_bringup/config/controllers/backup_16_04_2018/mini_twil_twist_pid_control.yaml b/twil_bringup/config/controllers/backup_16_04_2018/mini_twil_twist_pid_control.yaml new file mode 100644 index 0000000..662d02c --- /dev/null +++ b/twil_bringup/config/controllers/backup_16_04_2018/mini_twil_twist_pid_control.yaml @@ -0,0 +1,26 @@ +mini_twil: + joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 25 + + twist_pid_controller: + type: twil_controllers/TwistPIDController + joints: + - left_wheel_joint + - right_wheel_joint + + take: 1 + + left_wheel_joint_pid_parameters: + p: 1.44697 + i: 14.52408 + d: 0.00000 + i_clamp: 12000 + antiwindup: True + + right_wheel_joint_pid_parameters: + p: 1.07365 + i: 11.34182 + d: 0.00000 + i_clamp: 12000 + antiwindup: True diff --git a/twil_bringup/config/controllers/backup_16_04_2018/mini_twil_voltage_control.yaml b/twil_bringup/config/controllers/backup_16_04_2018/mini_twil_voltage_control.yaml new file mode 100644 index 0000000..f0290d5 --- /dev/null +++ b/twil_bringup/config/controllers/backup_16_04_2018/mini_twil_voltage_control.yaml @@ -0,0 +1,12 @@ +mini_twil: + joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 25 + + left_wheel_joint_voltage_controller: + type: twil_controllers/JointVoltageController + joint: left_wheel_joint + + right_wheel_joint_voltage_controller: + type: twil_controllers/JointVoltageController + joint: right_wheel_joint diff --git a/twil_bringup/config/controllers/backup_16_04_2018/twil_dynamics_mrac_backstepping_control.yaml b/twil_bringup/config/controllers/backup_16_04_2018/twil_dynamics_mrac_backstepping_control.yaml new file mode 100644 index 0000000..2e3c4f2 --- /dev/null +++ b/twil_bringup/config/controllers/backup_16_04_2018/twil_dynamics_mrac_backstepping_control.yaml @@ -0,0 +1,53 @@ +twil: + + joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 100 + + dynamics_mrac_backstepping_controller: + type: twil_controllers/DynamicsMRACBacksteppingController + joints: + - left_wheel_joint + - right_wheel_joint + + feedback_linearization: + A: -11.46461 + B: 0.02572 + C: 0.09761 + D: -8.32644 + E: 6.84786 + F: 0.52679 + + Alpha: [12.0, 10.0] + + take: 0 + + nonsmooth_controller: + lambda1: 1.0 + lambda2: 1.0 + lambda3: 1.0 + #gamma1: 2.0 + #gamma2: 0.01 + #gamma1: 0.2826 + #gamma2: 0.4995 + + #Tracking + gamma1: 0.8 + gamma2: 0.01 + + update_ratio: 5 + + #x_ini: 1.0 + #y_ini: 1.0 + #th_ini: 1.57 + #x_ini: 0.0 + #y_ini: 0.0 + #th_ini: 0.0 + + #Initial Tracking + x_ini: 0.0 + y_ini: -0.25 + th_ini: 1.57 + + #tolerances: [0.005, 0.01745] + tolerances: [0.0, 0.0] diff --git a/twil_bringup/config/controllers/backup_16_04_2018/twil_dynamics_pid_backstepping_control.yaml b/twil_bringup/config/controllers/backup_16_04_2018/twil_dynamics_pid_backstepping_control.yaml new file mode 100644 index 0000000..987fed2 --- /dev/null +++ b/twil_bringup/config/controllers/backup_16_04_2018/twil_dynamics_pid_backstepping_control.yaml @@ -0,0 +1,69 @@ +twil: + + joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 100 + + dynamics_pid_backstepping_controller: + type: twil_controllers/DynamicsPIDBacksteppingController + joints: + - left_wheel_joint + - right_wheel_joint + + feedback_linearization: + A: -11.46461 + B: 0.02572 + C: 0.09761 + D: -8.32644 + E: 6.84786 + F: 0.52679 + + linear_velocity_pid_parameters: + p: 16 + i: 64 + d: 0.0 + i_clamp: 120000 + antiwindup: false + + angular_velocity_pid_parameters: + p: 16 + i: 64 + d: 0.0 + i_clamp: 120000 + antiwindup: false + + take: 1 + + nonsmooth_controller: + lambda1: 1.0 + lambda2: 1.0 + lambda3: 1.0 + + #Tracking + #gamma1: 0.8 + #gamma2: 0.01 + + gamma1: 2.0 + gamma2: 0.01 + + #Stabilization + #gamma1: 0.2826 + #gamma2: 0.4995 + + #gamma1: 0.2658 + #gamma2: 0.4781 + + update_ratio: 5 + + #Initial Stabilization + #x_ini: 0.0 + #y_ini: -1.0 + #th_ini: 3.1416 + + #Initial Tracking + x_ini: 0.0 + y_ini: 0.0 + th_ini: 0.0 + + #tolerances: [0.005, 0.0175] + tolerances: [0.0, 0.0] diff --git a/twil_bringup/config/controllers/backup_16_04_2018/twil_dynamics_pid_control.yaml b/twil_bringup/config/controllers/backup_16_04_2018/twil_dynamics_pid_control.yaml new file mode 100644 index 0000000..558c76e --- /dev/null +++ b/twil_bringup/config/controllers/backup_16_04_2018/twil_dynamics_pid_control.yaml @@ -0,0 +1,39 @@ +twil: + joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 100 + + dynamics_pid_controller: + type: twil_controllers/DynamicsPIDController + joints: + - left_wheel_joint + - right_wheel_joint + + feedback_linearization: + A: -11.46461 + B: 0.02572 + C: 0.09761 + D: -8.32644 + E: 6.84786 + F: 0.52679 + + take: 0 + + linear_velocity_pid_parameters: + p: 0 + i: 0 + d: 0.0 + i_clamp: 120000 + antiwindup: false + publish_state: true + + angular_velocity_pid_parameters: + p: 0 + i: 0 + d: 0.0 + i_clamp: 120000 + antiwindup: false + publish_state: true + + + diff --git a/twil_bringup/config/controllers/backup_16_04_2018/twil_joint_velocity_control.yaml b/twil_bringup/config/controllers/backup_16_04_2018/twil_joint_velocity_control.yaml new file mode 100644 index 0000000..acfd34b --- /dev/null +++ b/twil_bringup/config/controllers/backup_16_04_2018/twil_joint_velocity_control.yaml @@ -0,0 +1,27 @@ +twil: + + joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 100 + + left_wheel_joint_velocity_controller: + type: effort_controllers/JointVelocityController + joint: left_wheel_joint + pid: + p: 10.7735 + i: 145.9399 + d: 0 + i_clamp: 120000 + antiwindup: false + + right_wheel_joint_velocity_controller: + type: effort_controllers/JointVelocityController + joint: right_wheel_joint + pid: + p: 11.8631 + i: 152.7907 + d: 0 + i_clamp: 120000 + antiwindup: false + + diff --git a/twil_bringup/config/controllers/backup_16_04_2018/twil_linearizing_control.yaml b/twil_bringup/config/controllers/backup_16_04_2018/twil_linearizing_control.yaml new file mode 100644 index 0000000..78c805f --- /dev/null +++ b/twil_bringup/config/controllers/backup_16_04_2018/twil_linearizing_control.yaml @@ -0,0 +1,21 @@ +twil: + + joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 100 + + cart_linearizing_controller: + type: twil_controllers/CartLinearizingController + joints: + - left_wheel_joint + - right_wheel_joint + + feedback_linearization: + A: -12.72037 + B: 0.00876 + C: 0.10451 + D: -8.02481 + E: 7.42193 + F: 0.51098 + + take: 4 diff --git a/twil_bringup/config/controllers/backup_16_04_2018/twil_twist_pid_backstepping_control.yaml b/twil_bringup/config/controllers/backup_16_04_2018/twil_twist_pid_backstepping_control.yaml new file mode 100644 index 0000000..708e53e --- /dev/null +++ b/twil_bringup/config/controllers/backup_16_04_2018/twil_twist_pid_backstepping_control.yaml @@ -0,0 +1,61 @@ +twil: + + joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 100 + + twist_pid_backstepping_controller: + type: twil_controllers/TwistPIDBacksteppingController + joints: + - left_wheel_joint + - right_wheel_joint + + left_wheel_joint_pid_parameters: + p: 2.7796 + i: 27.6931 + d: 0.0000 + i_clamp: 120000 + antiwindup: false + + right_wheel_joint_pid_parameters: + p: 2.7796 + i: 27.6931 + d: 0.0000 + i_clamp: 12000 + antiwindup: false + + take: 1 + + nonsmooth_controller: + lambda1: 1.0 + lambda2: 1.0 + lambda3: 1.0 + + #Tracking + #gamma1: 0.8 + #gamma2: 0.01 + + gamma1: 2.0 + gamma2: 0.01 + + #Stabilization + #gamma1: 0.2826 + #gamma2: 0.4995 + + #gamma1: 0.2658 + #gamma2: 0.4781 + + update_ratio: 5 + + #Initial Stabilization + #x_ini: 0.0 + #y_ini: -1.0 + #th_ini: 3.1416 + + #Initial Tracking + x_ini: 0.0 + y_ini: 0.0 + th_ini: 0.0 + + #tolerances: [0.005, 0.0175] + tolerances: [0.0, 0.0] diff --git a/twil_bringup/config/controllers/backup_16_04_2018/twil_twist_pid_control.yaml b/twil_bringup/config/controllers/backup_16_04_2018/twil_twist_pid_control.yaml new file mode 100644 index 0000000..fd05213 --- /dev/null +++ b/twil_bringup/config/controllers/backup_16_04_2018/twil_twist_pid_control.yaml @@ -0,0 +1,26 @@ +twil: + joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 100 + + twist_pid_controller: + type: twil_controllers/TwistPIDController + joints: + - left_wheel_joint + - right_wheel_joint + + take: 0 + + left_wheel_joint_pid_parameters: + p: 3.1741 + i: 27.0119 + d: 0.0000 + i_clamp: 120000 + antiwindup: False + + right_wheel_joint_pid_parameters: + p: 4.154 + i: 1.1943 + d: 2.260 + i_clamp: 120000 + antiwindup: False diff --git a/twil_bringup/config/controllers/backup_16_04_2018/twil_voltage_control.yaml b/twil_bringup/config/controllers/backup_16_04_2018/twil_voltage_control.yaml new file mode 100644 index 0000000..9678f60 --- /dev/null +++ b/twil_bringup/config/controllers/backup_16_04_2018/twil_voltage_control.yaml @@ -0,0 +1,12 @@ +twil: + joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 100 + + left_wheel_joint_voltage_controller: + type: twil_controllers/JointVoltageController + joint: left_wheel_joint + + right_wheel_joint_voltage_controller: + type: twil_controllers/JointVoltageController + joint: right_wheel_joint diff --git a/twil_bringup/config/controllers/mini_twil_dynamics_mrac_backstepping_control.yaml b/twil_bringup/config/controllers/mini_twil_dynamics_mrac_backstepping_control.yaml new file mode 100644 index 0000000..17eb69b --- /dev/null +++ b/twil_bringup/config/controllers/mini_twil_dynamics_mrac_backstepping_control.yaml @@ -0,0 +1,46 @@ +mini_twil: + + joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 100 + + dynamics_mrac_backstepping_controller: + type: twil_controllers/DynamicsMRACBacksteppingController + joints: + - left_wheel_joint + - right_wheel_joint + + feedback_linearization: + A: -9.13781 + B: -0.01999 + C: 0.36874 + D: -8.20998 + E: -1.98363 + F: 3.62234 + + Alpha: [12.0, 10.0] + + take: 5 + + nonsmooth_controller: + lambda1: 1.0 + lambda2: 1.0 + lambda3: 1.0 + gamma1: 2.0 + gamma2: 0.01 + #gamma1: 0.2826 + #gamma2: 0.4995 + #gamma1: 0.6297 + #gamma2: 1.2124 + + update_ratio: 5 + + #x_ini: 1.0 + #y_ini: 1.0 + #th_ini: 1.57 + x_ini: 0.0 + y_ini: 0.0 + th_ini: 0.0 + + #tolerances: [0.005, 0.01745] + tolerances: [0.0, 0.0] diff --git a/twil_bringup/config/controllers/mini_twil_dynamics_pid_backstepping_control.yaml b/twil_bringup/config/controllers/mini_twil_dynamics_pid_backstepping_control.yaml index 6070cec..4649d19 100644 --- a/twil_bringup/config/controllers/mini_twil_dynamics_pid_backstepping_control.yaml +++ b/twil_bringup/config/controllers/mini_twil_dynamics_pid_backstepping_control.yaml @@ -11,37 +11,51 @@ mini_twil: - right_wheel_joint feedback_linearization: - A: -7.969445e+00 - B: -1.876616e-02 - C: 3.454858e-01 - D: -9.405481e+00 - E: -5.987630e+00 - F: 3.472006e+00 + A: -9.13781 + B: -0.01999 + C: 0.36874 + D: -8.20998 + E: -1.98363 + F: 3.62234 + + #A: -8.75680 + #B: -0.01814 + #C: 0.34788 + #D: -7.92150 + #E: -1.48242 + #F: 3.43286 linear_velocity_pid_parameters: - p: 16 - i: 64 + p: 16.0 + i: 64.0 d: 0.0 i_clamp: 12000 antiwindup: false angular_velocity_pid_parameters: - p: 16 - i: 64 + p: 16.0 + i: 64.0 d: 0.0 i_clamp: 12000 antiwindup: false - take: 0 + take: 5 nonsmooth_controller: - lambda1: 0 + lambda1: 1 lambda2: 1 - lambda3: 0.5 - gamma1: 1 - gamma2: 1 + lambda3: 1 + gamma1: 2 + gamma2: 0.01 + #gamma1: 0.5577 + #gamma2: 0.5661 + #gamma1: 0.2826 + #gamma2: 0.4995 + update_ratio: 1 - x_ini: 0.0 - y_ini: 0.0 - th_ini: 0.0 + x_ini: 1.0 + y_ini: 1.0 + th_ini: 1.57 + #tolerances: [0.005, 0.04] + tolerances: [0.0, 0.0] diff --git a/twil_bringup/config/controllers/mini_twil_dynamics_pid_control.yaml b/twil_bringup/config/controllers/mini_twil_dynamics_pid_control.yaml index a4b4084..a27f5fd 100644 --- a/twil_bringup/config/controllers/mini_twil_dynamics_pid_control.yaml +++ b/twil_bringup/config/controllers/mini_twil_dynamics_pid_control.yaml @@ -10,14 +10,21 @@ mini_twil: - right_wheel_joint feedback_linearization: - A: -7.969445e+00 - B: -1.876616e-02 - C: 3.454858e-01 - D: -9.405481e+00 - E: -5.987630e+00 - F: 3.472006e+00 - - velocity_filter_taps: 5 + A: -9.13781 + B: -0.01999 + C: 0.36874 + D: -8.20998 + E: -1.98363 + F: 3.62234 + + #A: 1 + #B: 1 + #C: 1 + #D: 1 + #E: 1 + #F: 1 + + take: 0 linear_velocity_pid_parameters: p: 16 diff --git a/twil_bringup/config/controllers/mini_twil_joint_velocity_control.yaml b/twil_bringup/config/controllers/mini_twil_joint_velocity_control.yaml index 68491d1..b3e3ddb 100644 --- a/twil_bringup/config/controllers/mini_twil_joint_velocity_control.yaml +++ b/twil_bringup/config/controllers/mini_twil_joint_velocity_control.yaml @@ -8,9 +8,9 @@ mini_twil: type: effort_controllers/JointVelocityController joint: left_wheel_joint pid: - p: 0.5476848 - i: 4.457272 - d: 0.0 + p: 2.455561e-01 + i: 1.814526e+00 + d: 0 i_clamp: 12000 antiwindup: False diff --git a/twil_bringup/config/controllers/mini_twil_linearizing_control.yaml b/twil_bringup/config/controllers/mini_twil_linearizing_control.yaml index 604ec8f..589170f 100644 --- a/twil_bringup/config/controllers/mini_twil_linearizing_control.yaml +++ b/twil_bringup/config/controllers/mini_twil_linearizing_control.yaml @@ -9,13 +9,13 @@ mini_twil: - left_wheel_joint - right_wheel_joint - velocity_filter_taps: 5 + take: 0 feedback_linearization: - A: -7.969445e+00 - B: -1.876616e-02 - C: 3.454858e-01 - D: -9.405481e+00 - E: -5.987630e+00 - F: 3.472006e+00 + A: -8.75680 + B: -0.01814 + C: 0.34788 + D: -7.92150 + E: -1.48242 + F: 3.43286 diff --git a/twil_bringup/config/controllers/mini_twil_nonlinear_backstepping_control.yaml b/twil_bringup/config/controllers/mini_twil_nonlinear_backstepping_control.yaml new file mode 100644 index 0000000..20e877d --- /dev/null +++ b/twil_bringup/config/controllers/mini_twil_nonlinear_backstepping_control.yaml @@ -0,0 +1,53 @@ +mini_twil: + + joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 25 + + nonlinear_backstepping_controller: + type: twil_controllers/NonlinearBacksteppingController + joints: + - left_wheel_joint + - right_wheel_joint + + feedback_linearization: + #A: -7.969445e+00 + #B: -1.876616e-02 + #C: 3.454858e-01 + #D: -9.405481e+00 + #E: -5.987630e+00 + #F: 3.472006e+00 + A: -8.030481e+00 + B: -2.555340e-02 + C: 3.787045e-01 + D: -1.041838e+01 + E: -7.303194e+00 + F: 3.864439e+00 + + take: 0 + + nonsmooth_controller: + lambda1: 0.1 + lambda2: 0.1 + lambda3: 0.1 + lambda4: 0.1 + lambda5: 0.1 + gamma1: 1.0 + gamma2: 1.0 + gamma3: 1.0 + gamma4: 1.0 + update_ratio: 5 + + x_ini: 0.0 + y_ini: 0.0 + th_ini: 0.0 + +#lambda1: 10.0 +#lambda2: 0.1 +#lambda3: 0.1 +#lambda4: 50.0 +#lambda5: 100.0 +#gamma1: 10.0 +#gamma2: 1.0 +#gamma3: 10.0 +#gamma4: 10.0 diff --git a/twil_bringup/config/controllers/mini_twil_twist_pid_backstepping_control.yaml b/twil_bringup/config/controllers/mini_twil_twist_pid_backstepping_control.yaml index 29563b2..e6c0d11 100644 --- a/twil_bringup/config/controllers/mini_twil_twist_pid_backstepping_control.yaml +++ b/twil_bringup/config/controllers/mini_twil_twist_pid_backstepping_control.yaml @@ -11,31 +11,33 @@ mini_twil: - right_wheel_joint left_wheel_joint_pid_parameters: - p: 2.455561e-01 - i: 1.814526e+00 - d: 0 + p: 1.44697 + i: 14.52408 + d: 0.00000 i_clamp: 12000 antiwindup: true right_wheel_joint_pid_parameters: - p: 1.751695e-01 - i: 1.553672e+00 - d: 0 + p: 1.07365 + i: 11.34182 + d: 0.00000 i_clamp: 12000 antiwindup: False - velocity_filter_taps: 3 - take: 0 - + take: 4 nonsmooth_controller: - lambda1: 1000 - lambda2: 10 - lambda3: 0.1 - gamma1: 0.8 - gamma2: 0.8 + lambda1: 1 + lambda2: 1 + lambda3: 1 + gamma1: 2 + gamma2: 0.01 + #gamma1: 0.5577 + #gamma2: 0.5661 + update_ratio: 1 x_ini: 0.0 y_ini: 0.0 th_ini: 0.0 + tolerances: [0.005, 0.05] diff --git a/twil_bringup/config/controllers/mini_twil_twist_pid_control.yaml b/twil_bringup/config/controllers/mini_twil_twist_pid_control.yaml index c12f774..662d02c 100644 --- a/twil_bringup/config/controllers/mini_twil_twist_pid_control.yaml +++ b/twil_bringup/config/controllers/mini_twil_twist_pid_control.yaml @@ -9,18 +9,18 @@ mini_twil: - left_wheel_joint - right_wheel_joint - velocity_filter_taps: 2 + take: 1 left_wheel_joint_pid_parameters: - p: 2.455561e-01 - i: 1.814526e+00 - d: 0 + p: 1.44697 + i: 14.52408 + d: 0.00000 i_clamp: 12000 antiwindup: True right_wheel_joint_pid_parameters: - p: 1.751695e-01 - i: 1.553672e+00 - d: 0 + p: 1.07365 + i: 11.34182 + d: 0.00000 i_clamp: 12000 antiwindup: True diff --git a/twil_bringup/config/controllers/mini_twil_voltage_control.yaml b/twil_bringup/config/controllers/mini_twil_voltage_control.yaml new file mode 100644 index 0000000..f0290d5 --- /dev/null +++ b/twil_bringup/config/controllers/mini_twil_voltage_control.yaml @@ -0,0 +1,12 @@ +mini_twil: + joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 25 + + left_wheel_joint_voltage_controller: + type: twil_controllers/JointVoltageController + joint: left_wheel_joint + + right_wheel_joint_voltage_controller: + type: twil_controllers/JointVoltageController + joint: right_wheel_joint diff --git a/twil_bringup/config/controllers/twil_dynamics_mrac_backstepping_control.yaml b/twil_bringup/config/controllers/twil_dynamics_mrac_backstepping_control.yaml new file mode 100644 index 0000000..ecbf7ae --- /dev/null +++ b/twil_bringup/config/controllers/twil_dynamics_mrac_backstepping_control.yaml @@ -0,0 +1,53 @@ +twil: + + joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 100 + + dynamics_mrac_backstepping_controller: + type: twil_controllers/DynamicsMRACBacksteppingController + joints: + - left_wheel_joint + - right_wheel_joint + + feedback_linearization: + A: -11.25156 + B: 0.01177 + C: 0.09819 + D: -7.55114 + E: -8.40468 + F: 0.49782 + + Alpha: [12.0, 10.0] + + take: 0 + + nonsmooth_controller: + lambda1: 1.0 + lambda2: 1.0 + lambda3: 1.0 + #gamma1: 2.0 + #gamma2: 0.01 + #gamma1: 0.2826 + #gamma2: 0.4995 + + #Tracking + gamma1: 0.8 + gamma2: 0.01 + + update_ratio: 5 + + #x_ini: 1.0 + #y_ini: 1.0 + #th_ini: 1.57 + #x_ini: 0.0 + #y_ini: 0.0 + #th_ini: 0.0 + + #Initial Tracking + x_ini: 0.0 + y_ini: -0.25 + th_ini: 1.57 + + #tolerances: [0.005, 0.01745] + tolerances: [0.0, 0.0] diff --git a/twil_bringup/config/controllers/twil_dynamics_pid_backstepping_control.yaml b/twil_bringup/config/controllers/twil_dynamics_pid_backstepping_control.yaml index 9724795..04b79ea 100644 --- a/twil_bringup/config/controllers/twil_dynamics_pid_backstepping_control.yaml +++ b/twil_bringup/config/controllers/twil_dynamics_pid_backstepping_control.yaml @@ -11,12 +11,12 @@ twil: - right_wheel_joint feedback_linearization: - A: -14.778419353034346 - B: 0.009650740074675 - C: 0.133440027500232 - D: -9.731786200206576 - E: -13.056880796631793 - F: 0.666386266954281 + A: -11.25156 + B: 0.01177 + C: 0.09819 + D: -7.55114 + E: -8.40468 + F: 0.49782 linear_velocity_pid_parameters: p: 16 @@ -32,16 +32,38 @@ twil: i_clamp: 120000 antiwindup: false - take: 0 + take: 1 - nonsmooth_controller: - lambda1: 1000 - lambda2: 10 - lambda3: 0.00001 - gamma1: 1 - gamma2: 1 - update_ratio: 1 + nonsmooth_controller: + lambda1: 1.0 + lambda2: 1.0 + lambda3: 1.0 + + #Tracking + #gamma1: 0.8 + #gamma2: 0.01 - x_ini: 0.0 - y_ini: 0.0 - th_ini: 0 + #gamma1: 2.0 + #gamma2: 0.01 + + #Stabilization + #gamma1: 0.2826 + #gamma2: 0.4995 + + gamma1: 0.2658 + gamma2: 0.4781 + + update_ratio: 5 + + #Initial Stabilization + x_ini: -0.7071 + y_ini: -0.7071 + th_ini: 3.92699 + + #Initial Tracking + #x_ini: 0.0 + #y_ini: -0.25 + #th_ini: 1.57 + + tolerances: [0.005, 0.0175] + #tolerances: [0.0, 0.0] diff --git a/twil_bringup/config/controllers/twil_dynamics_pid_control.yaml b/twil_bringup/config/controllers/twil_dynamics_pid_control.yaml index 3be6e84..9f7d1b3 100644 --- a/twil_bringup/config/controllers/twil_dynamics_pid_control.yaml +++ b/twil_bringup/config/controllers/twil_dynamics_pid_control.yaml @@ -10,28 +10,30 @@ twil: - right_wheel_joint feedback_linearization: - A: -14.778419353034346 - B: 0.009650740074675 - C: 0.133440027500232 - D: -9.731786200206576 - E: -13.056880796631793 - F: 0.666386266954281 + A: -11.25156 + B: 0.01177 + C: 0.09819 + D: -7.55114 + E: -8.40468 + F: 0.49782 - velocity_filter_taps: 10 + take: 5 linear_velocity_pid_parameters: p: 16 i: 64 d: 0.0 i_clamp: 120000 - antiwindup: True + antiwindup: false + publish_state: true angular_velocity_pid_parameters: p: 16 i: 64 d: 0.0 i_clamp: 120000 - antiwindup: True + antiwindup: false + publish_state: true diff --git a/twil_bringup/config/controllers/twil_joint_velocity_control.yaml b/twil_bringup/config/controllers/twil_joint_velocity_control.yaml index 1ec7199..acfd34b 100644 --- a/twil_bringup/config/controllers/twil_joint_velocity_control.yaml +++ b/twil_bringup/config/controllers/twil_joint_velocity_control.yaml @@ -8,8 +8,8 @@ twil: type: effort_controllers/JointVelocityController joint: left_wheel_joint pid: - p: 4.622505e-01 - i: 6.617994e+01 + p: 10.7735 + i: 145.9399 d: 0 i_clamp: 120000 antiwindup: false @@ -18,8 +18,8 @@ twil: type: effort_controllers/JointVelocityController joint: right_wheel_joint pid: - p: 1.158977e+00 - i: 7.266055e+01 + p: 11.8631 + i: 152.7907 d: 0 i_clamp: 120000 antiwindup: false diff --git a/twil_bringup/config/controllers/twil_linearizing_control.yaml b/twil_bringup/config/controllers/twil_linearizing_control.yaml index 16d8ec7..a280426 100644 --- a/twil_bringup/config/controllers/twil_linearizing_control.yaml +++ b/twil_bringup/config/controllers/twil_linearizing_control.yaml @@ -1,20 +1,21 @@ twil: - joint_state_controller: - type: joint_state_controller/JointStateController - publish_rate: 100 + + joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 100 - cart_linearizing_controller: - type: twil_controllers/CartLinearizingController - joints: - - left_wheel_joint - - right_wheel_joint + cart_linearizing_controller: + type: twil_controllers/CartLinearizingController + joints: + - left_wheel_joint + - right_wheel_joint - velocity_filter_taps: 5 - - feedback_linearization: - A: -14.778419353034346 - B: 0.009650740074675 - C: 0.133440027500232 - D: -9.731786200206576 - E: -13.056880796631793 - F: 0.666386266954281 + feedback_linearization: + A: -11.25156 + B: 0.01177 + C: 0.09819 + D: -7.55114 + E: -8.40468 + F: 0.49782 + + take: 16 diff --git a/twil_bringup/config/controllers/twil_twist_pid_backstepping_control.yaml b/twil_bringup/config/controllers/twil_twist_pid_backstepping_control.yaml index 9d5b20d..2aa1689 100644 --- a/twil_bringup/config/controllers/twil_twist_pid_backstepping_control.yaml +++ b/twil_bringup/config/controllers/twil_twist_pid_backstepping_control.yaml @@ -11,29 +11,51 @@ twil: - right_wheel_joint left_wheel_joint_pid_parameters: - p: 4.622505e-01 - i: 6.617994e+01 - d: 0 + p: 3.8102 + i: 31.0213 + d: 0.0000 i_clamp: 120000 antiwindup: false right_wheel_joint_pid_parameters: - p: 1.158977e+00 - i: 7.266055e+01 - d: 0 - i_clamp: 1000 + p: 3.8102 + i: 31.0213 + d: 0.0000 + i_clamp: 12000 antiwindup: false - take: 1 + take: 4 nonsmooth_controller: - lambda1: 1000 - lambda2: 10 - lambda3: 0.00001 - gamma1: 1 - gamma2: 1 - update_ratio: 1 + lambda1: 1.0 + lambda2: 1.0 + lambda3: 1.0 + + #Tracking + #gamma1: 0.8 + #gamma2: 0.01 + #gamma1: 2.0 + #gamma2: 0.01 + + #Stabilization + #gamma1: 0.2826 + #gamma2: 0.4995 + + gamma1: 0.2658 + gamma2: 0.4781 + + update_ratio: 5 + + #Initial Stabilization x_ini: 0.0 - y_ini: 0.0 - th_ini: 0.0 + y_ini: -1.0 + th_ini: 3.1416 + + #Initial Tracking + #x_ini: 0.0 + #y_ini: -0.25 + #th_ini: 1.57 + + tolerances: [0.005, 0.0175] + #tolerances: [0.0, 0.0] diff --git a/twil_bringup/config/controllers/twil_twist_pid_control.yaml b/twil_bringup/config/controllers/twil_twist_pid_control.yaml index bd48c43..1568704 100644 --- a/twil_bringup/config/controllers/twil_twist_pid_control.yaml +++ b/twil_bringup/config/controllers/twil_twist_pid_control.yaml @@ -9,19 +9,18 @@ twil: - left_wheel_joint - right_wheel_joint - velocity_filter_taps: 1 - + take: 5 left_wheel_joint_pid_parameters: - p: 4.622505e-01 - i: 6.617994e+01 - d: 0 + p: 3.8102 + i: 31.0213 + d: 0.0000 i_clamp: 120000 antiwindup: False right_wheel_joint_pid_parameters: - p: 1.158977e+00 - i: 7.266055e+01 - d: 0 + p: 3.8102 + i: 31.0213 + d: 0.0000 i_clamp: 120000 antiwindup: False diff --git a/twil_bringup/config/controllers/twil_voltage_control.yaml b/twil_bringup/config/controllers/twil_voltage_control.yaml new file mode 100644 index 0000000..9678f60 --- /dev/null +++ b/twil_bringup/config/controllers/twil_voltage_control.yaml @@ -0,0 +1,12 @@ +twil: + joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 100 + + left_wheel_joint_voltage_controller: + type: twil_controllers/JointVoltageController + joint: left_wheel_joint + + right_wheel_joint_voltage_controller: + type: twil_controllers/JointVoltageController + joint: right_wheel_joint diff --git a/twil_bringup/config/electronics/mini_twil_hw.yaml b/twil_bringup/config/electronics/mini_twil_hw.yaml index d8465fd..fef8b3b 100644 --- a/twil_bringup/config/electronics/mini_twil_hw.yaml +++ b/twil_bringup/config/electronics/mini_twil_hw.yaml @@ -5,17 +5,24 @@ mini_twil: can_bus_number: 0 can_iface: 0 can_id_number: 2 - dead_zone_compensation: False - neg_dead_zone: 0 - pos_dead_zone: 0 + dead_zone_compensation: false + #pos_dead_zone: 1.7 + #neg_dead_zone: -2.0 + pos_dead_zone: 1.4 + neg_dead_zone: -1.4 right_wheel_joint: aic_com_device: 2 can_bus_number: 0 can_iface: 0 can_id_number: 3 - dead_zone_compensation: False - neg_dead_zone: 0 - pos_dead_zone: 0 + dead_zone_compensation: false + #pos_dead_zone: 1.6 + #neg_dead_zone: -1.4 + pos_dead_zone: 1.3 + neg_dead_zone: -1.3 + + motor_nominal_voltage: 6 + battery_nominal_voltage: 11.2 control_loop_rate: 25 diff --git a/twil_bringup/config/electronics/twil_hw.yaml b/twil_bringup/config/electronics/twil_hw.yaml index f034707..7c979f9 100644 --- a/twil_bringup/config/electronics/twil_hw.yaml +++ b/twil_bringup/config/electronics/twil_hw.yaml @@ -4,17 +4,24 @@ twil: aic_com_device: 1 serial_port: /dev/ttyUSB0 - dead_zone_compensation: False - neg_dead_zone: -4.9 - pos_dead_zone: 3.4 + dead_zone_compensation: false + neg_dead_zone: -1.8 + pos_dead_zone: 0.7 + #neg_dead_zone: -4.8 + #pos_dead_zone: 3.4 right_wheel_joint: aic_com_device: 1 serial_port: /dev/ttyUSB1 - dead_zone_compensation: False - neg_dead_zone: -3.3 - pos_dead_zone: 3.7 + dead_zone_compensation: false + neg_dead_zone: -0.7 + pos_dead_zone: 1.5 + #neg_dead_zone: -3.1 + #pos_dead_zone: 3.7 + + motor_nominal_voltage: 12 + battery_nominal_voltage: 12 control_loop_rate: 100 diff --git a/twil_bringup/launch/controllers/adaptive_dynamics_mrac_backstepping_controller.launch b/twil_bringup/launch/controllers/adaptive_dynamics_mrac_backstepping_controller.launch new file mode 100644 index 0000000..0e7e17e --- /dev/null +++ b/twil_bringup/launch/controllers/adaptive_dynamics_mrac_backstepping_controller.launch @@ -0,0 +1,32 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/twil_bringup/launch/controllers/adaptive_dynamics_pid_backstepping_controller.launch b/twil_bringup/launch/controllers/adaptive_dynamics_pid_backstepping_controller.launch new file mode 100644 index 0000000..fd086e0 --- /dev/null +++ b/twil_bringup/launch/controllers/adaptive_dynamics_pid_backstepping_controller.launch @@ -0,0 +1,32 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/twil_bringup/launch/controllers/adaptive_dynamics_pid_controller.launch b/twil_bringup/launch/controllers/adaptive_dynamics_pid_controller.launch new file mode 100644 index 0000000..2849c2c --- /dev/null +++ b/twil_bringup/launch/controllers/adaptive_dynamics_pid_controller.launch @@ -0,0 +1,35 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/twil_bringup/launch/controllers/dynamics_mrac_backstepping_controller.launch b/twil_bringup/launch/controllers/dynamics_mrac_backstepping_controller.launch new file mode 100644 index 0000000..f956d5e --- /dev/null +++ b/twil_bringup/launch/controllers/dynamics_mrac_backstepping_controller.launch @@ -0,0 +1,29 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/twil_bringup/launch/controllers/dynamics_pid_backstepping_controller.launch b/twil_bringup/launch/controllers/dynamics_pid_backstepping_controller.launch index 2028903..e9e0245 100644 --- a/twil_bringup/launch/controllers/dynamics_pid_backstepping_controller.launch +++ b/twil_bringup/launch/controllers/dynamics_pid_backstepping_controller.launch @@ -3,11 +3,18 @@ + - - + + + + + + + + @@ -16,7 +23,7 @@ output="screen" ns="$(arg Robot_Name)" args="dynamics_pid_backstepping_controller joint_state_controller"> - + diff --git a/twil_bringup/launch/controllers/dynamics_pid_controller.launch b/twil_bringup/launch/controllers/dynamics_pid_controller.launch index 30176da..f7a8f17 100644 --- a/twil_bringup/launch/controllers/dynamics_pid_controller.launch +++ b/twil_bringup/launch/controllers/dynamics_pid_controller.launch @@ -3,11 +3,18 @@ + - - + + + + + + + + diff --git a/twil_bringup/launch/controllers/joint_velocity_controller.launch b/twil_bringup/launch/controllers/joint_velocity_controller.launch index 3de11de..b3b1ac4 100644 --- a/twil_bringup/launch/controllers/joint_velocity_controller.launch +++ b/twil_bringup/launch/controllers/joint_velocity_controller.launch @@ -3,11 +3,18 @@ + - - + + + + + + + + diff --git a/twil_bringup/launch/controllers/joint_voltage_controller.launch b/twil_bringup/launch/controllers/joint_voltage_controller.launch new file mode 100644 index 0000000..5565804 --- /dev/null +++ b/twil_bringup/launch/controllers/joint_voltage_controller.launch @@ -0,0 +1,29 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/twil_bringup/launch/controllers/linearizing_controller.launch b/twil_bringup/launch/controllers/linearizing_controller.launch index bb1e5f8..b9e7db9 100644 --- a/twil_bringup/launch/controllers/linearizing_controller.launch +++ b/twil_bringup/launch/controllers/linearizing_controller.launch @@ -3,11 +3,18 @@ + - - + + + + + + + + diff --git a/twil_bringup/launch/controllers/nonlinear_backstepping_controller.launch b/twil_bringup/launch/controllers/nonlinear_backstepping_controller.launch new file mode 100644 index 0000000..1f713af --- /dev/null +++ b/twil_bringup/launch/controllers/nonlinear_backstepping_controller.launch @@ -0,0 +1,29 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/twil_bringup/launch/controllers/twist_pid_backstepping_controller.launch b/twil_bringup/launch/controllers/twist_pid_backstepping_controller.launch index 37dd661..0d36a3d 100644 --- a/twil_bringup/launch/controllers/twist_pid_backstepping_controller.launch +++ b/twil_bringup/launch/controllers/twist_pid_backstepping_controller.launch @@ -3,11 +3,18 @@ + - - + + + + + + + + diff --git a/twil_bringup/launch/controllers/twist_pid_controller.launch b/twil_bringup/launch/controllers/twist_pid_controller.launch index 1bdc7e6..e73b60c 100644 --- a/twil_bringup/launch/controllers/twist_pid_controller.launch +++ b/twil_bringup/launch/controllers/twist_pid_controller.launch @@ -3,11 +3,18 @@ + - - + + + + + + + + diff --git a/twil_bringup/launch/robot_hw.launch b/twil_bringup/launch/robot_hw.launch index 01a5a1f..23b5166 100644 --- a/twil_bringup/launch/robot_hw.launch +++ b/twil_bringup/launch/robot_hw.launch @@ -9,13 +9,9 @@ - - - - - + diff --git a/twil_bringup/launch/robot_mechanics.launch b/twil_bringup/launch/robot_mechanics.launch new file mode 100644 index 0000000..f117d65 --- /dev/null +++ b/twil_bringup/launch/robot_mechanics.launch @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/twil_bringup/launch/trajectory/mini_twil_circle_trajectory.launch b/twil_bringup/launch/trajectory/mini_twil_circle_trajectory.launch index a1b197b..c5b4022 100644 --- a/twil_bringup/launch/trajectory/mini_twil_circle_trajectory.launch +++ b/twil_bringup/launch/trajectory/mini_twil_circle_trajectory.launch @@ -9,9 +9,9 @@ - - - + + + diff --git a/twil_bringup/launch/trajectory/mini_twil_eight_trajectory.launch b/twil_bringup/launch/trajectory/mini_twil_eight_trajectory.launch index 9bbc561..312f871 100644 --- a/twil_bringup/launch/trajectory/mini_twil_eight_trajectory.launch +++ b/twil_bringup/launch/trajectory/mini_twil_eight_trajectory.launch @@ -10,8 +10,8 @@ - - + + diff --git a/twil_bringup/launch/trajectory/twil_circle_trajectory.launch b/twil_bringup/launch/trajectory/twil_circle_trajectory.launch index 31babb9..bd31710 100644 --- a/twil_bringup/launch/trajectory/twil_circle_trajectory.launch +++ b/twil_bringup/launch/trajectory/twil_circle_trajectory.launch @@ -9,9 +9,9 @@ - + - + diff --git a/twil_bringup/launch/trajectory/twil_eight_trajectory.launch b/twil_bringup/launch/trajectory/twil_eight_trajectory.launch index 1f14b76..98d297f 100644 --- a/twil_bringup/launch/trajectory/twil_eight_trajectory.launch +++ b/twil_bringup/launch/trajectory/twil_eight_trajectory.launch @@ -9,9 +9,9 @@ - - - + + + diff --git a/twil_bringup/package.xml b/twil_bringup/package.xml index 2c60d71..709da8c 100644 --- a/twil_bringup/package.xml +++ b/twil_bringup/package.xml @@ -40,26 +40,16 @@ catkin - controller_interface - controller_manager - hardware_interface - joint_state_controller roscpp rospy std_msgs - twil_hw sensor_msgs trajectory_msgs geometry_msgs - controller_interface - controller_manager - hardware_interface - joint_state_controller roscpp rospy std_msgs - twil_hw sensor_msgs trajectory_msgs geometry_msgs diff --git a/twil_controllers/CMakeLists.txt b/twil_controllers/CMakeLists.txt index a0fe050..e41cb6c 100644 --- a/twil_controllers/CMakeLists.txt +++ b/twil_controllers/CMakeLists.txt @@ -1,36 +1,31 @@ -cmake_minimum_required(VERSION 2.8.3) + cmake_minimum_required(VERSION 2.8.3) project(twil_controllers) -## Add support for C++11, supported in ROS Kinetic and newer add_definitions(-std=c++11) -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages find_package(catkin REQUIRED COMPONENTS - controller_interface - control_msgs - control_toolbox - urdf - roscpp - rospy - ) + controller_interface + effort_controllers + geometry_msgs + control_msgs + control_toolbox + nav_msgs + roscpp + tf + arc_odometry + roscpp + rospy + twil_msgs + twil_hardware_interfaces + ) find_package(cmake_modules REQUIRED) - - -## System dependencies are found with CMake's conventions # find_package(Boost REQUIRED COMPONENTS system) find_package(Eigen REQUIRED) find_package(orocos_kdl REQUIRED) find_package(kdl_parser REQUIRED) -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() - ################################################ ## Declare ROS messages, services and actions ## ################################################ @@ -57,10 +52,8 @@ find_package(kdl_parser REQUIRED) ## Generate messages in the 'msg' folder # add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) +# FILES +# ) ## Generate services in the 'srv' folder # add_service_files( @@ -78,9 +71,10 @@ find_package(kdl_parser REQUIRED) ## Generate added messages and services with any dependencies listed here # generate_messages( -# DEPENDENCIES -# std_msgs # Or other packages containing msgs -# ) +# DEPENDENCIES + # std_msgs # Or other packages containing msgs + # geometry_msgs +# ) ################################### ## catkin specific configuration ## @@ -92,78 +86,83 @@ find_package(kdl_parser REQUIRED) ## CATKIN_DEPENDS: catkin_packages dependent projects also need ## DEPENDS: system dependencies of this project that dependent projects also need catkin_package( - # INCLUDE_DIRS include - LIBRARIES ${PROJECT_NAME} - CATKIN_DEPENDS controller_interface control_toolbox - # DEPENDS system_lib - DEPENDS Eigen - ) + INCLUDE_DIRS include + LIBRARIES ${PROJECT_NAME} + CATKIN_DEPENDS controller_interface + control_toolbox + effort_controllers + geometry_msgs + nav_msgs + roscpp + tf + arc_odometry + twil_msgs + DEPENDS Eigen + ) ########### ## Build ## ########### -## Specify additional locations of header files -## Your package locations should be listed before other locations include_directories( - include - ${catkin_INCLUDE_DIRS} - ${Eigen_INCLUDE_DIRS} - ) + include + ${catkin_INCLUDE_DIRS} + ${Eigen_INCLUDE_DIRS} + ) -## Declare a cpp library add_library(twil_controllers - src/odometry_tool.cpp - src/pose_tool.cpp - src/cart_linearizing_controller.cpp - src/dynamics_pid_controller.cpp - src/dynamics_pid_backstepping_controller.cpp - src/nonlinear_backstepping_controller.cpp - src/twist_pid_backstepping_controller.cpp - src/twist_pid_controller.cpp - ) - -## Declare a cpp executable -# add_executable(twil_controllers_node src/twil_controllers_node.cpp) + src/cart_linearizing_controller.cpp + src/dynamics_pid_controller.cpp + src/dynamics_pid_backstepping_controller.cpp + src/dynamics_mrac_backstepping_controller.cpp + src/nonlinear_backstepping_controller.cpp + src/twist_pid_backstepping_controller.cpp + src/twist_pid_controller.cpp + src/state_feedback_linearization.cpp +# Bypass Controllers + src/joint_voltage_controller.cpp + src/joint_group_voltage_controller.cpp + ) + +add_executable( + # Adaptive Dynamics + adaptive_dynamics_identification src/adaptive_dynamics_identification.cpp + ) ## Add cmake target dependencies of the executable/library ## as an example, message headers may need to be generated before nodes -# add_dependencies(twil_controllers_node twil_controllers_generate_messages_cpp) +add_dependencies(twil_controllers ${catkin_EXPORTED_TARGETS}) ## Specify libraries to link a library or executable target against target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} - ${orocos-kdl_LIBRARIES} - ${kdl_parser_LIBRARIES} - ) + ${catkin_LIBRARIES} + ${orocos-kdl_LIBRARIES} + ${kdl_parser_LIBRARIES} + ) + +target_link_libraries(adaptive_dynamics_identification + ${catkin_LIBRARIES} + ${orocos-kdl_LIBRARIES} + ${kdl_parser_LIBRARIES} + ) ############# ## Install ## ############# -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# install(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables and/or libraries for installation install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} - ) + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ) + -## Mark cpp header files for installation install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} - # FILES_MATCHING PATTERN "*.h" - # PATTERN ".svn" EXCLUDE - ) + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE + ) ## Mark other files for installation (e.g. launch and bag files, etc.) # install(FILES @@ -172,15 +171,4 @@ install(DIRECTORY include/${PROJECT_NAME}/ # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} # ) -############# -## Testing ## -############# - -## Add gtest twistd cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_twil_controllers.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) diff --git a/twil_controllers/include/twil_controllers/cart_linearizing_controller.h b/twil_controllers/include/twil_controllers/cart_linearizing_controller.h index 79c5a80..cc89bcd 100644 --- a/twil_controllers/include/twil_controllers/cart_linearizing_controller.h +++ b/twil_controllers/include/twil_controllers/cart_linearizing_controller.h @@ -27,14 +27,16 @@ #define TWIL_CONTROLLERS_CART_LINEARIZING_CONTROLLER_H #include -#include +//#include #include #include +#include #include #include "Eigen/Dense" #include "state_feedback_linearization.h" -#include "velocity_transformation.h" +#include +#include "twil_hardware_interfaces/joint_command_extended_interface.h" #define CART_LIN_DEBUG @@ -44,31 +46,42 @@ namespace twil_controllers { -class CartLinearizingController: public controller_interface::Controller +class CartLinearizingController: public controller_interface::Controller { public: CartLinearizingController(void); ~CartLinearizingController(void); - bool init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n); + bool init(twil_hardware_interfaces::VoltageJointInterface *robot, ros::NodeHandle &n); void starting(const ros::Time& time); void update(const ros::Time& time,const ros::Duration& duration); void stopping(void); - void commandCB(const std_msgs::Float64MultiArray::ConstPtr &command); + void commandCB(const geometry_msgs::Accel &command); EIGEN_MAKE_ALIGNED_OPERATOR_NEW private: ros::NodeHandle node_; - hardware_interface::EffortJointInterface *robot_; + twil_hardware_interfaces::VoltageJointInterface *robot_; std::vector joints_; - StateFeedbackLinearization * feedback_linearization; + StateFeedbackLinearization * feedback_linearization_; std::string robot_ns; ros::Subscriber sub_command_; - Eigen::Vector2d Effort; + Eigen::Vector2d voltage_effort_; Eigen::Vector2d nu_ref; - Mat2_4 Fu; + K_parameters_t Dyn_params; + + /* Model Variables */ + std::vector WheelRadius; + double WheelBase; + + /* Odometer Variables */ + arc_odometry::DiffOdometry odom_; + double time_step_; + ros::Time lastSamplingTime_; + Eigen::Vector2d phi_; + Eigen::Vector3d Pose_; #ifdef CART_LIN_DEBUG std::ofstream fd; @@ -77,8 +90,7 @@ private: int take; #endif - std::vector WheelRadius; - double WheelBase; + }; } #endif diff --git a/twil_controllers/include/twil_controllers/dynamics_mrac_backstepping_controller.h b/twil_controllers/include/twil_controllers/dynamics_mrac_backstepping_controller.h new file mode 100644 index 0000000..6567ad7 --- /dev/null +++ b/twil_controllers/include/twil_controllers/dynamics_mrac_backstepping_controller.h @@ -0,0 +1,130 @@ +/****************************************************************************** + Twil Controllers + Linearizing MRAC Backstep Controller + + Copyright (C) 2017 Tiago G. Alves + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program; if not, write to the Free Software + Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + + You can also obtain a copy of the GNU General Public License + at . + +*******************************************************************************/ + +#ifndef DYNAMICS_MRAC_BACKSTEPPING_CONTROLLER_H +#define DYNAMICS_MRAC_BACKSTEPPING_CONTROLLER_H + +#include +//#include +#include + +#include +#include +#include + +#include "non_smooth_control.h" +#include "state_feedback_linearization.h" +#include + +#include +#include +#include "twil_msgs/PoseControllerStatus.h" +#include "twil_msgs/non_smooth_parameters.h" +#include "twil_msgs/robot_dynamics.h" +#include "twil_hardware_interfaces/joint_command_extended_interface.h" + +#define DYNAMICS_MRAC_BACKSTEPPING_CONTROLLER_DEBUG + +#ifdef DYNAMICS_MRAC_BACKSTEPPING_CONTROLLER_DEBUG +#include +#endif + + +namespace twil_controllers{ + +class DynamicsMRACBacksteppingController: public controller_interface::Controller +{ + +public: + DynamicsMRACBacksteppingController(); + ~DynamicsMRACBacksteppingController(); + bool init(twil_hardware_interfaces::VoltageJointInterface *robot, ros::NodeHandle &n); + void starting(const ros::Time &time); + void update(const ros::Time &time, const ros::Duration &duration); + void stopping(const ros::Time &time); + void commandCB(const geometry_msgs::PoseStampedConstPtr &command); + void command2dCB(const geometry_msgs::Pose2DConstPtr &command); + void parametersCB(const twil_msgs::non_smooth_parametersConstPtr ¶m); + void dynParametersCB(const twil_msgs::robot_dynamicsConstPtr ¶m); + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + +private: + + /* ROS Variables and Structures */ + ros::Subscriber position_sub_[2]; + ros::Subscriber controller_parameters_sub_; + ros::Subscriber dynamics_parameters_sub_; + + ros::NodeHandle node_; + twil_hardware_interfaces::VoltageJointInterface *robot_; + std::vector joints_; + std::string robot_ns_; + + /* Dynamics MRAC Controller Variables */ + StateFeedbackLinearization * feedback_linearization_; + Eigen::Vector2d voltage_effort_; + Eigen::Vector2d nu_ref_; + Eigen::Vector2d u_ref_; + K_parameters_t Dyn_params; + boost::mutex feedback_linearization_mutex_; + + /* MRAC Controller Variables */ + Eigen::Vector2d uModel_; + Eigen::DiagonalMatrix Alpha_; + + /* Nonlinear Kinematic Controller Variables */ + Eigen::Vector3d lambda; + Eigen::Vector2d gamma; + Eigen::Vector3d pose_ref_; + NonSmoothControl * nonsmooth_control_; + int update_ratio; + + /* Model Variables */ + std::vector WheelRadius; + double WheelBase; + + /* Odometer Variables */ + arc_odometry::DiffOdometry odom_; + double time_step_; + ros::Time lastSamplingTime_; + Eigen::Vector2d phi_; + Eigen::Vector3d pose_; + + /* Controller Status */ + boost::scoped_ptr > status_publisher_ ; + +#ifdef DYNAMICS_MRAC_BACKSTEPPING_CONTROLLER_DEBUG + std::ofstream fd; + bool makeLog; + std::string log_date; + int take; +#endif + +}; + +} + +#endif + diff --git a/twil_controllers/include/twil_controllers/dynamics_pid_backstepping_controller.h b/twil_controllers/include/twil_controllers/dynamics_pid_backstepping_controller.h index c969434..f7bfec0 100644 --- a/twil_controllers/include/twil_controllers/dynamics_pid_backstepping_controller.h +++ b/twil_controllers/include/twil_controllers/dynamics_pid_backstepping_controller.h @@ -27,15 +27,23 @@ #define DYNAMICS_PID_BACKSTEPPING_CONTROLLER_H #include -#include +//#include #include #include #include #include -#include "twil_controllers/odometry_tool.h" -#include "twil_controllers/velocity_transformation.h" +#include + #include "non_smooth_control.h" #include "state_feedback_linearization.h" +#include + +#include +#include +#include "twil_msgs/PoseControllerStatus.h" +#include "twil_msgs/non_smooth_parameters.h" +#include "twil_msgs/robot_dynamics.h" +#include "twil_hardware_interfaces/joint_command_extended_interface.h" #define DYNAMICS_PID_BACKSTEPPING_CONTROLLER_DEBUG @@ -46,50 +54,63 @@ namespace twil_controllers{ -class DynamicsPIDBacksteppingController: public controller_interface::Controller +class DynamicsPIDBacksteppingController: public controller_interface::Controller { public: DynamicsPIDBacksteppingController(); ~DynamicsPIDBacksteppingController(); - bool init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n); - void starting(const ros::Time &); + bool init(twil_hardware_interfaces::VoltageJointInterface *robot, ros::NodeHandle &n); + void starting(const ros::Time &time); void update(const ros::Time &time, const ros::Duration &duration); - void stopping(const ros::Time &); + void stopping(const ros::Time &time); void commandCB(const geometry_msgs::PoseStampedConstPtr &command); void command2dCB(const geometry_msgs::Pose2DConstPtr &command); + void parametersCB(const twil_msgs::non_smooth_parametersConstPtr ¶m); + void dynParametersCB(const twil_msgs::robot_dynamicsConstPtr ¶m); EIGEN_MAKE_ALIGNED_OPERATOR_NEW private: /* ROS Variables and Structures */ - ros::Subscriber PositionSub[2]; - control_toolbox::Pid PID[2]; + ros::Subscriber position_sub_[2]; + ros::Subscriber controller_parameters_sub_; + ros::Subscriber dynamics_parameters_sub_; + control_toolbox::Pid pid_[2]; ros::NodeHandle node_; - hardware_interface::EffortJointInterface *robot_; + twil_hardware_interfaces::VoltageJointInterface *robot_; std::vector joints_; std::string robot_ns; /* Dynamics PID Controller Variables */ - StateFeedbackLinearization * feedback_linearization; - Eigen::Vector2d Effort; - Eigen::Vector2d nu_ref; - Eigen::Vector2d u_ref; - Mat2_4 Fu; + StateFeedbackLinearization * feedback_linearization_; + Eigen::Vector2d voltage_effort_; + Eigen::Vector2d nu_ref_; + Eigen::Vector2d u_ref_; + K_parameters_t Dyn_params; + boost::mutex feedback_linearization_mutex_; /* Nonlinear Kinematic Controller Variables */ Eigen::Vector3d lambda; Eigen::Vector2d gamma; - Eigen::Vector3d pose_ref; - OdometryTool * Odometer; - NonSmoothControl * Kinematics; + Eigen::Vector3d pose_ref_; + NonSmoothControl * nonsmooth_control_; int update_ratio; - /* Model Variables */ - Eigen::Vector2d WheelRadius; + std::vector WheelRadius; double WheelBase; + /* Odometer Variables */ + arc_odometry::DiffOdometry odom_; + double time_step_; + ros::Time lastSamplingTime_; + Eigen::Vector2d phi_; + Eigen::Vector3d pose_; + + /* Controller Status */ + boost::scoped_ptr > status_publisher_ ; + #ifdef DYNAMICS_PID_BACKSTEPPING_CONTROLLER_DEBUG std::ofstream fd; bool makeLog; diff --git a/twil_controllers/include/twil_controllers/dynamics_pid_controller.h b/twil_controllers/include/twil_controllers/dynamics_pid_controller.h index d70de47..319f6c3 100644 --- a/twil_controllers/include/twil_controllers/dynamics_pid_controller.h +++ b/twil_controllers/include/twil_controllers/dynamics_pid_controller.h @@ -28,12 +28,20 @@ #include #include -#include +//#include #include #include #include -#include "velocity_transformation.h" +#include + #include "state_feedback_linearization.h" +#include + +#include +#include +#include "twil_msgs/VelocityControllerStatus.h" +#include "twil_msgs/robot_dynamics.h" +#include "twil_hardware_interfaces/joint_command_extended_interface.h" #define DYNAMICS_PID_DEBUG @@ -43,40 +51,56 @@ namespace twil_controllers { -class DynamicsPIDController: public controller_interface::Controller +class DynamicsPIDController: public controller_interface::Controller { public: DynamicsPIDController(void); ~DynamicsPIDController(void); - bool init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n); + bool init(twil_hardware_interfaces::VoltageJointInterface *robot, ros::NodeHandle &n); void starting(const ros::Time& time); void update(const ros::Time& time,const ros::Duration& duration); void stopping(void); void commandCB(const geometry_msgs::TwistConstPtr &CommandVel); + void dynParametersCB(const twil_msgs::robot_dynamicsConstPtr ¶m); EIGEN_MAKE_ALIGNED_OPERATOR_NEW - private: +private: /* ROS Variables and Structures */ - ros::Subscriber VelocitySub; - control_toolbox::Pid PID[2]; + ros::Subscriber velocity_sub_; + ros::Subscriber dynamics_parameters_sub_; + control_toolbox::Pid pid_[2]; ros::NodeHandle node_; - hardware_interface::EffortJointInterface *robot_; + twil_hardware_interfaces::VoltageJointInterface *robot_; std::vector joints_; - std::string robot_ns; + std::string robot_ns_; + + /* Dynamics PID Controller Variables */ + StateFeedbackLinearization * feedback_linearization_; + Eigen::Vector2d voltage_effort_; + Eigen::Vector2d nu_ref_; + Eigen::Vector2d u_ref_; + K_parameters_t Dyn_params; + boost::mutex feedback_linearization_mutex_; - /* Dynamics Controller Variables */ - StateFeedbackLinearization * feedback_linearization; - Eigen::Vector2d Effort; - Eigen::Vector2d nu_ref; - Eigen::Vector2d u_ref; - Mat2_4 Fu; + /* Model Variables */ + std::vector WheelRadius; + double WheelBase; + /* Odometer Variables */ + arc_odometry::DiffOdometry odom_; + double time_step_; + ros::Time lastSamplingTime_; + Eigen::Vector2d phi_; + Eigen::Vector3d pose_; + + /* Controller Status */ + boost::scoped_ptr > status_publisher_ ; #ifdef DYNAMICS_PID_DEBUG std::ofstream fd; @@ -85,8 +109,7 @@ public: int take; #endif - std::vector WheelRadius; - double WheelBase; + }; } #endif diff --git a/twil_controllers/include/twil_controllers/forward_command_controller.h b/twil_controllers/include/twil_controllers/forward_command_controller.h new file mode 100644 index 0000000..0af9b8c --- /dev/null +++ b/twil_controllers/include/twil_controllers/forward_command_controller.h @@ -0,0 +1,64 @@ +#ifndef FORWARD_COMMAND_CONTROLLER_FORWARD_COMMAND_CONTROLLER_H +#define FORWARD_COMMAND_CONTROLLER_FORWARD_COMMAND_CONTROLLER_H + +#include +#include +#include +#include +#include + + +namespace twil_controllers +{ + +/** + * \brief Single joint controller. + * + * This class passes the command signal down to the joint. + * Command signal and joint hardware interface are of the same type, e.g. effort commands for an effort-controlled + * joint. + * + * \tparam T Type implementing the JointCommandInterface. + * + * \section ROS interface + * + * \param type hardware interface type. + * \param joint Name of the joint to control. + * + * Subscribes to: + * - \b command (std_msgs::Float64) : The joint command to apply. + */ +template +class ForwardCommandController: public controller_interface::Controller +{ +public: + ForwardCommandController() {} + ~ForwardCommandController() {sub_command_.shutdown();} + + bool init(T* hw, ros::NodeHandle &n) + { + std::string joint_name; + if (!n.getParam("joint", joint_name)) + { + ROS_ERROR("No joint given (namespace: %s)", n.getNamespace().c_str()); + return false; + } + joint_ = hw->getHandle(joint_name); + sub_command_ = n.subscribe("command", 1, &ForwardCommandController::commandCB, this); + return true; + } + + void starting(const ros::Time& time); + void update(const ros::Time& /*time*/, const ros::Duration& /*period*/) {joint_.setCommand(*command_buffer_.readFromRT());} + + hardware_interface::JointHandle joint_; + realtime_tools::RealtimeBuffer command_buffer_; + +private: + ros::Subscriber sub_command_; + void commandCB(const std_msgs::Float64ConstPtr& msg) {command_buffer_.writeFromNonRT(msg->data);} +}; + +} + +#endif diff --git a/twil_controllers/include/twil_controllers/forward_joint_group_command_controller.h b/twil_controllers/include/twil_controllers/forward_joint_group_command_controller.h new file mode 100644 index 0000000..d490d1c --- /dev/null +++ b/twil_controllers/include/twil_controllers/forward_joint_group_command_controller.h @@ -0,0 +1,103 @@ +#ifndef FORWARD_COMMAND_CONTROLLER_FORWARD_JOINT_GROUP_COMMAND_CONTROLLER_H +#define FORWARD_COMMAND_CONTROLLER_FORWARD_JOINT_GROUP_COMMAND_CONTROLLER_H + +#include +#include + +#include +#include +#include +#include +#include + + +namespace twil_controllers +{ + +/** + * \brief Forward command controller for a set of joints. + * + * This class forwards the command signal down to a set of joints. + * Command signal and joint hardware interface are of the same type, e.g. effort commands for an effort-controlled + * joint. + * + * \tparam T Type implementing the JointCommandInterface. + * + * \section ROS interface + * + * \param type hardware interface type. + * \param joints Names of the joints to control. + * + * Subscribes to: + * - \b command (std_msgs::Float64MultiArray) : The joint commands to apply. + */ +template +class ForwardJointGroupCommandController: public controller_interface::Controller +{ +public: + ForwardJointGroupCommandController() {} + ~ForwardJointGroupCommandController() {sub_command_.shutdown();} + + bool init(T* hw, ros::NodeHandle &n) + { + // List of controlled joints + std::string param_name = "joints"; + if(!n.getParam(param_name, joint_names_)) + { + ROS_ERROR_STREAM("Failed to getParam '" << param_name << "' (namespace: " << n.getNamespace() << ")."); + return false; + } + n_joints_ = joint_names_.size(); + + if(n_joints_ == 0){ + ROS_ERROR_STREAM("List of joint names is empty."); + return false; + } + for(unsigned int i=0; igetHandle(joint_names_[i])); + } + catch (const hardware_interface::HardwareInterfaceException& e) + { + ROS_ERROR_STREAM("Exception thrown: " << e.what()); + return false; + } + } + + commands_buffer_.writeFromNonRT(std::vector(n_joints_, 0.0)); + + sub_command_ = n.subscribe("command", 1, &ForwardJointGroupCommandController::commandCB, this); + return true; + } + + void starting(const ros::Time& time); + void update(const ros::Time& /*time*/, const ros::Duration& /*period*/) + { + std::vector & commands = *commands_buffer_.readFromRT(); + for(unsigned int i=0; i joint_names_; + std::vector< hardware_interface::JointHandle > joints_; + realtime_tools::RealtimeBuffer > commands_buffer_; + unsigned int n_joints_; + +private: + ros::Subscriber sub_command_; + void commandCB(const std_msgs::Float64MultiArrayConstPtr& msg) + { + if(msg->data.size()!=n_joints_) + { + ROS_ERROR_STREAM("Dimension of command (" << msg->data.size() << ") does not match number of joints (" << n_joints_ << ")! Not executing!"); + return; + } + commands_buffer_.writeFromNonRT(msg->data); + } +}; + +} + +#endif diff --git a/twil_controllers/include/twil_controllers/joint_group_voltage_controller.h b/twil_controllers/include/twil_controllers/joint_group_voltage_controller.h new file mode 100644 index 0000000..1bfcc5b --- /dev/null +++ b/twil_controllers/include/twil_controllers/joint_group_voltage_controller.h @@ -0,0 +1,27 @@ +#ifndef EFFORT_CONTROLLERS_JOINT_GROUP_VOLTAGE_CONTROLLER_H +#define EFFORT_CONTROLLERS_JOINT_GROUP_VOLTAGE_CONTROLLER_H + +#include +#include "twil_hardware_interfaces/joint_command_extended_interface.h" + +namespace twil_controllers +{ + +/** + * \brief Forward command controller for a set of effort controlled joints (torque or force). + * + * This class forwards the commanded efforts down to a set of joints. + * + * \section ROS interface + * + * \param type Must be "JointGroupEffortController". + * \param joints List of names of the joints to control. + * + * Subscribes to: + * - \b command (std_msgs::Float64MultiArray) : The joint efforts to apply + */ +typedef twil_controllers::ForwardJointGroupCommandController + JointGroupVoltageController; +} + +#endif diff --git a/twil_controllers/include/twil_controllers/joint_voltage_controller.h b/twil_controllers/include/twil_controllers/joint_voltage_controller.h new file mode 100644 index 0000000..5208906 --- /dev/null +++ b/twil_controllers/include/twil_controllers/joint_voltage_controller.h @@ -0,0 +1,27 @@ +#ifndef EFFORT_CONTROLLERS_JOINT_VOLTAGE_CONTROLLER_H +#define EFFORT_CONTROLLERS_JOINT_VOLTAGE_CONTROLLER_H + +#include +#include "twil_hardware_interfaces/joint_command_extended_interface.h" + +namespace twil_controllers +{ + +/** + * \brief Joint Effort Controller (torque or force) + * + * This class passes the commanded effort down to the joint. + * + * \section ROS interface + * + * \param type Must be "JointEffortController". + * \param joint Name of the joint to control. + * + * Subscribes to: + * - \b command (std_msgs::Float64) : The joint effort to apply. + */ +typedef twil_controllers::ForwardCommandController + JointVoltageController; +} + +#endif diff --git a/twil_controllers/include/twil_controllers/non_smooth_control.h b/twil_controllers/include/twil_controllers/non_smooth_control.h index 631c2a0..87c2162 100644 --- a/twil_controllers/include/twil_controllers/non_smooth_control.h +++ b/twil_controllers/include/twil_controllers/non_smooth_control.h @@ -30,72 +30,132 @@ #include #include -//#define NONSMOOTH_CONTROLLER_OUTPUT_SCREEN +#include + +#define NONSMOOTH_CONTROLLER_OUTPUT_SCREEN class NonSmoothControl { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW NonSmoothControl(const Eigen::Vector3d &lambda, const Eigen::Vector2d &gamma) - : lamb(lambda), gam(gamma) { + : lamb(lambda), gam(gamma) +#ifdef NONSMOOTH_CONTROLLER_OUTPUT_SCREEN + , fd("~/output_fetter.txt") +#endif + { + std::cout << "Parametros [lb, gm]: "< 1e-6) ? - (gam[0]*(lamb[2]/lamb[1])*cos(alpha)*(sin(alpha)/alpha)*psi) : (gam[0]*(lamb[2]/lamb[1])*cos(alpha)*(1)*psi); - } + // Compute velocity references + u_r[0] = -gam[0]*e*cos(alpha); + u_r[1] = -(gam[1]*alpha) -(gam[0]*sin(alpha)*cos(alpha)); + fd << u_r[0] << "\t" << u_r[1] << "\t"; + + // Avoid singularity + // if (fabs(e) < DBL_EPSILON) psi = fabs(psi); + // if (fabs(e) < DBL_EPSILON) psi = 0; + u_r[1] += (fabs(alpha) > DBL_EPSILON) ? + (gam[0]*(lamb[2]/lamb[1])*cos(alpha)*(sin(alpha)/alpha)*psi) : (gam[0]*(lamb[2]/lamb[1])*cos(alpha)*(1)*psi); + fd << u_r[1] << std::endl; if (std::isnan(u_r[0])) { u_r[0] = 0; std::cout << "u_r[0] generated a nan-value..." << std::endl; + exit(-1); } if (std::isnan(u_r[1])) { u_r[1] = 0; std::cout << "u_r[1] generated a nan-value..." << std::endl; + exit(-1); } + // Check the tolerances + if ( (fabs(e) < eps[0]) && (fabs(alpha) < eps[1]) && (fabs(psi) < eps[1]) ){ + u_r[0] = 0; + u_r[1] = 0; + } + // u_r[0] = (fabs(e) > eps[0]) ? u_r[0] : 0; + // u_r[1] = (fabs(alpha) > eps[1]) ? u_r[1] : 0; + #ifdef NONSMOOTH_CONTROLLER_OUTPUT_SCREEN - std::cout << "\ne: " << e << " - alpha: " << alpha << " - psi: " << psi; - std::cout << " - ur0: " << u_r[0] << " - ur1: " << u_r[1] << std::endl; + // std::cout << "\ne: " << e << " - alpha: " << alpha << " - psi: " << psi << " - eps: " << eps[0] << " - " << eps[1]; + // std::cout << " - ur0: " << u_r[0] << " - ur1: " << u_r[1] << std::endl; #endif return u_r; } - bool StopCondition(const Eigen::Vector3d &xc, const Eigen::Vector3d &x_r, double eps){ - return StopCondition(xc, x_r, eps, eps); + Eigen::Vector3d getLamb() const{ + return lamb; + } + + Eigen::Vector2d getGam() const + { + return gam; + } + + void setGam(const Eigen::Vector2d &value) + { + gam = value; + } + + void setLamb(const Eigen::Vector3d &value){ + lamb = value; } - bool StopCondition(const Eigen::Vector3d &xc, const Eigen::Vector3d &x_r, double eps_pos, double eps_heading){ - double err_pos = 0; - err_pos += pow((xc[0]-x_r[0]),2); - err_pos += pow((xc[1]-x_r[1]),2); - if(sqrt(err_pos)>=eps_pos) return false; + double getAlpha() const + { + return alpha; + } - double err_heading = pow((xc[2]-x_r[2]),2); - if(sqrt(err_heading)>=eps_pos) return false; + double getPsi() const + { + return psi; + } - return true; + double getE() const + { + return e; + } + + Eigen::Vector2d getEps() const + { + return eps; + } + + void setEps(const Eigen::Vector2d &value) + { + eps = value; } private: @@ -116,6 +176,14 @@ private: Eigen::Vector3d lamb; Eigen::Vector2d gam; + Eigen::Vector2d eps; + double e; + double psi; + double alpha; + +#ifdef NONSMOOTH_CONTROLLER_OUTPUT_SCREEN + std::ofstream fd; +#endif }; diff --git a/twil_controllers/include/twil_controllers/nonlinear_backstepping_controller.h b/twil_controllers/include/twil_controllers/nonlinear_backstepping_controller.h index 7c50782..a5cd67d 100644 --- a/twil_controllers/include/twil_controllers/nonlinear_backstepping_controller.h +++ b/twil_controllers/include/twil_controllers/nonlinear_backstepping_controller.h @@ -1,5 +1,99 @@ +/****************************************************************************** + Twil Controllers + Nonlinear Backstep Controller + + Copyright (C) 2017 Tiago G. Alves + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program; if not, write to the Free Software + Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + + You can also obtain a copy of the GNU General Public License + at . + +*******************************************************************************/ + #ifndef NONLINEAR_BACKSTEPPING_CONTROLLER_H #define NONLINEAR_BACKSTEPPING_CONTROLLER_H +#include +//#include +#include +#include +#include +#include +#include "twil_controllers/velocity_transformation.h" +#include "state_feedback_linearization.h" +#include "twil_hardware_interfaces/joint_command_extended_interface.h" + +#define NONLINEAR_PID_BACKSTEPPING_CONTROLLER_DEBUG + +#ifdef NONLINEAR_PID_BACKSTEPPING_CONTROLLER_DEBUG +#include +#endif + + +namespace twil_controllers{ + +class NonlinearBacksteppingController: public controller_interface::Controller +{ + +public: + NonlinearBacksteppingController(void); + ~NonlinearBacksteppingController(void); + bool init(twil_hardware_interfaces::VoltageJointInterface *robot, ros::NodeHandle &n); + void starting(const ros::Time &); + void update(const ros::Time &time, const ros::Duration &duration); + void stopping(const ros::Time &); + void commandCB(const geometry_msgs::PoseStampedConstPtr &command); + void command2dCB(const geometry_msgs::Pose2DConstPtr &command); + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + +private: + + /* ROS Variables and Structures */ + ros::Subscriber PositionSub[2]; + ros::NodeHandle node_; + twil_hardware_interfaces::VoltageJointInterface *robot_; + std::vector joints_; + std::string robot_ns; + + /* Nonlinear Backstepping Controller Variables */ + StateFeedbackLinearization * feedback_linearization_; + Eigen::Vector2d voltage_effort_; + Eigen::Vector2d nu_ref_; + Mat2_4 Fu; + Eigen::Vector2d eta_; + Eigen::Vector2d deta_; + Eigen::Vector2d nuBar; + + std::vector lambda; + std::vector gamma; + Eigen::Vector3d pose_ref_; + + /* Model Variables */ + Eigen::Vector2d WheelRadius; + double WheelBase; + +#ifdef NONLINEAR_PID_BACKSTEPPING_CONTROLLER_DEBUG + std::ofstream fd; + bool makeLog; + std::string log_date; + int take; +#endif + +}; + +} #endif diff --git a/twil_controllers/include/twil_controllers/state_feedback_linearization.h b/twil_controllers/include/twil_controllers/state_feedback_linearization.h index 627aad5..0613780 100644 --- a/twil_controllers/include/twil_controllers/state_feedback_linearization.h +++ b/twil_controllers/include/twil_controllers/state_feedback_linearization.h @@ -33,57 +33,65 @@ typedef Eigen::Matrix Mat2_4; +typedef struct K_parameters { + double A; + double B; + double C; + double D; + double E; + double F; + + K_parameters(); + +public: + double getA() const; + double getB() const; + double getC() const; + double getD() const; + double getE() const; + double getF() const; + void setA(double value); + void setB(double value); + void setC(double value); + void setD(double value); + void setE(double value); + void setF(double value); +} K_parameters_t; + class StateFeedbackLinearization { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW - StateFeedbackLinearization(const Eigen::Matrix2d &G_, - const Eigen::Matrix2d &H_beta_) - { - H_beta = H_beta_; - Ginv = G_.inverse(); - Tau_zero.setZero(); - std::cout << "\n Value of H(beta): " << H_beta << std::endl; - std::cout << "\n Value of G: " << G_ << std::endl; - std::cout << "\n Value of Ginv: " << Ginv << std::endl; - std::cout << "\n Value of Tau0: " << Tau_zero << std::endl; - } + StateFeedbackLinearization(const K_parameters_t params); + +// StateFeedbackLinearization(const Eigen::Matrix2d &G_, +// const Eigen::Matrix2d &H_beta_); Eigen::Vector2d computeInputs(const Eigen::Vector2d &v_, - const Eigen::Vector2d &F_Beta_u_) - { - /** - - * This class follows the standard feedback linearization defined by - * Tau_0 = G^-1(Beta)*( H(Beta)v + f(Beta,u) ) - * - * Input -> Joint Velocities - * v_(0): Linear Acceleration (m/s^2) - * v_(1): Angular Acceleration (rd/s^2) - * - * Return -> Robot Base Velocities - * Tau_zero(0): Right Actuator Effort - * Tau_zero(1): Left Actuator Effort - */ - - v = v_; - F_Beta_u = F_Beta_u_; - // std::cout << "\n Fbeta -v: " << F_Beta_u - v << std::endl; - // std::cout << "\n Fbeta: " << F_Beta_u << std::endl; - Tau_zero = Ginv*(H_beta*v + F_Beta_u); - - // std::cout << "\n Tau: " << Tau_zero << std::endl; - return Tau_zero; - } + const Eigen::Vector2d &F_Beta_u_); ~StateFeedbackLinearization() {} + Mat2_4 getFu() const; + void setFu(const Mat2_4 &value); + Mat2_4 computeFu(const K_parameters_t ¶ms) const; + + Eigen::Matrix2d getGinv() const; + void setGinv(const Eigen::Matrix2d &value); + Eigen::Matrix2d computeGinv(const K_parameters_t ¶ms) const; + private: Eigen::Matrix2d Ginv; Eigen::Matrix2d H_beta; Eigen::Vector2d F_Beta_u; Eigen::Vector2d v; Eigen::Vector2d Tau_zero; + + // Robot_Specifics + Mat2_4 Fu; }; #endif + + + diff --git a/twil_controllers/include/twil_controllers/twist_pid_backstepping_controller.h b/twil_controllers/include/twil_controllers/twist_pid_backstepping_controller.h index bea6ac3..d546e73 100644 --- a/twil_controllers/include/twil_controllers/twist_pid_backstepping_controller.h +++ b/twil_controllers/include/twil_controllers/twist_pid_backstepping_controller.h @@ -27,18 +27,21 @@ #define TWIST_PID_BACKSTEPPING_CONTROLLER_H #include -#include +//#include #include #include #include #include #include -#include "twil_controllers/odometry_tool.h" -#include "twil_controllers/pose_tool.h" -#include "twil_controllers/velocity_transformation.h" -#include "non_smooth_control.h" +#include "non_smooth_control.h" +#include +#include +#include +#include "twil_msgs/PoseControllerStatus.h" +#include "twil_msgs/non_smooth_parameters.h" +#include "twil_hardware_interfaces/joint_command_extended_interface.h" #define TWIST_PID_BACKSTEPPING_CONTROLLER_DEBUG @@ -48,48 +51,60 @@ namespace twil_controllers{ -class TwistPIDBacksteppingController: public controller_interface::Controller +class TwistPIDBacksteppingController: public controller_interface::Controller { public: TwistPIDBacksteppingController(); ~TwistPIDBacksteppingController(); - bool init(hardware_interface::EffortJointInterface *, ros::NodeHandle &); - void starting(const ros::Time &); + bool init(twil_hardware_interfaces::VoltageJointInterface *, ros::NodeHandle &); + void starting(const ros::Time &time); void update(const ros::Time &time, const ros::Duration &duration); - void stopping(const ros::Time &); + void stopping(const ros::Time &time); void commandCB(const geometry_msgs::PoseStampedConstPtr &command); void command2dCB(const geometry_msgs::Pose2DConstPtr &command); + void parametersCB(const twil_msgs::non_smooth_parametersConstPtr ¶m); EIGEN_MAKE_ALIGNED_OPERATOR_NEW private: /* ROS Variables and Structures */ - ros::Subscriber PositionSub[2]; - control_toolbox::Pid PID[2]; + ros::Subscriber position_sub_[2]; + ros::Subscriber controller_parameters_sub_; + control_toolbox::Pid pid_[2]; ros::NodeHandle node_; - hardware_interface::EffortJointInterface *robot_; + twil_hardware_interfaces::VoltageJointInterface *robot_; std::vector joints_; std::string robot_ns; /* Twist PID Controller Variables */ - Eigen::Vector2d dphi_ref, dphi, Effort; - Eigen::Vector2d u_ref; + Eigen::Vector2d phi_dot_ref_; + Eigen::Vector2d phi_dot_; + Eigen::Vector2d voltage_effort_; + Eigen::Vector2d u_ref_; /* Nonlinear Kinematic Controller Variables */ Eigen::Vector3d lambda; Eigen::Vector2d gamma; - Eigen::Vector3d pose_ref; - OdometryTool * Odometer; - NonSmoothControl * Kinematics; + Eigen::Vector3d pose_ref_; + NonSmoothControl * nonsmooth_control_; int update_ratio; - /* Model Variables */ - Eigen::Vector2d WheelRadius; + std::vector WheelRadius; double WheelBase; + /* Odometer Variables */ + arc_odometry::DiffOdometry odom_; + double time_step_; + ros::Time lastSamplingTime_; + Eigen::Vector2d phi_; + Eigen::Vector3d pose_; + + /* Controller Status */ + boost::scoped_ptr > status_publisher_ ; + #ifdef TWIST_PID_BACKSTEPPING_CONTROLLER_DEBUG std::ofstream fd; bool makeLog; diff --git a/twil_controllers/include/twil_controllers/twist_pid_controller.h b/twil_controllers/include/twil_controllers/twist_pid_controller.h index a3d840d..45ba05b 100644 --- a/twil_controllers/include/twil_controllers/twist_pid_controller.h +++ b/twil_controllers/include/twil_controllers/twist_pid_controller.h @@ -28,12 +28,18 @@ #include #include -#include +//#include #include #include #include -#include "velocity_transformation.h" + #include "state_feedback_linearization.h" +#include + +#include +#include +#include "twil_msgs/VelocityControllerStatus.h" +#include "twil_hardware_interfaces/joint_command_extended_interface.h" #define TWIST_PID_DEBUG @@ -41,17 +47,15 @@ #include #endif -typedef Eigen::Matrix Mat2_4; - namespace twil_controllers { -class TwistPIDController: public controller_interface::Controller +class TwistPIDController: public controller_interface::Controller { public: TwistPIDController(void); ~TwistPIDController(void); - bool init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n); + bool init(twil_hardware_interfaces::VoltageJointInterface *robot, ros::NodeHandle &n); void starting(const ros::Time& time); void update(const ros::Time& time,const ros::Duration& duration); void stopping(void); @@ -59,27 +63,38 @@ public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW private: - /* ROS Variables and Structures */ - ros::Subscriber VelocitySub; - control_toolbox::Pid PID[2]; + ros::Subscriber velocity_sub_; + control_toolbox::Pid pid_[2]; ros::NodeHandle node_; - hardware_interface::EffortJointInterface *robot_; + twil_hardware_interfaces::VoltageJointInterface *robot_; std::vector joints_; - std::string robot_ns; + std::string robot_ns_; /* Velocity Controller Variables */ - Eigen::Vector2d dphi_ref, dphi, Effort; - Eigen::Vector2d u_ref; + Eigen::Vector2d phi_dot_ref_; + Eigen::Vector2d phi_dot_; + Eigen::Vector2d voltage_effort_; + Eigen::Vector2d u_ref_; /* Model Variables */ - Eigen::Vector2d WheelRadius; + std::vector WheelRadius; double WheelBase; + /* Odometer Variables */ + arc_odometry::DiffOdometry odom_; + double time_step_; + ros::Time lastSamplingTime_; + Eigen::Vector2d phi_; + Eigen::Vector3d pose_; + + /* Controller Status */ + boost::scoped_ptr > status_publisher_ ; + #ifdef TWIST_PID_DEBUG std::ofstream fd; bool makeLog; - std::string log_date; + std::string log_date; int take; #endif diff --git a/twil_controllers/package.xml b/twil_controllers/package.xml index d644f67..c1ee25f 100644 --- a/twil_controllers/package.xml +++ b/twil_controllers/package.xml @@ -4,74 +4,47 @@ 2.0.0 The twil_controllers package - - - Walter Fetter Lages - - - - GPLv3 - - - - - - - - - Walter Fetter Lages - - - - - - - - - - - - catkin + twil_msgs controller_interface effort_controllers kdl_parser - orocos_kdl - twil_gazebo_ros_control - control_toolbox - - - cmake_modules - - + control_toolbox + Eigen + geometry_msgs + nav_msgs + roscpp + tf + arc_odometry + twil_hardware_interfaces + + twil_msgs controller_interface controller_manager effort_controllers joint_state_controller kdl_parser - orocos_kdl - twil_gazebo_ros_control control_toolbox - cmake_modules + Eigen + geometry_msgs + nav_msgs + roscpp + tf + arc_odometry + twil_hardware_interfaces - - - - - - diff --git a/twil_controllers/scripts/pub_array_linear.sh b/twil_controllers/scripts/pub_array_linear.sh new file mode 100644 index 0000000..f0e5298 --- /dev/null +++ b/twil_controllers/scripts/pub_array_linear.sh @@ -0,0 +1,4 @@ +#!/bin/bash +rostopic pub -1 /twil/cart_linearizing_controller/command std_msgs/Float64MultiArray '{data: [0.0, 1.60]}' +sleep 1 +rostopic pub -1 /twil/cart_linearizing_controller/command std_msgs/Float64MultiArray '{data: [0.0, 0.00]}' diff --git a/twil_controllers/scripts/pub_dyn_pid_vel.sh b/twil_controllers/scripts/pub_dyn_pid_vel.sh new file mode 100755 index 0000000..2b6663a --- /dev/null +++ b/twil_controllers/scripts/pub_dyn_pid_vel.sh @@ -0,0 +1,9 @@ +#rostopic pub /twil/dynamics_pid_controller/cmd_vel geometry_msgs/Twist '{linear: {x: 0.18, y: 0.0, z: 0.0}, angular: {x: 0.0,y: 0.0,z: 0.0}}' -1 +#sleep 2 +#rostopic pub /twil/dynamics_pid_controller/cmd_vel geometry_msgs/Twist '{linear: {x: -0.18, y: 0.0, z: 0.0}, angular: {x: 0.0,y: 0.0,z: 0.0}}' -1 +#sleep 2 +rostopic pub /twil/dynamics_pid_controller/cmd_vel geometry_msgs/Twist '{linear: {x: 0.0, y: 0.0, z: 0.0}, angular: {x: 0.0,y: 0.0,z: 1.3}}' -1 +sleep 2 +rostopic pub /twil/dynamics_pid_controller/cmd_vel geometry_msgs/Twist '{linear: {x: 0.0, y: 0.0, z: 0.0}, angular: {x: 0.0,y: 0.0,z: -1.3}}' -1 +sleep 2 +rostopic pub /twil/dynamics_pid_controller/cmd_vel geometry_msgs/Twist '{linear: {x: 0.0, y: 0.0, z: 0.0}, angular: {x: 0.0,y: 0.0,z: 0.0}}' -1 diff --git a/twil_controllers/scripts/pub_nonsmooth_param.sh b/twil_controllers/scripts/pub_nonsmooth_param.sh new file mode 100755 index 0000000..b4a9396 --- /dev/null +++ b/twil_controllers/scripts/pub_nonsmooth_param.sh @@ -0,0 +1,10 @@ +rostopic pub /mini_twil/twist_pid_backstepping_controller/control_param twil_msgs/non_smooth_parameters "lambda_: +- 1.0 +- 1.0 +- 10.0 +gamma: +- 1.0 +- 0.3 +eps: +- 0.0 +- 0.0" -1 diff --git a/twil_controllers/scripts/pub_twist_vel.sh b/twil_controllers/scripts/pub_twist_vel.sh new file mode 100755 index 0000000..10b3a48 --- /dev/null +++ b/twil_controllers/scripts/pub_twist_vel.sh @@ -0,0 +1,18 @@ +#rostopic pub /twil/twist_pid_controller/cmd_vel geometry_msgs/Twist '{linear: {x: 0.18, y: 0.0, z: 0.0}, angular: {x: 0.0,y: 0.0,z: 0.0}}' -1 +#sleep 2 +#rostopic pub /twil/twist_pid_controller/cmd_vel geometry_msgs/Twist '{linear: {x: -0.18, y: 0.0, z: 0.0}, angular: {x: 0.0,y: 0.0,z: 0.0}}' -1 +#sleep 2 +#rostopic pub /twil/twist_pid_controller/cmd_vel geometry_msgs/Twist '{linear: {x: 0.0, y: 0.0, z: 0.0}, angular: {x: 0.0,y: 0.0,z: 1.3}}' -1 +#sleep 2 +#rostopic pub /twil/twist_pid_controller/cmd_vel geometry_msgs/Twist '{linear: {x: 0.0, y: 0.0, z: 0.0}, angular: {x: 0.0,y: 0.0,z: -1.3}}' -1 +#sleep 2 + +rostopic pub /twil/twist_pid_controller/cmd_vel geometry_msgs/Twist '{linear: {x: 0.0, y: 0.0, z: 0.0}, angular: {x: 0.0,y: 0.0,z: 1.3}}' -1 +sleep 2 +rostopic pub /twil/twist_pid_controller/cmd_vel geometry_msgs/Twist '{linear: {x: 0.18, y: 0.0, z: 0.0}, angular: {x: 0.0,y: 0.0,z: 0.0}}' -1 +sleep 2 +rostopic pub /twil/twist_pid_controller/cmd_vel geometry_msgs/Twist '{linear: {x: 0.1, y: 0.0, z: 0.0}, angular: {x: 0.0,y: 0.0,z: 0.5}}' -1 +sleep 4 +rostopic pub /twil/twist_pid_controller/cmd_vel geometry_msgs/Twist '{linear: {x: -0.1, y: 0.0, z: 0.0}, angular: {x: 0.0,y: 0.0,z: -0.5}}' -1 +sleep 4 +rostopic pub /twil/twist_pid_controller/cmd_vel geometry_msgs/Twist '{linear: {x: 0.0, y: 0.0, z: 0.0}, angular: {x: 0.0,y: 0.0,z: 0.0}}' -1 diff --git a/twil_controllers/src/adaptive_dynamics_identification.cpp b/twil_controllers/src/adaptive_dynamics_identification.cpp new file mode 100644 index 0000000..8e709e9 --- /dev/null +++ b/twil_controllers/src/adaptive_dynamics_identification.cpp @@ -0,0 +1,356 @@ +#include "ros/ros.h" + +#include + +#include "arc_odometry/diff_odometry.h" +#include "twil_msgs/robot_dynamics.h" + +#include "sensor_msgs/JointState.h" + +#include + +//#define ADAPTIVE_DYNAMICS_IDENTIFICATION_DEBUG +int log_t = 0; + +typedef struct rls_return +{ + Eigen::Vector3d theta_hat; + double y_hat; + double error; + Eigen::Matrix3d P; + rls_return() { + theta_hat.setZero(); + y_hat = 0; + error = 0; + P.setZero(); + } + +public: + Eigen::Vector3d getTheta_hat() const; + void setTheta_hat(const Eigen::Vector3d &value); + double getY_hat() const; + void setY_hat(double value); + double getError() const; + void setError(double value); + Eigen::Matrix3d getP() const; + void setP(const Eigen::Matrix3d &value); +} rls_return_t; + +const rls_return_t compute_rls(const double &y, const Eigen::Vector3d &phi, const Eigen::Matrix3d &P_last, const Eigen::Vector3d &theta_hat_last) +{ + double y_hat = phi.transpose() * theta_hat_last; + + Eigen::Vector3d K = P_last*phi/(1+(phi.transpose()*P_last*phi)); + + Eigen::Vector3d theta_hat = theta_hat_last + K*(y-y_hat); + + double error = fabs(y-y_hat); + + Eigen::Matrix3d P = P_last - (K*phi.transpose()*P_last); + + rls_return_t ret; + + ret.setY_hat(y_hat); + ret.setTheta_hat(theta_hat); + ret.setP(P); + ret.setError(error); + +#ifdef ADAPTIVE_DYNAMICS_IDENTIFICATION_DEBUG + std::cout << "Log_t: " << log_t << " - phi: " << phi << " - theta_hat_last: " << theta_hat_last << std::endl; + std::cout << "y_hat: " << ret.y_hat << std::endl; + std::cout << "error: " << ret.error << std::endl; + std::cout << "theta_hat: " << ret.theta_hat << std::endl; + std::cout << "P: " << ret.P << std::endl; + printf("\n\n"); +#endif + std::cout << "theta_hat: " << ret.theta_hat << std::endl; + std::cout << "P: " << P << "\n" << std::endl; + + return ret; +} + +class recursive_identificator +{ +public: + recursive_identificator(ros::NodeHandle node); + + void stateCB(const sensor_msgs::JointState::ConstPtr &jointStates); + void parameter_publisher(void); + void resetEstimator(void); + +private: + ros::NodeHandle node_; + ros::Subscriber jointStatesSubscriber; + ros::Publisher dynParamPublisher; + + Eigen::Vector2d u_last_; + double sum_efforts_last; + double dif_efforts_last; + rls_return_t ret_rls1, ret_rls2; + + int seq; + + /* Model Variables */ + std::vector WheelRadius; + double WheelBase; + std::string robot_ns; + + /* Odometer Variables */ + arc_odometry::DiffOdometry odom_; + double time_step_; + ros::Time lastSamplingTime_; + Eigen::Vector2d phi_; + Eigen::Vector3d pose_; +}; + + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "dynamics_identification"); + ros::NodeHandle nh; + + recursive_identificator ident(nh); + ros::spin(); + + return 0; +} + +recursive_identificator::recursive_identificator(ros::NodeHandle node) + : node_(node), WheelRadius(2), odom_(0.0,std::vector(2)) +{ + // Publishers and Subscribers + dynParamPublisher=node_.advertise("ident/dynamic_parameters",1); + jointStatesSubscriber=node_.subscribe("joint_states",1,&recursive_identificator::stateCB,this); + + // Parameter configuration + if(!node_.getParam("/robot_config",robot_ns)) + { + ROS_ERROR("Could not find Robot namespace in '/robot_config'."); + exit(-1); + } else ROS_INFO("Robot namespace: %s", robot_ns.c_str()); + + if (node_.getParam("/"+robot_ns+"/robot_baseline",WheelBase)) + ROS_INFO("Robot baseline: %f", WheelBase); + else ROS_ERROR("Failed to get param 'robot_baseline'"); + + if (node_.getParam("/" + robot_ns + "/wheel_radius",WheelRadius[0])){ + WheelRadius[1] = WheelRadius[0]; + ROS_INFO("Wheel Radius: %f", WheelRadius[1]); + } else ROS_ERROR("Failed to get param 'wheel_radius'"); + + + // Odom Config + std::string odom_frame_id="odom"; + node_.param("odom_frame_id",odom_frame_id,odom_frame_id); + + odom_.setParams(WheelBase,WheelRadius); + odom_.publisherConfig(node_); + pose_.setZero(); + odom_.setPose(pose_); + lastSamplingTime_ = ros::Time::now(); + + // Ident Parameters Config + ret_rls1.P.setIdentity(); + ret_rls1.P*=1000; + ret_rls2.P.setIdentity(); + ret_rls2.P*=1000; + + ret_rls1.theta_hat.setZero(); + ret_rls2.theta_hat.setZero(); + + u_last_.setZero(); + sum_efforts_last = 0; + dif_efforts_last = 0; + seq = 0; +} + +void recursive_identificator::stateCB(const sensor_msgs::JointState::ConstPtr &jointStates){ + + static int init = 0; + + ros::Duration dt=jointStates->header.stamp-lastSamplingTime_; +#ifdef ADAPTIVE_DYNAMICS_IDENTIFICATION_DEBUG + std::cout << "Dt: " << dt.toSec() << std::endl; +#endif + lastSamplingTime_=jointStates->header.stamp; + + Eigen::Vector2d phi_dot_, deltaPhi=-phi_; + for(unsigned int i=0; i < 2; i++) + { + phi_[i]=jointStates->position[i]; +#ifdef ADAPTIVE_DYNAMICS_IDENTIFICATION_DEBUG + std::cout <<"Phi_vl: " << phi_[i] << std::endl; +#endif + } + deltaPhi+=phi_; + phi_dot_ = deltaPhi/dt.toSec(); + + odom_.update(deltaPhi,dt); + odom_.getPose(pose_); + + if (init++ > 0){ + + Eigen::Vector2d u; + odom_.getVelocity(u); + double sum_efforts = jointStates->effort[1] + jointStates->effort[0]; + double dif_efforts = jointStates->effort[1] - jointStates->effort[0]; +#ifdef ADAPTIVE_DYNAMICS_IDENTIFICATION_DEBUG + std::cout << "Sum_eff: " << sum_efforts << "- Dif_Eff: " << dif_efforts << std::endl; + std::cout <<"u: " << u << std::endl; +#endif + + // y[0] : linear acceleration + // y[1] : angular acceleration + + Eigen::Vector2d y = (u-u_last_)/dt.toSec(); + + Eigen::Vector3d Phi1, Phi2; + Phi1 << u_last_[0], u_last_[1]*u_last_[1], sum_efforts_last; + Phi2 << u_last_[1], u_last_[0]*u_last_[1], dif_efforts_last; + + u_last_ = u; + sum_efforts_last = sum_efforts; + dif_efforts_last = dif_efforts; + +#ifdef ADAPTIVE_DYNAMICS_IDENTIFICATION_DEBUG + std::cout << "Phi1: " << Phi1 << std::endl; + std::cout << "Phi2: " << Phi1 << std::endl; + std::cout << "y: " << y << std::endl; +#endif + + bool publish_ok = true; + log_t = 1; + ret_rls1 = compute_rls(y[0], Phi1, ret_rls1.getP(), ret_rls1.getTheta_hat()); + log_t = 2; + ret_rls2 = compute_rls(y[1], Phi2, ret_rls2.getP(), ret_rls2.getTheta_hat()); + + Eigen::Matrix3d P_ch1 = ret_rls1.getP(); + Eigen::Matrix3d P_ch2 = ret_rls2.getP(); + Eigen::Vector3d th_ch1 = ret_rls1.getTheta_hat(); + Eigen::Vector3d th_ch2 = ret_rls2.getTheta_hat(); + + for (int i=0; i< 2; i++) + { + if ((std::isnan(th_ch1[i])) || (std::isinf(th_ch1[i]))){ + std::cout << "Error in Theta_1" << std::endl; + resetEstimator(); + ret_rls1.theta_hat[i] = 1; + publish_ok = false; + } + + if ((std::isnan(th_ch2[i])) || (std::isinf(th_ch2[i]))){ + std::cout << "Error in Theta_1" << std::endl; + resetEstimator(); + ret_rls2.theta_hat[i] = 1; + publish_ok = false; + } + + for (int j = 0; j< 2; j++){ + + if ((std::isnan(P_ch1(i,j))) || (std::isinf(P_ch1(i,j)))){ + std::cout << "Error in P_1" << std::endl; + resetEstimator(); + publish_ok = false; + } + + if ((std::isnan(P_ch2(i,j))) || (std::isinf(P_ch2(i,j)))){ + std::cout << "Error in P_2" << std::endl; + resetEstimator(); + publish_ok = false; + } + + } + } + + + parameter_publisher(); + } +} + +void recursive_identificator::parameter_publisher(void){ + twil_msgs::robot_dynamics param_msg; + + param_msg.header.seq = ++seq; + param_msg.header.frame_id = "dynamics"; + param_msg.header.stamp = ros::Time::now(); + + Eigen::Vector3d theta; + Eigen::Matrix3d P; + + theta=ret_rls1.getTheta_hat(); + P=ret_rls1.getP(); + + for (int i = 0; i<3; i++) { + param_msg.parameters[i] = theta[i]; + param_msg.variances[i] = P(i,i); +#ifdef ADAPTIVE_DYNAMICS_IDENTIFICATION_DEBUG + std::cout << "Th1: " << param_msg.parameters[i] << " - P1: " << param_msg.variances[i] << std::endl; +#endif + } + + theta=ret_rls2.getTheta_hat(); + P=ret_rls2.getP(); + + for (int i = 0; i<3; i++) { + param_msg.parameters[i+3] = theta[i]; + param_msg.variances[i+3] = P(i,i); +#ifdef ADAPTIVE_DYNAMICS_IDENTIFICATION_DEBUG + std::cout << "Th2: " << param_msg.parameters[i] << " - P2: " << param_msg.variances[i] << std::endl; +#endif + } + + param_msg.error[0] = ret_rls1.getError(); + param_msg.error[1] = ret_rls2.getError(); + + dynParamPublisher.publish(param_msg); +} + +void recursive_identificator::resetEstimator(void) +{ + ret_rls1.P.setIdentity(); + ret_rls1.P*=1000; + ret_rls2.P.setIdentity(); + ret_rls2.P*=1000; + // ret_rls1.theta_hat.setZero(); + // ret_rls2.theta_hat.setZero(); +} + +double rls_return::getY_hat() const +{ + return y_hat; +} + +void rls_return::setY_hat(double value) +{ + y_hat = value; +} + +double rls_return::getError() const +{ + return error; +} + +void rls_return::setError(double value) +{ + error = value; +} + +Eigen::Matrix3d rls_return::getP() const +{ + return P; +} + +void rls_return::setP(const Eigen::Matrix3d &value) +{ + P = value; +} + +Eigen::Vector3d rls_return::getTheta_hat() const +{ + return theta_hat; +} + +void rls_return::setTheta_hat(const Eigen::Vector3d &value) +{ + theta_hat = value; +} diff --git a/twil_controllers/src/cart_linearizing_controller.cpp b/twil_controllers/src/cart_linearizing_controller.cpp index 8f3acfc..fab2c3c 100644 --- a/twil_controllers/src/cart_linearizing_controller.cpp +++ b/twil_controllers/src/cart_linearizing_controller.cpp @@ -23,6 +23,8 @@ *******************************************************************************/ +#include + #include #include #include @@ -32,17 +34,17 @@ namespace twil_controllers { -CartLinearizingController::CartLinearizingController(void): - WheelRadius(2){} +CartLinearizingController::CartLinearizingController(void) + : WheelRadius(2), odom_(0.0,std::vector(2)){} CartLinearizingController::~CartLinearizingController(void) { sub_command_.~Subscriber(); - feedback_linearization->~StateFeedbackLinearization(); + feedback_linearization_->~StateFeedbackLinearization(); } -bool CartLinearizingController::init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n) { +bool CartLinearizingController::init(twil_hardware_interfaces::VoltageJointInterface *robot, ros::NodeHandle &n) { node_=n; - robot_=robot; + robot_=robot; if(!node_.getParam("/robot_config",robot_ns)) { @@ -75,7 +77,8 @@ bool CartLinearizingController::init(hardware_interface::EffortJointInterface *r hardware_interface::JointHandle j=robot->getHandle((std::string)name_value); joints_.push_back(j); } - sub_command_ = node_.subscribe("command",1000,&CartLinearizingController::commandCB,this); + + sub_command_ = node_.subscribe("command",1,&CartLinearizingController::commandCB,this); #ifdef urdf std::string robot_desc_string; @@ -129,6 +132,9 @@ bool CartLinearizingController::init(hardware_interface::EffortJointInterface *r } else ROS_ERROR("Failed to get param 'wheel_radius'"); #endif + odom_.setParams(WheelBase,WheelRadius); + odom_.publisherConfig(node_); + /* * Dynamics Controller Parameters * --------------------------------------------------------------------------------------------------------------------- @@ -161,26 +167,47 @@ bool CartLinearizingController::init(hardware_interface::EffortJointInterface *r else ROS_ERROR("Failed to get param '%s'", param_name[i].c_str()); } - const double A = k_parameters[0]; - const double B = k_parameters[1]; - const double C = k_parameters[2]; - const double D = k_parameters[3]; - const double E = k_parameters[4]; - const double F = k_parameters[5]; + Dyn_params.setA(k_parameters[0]); + Dyn_params.setB(k_parameters[1]); + Dyn_params.setC(k_parameters[2]); + Dyn_params.setD(k_parameters[3]); + Dyn_params.setE(k_parameters[4]); + Dyn_params.setF(k_parameters[5]); + + feedback_linearization_ = new StateFeedbackLinearization(Dyn_params); - Eigen::Matrix2d G; - G << C, C, F, -F; - Fu << A, 0, 0, B, 0, D, E, 0; - std::cout << "Value of constants of F(u): " << Fu << std::endl; + node_.param("time_step",time_step_,0.01); - feedback_linearization = new StateFeedbackLinearization(G, Eigen::MatrixXd::Identity(2,2)); return true; } void CartLinearizingController::starting(const ros::Time& time) { nu_ref.setZero(); - Effort.setZero(); + voltage_effort_.setZero(); + Pose_.setZero(); + odom_.setPose(Pose_); + lastSamplingTime_ = time; + + for(unsigned int i=0;i < joints_.size();i++) + { + phi_[i]=joints_[i].getPosition(); + } + + struct sched_param param; + if(!node_.getParam("priority",param.sched_priority)) + { + ROS_WARN("No 'priority' configured for controller %s. Using highest possible priority.",node_.getNamespace().c_str()); + param.sched_priority=sched_get_priority_max(SCHED_FIFO); + } + if(sched_setscheduler(0,SCHED_FIFO,¶m) == -1) + { + ROS_WARN("Failed to set real-time scheduler."); + return; + } + if(mlockall(MCL_CURRENT|MCL_FUTURE) == -1) + ROS_WARN("Failed to lock memory."); + #ifdef CART_LIN_DEBUG if (makeLog == false) @@ -189,14 +216,14 @@ void CartLinearizingController::starting(const ros::Time& time) if (robot_ns == "mini_twil") { - filename = "/home/talves/Dropbox/Mini_Twil/ros/exp/" + log_date + "/cart_lin_0" + std::to_string(take) + ".txt"; + filename = "/home/talves/Dropbox/Projects/Mini_Twil/ros/exp/" + log_date + "/cart_lin_0" + std::to_string(take) + ".txt"; ROS_INFO("Output File: %s", filename.c_str()); fd.open(filename); makeLog = true; } else if (robot_ns == "twil") { - filename = "/home/talves/Dropbox/Twil/ros/exp/" + log_date + "/cart_lin_0" + std::to_string(take) + ".txt"; + filename = "/home/talves/Dropbox/Projects/Twil/ros/exp/" + log_date + "/cart_lin_0" + std::to_string(take) + ".txt"; ROS_INFO("Output File: %s", filename.c_str());; fd.open(filename); makeLog = true; @@ -215,40 +242,88 @@ void CartLinearizingController::update(const ros::Time& time, if (k==0) { ROS_INFO("Starting the LOG for the '%s_robot'...",robot_ns.c_str()); - fd << "seq," << "dur," << "u_ref1," <<"u_ref2," << "nu_ref1," <<"nu_ref2,"<< "u1," << "u2," << "eff1," << "eff2," << "dphi1," << "dphi2" << std::endl; + fd << "seq," << "dur," << "pose_ref1," << "pose_ref2," << "pose_ref3," << "pose1," << "pose2," + << "pose3," << "u_ref1," <<"u_ref2," << "nu_ref1," <<"nu_ref2," << "u1," << "u2," + << "eff1," << "eff2," << "dphi1," << "dphi2" << std::endl; } k++; } } #endif - Eigen::Vector2d u, fu, joint_ang_vel; + ros::Duration dt=time-lastSamplingTime_; + +// if(fabs(dt.toSec()-time_step_) > time_step_/20) { +// ROS_INFO("Real-time problem..."); +// return; +// } + lastSamplingTime_=time; + + // Incremental encoders sense angular displacement and + // not velocity + // phi[0] is the left wheel angular displacement + // phi[1] is the right wheel angular displacement + + // phi_dot[0] is the left wheel angular rate + // phi_dot[1] is the right wheel angular rate + + Eigen::Vector2d phi_dot, deltaPhi=-phi_; + for(unsigned int i=0;i < joints_.size();i++) + { + phi_[i]=joints_[i].getPosition(); + } + deltaPhi+=phi_; + phi_dot = deltaPhi/dt.toSec(); + + odom_.update(deltaPhi,dt); + odom_.getPose(Pose_); + + Eigen::Vector2d u; + + Eigen::Vector2d fu; Eigen::Vector4d uf; + Mat2_4 Fu_ = feedback_linearization_->getFu(); // Compute Velocities - for(unsigned int i=0;i < joints_.size();i++) joint_ang_vel[i]=joints_[i].getVelocity(); - Eigen::Vector2d dphi; - dphi << joint_ang_vel[1], joint_ang_vel[0]; + odom_.getVelocity(u); - u = VelocityTransformation::WheelToBase(dphi,WheelBase,WheelRadius[0]); - - // Compute linearization + // Compute Feedback Linearization uf << u[0], u[1], u[1]*u[0], u[1]*u[1]; - fu = -Fu*uf; - Effort = feedback_linearization->computeInputs(nu_ref,fu); + fu = -Fu_*uf; + + // Computes Armature Voltages + // Va[0] is the Right Armature Voltage + // Va[1] is the Left Armature Voltage + Eigen::Vector2d Va = feedback_linearization_->computeInputs(nu_ref,fu); + + /* + * To return to ros space joint numbers, the efforts are calculated as + * effort[0] equals to left joint effort and + * effort[1] equals to right joint effort + */ + voltage_effort_.setZero(); + voltage_effort_ << Va[1], Va[0]; + // Apply Control Efforts - joints_[0].setCommand(Effort[1]); - joints_[1].setCommand(Effort[0]); + joints_[0].setCommand(voltage_effort_[0]); + joints_[1].setCommand(voltage_effort_[1]); #ifdef CART_LIN_DEBUG if (makeLog==true){ if (k>0) { - fd << k << ","<< duration.toSec() << "," << 0 << "," << 0 << "," << nu_ref[0] << "," << nu_ref[1] << "," << uf[0] << "," << uf[1] << "," - << Effort[0] << "," << Effort[1] << "," << dphi[1] << "," << dphi[0] << std::endl; +// fd << "seq," << "dur," << "pose_ref1," << "pose_ref2," << "pose_ref3," << "pose1," << "pose2," +// << "pose3," << "u_ref1," <<"u_ref2," << "nu_ref1," <<"nu_ref2," << "u1," << "u2," +// << "eff1," << "eff2," << "dphi1," << "dphi2" << std::endl; + fd << k << "," << duration.toSec() << "," << 0 << "," << 0 + << "," << 0 << "," << Pose_[0] << "," << Pose_[1] << "," << Pose_[2] + << "," << 0 << "," << 0 << "," << nu_ref[0] << "," << nu_ref[1] << "," << u[0] << "," << u[1] + << "," << voltage_effort_[1] << "," << voltage_effort_[0] << "," << phi_dot[1] << "," << phi_dot[0] + << std::endl; } - } + } #endif + odom_.publish(time); } void CartLinearizingController::stopping() @@ -260,13 +335,13 @@ void CartLinearizingController::stopping() sub_command_.shutdown(); } -void CartLinearizingController::commandCB(const std_msgs::Float64MultiArray::ConstPtr &command) -{ - for(unsigned int i=0;i < command->data.size();i++){ - nu_ref[i]=command->data[i]; - } +void CartLinearizingController::commandCB(const geometry_msgs::Accel &command) +{ + nu_ref[0]=command.linear.x; + nu_ref[1]=command.angular.z; + #ifdef CONTROLLER_OUTPUT_SCREEN - ROS_INFO("nu_ref: {%f, %f}", nu_ref[0], nu_ref[1]); + ROS_INFO("nu_ref: {%f, %f}", nu_ref[0], nu_ref[1]); #endif } diff --git a/twil_controllers/src/dynamics_mrac_backstepping_controller.cpp b/twil_controllers/src/dynamics_mrac_backstepping_controller.cpp new file mode 100644 index 0000000..b37a83d --- /dev/null +++ b/twil_controllers/src/dynamics_mrac_backstepping_controller.cpp @@ -0,0 +1,643 @@ +/****************************************************************************** + Twil Controllers + Linearizing MRAC Backstep Controller + + Copyright (C) 2017 Tiago G. Alves + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program; if not, write to the Free Software + Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + + You can also obtain a copy of the GNU General Public License + at . + +*******************************************************************************/ + +#include + +#include "twil_controllers/dynamics_mrac_backstepping_controller.h" +#include +#include +#include + +//#define URDF +//#define CONTROLLER_OUTPUT_SCREEN + +namespace twil_controllers +{ + +DynamicsMRACBacksteppingController::DynamicsMRACBacksteppingController() + : WheelRadius(2), odom_(0.0,std::vector(2)){} + +DynamicsMRACBacksteppingController::~DynamicsMRACBacksteppingController() +{ + position_sub_[0].~Subscriber(); + position_sub_[1].~Subscriber(); + + controller_parameters_sub_.~Subscriber(); + dynamics_parameters_sub_.~Subscriber(); +} + +bool DynamicsMRACBacksteppingController::init(twil_hardware_interfaces::VoltageJointInterface *robot, ros::NodeHandle &n) { + node_ = n; + robot_ = robot; + + if(!node_.getParam("/robot_config",robot_ns_)) + { + ROS_ERROR("Could not find Robot namespace in '/robot_config'."); + return false; + } else ROS_INFO("Robot namespace: %s", robot_ns_.c_str()); + + XmlRpc::XmlRpcValue joint_names; + if(!node_.getParam("joints",joint_names)) + { + ROS_ERROR("No 'joints' parameter in controller. (namespace: %s)",node_.getNamespace().c_str()); + return false; + } + + if(joint_names.getType() != XmlRpc::XmlRpcValue::TypeArray) + { + ROS_ERROR("The 'joints' parameter is not a struct. (namespace: %s)",node_.getNamespace().c_str()); + return false; + } + + for(int i=0; i < joint_names.size();i++) + { + XmlRpc::XmlRpcValue &name_value=joint_names[i]; + if(name_value.getType() != XmlRpc::XmlRpcValue::TypeString) + { + ROS_ERROR("Array of joint names should contain only strings. (namespace: %s)",node_.getNamespace().c_str()); + return false; + } + + hardware_interface::JointHandle j=robot->getHandle((std::string)name_value); + joints_.push_back(j); + } + + // PositionSub[0] = node_.subscribe("/move_base_simple/goal",1,&DynamicsMRACBacksteppingController::commandCB,this); + position_sub_[1] = node_.subscribe("cmd_pos2d",1,&DynamicsMRACBacksteppingController::command2dCB,this); + controller_parameters_sub_ = node_.subscribe("control_param",1,&DynamicsMRACBacksteppingController::parametersCB,this); + dynamics_parameters_sub_ = node_.subscribe("/"+robot_ns_+"/ident/dynamic_parameters",1,&DynamicsMRACBacksteppingController::dynParametersCB,this); + +#ifdef URDF + std::string robot_desc_string; + if(!node_.getParam("/robot_description",robot_desc_string)) + { + ROS_ERROR("Could not find '/robot_description'."); + return false; + } + + KDL::Tree tree; + if (!kdl_parser::treeFromString(robot_desc_string,tree)) + { + ROS_ERROR("Failed to construct KDL tree."); + return false; + } + + // get wheelBase from URDF (actually from KDL tree) + KDL::SegmentMap::const_iterator segmentMapIter=tree.getSegment("left_wheel_support"); + KDL::Frame leftSupportFrame=segmentMapIter->second.segment.getFrameToTip(); + + segmentMapIter=tree.getSegment("right_wheel_support"); + KDL::Frame rightSupportFrame=segmentMapIter->second.segment.getFrameToTip(); + + WheelRadius.resize(joints_.size()); + WheelBase=leftSupportFrame(1,3)-rightSupportFrame(1,3); + + // get wheelRadius from URDF (actually from KDL tree) + segmentMapIter=tree.getSegment("chassis"); + KDL::Frame chassisFrame=segmentMapIter->second.segment.getFrameToTip(); + + segmentMapIter=tree.getSegment("left_wheel"); + KDL::Joint leftWheelJoint=segmentMapIter->second.segment.getJoint(); + WheelRadius[0]=chassisFrame(2,3)+leftSupportFrame(2,3)+leftWheelJoint.JointOrigin().z(); + + segmentMapIter=tree.getSegment("right_wheel"); + KDL::Joint rightWheelJoint=segmentMapIter->second.segment.getJoint(); + WheelRadius[1]=chassisFrame(2,3)+rightSupportFrame(2,3)+rightWheelJoint.JointOrigin().z(); + + std::cout << "Wheel Radius: " << WheelRadius[0] << " - " << WheelRadius[1] << std::endl; + std::cout << "Wheel base: " << WheelBase << std::endl; + +#else + if (node_.getParam("/"+robot_ns_+"/robot_baseline",WheelBase)) + ROS_INFO("Robot baseline: %f", WheelBase); + else ROS_ERROR("Failed to get param 'robot_baseline'"); + + if (node_.getParam("/" + robot_ns_ + "/wheel_radius",WheelRadius[0])){ + WheelRadius[1] = WheelRadius[0]; + ROS_INFO("Wheel Radius: %f", WheelRadius[1]); + } else ROS_ERROR("Failed to get param 'wheel_radius'"); +#endif + + std::string odom_frame_id="odom"; + node_.param("odom_frame_id",odom_frame_id,odom_frame_id); + + odom_.setParams(WheelBase,WheelRadius); + odom_.publisherConfig(node_); + + status_publisher_.reset(new realtime_tools::RealtimePublisher(node_,"status",1)); + status_publisher_->msg_.header.frame_id=odom_frame_id; + + /* + * Velocity MRAC Controller Parameters + * --------------------------------------------------------------------------------------------------------------------- + */ + + std::vector AlphaVec; + if(!node_.getParam("Alpha",AlphaVec)) + { + ROS_ERROR("No 'Alpha' in controller %s.",node_.getNamespace().c_str()); + return false; + } + Alpha_.diagonal()=Eigen::Map(AlphaVec.data(),2); + + + double low_level_control_rate = 0.0; + if(node_.getParam("/" + robot_ns_ + "/control_loop_rate", low_level_control_rate)) { + ROS_INFO("Low_level Control Frequency: %fHz", low_level_control_rate); + } else return false; + + if (!node_.getParam("/log_date",log_date)){ + ROS_ERROR("Parameter '/robot_date' not found! The log will be disabled..."); + log_date=""; + } + + /* + * Dynamics Controller Parameters + * --------------------------------------------------------------------------------------------------------------------- + */ + + { + + + double k_parameters[6]; + + std::string param_name[] = {"feedback_linearization/A", + "feedback_linearization/B", + "feedback_linearization/C", + "feedback_linearization/D", + "feedback_linearization/E", + "feedback_linearization/F"}; + + for (int i = 0; i < 6; i++){ + if (node_.getParam(param_name[i].c_str(),k_parameters[i])) + ROS_INFO("%s: %f", param_name[i].c_str(), k_parameters[i]); + else ROS_ERROR("Failed to get param '%s'", param_name[i].c_str()); + } + + Dyn_params.setA(k_parameters[0]); + Dyn_params.setB(k_parameters[1]); + Dyn_params.setC(k_parameters[2]); + Dyn_params.setD(k_parameters[3]); + Dyn_params.setE(k_parameters[4]); + Dyn_params.setF(k_parameters[5]); + + feedback_linearization_ = new StateFeedbackLinearization(Dyn_params); + + } + +#ifdef DYNAMICS_MRAC_BACKSTEPPING_CONTROLLER_DEBUG + if (!node_.getParam("take",take)){ + ROS_ERROR("Execution take '%s/take' not found! The parameter will be assumed as '0'",node_.getNamespace().c_str()); + take = 0; + } +#endif + + /* + * Nonlinear Kinematic Controller Parameters + * --------------------------------------------------------------------------------------------------------------------- + */ + + // Configure the Odometer Initial Pose + + if (!node_.getParam("x_ini",pose_[0])){ + ROS_ERROR("Initial pose parameter '%s/x_ini' not found! The parameter will be disabled as '0'",node_.getNamespace().c_str()); + pose_[0] = 0; + } + + if (!node_.getParam("y_ini",pose_[1])){ + ROS_ERROR("Initial pose parameter '%s/y_ini' not found! The parameter will be disabled as '0'",node_.getNamespace().c_str()); + pose_[1] = 0; + } + + if (!node_.getParam("th_ini",pose_[2])){ + ROS_ERROR("Initial pose parameter '%s/th_ini' not found! The parameter will be disabled as '0'",node_.getNamespace().c_str()); + pose_[2] = 0; + } + + // Starting Kinematic Controller + + double non_smooth_parameters[6]; + std::string param_name[] = {"nonsmooth_controller/lambda1", + "nonsmooth_controller/lambda2", + "nonsmooth_controller/lambda3", + "nonsmooth_controller/gamma1", + "nonsmooth_controller/gamma2", + "nonsmooth_controller/update_ratio"}; + + for (int i = 0; i < 6; i++){ + if (node_.getParam(param_name[i].c_str(),non_smooth_parameters[i])) + ROS_INFO("%s: %f", param_name[i].c_str(), non_smooth_parameters[i]); + else { + ROS_ERROR("Failed to get param '%s'", param_name[i].c_str()); + return false; + } + } + + lambda[0] = non_smooth_parameters[0]; + lambda[1] = non_smooth_parameters[1]; + lambda[2] = non_smooth_parameters[2]; + gamma[0] = non_smooth_parameters[3]; + gamma[1] = non_smooth_parameters[4]; + update_ratio = non_smooth_parameters[5]; + + std::vector tolerances; + if (node_.getParam("tolerances",tolerances)) + { ROS_INFO("Tolerances: [%f, %f]", tolerances[0], tolerances[1]); + nonsmooth_control_ = new NonSmoothControl(lambda,gamma,tolerances[0],tolerances[1]); + } else { + ROS_ERROR("Failed to get param 'tolerances'"); + nonsmooth_control_ = new NonSmoothControl(lambda,gamma); + } + + ROS_INFO("%s: %f", "Kinematic Controller Frequency: ", low_level_control_rate/update_ratio); + + node_.param("time_step",time_step_,0.01); + + return true; +} + +void DynamicsMRACBacksteppingController::starting(const ros::Time &time) +{ + nu_ref_.setZero(); + u_ref_.setZero(); + voltage_effort_.setZero(); + pose_ref_ = pose_; + odom_.setPose(pose_); + lastSamplingTime_ = time; + + for(unsigned int i=0;i < joints_.size();i++) + { + phi_[i]=joints_[i].getPosition(); + } + + struct sched_param param; + if(!node_.getParam("priority",param.sched_priority)) + { + ROS_WARN("No 'priority' configured for controller %s. Using highest possible priority.",node_.getNamespace().c_str()); + param.sched_priority=sched_get_priority_max(SCHED_FIFO); + } + if(sched_setscheduler(0,SCHED_FIFO,¶m) == -1) + { + ROS_WARN("Failed to set real-time scheduler."); + return; + } + if(mlockall(MCL_CURRENT|MCL_FUTURE) == -1) + ROS_WARN("Failed to lock memory."); + +#ifdef DYNAMICS_MRAC_BACKSTEPPING_CONTROLLER_DEBUG + if (makeLog == false) + { + std::string filename = "none"; + + if (robot_ns_ == "mini_twil") + { + filename = "/home/talves/Dropbox/Projects/Mini_Twil/ros/exp/" + log_date + "/dynamics_mrac_backstepping_0" + std::to_string(take) + ".txt"; + ROS_INFO("Output File: %s", filename.c_str()); + fd.open(filename); + makeLog = true; + } + else if (robot_ns_ == "twil") + { + filename = "/home/talves/Dropbox/Projects/Twil/ros/exp/" + log_date + "/dynamics_mrac_backstepping_0" + std::to_string(take) + ".txt"; + ROS_INFO("Output File: %s", filename.c_str()); + fd.open(filename); + makeLog = true; + } + } +#endif +} + +void DynamicsMRACBacksteppingController::update(const ros::Time &time, const ros::Duration &duration) +{ + static int kinematic_cycle = 0; + +#ifdef DYNAMICS_MRAC_BACKSTEPPING_CONTROLLER_DEBUG + static int k = 0; + if (makeLog == true){ + if (this->fd.is_open()){ + if (k==0) + { + ROS_INFO("Starting the LOG for the '%s_robot'...",robot_ns_.c_str()); + fd << "seq," << "dur," << "pose_ref1," << "pose_ref2," << "pose_ref3," << "pose1," << "pose2," + << "pose3," << "u_ref1," <<"u_ref2," << "nu_ref1," <<"nu_ref2," << "u1," << "u2," + << "eff1," << "eff2," << "dphi1," << "dphi2" << std::endl; + } + k++; + } + } +#endif + + ros::Duration dt=time-lastSamplingTime_; + + // if(fabs(dt.toSec()-time_step_) > time_step_/20) { + // ROS_INFO("Real-time problem..."); + // return; + // } + lastSamplingTime_=time; + + // Incremental encoders sense angular displacement and + // not velocity + // phi[0] is the left wheel angular displacement + // phi[1] is the right wheel angular displacement + + // phi_dot[0] is the left wheel angular rate + // phi_dot[1] is the right wheel angular rate + + Eigen::Vector2d phi_dot, deltaPhi=-phi_; + for(unsigned int i=0;i < joints_.size();i++) + { + phi_[i]=joints_[i].getPosition(); + } + deltaPhi+=phi_; + phi_dot = deltaPhi/dt.toSec(); + + odom_.update(deltaPhi,dt); + odom_.getPose(pose_); + + /* --------------------------------------------------------------------------------------------------------------------- + * Nonlinear Pose Controller + * --------------------------------------------------------------------------------------------------------------------- + */ + + if (++kinematic_cycle == update_ratio) + { + kinematic_cycle = 0; +#ifdef CONTROLLER_OUTPUT_SCREEN + ROS_INFO("Updating kinematic controller"); +#endif + + // if (!Kinematics->StopCondition(pose,pose_ref,10e-3,1e-3)){ + u_ref_ = nonsmooth_control_->compute(pose_,pose_ref_); // Here will be generated the linear and angular velocities setpoints + // } else u_ref.setZero(); + } + + /* --------------------------------------------------------------------------------------------------------------------- + * Velocity Controller + * --------------------------------------------------------------------------------------------------------------------- + */ + + Eigen::Vector2d error, u; + + // Compute Velocities + odom_.getVelocity(u); + + // error[0] = u_ref_[0]-u[0]; + // error[1] = u_ref_[1]-u[1]; + + // Compute Control Actions + // nu_ref_[0] = pid_[0].computeCommand(error[0],duration); // Here will be generated the linear acceleration setpoint (m/s^2) + // nu_ref_[1] = pid_[1].computeCommand(error[1],duration); // Here will be generated the angular acceleration setpoint (rd/s^2) + + // Compute reference model + Eigen::Vector2d deltaUModel = Alpha_*(u_ref_-uModel_); + + nu_ref_ = Alpha_*(uModel_-u)+deltaUModel; + + //update reference model + uModel_+=deltaUModel*dt.toSec(); + + + // Compute Feedback Linearization + + Eigen::Vector2d fu; + Eigen::Vector4d uf; + feedback_linearization_mutex_.lock(); + Mat2_4 Fu_ = feedback_linearization_->getFu(); + + // Feedback Linearization ----------------------------------------------------------------------------------------------- + uf << u[0], u[1], u[1]*u[0], u[1]*u[1]; + fu = -Fu_*uf; + + // Computes Armature Voltages + // Va[0] is the Right Armature Voltage + // Va[1] is the Left Armature Voltage + Eigen::Vector2d Va = feedback_linearization_->computeInputs(nu_ref_,fu); + feedback_linearization_mutex_.unlock(); + + /* + * To return to ros space joint numbers, the efforts are calculated as + * effort[0] equals to left joint effort and + * effort[1] equals to right joint effort + */ + voltage_effort_.setZero(); + voltage_effort_ << Va[1], Va[0]; + + // Apply Control Efforts + joints_[0].setCommand(voltage_effort_[0]); + joints_[1].setCommand(voltage_effort_[1]); + +#ifdef CONTROLLER_OUTPUT_SCREEN + ROS_INFO("uref[0]: %f; u[0]: %f",u_ref[0],u[0]); + ROS_INFO("uref[1]: %f; u[1]: %f",u_ref[1],u[1]); + ROS_INFO("Ref: {%f,%f,%f}; Pos: {%f,%f,%f}",pose_ref[0], pose_ref[1], pose_ref[2], pose[0], pose[1], pose[2]); +#endif + +#ifdef DYNAMICS_MRAC_BACKSTEPPING_CONTROLLER_DEBUG + if (makeLog==true){ + if (k>0){ + // fd << "seq," << "dur," << "pose_ref1," << "pose_ref2," << "pose_ref3," << "pose1," << "pose2," + // << "pose3," << "u_ref1," <<"u_ref2," << "nu_ref1," <<"nu_ref2," << "u1," << "u2," + // << "eff1," << "eff2," << "dphi1," << "dphi2" << std::endl; + fd << k << "," << duration.toSec() << "," << pose_ref_[0] << "," << pose_ref_[1] + << "," << pose_ref_[2] << "," << pose_[0] << "," << pose_[1] << "," << pose_[2] + << "," << u_ref_[0] << "," << u_ref_[1] << "," << nu_ref_[0] << "," << nu_ref_[1] << "," << u[0] << "," << u[1] + << "," << voltage_effort_[1] << "," << voltage_effort_[0] << "," << phi_dot[1] << "," << phi_dot[0] + << std::endl; + } + } +#endif + + odom_.publish(time); + + if(status_publisher_ && status_publisher_->trylock()) + { + status_publisher_->msg_.header.stamp=time; + + status_publisher_->msg_.set_point.x=pose_ref_[0]; + status_publisher_->msg_.set_point.y=pose_ref_[1]; + status_publisher_->msg_.set_point.theta=pose_ref_[2]; + + status_publisher_->msg_.process_value.x=pose_[0]; + status_publisher_->msg_.process_value.y=pose_[1]; + status_publisher_->msg_.process_value.theta=pose_[2]; + + status_publisher_->msg_.process_value_dot.x=u[0]*cos(pose_[2]); + status_publisher_->msg_.process_value_dot.y=u[0]*sin(pose_[2]); + status_publisher_->msg_.process_value_dot.theta=u[1]; + + status_publisher_->msg_.error.x=pose_ref_[0]-pose_[0]; + status_publisher_->msg_.error.y=pose_ref_[1]-pose_[1]; + status_publisher_->msg_.error.theta=pose_ref_[2]-pose_[2]; + + status_publisher_->msg_.time_step=dt.toSec(); + + for(int i=0;i < voltage_effort_.size();i++) + status_publisher_->msg_.command[i]=voltage_effort_[i]; + + Eigen::Vector3d lb_value = nonsmooth_control_->getLamb(); + for(int i=0;i < lb_value.size();i++) + status_publisher_->msg_.lambda[i]=lb_value[i]; + + Eigen::Vector2d gm_value = nonsmooth_control_->getGam(); + for(int i=0;i < gm_value.size();i++) + status_publisher_->msg_.gamma[i]=gm_value[i]; + + status_publisher_->msg_.polar_error.range=nonsmooth_control_->getE(); + status_publisher_->msg_.polar_error.angle=nonsmooth_control_->getPsi(); + status_publisher_->msg_.polar_error.orientation=nonsmooth_control_->getAlpha(); + + for(int i=0;i < u_ref_.size();i++) + status_publisher_->msg_.backstep_set_point[i]=u_ref_[i]; + + for(int i=0;i < u.size();i++) + status_publisher_->msg_.backstep_process_value[i]=u[i]; + + Eigen::Vector2d eb=u-u_ref_; + + for(int i=0;i < eb.size();i++) + status_publisher_->msg_.backstep_error[i]=eb[i]; + + for(int i=0;i < nu_ref_.size();i++) + status_publisher_->msg_.linear_dynamics_command[i]=nu_ref_[i]; + + status_publisher_->unlockAndPublish(); + } +} + +void DynamicsMRACBacksteppingController::stopping(const ros::Time &) +{ +#ifdef DYNAMICS_MRAC_BACKSTEPPING_CONTROLLER_DEBUG + fd.close(); +#endif + position_sub_[0].shutdown(); + position_sub_[1].shutdown(); +} + +void DynamicsMRACBacksteppingController::commandCB(const geometry_msgs::PoseStampedConstPtr &command) +{ + pose_ref_[0] = command->pose.position.x; + pose_ref_[1] = command->pose.position.y; + tf::Quaternion quat_aux(command->pose.orientation.x, + command->pose.orientation.y, + command->pose.orientation.z, + command->pose.orientation.w); + tf::Matrix3x3 m(quat_aux); + double roll, pitch, yaw; + m.getRPY(roll, pitch, yaw); + pose_ref_[2] = yaw; +#ifdef CONTROLLER_OUTPUT_SCREEN + ROS_INFO("Pose_ref: {%f, %f, %f}", pose_ref[0], pose_ref[1], pose_ref[2]); +#endif +} + +void DynamicsMRACBacksteppingController::command2dCB(const geometry_msgs::Pose2DConstPtr &command) +{ + + pose_ref_[0] = command->x; + pose_ref_[1] = command->y; + pose_ref_[2] = command->theta; + +#ifdef CONTROLLER_OUTPUT_SCREEN + ROS_INFO("Pose_ref: {%f, %f, %f}", pose_ref[0], pose_ref[1], pose_ref[2]); +#endif +} + +void DynamicsMRACBacksteppingController::parametersCB(const twil_msgs::non_smooth_parametersConstPtr ¶m) +{ + ROS_INFO("\nUpdate in Nonsmooth Controller parameters..."); + Eigen::Vector3d lb_par; + Eigen::Vector2d gm_par; + Eigen::Vector2d eps_par; + + lb_par << param->lambda[0], param->lambda[1], param->lambda[2]; + gm_par << param->gamma[0], param->gamma[1]; + eps_par << param->eps[0], param->eps[1]; + + nonsmooth_control_->setLamb(lb_par); + nonsmooth_control_->setGam(gm_par); + nonsmooth_control_->setEps(eps_par); +} + +void DynamicsMRACBacksteppingController::dynParametersCB(const twil_msgs::robot_dynamicsConstPtr ¶m) +{ + + Dyn_params; + bool update = false; + double max_variance_vl = 1e-6; + // double min_variance_vl = 1e-2; + + if ((sqrt(param->variances[0]*param->variances[0]) < max_variance_vl) )//&& (fabs(param->parameters[0]) > 0)) + { + Dyn_params.setA(param->parameters[0]); + std::cout << "Variance_A: " << param->variances[0] << std::endl; + update = true; + } + if ((sqrt(param->variances[1]*param->variances[1]) < max_variance_vl) )//&& (fabs(param->parameters[1]) > 0)) + { + Dyn_params.setB(param->parameters[1]); + std::cout << "Variance_B: " << param->variances[1] << std::endl; + update = true; + } + if ((sqrt(param->variances[2]*param->variances[2]) < max_variance_vl) )//&& (fabs(param->parameters[2]) > 0)) + { + Dyn_params.setC(param->parameters[2]); + std::cout << "Variance_C: " << param->variances[2] << std::endl; + update = true; + } + + if ((sqrt(param->variances[3]*param->variances[3]) < max_variance_vl) )//&& (fabs(param->parameters[3]) > 0)) + { + Dyn_params.setD(param->parameters[3]); + std::cout << "Variance_D: " << param->variances[3] << std::endl; + update = true; + } + if ((sqrt(param->variances[4]*param->variances[4]) < max_variance_vl) )//&& (fabs(param->parameters[4]) > 0)) + { + Dyn_params.setE(param->parameters[4]*param->parameters[4]); + std::cout << "Variance_E: " << param->variances[4] << std::endl; + update = true; + } + if ((sqrt(param->variances[5]*param->variances[5]) < max_variance_vl) )//&& (fabs(param->parameters[5]) > 0)) + { + Dyn_params.setF(param->parameters[5]); + std::cout << "Variance_F: " << param->variances[5] << std::endl; + update = true; + } + + if (update) { + ROS_INFO("Dynamic_parameters Updated..."); + feedback_linearization_mutex_.lock(); + Mat2_4 Fu_ = feedback_linearization_->computeFu(Dyn_params); + feedback_linearization_->setFu(Fu_); + Eigen::Matrix2d Ginv_ = feedback_linearization_->computeGinv(Dyn_params); + feedback_linearization_->setGinv(Ginv_); + feedback_linearization_mutex_.unlock(); + } + +} + +} +PLUGINLIB_EXPORT_CLASS(twil_controllers::DynamicsMRACBacksteppingController,controller_interface::ControllerBase) + diff --git a/twil_controllers/src/dynamics_pid_backstepping_controller.cpp b/twil_controllers/src/dynamics_pid_backstepping_controller.cpp index 67a8020..0fb79d3 100644 --- a/twil_controllers/src/dynamics_pid_backstepping_controller.cpp +++ b/twil_controllers/src/dynamics_pid_backstepping_controller.cpp @@ -23,6 +23,8 @@ *******************************************************************************/ +#include + #include "twil_controllers/dynamics_pid_backstepping_controller.h" #include #include @@ -35,21 +37,20 @@ namespace twil_controllers { DynamicsPIDBacksteppingController::DynamicsPIDBacksteppingController() -{ - -} + : WheelRadius(2), odom_(0.0,std::vector(2)){} DynamicsPIDBacksteppingController::~DynamicsPIDBacksteppingController() { - PositionSub[0].~Subscriber(); - PositionSub[1].~Subscriber(); - PID[0].~Pid(); - PID[1].~Pid(); -} + position_sub_[0].~Subscriber(); + position_sub_[1].~Subscriber(); -bool DynamicsPIDBacksteppingController::init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n) -{ + pid_[0].~Pid(); + pid_[1].~Pid(); + controller_parameters_sub_.~Subscriber(); + dynamics_parameters_sub_.~Subscriber(); +} +bool DynamicsPIDBacksteppingController::init(twil_hardware_interfaces::VoltageJointInterface *robot, ros::NodeHandle &n) { node_ = n; robot_ = robot; @@ -85,9 +86,10 @@ bool DynamicsPIDBacksteppingController::init(hardware_interface::EffortJointInte joints_.push_back(j); } - // PositionSub[0] = node_.subscribe("cmd_pos",1,&DynamicsPIDBacksteppingController::commandCB,this); - PositionSub[0] = node_.subscribe("/move_base_simple/goal",1,&DynamicsPIDBacksteppingController::commandCB,this); - PositionSub[1] = node_.subscribe("cmd_pos2d",1,&DynamicsPIDBacksteppingController::command2dCB,this); +// PositionSub[0] = node_.subscribe("/move_base_simple/goal",1,&DynamicsPIDBacksteppingController::commandCB,this); + position_sub_[1] = node_.subscribe("cmd_pos2d",1,&DynamicsPIDBacksteppingController::command2dCB,this); + controller_parameters_sub_ = node_.subscribe("control_param",1,&DynamicsPIDBacksteppingController::parametersCB,this); + dynamics_parameters_sub_ = node_.subscribe("/"+robot_ns+"/ident/dynamic_parameters",1,&DynamicsPIDBacksteppingController::dynParametersCB,this); #ifdef URDF std::string robot_desc_string; @@ -140,18 +142,26 @@ bool DynamicsPIDBacksteppingController::init(hardware_interface::EffortJointInte } else ROS_ERROR("Failed to get param 'wheel_radius'"); #endif + std::string odom_frame_id="odom"; + node_.param("odom_frame_id",odom_frame_id,odom_frame_id); + + odom_.setParams(WheelBase,WheelRadius); + odom_.publisherConfig(node_); + + status_publisher_.reset(new realtime_tools::RealtimePublisher(node_,"status",1)); + status_publisher_->msg_.header.frame_id=odom_frame_id; /* * Velocity PID Controller Parameters * --------------------------------------------------------------------------------------------------------------------- */ - if (!PID[0].init(ros::NodeHandle(node_, "linear_velocity_pid_parameters"))){ + if (!pid_[0].init(ros::NodeHandle(node_, "linear_velocity_pid_parameters"))){ ROS_ERROR("This class could not construct PID controller for linear velocity"); return false; - } else ROS_INFO("\n-------------------------------------------------->PID01_OK\n-------------------------------------------------->"); + } - if (!PID[1].init(ros::NodeHandle(node_, "angular_velocity_pid_parameters"))){ + if (!pid_[1].init(ros::NodeHandle(node_, "angular_velocity_pid_parameters"))){ ROS_ERROR("This class could not construct PID controller for angular velocity"); return false; } @@ -160,9 +170,9 @@ bool DynamicsPIDBacksteppingController::init(hardware_interface::EffortJointInte bool awdup; - PID[0].getGains(p,i,d,imax,imin,awdup); + pid_[0].getGains(p,i,d,imax,imin,awdup); ROS_INFO("Left PID: {p: %f, i: %f, d: %f, imax: %f, imin: %f, awdup: %d", p, i, d, imin, imax, awdup); - PID[1].getGains(p,i,d,imax,imin,awdup); + pid_[1].getGains(p,i,d,imax,imin,awdup); ROS_INFO("Right PID: {p: %f, i: %f, d: %f, imax: %f, imin: %f, awdup: %d", p, i, d, imin, imax, awdup); double low_level_control_rate = 0.0; @@ -198,20 +208,14 @@ bool DynamicsPIDBacksteppingController::init(hardware_interface::EffortJointInte else ROS_ERROR("Failed to get param '%s'", param_name[i].c_str()); } - const double A = k_parameters[0]; - const double B = k_parameters[1]; - const double C = k_parameters[2]; - const double D = k_parameters[3]; - const double E = k_parameters[4]; - const double F = k_parameters[5]; - - Eigen::Matrix2d G; - G << C, C, F, -F; - Fu << A, 0, 0, B, 0, D, E, 0; - std::cout << "Value of constants of F(u): " << Fu << std::endl; - - feedback_linearization = new StateFeedbackLinearization(G, Eigen::MatrixXd::Identity(2,2)); + Dyn_params.setA(k_parameters[0]); + Dyn_params.setB(k_parameters[1]); + Dyn_params.setC(k_parameters[2]); + Dyn_params.setD(k_parameters[3]); + Dyn_params.setE(k_parameters[4]); + Dyn_params.setF(k_parameters[5]); + feedback_linearization_ = new StateFeedbackLinearization(Dyn_params); } #ifdef DYNAMICS_PID_BACKSTEPPING_CONTROLLER_DEBUG @@ -226,71 +230,92 @@ bool DynamicsPIDBacksteppingController::init(hardware_interface::EffortJointInte * --------------------------------------------------------------------------------------------------------------------- */ - { - // Starting Odometer - Eigen::Vector3d pos_ini; + // Configure the Odometer Initial Pose - if (!node_.getParam("x_ini",pos_ini[0])){ - ROS_ERROR("Initial pose parameter '%s/x_ini' not found! The parameter will be disabled as '0'",node_.getNamespace().c_str()); - pos_ini[0] = 0; - } - - if (!node_.getParam("y_ini",pos_ini[1])){ - ROS_ERROR("Initial pose parameter '%s/y_ini' not found! The parameter will be disabled as '0'",node_.getNamespace().c_str()); - pos_ini[1] = 0; - } - - if (!node_.getParam("th_ini",pos_ini[2])){ - ROS_ERROR("Initial pose parameter '%s/th_ini' not found! The parameter will be disabled as '0'",node_.getNamespace().c_str()); - pos_ini[2] = 0; - } - - - Odometer = new OdometryTool(); - Odometer->init(node_, robot_ns, pos_ini); - - // TODO - // Start odometer in a arbitrary pose + if (!node_.getParam("x_ini",pose_[0])){ + ROS_ERROR("Initial pose parameter '%s/x_ini' not found! The parameter will be disabled as '0'",node_.getNamespace().c_str()); + pose_[0] = 0; + } - // Starting Kinematic Controller + if (!node_.getParam("y_ini",pose_[1])){ + ROS_ERROR("Initial pose parameter '%s/y_ini' not found! The parameter will be disabled as '0'",node_.getNamespace().c_str()); + pose_[1] = 0; + } - double non_smooth_parameters[6]; - std::string param_name[] = {"nonsmooth_controller/lambda1", - "nonsmooth_controller/lambda2", - "nonsmooth_controller/lambda3", - "nonsmooth_controller/gamma1", - "nonsmooth_controller/gamma2", - "nonsmooth_controller/update_ratio"}; + if (!node_.getParam("th_ini",pose_[2])){ + ROS_ERROR("Initial pose parameter '%s/th_ini' not found! The parameter will be disabled as '0'",node_.getNamespace().c_str()); + pose_[2] = 0; + } - for (int i = 0; i < 6; i++){ - if (node_.getParam(param_name[i].c_str(),non_smooth_parameters[i])) - ROS_INFO("%s: %f", param_name[i].c_str(), non_smooth_parameters[i]); - else { - ROS_ERROR("Failed to get param '%s'", param_name[i].c_str()); - return false; - } + // Starting Kinematic Controller + + double non_smooth_parameters[6]; + std::string param_name[] = {"nonsmooth_controller/lambda1", + "nonsmooth_controller/lambda2", + "nonsmooth_controller/lambda3", + "nonsmooth_controller/gamma1", + "nonsmooth_controller/gamma2", + "nonsmooth_controller/update_ratio"}; + + for (int i = 0; i < 6; i++){ + if (node_.getParam(param_name[i].c_str(),non_smooth_parameters[i])) + ROS_INFO("%s: %f", param_name[i].c_str(), non_smooth_parameters[i]); + else { + ROS_ERROR("Failed to get param '%s'", param_name[i].c_str()); + return false; } + } - lambda[0] = non_smooth_parameters[0]; - lambda[1] = non_smooth_parameters[1]; - lambda[2] = non_smooth_parameters[2]; - gamma[0] = non_smooth_parameters[3]; - gamma[1] = non_smooth_parameters[4]; - update_ratio = non_smooth_parameters[5]; - - Kinematics = new NonSmoothControl(lambda,gamma); + lambda[0] = non_smooth_parameters[0]; + lambda[1] = non_smooth_parameters[1]; + lambda[2] = non_smooth_parameters[2]; + gamma[0] = non_smooth_parameters[3]; + gamma[1] = non_smooth_parameters[4]; + update_ratio = non_smooth_parameters[5]; + + std::vector tolerances; + if (node_.getParam("tolerances",tolerances)) + { ROS_INFO("Tolerances: [%f, %f]", tolerances[0], tolerances[1]); + nonsmooth_control_ = new NonSmoothControl(lambda,gamma,tolerances[0],tolerances[1]); + } else { + ROS_ERROR("Failed to get param 'tolerances'"); + nonsmooth_control_ = new NonSmoothControl(lambda,gamma); } + ROS_INFO("%s: %f", "Kinematic Controller Frequency: ", low_level_control_rate/update_ratio); + node_.param("time_step",time_step_,0.01); + return true; } -void DynamicsPIDBacksteppingController::starting(const ros::Time &) +void DynamicsPIDBacksteppingController::starting(const ros::Time &time) { - Effort.setZero(); - nu_ref.setZero(); - u_ref.setZero(); - pose_ref = Odometer->getPose(); + nu_ref_.setZero(); + u_ref_.setZero(); + voltage_effort_.setZero(); + pose_ref_ = pose_; + odom_.setPose(pose_); + lastSamplingTime_ = time; + + for(unsigned int i=0;i < joints_.size();i++) + { + phi_[i]=joints_[i].getPosition(); + } + + struct sched_param param; + if(!node_.getParam("priority",param.sched_priority)) + { + ROS_WARN("No 'priority' configured for controller %s. Using highest possible priority.",node_.getNamespace().c_str()); + param.sched_priority=sched_get_priority_max(SCHED_FIFO); + } + if(sched_setscheduler(0,SCHED_FIFO,¶m) == -1) + { + ROS_WARN("Failed to set real-time scheduler."); + return; + } + if(mlockall(MCL_CURRENT|MCL_FUTURE) == -1) + ROS_WARN("Failed to lock memory."); #ifdef DYNAMICS_PID_BACKSTEPPING_CONTROLLER_DEBUG if (makeLog == false) @@ -299,14 +324,14 @@ void DynamicsPIDBacksteppingController::starting(const ros::Time &) if (robot_ns == "mini_twil") { - filename = "/home/talves/Dropbox/Mini_Twil/ros/exp/" + log_date + "/dynamics_backstepping_0" + std::to_string(take) + ".txt"; + filename = "/home/talves/Dropbox/Projects/Mini_Twil/ros/exp/" + log_date + "/dynamics_backstepping_0" + std::to_string(take) + ".txt"; ROS_INFO("Output File: %s", filename.c_str()); fd.open(filename); makeLog = true; } else if (robot_ns == "twil") { - filename = "/home/talves/Dropbox/Twil/ros/exp/" + log_date + "/dynamics_backstepping_0" + std::to_string(take) + ".txt"; + filename = "/home/talves/Dropbox/Projects/Twil/ros/exp/" + log_date + "/dynamics_backstepping_0" + std::to_string(take) + ".txt"; ROS_INFO("Output File: %s", filename.c_str()); fd.open(filename); makeLog = true; @@ -318,8 +343,6 @@ void DynamicsPIDBacksteppingController::starting(const ros::Time &) void DynamicsPIDBacksteppingController::update(const ros::Time &time, const ros::Duration &duration) { static int kinematic_cycle = 0; - static int filter_cycle = 0; - static double last_vel[2] = {0.0f, 0.0f}, alph = 0.5; #ifdef DYNAMICS_PID_BACKSTEPPING_CONTROLLER_DEBUG static int k = 0; @@ -337,45 +360,47 @@ void DynamicsPIDBacksteppingController::update(const ros::Time &time, const ros: } #endif - /* --------------------------------------------------------------------------------------------------------------------- - * Nonlinear Pose Controller - * --------------------------------------------------------------------------------------------------------------------- - */ + ros::Duration dt=time-lastSamplingTime_; - Eigen::Vector2d u, joint_ang_vel; + // if(fabs(dt.toSec()-time_step_) > time_step_/20) { + // ROS_INFO("Real-time problem..."); + // return; + // } + lastSamplingTime_=time; - // Compute Velocities - for(int i=0; i < joints_.size(); i++) joint_ang_vel[i]=joints_[i].getVelocity(); + // Incremental encoders sense angular displacement and + // not velocity + // phi[0] is the left wheel angular displacement + // phi[1] is the right wheel angular displacement - /* - * dphi[0] equals to right joint velocity and - * dphi[1] equals to left joint velocity (as in the formulation). - * In ros the left joint is defined as 0 and right joint as 1 - * to correct the mismatch, a conversion denoted by dphi=[joint_ang_vel[1], joint_ang_vel[0]] is defined - */ - Eigen::Vector2d dphi; - if (filter_cycle == 0){ - dphi << joint_ang_vel[1], joint_ang_vel[0]; - filter_cycle++; - } else { - dphi << (alph*joint_ang_vel[1]) +((1-alph)*last_vel[1]), (alph*joint_ang_vel[0]) +((1-alph)*last_vel[0]); + // phi_dot[0] is the left wheel angular rate + // phi_dot[1] is the right wheel angular rate + + Eigen::Vector2d phi_dot, deltaPhi=-phi_; + for(unsigned int i=0;i < joints_.size();i++) + { + phi_[i]=joints_[i].getPosition(); } - last_vel[0]=joint_ang_vel[0]; - last_vel[1]=joint_ang_vel[1]; + deltaPhi+=phi_; + phi_dot = deltaPhi/dt.toSec(); - u = VelocityTransformation::WheelToBase(dphi,WheelBase,WheelRadius[0]); + odom_.update(deltaPhi,dt); + odom_.getPose(pose_); - Odometer->compute(duration,u); - Eigen::Vector3d pose = Odometer->getPose(); + /* --------------------------------------------------------------------------------------------------------------------- + * Nonlinear Pose Controller + * --------------------------------------------------------------------------------------------------------------------- + */ - if (++kinematic_cycle == update_ratio){ + if (++kinematic_cycle == update_ratio) + { kinematic_cycle = 0; #ifdef CONTROLLER_OUTPUT_SCREEN ROS_INFO("Updating kinematic controller"); #endif // if (!Kinematics->StopCondition(pose,pose_ref,10e-3,1e-3)){ - u_ref = Kinematics->compute(pose,pose_ref); // Here will be generated the linear and angular velocities setpoints + u_ref_ = nonsmooth_control_->compute(pose_,pose_ref_); // Here will be generated the linear and angular velocities setpoints // } else u_ref.setZero(); } @@ -384,27 +409,47 @@ void DynamicsPIDBacksteppingController::update(const ros::Time &time, const ros: * --------------------------------------------------------------------------------------------------------------------- */ - Eigen::Vector2d error; - error[0] = u_ref[0]-u[0]; - error[1] = u_ref[1]-u[1]; - nu_ref[0] = PID[0].computeCommand(error[0],duration); // Here will be generated the linear acceleration setpoint (m/s^2) - nu_ref[1] = PID[1].computeCommand(error[1],duration); // Here will be generated the angular acceleration setpoint (rd/s^2) + Eigen::Vector2d error, u; - /* --------------------------------------------------------------------------------------------------------------------- - * Dynamics Feedback Linearization - * --------------------------------------------------------------------------------------------------------------------- - */ + // Compute Velocities + odom_.getVelocity(u); + + error[0] = u_ref_[0]-u[0]; + error[1] = u_ref_[1]-u[1]; + + // Compute Control Actions + nu_ref_[0] = pid_[0].computeCommand(error[0],duration); // Here will be generated the linear acceleration setpoint (m/s^2) + nu_ref_[1] = pid_[1].computeCommand(error[1],duration); // Here will be generated the angular acceleration setpoint (rd/s^2) + + // Compute Feedback Linearization Eigen::Vector2d fu; Eigen::Vector4d uf; + feedback_linearization_mutex_.lock(); + Mat2_4 Fu_ = feedback_linearization_->getFu(); // Feedback Linearization ----------------------------------------------------------------------------------------------- uf << u[0], u[1], u[1]*u[0], u[1]*u[1]; - fu = -Fu*uf; - Effort = feedback_linearization->computeInputs(nu_ref,fu); - // Control Efforts ------------------------------------------------------------------------------------------------------ - joints_[0].setCommand(Effort(1)); - joints_[1].setCommand(Effort(0)); + fu = -Fu_*uf; + + + // Computes Armature Voltages + // Va[0] is the Right Armature Voltage + // Va[1] is the Left Armature Voltage + Eigen::Vector2d Va = feedback_linearization_->computeInputs(nu_ref_,fu); + feedback_linearization_mutex_.unlock(); + + /* + * To return to ros space joint numbers, the efforts are calculated as + * effort[0] equals to left joint effort and + * effort[1] equals to right joint effort + */ + voltage_effort_.setZero(); + voltage_effort_ << Va[1], Va[0]; + + // Apply Control Efforts + joints_[0].setCommand(voltage_effort_[0]); + joints_[1].setCommand(voltage_effort_[1]); #ifdef CONTROLLER_OUTPUT_SCREEN ROS_INFO("uref[0]: %f; u[0]: %f",u_ref[0],u[0]); @@ -412,20 +457,76 @@ void DynamicsPIDBacksteppingController::update(const ros::Time &time, const ros: ROS_INFO("Ref: {%f,%f,%f}; Pos: {%f,%f,%f}",pose_ref[0], pose_ref[1], pose_ref[2], pose[0], pose[1], pose[2]); #endif - Odometer->publish(); - #ifdef DYNAMICS_PID_BACKSTEPPING_CONTROLLER_DEBUG if (makeLog==true){ if (k>0){ - fd << k << "," << duration.toSec() << "," << pose_ref[0] << "," << pose_ref[1] - << "," << pose_ref[2] << "," << pose[0] << "," << pose[1] << "," << pose[2] - << "," << u_ref[0] << "," << u_ref[1] << nu_ref[0] << "," << nu_ref[1] << "," << u[0] << "," << u[1] - << "," << Effort[0] << "," << Effort[1] << "," << dphi[1] << "," << dphi[0] + // fd << "seq," << "dur," << "pose_ref1," << "pose_ref2," << "pose_ref3," << "pose1," << "pose2," + // << "pose3," << "u_ref1," <<"u_ref2," << "nu_ref1," <<"nu_ref2," << "u1," << "u2," + // << "eff1," << "eff2," << "dphi1," << "dphi2" << std::endl; + fd << k << "," << duration.toSec() << "," << pose_ref_[0] << "," << pose_ref_[1] + << "," << pose_ref_[2] << "," << pose_[0] << "," << pose_[1] << "," << pose_[2] + << "," << u_ref_[0] << "," << u_ref_[1] << "," << nu_ref_[0] << "," << nu_ref_[1] << "," << u[0] << "," << u[1] + << "," << voltage_effort_[1] << "," << voltage_effort_[0] << "," << phi_dot[1] << "," << phi_dot[0] << std::endl; } } #endif + odom_.publish(time); + + if(status_publisher_ && status_publisher_->trylock()) + { + status_publisher_->msg_.header.stamp=time; + + status_publisher_->msg_.set_point.x=pose_ref_[0]; + status_publisher_->msg_.set_point.y=pose_ref_[1]; + status_publisher_->msg_.set_point.theta=pose_ref_[2]; + + status_publisher_->msg_.process_value.x=pose_[0]; + status_publisher_->msg_.process_value.y=pose_[1]; + status_publisher_->msg_.process_value.theta=pose_[2]; + + status_publisher_->msg_.process_value_dot.x=u[0]*cos(pose_[2]); + status_publisher_->msg_.process_value_dot.y=u[0]*sin(pose_[2]); + status_publisher_->msg_.process_value_dot.theta=u[1]; + + status_publisher_->msg_.error.x=pose_ref_[0]-pose_[0]; + status_publisher_->msg_.error.y=pose_ref_[1]-pose_[1]; + status_publisher_->msg_.error.theta=pose_ref_[2]-pose_[2]; + + status_publisher_->msg_.time_step=dt.toSec(); + + for(int i=0;i < voltage_effort_.size();i++) + status_publisher_->msg_.command[i]=voltage_effort_[i]; + + Eigen::Vector3d lb_value = nonsmooth_control_->getLamb(); + for(int i=0;i < lb_value.size();i++) + status_publisher_->msg_.lambda[i]=lb_value[i]; + + Eigen::Vector2d gm_value = nonsmooth_control_->getGam(); + for(int i=0;i < gm_value.size();i++) + status_publisher_->msg_.gamma[i]=gm_value[i]; + + status_publisher_->msg_.polar_error.range=nonsmooth_control_->getE(); + status_publisher_->msg_.polar_error.angle=nonsmooth_control_->getPsi(); + status_publisher_->msg_.polar_error.orientation=nonsmooth_control_->getAlpha(); + + for(int i=0;i < u_ref_.size();i++) + status_publisher_->msg_.backstep_set_point[i]=u_ref_[i]; + + for(int i=0;i < u.size();i++) + status_publisher_->msg_.backstep_process_value[i]=u[i]; + + Eigen::Vector2d eb=u-u_ref_; + + for(int i=0;i < eb.size();i++) + status_publisher_->msg_.backstep_error[i]=eb[i]; + + for(int i=0;i < nu_ref_.size();i++) + status_publisher_->msg_.linear_dynamics_command[i]=nu_ref_[i]; + + status_publisher_->unlockAndPublish(); + } } void DynamicsPIDBacksteppingController::stopping(const ros::Time &) @@ -433,14 +534,14 @@ void DynamicsPIDBacksteppingController::stopping(const ros::Time &) #ifdef DYNAMICS_PID_BACKSTEPPING_CONTROLLER_DEBUG fd.close(); #endif - PositionSub[0].shutdown(); - PositionSub[1].shutdown(); + position_sub_[0].shutdown(); + position_sub_[1].shutdown(); } void DynamicsPIDBacksteppingController::commandCB(const geometry_msgs::PoseStampedConstPtr &command) { - pose_ref[0] = command->pose.position.x; - pose_ref[1] = command->pose.position.y; + pose_ref_[0] = command->pose.position.x; + pose_ref_[1] = command->pose.position.y; tf::Quaternion quat_aux(command->pose.orientation.x, command->pose.orientation.y, command->pose.orientation.z, @@ -448,7 +549,7 @@ void DynamicsPIDBacksteppingController::commandCB(const geometry_msgs::PoseStamp tf::Matrix3x3 m(quat_aux); double roll, pitch, yaw; m.getRPY(roll, pitch, yaw); - pose_ref[2] = yaw; + pose_ref_[2] = yaw; #ifdef CONTROLLER_OUTPUT_SCREEN ROS_INFO("Pose_ref: {%f, %f, %f}", pose_ref[0], pose_ref[1], pose_ref[2]); #endif @@ -456,19 +557,87 @@ void DynamicsPIDBacksteppingController::commandCB(const geometry_msgs::PoseStamp void DynamicsPIDBacksteppingController::command2dCB(const geometry_msgs::Pose2DConstPtr &command) { - // pose_ref[0] = command->x; - // pose_ref[1] = command->y; - // pose_ref[2] = command->theta; - // std::cout << "Pose_ref: " << pose_ref << std::endl; - pose_ref[0] = command->x; - pose_ref[1] = command->y; - pose_ref[2] = command->theta; + pose_ref_[0] = command->x; + pose_ref_[1] = command->y; + pose_ref_[2] = command->theta; #ifdef CONTROLLER_OUTPUT_SCREEN ROS_INFO("Pose_ref: {%f, %f, %f}", pose_ref[0], pose_ref[1], pose_ref[2]); #endif } +void DynamicsPIDBacksteppingController::parametersCB(const twil_msgs::non_smooth_parametersConstPtr ¶m) +{ + ROS_INFO("\nUpdate in Nonsmooth Controller parameters..."); + Eigen::Vector3d lb_par; + Eigen::Vector2d gm_par; + Eigen::Vector2d eps_par; + + lb_par << param->lambda[0], param->lambda[1], param->lambda[2]; + gm_par << param->gamma[0], param->gamma[1]; + eps_par << param->eps[0], param->eps[1]; + + nonsmooth_control_->setLamb(lb_par); + nonsmooth_control_->setGam(gm_par); + nonsmooth_control_->setEps(eps_par); +} + +void DynamicsPIDBacksteppingController::dynParametersCB(const twil_msgs::robot_dynamicsConstPtr ¶m) +{ + Dyn_params; + bool update = false; + double max_variance_vl = 1e-6; + // double min_variance_vl = 1e-2; + + if ((sqrt(param->variances[0]*param->variances[0]) < max_variance_vl) )//&& (fabs(param->parameters[0]) > 0)) + { + Dyn_params.setA(param->parameters[0]); + std::cout << "Variance_A: " << param->variances[0] << std::endl; + update = true; + } + if ((sqrt(param->variances[1]*param->variances[1]) < max_variance_vl) )//&& (fabs(param->parameters[1]) > 0)) + { + Dyn_params.setB(param->parameters[1]); + std::cout << "Variance_B: " << param->variances[1] << std::endl; + update = true; + } + if ((sqrt(param->variances[2]*param->variances[2]) < max_variance_vl) )//&& (fabs(param->parameters[2]) > 0)) + { + Dyn_params.setC(param->parameters[2]); + std::cout << "Variance_C: " << param->variances[2] << std::endl; + update = true; + } + + if ((sqrt(param->variances[3]*param->variances[3]) < max_variance_vl) )//&& (fabs(param->parameters[3]) > 0)) + { + Dyn_params.setD(param->parameters[3]); + std::cout << "Variance_D: " << param->variances[3] << std::endl; + update = true; + } + if ((sqrt(param->variances[4]*param->variances[4]) < max_variance_vl) )//&& (fabs(param->parameters[4]) > 0)) + { + Dyn_params.setE(param->parameters[4]*param->parameters[4]); + std::cout << "Variance_E: " << param->variances[4] << std::endl; + update = true; + } + if ((sqrt(param->variances[5]*param->variances[5]) < max_variance_vl) )//&& (fabs(param->parameters[5]) > 0)) + { + Dyn_params.setF(param->parameters[5]); + std::cout << "Variance_F: " << param->variances[5] << std::endl; + update = true; + } + + if (update) { + ROS_INFO("Dynamic_parameters Updated..."); + feedback_linearization_mutex_.lock(); + Mat2_4 Fu_ = feedback_linearization_->computeFu(Dyn_params); + feedback_linearization_->setFu(Fu_); + Eigen::Matrix2d Ginv_ = feedback_linearization_->computeGinv(Dyn_params); + feedback_linearization_->setGinv(Ginv_); + feedback_linearization_mutex_.unlock(); + } +} + } PLUGINLIB_EXPORT_CLASS(twil_controllers::DynamicsPIDBacksteppingController,controller_interface::ControllerBase) diff --git a/twil_controllers/src/dynamics_pid_controller.cpp b/twil_controllers/src/dynamics_pid_controller.cpp index 71007ae..8fee65f 100644 --- a/twil_controllers/src/dynamics_pid_controller.cpp +++ b/twil_controllers/src/dynamics_pid_controller.cpp @@ -23,6 +23,8 @@ *******************************************************************************/ +#include + #include #include #include @@ -32,25 +34,27 @@ namespace twil_controllers { -DynamicsPIDController::DynamicsPIDController(void): - WheelRadius(2){} +DynamicsPIDController::DynamicsPIDController(void) + : WheelRadius(2), odom_(0.0,std::vector(2)){} DynamicsPIDController::~DynamicsPIDController(void) { - VelocitySub.~Subscriber(); - feedback_linearization->~StateFeedbackLinearization(); - PID[0].~Pid(); - PID[1].~Pid(); + velocity_sub_.~Subscriber(); + feedback_linearization_->~StateFeedbackLinearization(); + + pid_[0].~Pid(); + pid_[1].~Pid(); + dynamics_parameters_sub_.~Subscriber(); } -bool DynamicsPIDController::init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n) { +bool DynamicsPIDController::init(twil_hardware_interfaces::VoltageJointInterface *robot, ros::NodeHandle &n) { node_=n; robot_=robot; - if(!node_.getParam("/robot_config",robot_ns)) + if(!node_.getParam("/robot_config",robot_ns_)) { ROS_ERROR("Could not find Robot namespace in '/robot_config'."); return false; - } else ROS_INFO("Robot namespace: %s", robot_ns.c_str()); + } else ROS_INFO("Robot namespace: %s", robot_ns_.c_str()); XmlRpc::XmlRpcValue joint_names; if(!node_.getParam("joints",joint_names)) @@ -78,7 +82,8 @@ bool DynamicsPIDController::init(hardware_interface::EffortJointInterface *robot joints_.push_back(j); } - VelocitySub = node_.subscribe("cmd_vel",1,&DynamicsPIDController::commandCB,this); + velocity_sub_ = node_.subscribe("cmd_vel",1,&DynamicsPIDController::commandCB,this); + dynamics_parameters_sub_ = node_.subscribe("/"+robot_ns_+"/ident/dynamic_parameters",1,&DynamicsPIDController::dynParametersCB,this); #ifdef urdf std::string robot_desc_string; @@ -119,30 +124,37 @@ bool DynamicsPIDController::init(hardware_interface::EffortJointInterface *robot std::cout << "Wheel Radius: " << WheelRadius[0] << " - " << WheelRadius[1] << std::endl; std::cout << "Wheel base: " << WheelBase << std::endl; #else - if (node_.getParam("/"+robot_ns+"/robot_baseline",WheelBase)) + if (node_.getParam("/"+robot_ns_+"/robot_baseline",WheelBase)) ROS_INFO("Robot baseline: %f", WheelBase); else ROS_ERROR("Failed to get param 'robot_baseline'"); - if (node_.getParam("/" + robot_ns + "/wheel_radius",WheelRadius[0])){ + if (node_.getParam("/" + robot_ns_ + "/wheel_radius",WheelRadius[0])){ WheelRadius[1] = WheelRadius[0]; ROS_INFO("Wheel Radius: %f", WheelRadius[1]); } else ROS_ERROR("Failed to get param 'wheel_radius'"); #endif + std::string odom_frame_id="odom"; + node_.param("odom_frame_id",odom_frame_id,odom_frame_id); + + odom_.setParams(WheelBase,WheelRadius); + odom_.publisherConfig(node_); + status_publisher_.reset(new realtime_tools::RealtimePublisher(node_,"status",1)); + status_publisher_->msg_.header.frame_id=odom_frame_id; /* * Velocity PID Controller Parameters * --------------------------------------------------------------------------------------------------------------------- */ - if (!PID[0].init(ros::NodeHandle(node_, "linear_velocity_pid_parameters"))){ + if (!pid_[0].init(ros::NodeHandle(node_, "linear_velocity_pid_parameters"))){ ROS_ERROR("This class could not construct PID controller for linear velocity"); return false; } - if (!PID[1].init(ros::NodeHandle(node_, "angular_velocity_pid_parameters"))){ + if (!pid_[1].init(ros::NodeHandle(node_, "angular_velocity_pid_parameters"))){ ROS_ERROR("This class could not construct PID controller for angular velocity"); return false; } @@ -151,9 +163,9 @@ bool DynamicsPIDController::init(hardware_interface::EffortJointInterface *robot bool awdup; - PID[0].getGains(p,i,d,imax,imin,awdup); + pid_[0].getGains(p,i,d,imax,imin,awdup); ROS_INFO("Left PID: {p: %f, i: %f, d: %f, imax: %f, imin: %f, awdup: %d", p, i, d, imin, imax, awdup); - PID[1].getGains(p,i,d,imax,imin,awdup); + pid_[1].getGains(p,i,d,imax,imin,awdup); ROS_INFO("Right PID: {p: %f, i: %f, d: %f, imax: %f, imin: %f, awdup: %d", p, i, d, imin, imax, awdup); if (!node_.getParam("/log_date",log_date)){ @@ -161,7 +173,7 @@ bool DynamicsPIDController::init(hardware_interface::EffortJointInterface *robot log_date=""; } -#ifdef TWIST_PID_DEBUG +#ifdef DYNAMICS_PID_DEBUG if (!node_.getParam("take",take)){ ROS_ERROR("Execution take '%s/take' not found! The parameter will be assumed as '0'",node_.getNamespace().c_str()); take = 0; @@ -187,43 +199,63 @@ bool DynamicsPIDController::init(hardware_interface::EffortJointInterface *robot else ROS_ERROR("Failed to get param '%s'", param_name[i].c_str()); } - const double A = k_parameters[0]; - const double B = k_parameters[1]; - const double C = k_parameters[2]; - const double D = k_parameters[3]; - const double E = k_parameters[4]; - const double F = k_parameters[5]; + Dyn_params.setA(k_parameters[0]); + Dyn_params.setB(k_parameters[1]); + Dyn_params.setC(k_parameters[2]); + Dyn_params.setD(k_parameters[3]); + Dyn_params.setE(k_parameters[4]); + Dyn_params.setF(k_parameters[5]); + + feedback_linearization_ = new StateFeedbackLinearization(Dyn_params); - Eigen::Matrix2d G; - G << C, C, F, -F; - Fu << A, 0, 0, B, 0, D, E, 0; - std::cout << "Value of constants of F(u): " << Fu << std::endl; + node_.param("time_step",time_step_,0.01); - feedback_linearization = new StateFeedbackLinearization(G, Eigen::MatrixXd::Identity(2,2)); return true; } void DynamicsPIDController::starting(const ros::Time& time) { - nu_ref.setZero(); - u_ref.setZero(); - Effort.setZero(); + nu_ref_.setZero(); + u_ref_.setZero(); + voltage_effort_.setZero(); + pose_.setZero(); + odom_.setPose(pose_); + lastSamplingTime_ = time; + + for(unsigned int i=0;i < joints_.size();i++) + { + phi_[i]=joints_[i].getPosition(); + } + + struct sched_param param; + if(!node_.getParam("priority",param.sched_priority)) + { + ROS_WARN("No 'priority' configured for controller %s. Using highest possible priority.",node_.getNamespace().c_str()); + param.sched_priority=sched_get_priority_max(SCHED_FIFO); + } + if(sched_setscheduler(0,SCHED_FIFO,¶m) == -1) + { + ROS_WARN("Failed to set real-time scheduler."); + return; + } + if(mlockall(MCL_CURRENT|MCL_FUTURE) == -1) + ROS_WARN("Failed to lock memory."); #ifdef DYNAMICS_PID_DEBUG if (makeLog == false) { std::string filename = "none"; - if (robot_ns == "mini_twil") + if (robot_ns_ == "mini_twil") { - filename = "/home/talves/Dropbox/Mini_Twil/ros/exp/" + log_date + "/dynamics_pid_0" + std::to_string(take) + ".txt"; + filename = "/home/talves/Dropbox/Projects/Mini_Twil/ros/exp/" + log_date + "/dynamics_pid_0" + std::to_string(take) + ".txt"; ROS_INFO("Output File: %s", filename.c_str()); fd.open(filename); makeLog = true; } - else if (robot_ns == "twil") + else if (robot_ns_ == "twil") { - filename = "/home/talves/Dropbox/Twil/ros/exp/" + log_date + "/dynamics_pid_0" + std::to_string(take) + ".txt"; + filename = "/home/talves/Dropbox/Projects/Twil/ros/exp/" + log_date + "/dynamics_pid_0" + std::to_string(take) + ".txt"; ROS_INFO("Output File: %s", filename.c_str()); fd.open(filename); makeLog = true; @@ -234,73 +266,94 @@ void DynamicsPIDController::starting(const ros::Time& time) void DynamicsPIDController::update(const ros::Time& time, const ros::Duration& duration) -{ +{ #ifdef DYNAMICS_PID_DEBUG static int k = 0; if (makeLog==true){ if (this->fd.is_open()){ if (k==0) { - ROS_INFO("Starting the LOG for the '%s_robot'...",robot_ns.c_str()); - fd << "seq," << "dur," << "u_ref1," <<"u_ref2," << "nu_ref1," <<"nu_ref2,"<< "u1," << "u2," << "eff1," << "eff2," << "dphi1," << "dphi2" << std::endl; + ROS_INFO("Starting the LOG for the '%s_robot'...",robot_ns_.c_str()); + fd << "seq," << "dur," << "pose_ref1," << "pose_ref2," << "pose_ref3," << "pose1," << "pose2," + << "pose3," << "u_ref1," <<"u_ref2," << "nu_ref1," <<"nu_ref2," << "u1," << "u2," + << "eff1," << "eff2," << "dphi1," << "dphi2" << std::endl; } k++; } } #endif - /* --------------------------------------------------------------------------------------------------------------------- - * Velocity Controller - * --------------------------------------------------------------------------------------------------------------------- - */ - Eigen::Vector2d u, joint_ang_vel; - - // Compute Velocities - for(int i=0; i < joints_.size(); i++) joint_ang_vel[i]=joints_[i].getVelocity(); + ros::Duration dt=time-lastSamplingTime_; - /* - * dphi[0] equals to right joint velocity and - * dphi[1] equals to left joint velocity (as in the formulation). - * In ros the left joint is defined as 0 and right joint as 1 - * to correct the mismatch, a conversion denoted by dphi=[joint_ang_vel[1], joint_ang_vel[0]] is defined - */ - Eigen::Vector2d dphi; - dphi << joint_ang_vel[1], joint_ang_vel[0]; - u = VelocityTransformation::WheelToBase(dphi,WheelBase,WheelRadius[0]); + // if(fabs(dt.toSec()-time_step_) > time_step_/20) { + // ROS_INFO("Real-time problem..."); + // return; + // } + lastSamplingTime_=time; + // Incremental encoders sense angular displacement and + // not velocity + // phi[0] is the left wheel angular displacement + // phi[1] is the right wheel angular displacement + // phi_dot[0] is the left wheel angular rate + // phi_dot[1] is the right wheel angular rate - // Velocity PID Calculation --------------------------------------------------------------------------------------------- - Eigen::Vector2d error; - error[0] = u_ref[0]-u[0]; - error[1] = u_ref[1]-u[1]; - nu_ref[0] = PID[0].computeCommand(error[0],duration); // Here will be generated the linear acceleration setpoint (m/s^2) - nu_ref[1] = PID[1].computeCommand(error[1],duration); // Here will be generated the angular acceleration setpoint (rd/s^2) - /* - * ---------------------------------------------------------------------------------------------------------------------- - */ + Eigen::Vector2d phi_dot, deltaPhi=-phi_; + for(unsigned int i=0;i < joints_.size();i++) + { + phi_[i]=joints_[i].getPosition(); + } + deltaPhi+=phi_; + phi_dot = deltaPhi/dt.toSec(); + odom_.update(deltaPhi,dt); + odom_.getPose(pose_); /* --------------------------------------------------------------------------------------------------------------------- - * Dynamics Feedback Linearization + * Velocity Controller * --------------------------------------------------------------------------------------------------------------------- */ + Eigen::Vector2d error, u; + + // Compute Velocities + odom_.getVelocity(u); + + error[0] = u_ref_[0]-u[0]; + error[1] = u_ref_[1]-u[1]; + + // Compute Control Actions + nu_ref_[0] = pid_[0].computeCommand(error[0],duration); // Here will be generated the linear acceleration setpoint (m/s^2) + nu_ref_[1] = pid_[1].computeCommand(error[1],duration); // Here will be generated the angular acceleration setpoint (rd/s^2) + + // Compute Feedback Linearization Eigen::Vector2d fu; Eigen::Vector4d uf; + feedback_linearization_mutex_.lock(); + Mat2_4 Fu_ = feedback_linearization_->getFu(); // Feedback Linearization ----------------------------------------------------------------------------------------------- uf << u[0], u[1], u[1]*u[0], u[1]*u[1]; - fu = -Fu*uf; - Effort = feedback_linearization->computeInputs(nu_ref,fu); - // Control Efforts ------------------------------------------------------------------------------------------------------ - joints_[0].setCommand(Effort(1)); - joints_[1].setCommand(Effort(0)); + fu = -Fu_*uf; + + // Computes Armature Voltages + // Va[0] is the Right Armature Voltage + // Va[1] is the Left Armature Voltage + Eigen::Vector2d Va = feedback_linearization_->computeInputs(nu_ref_,fu); + feedback_linearization_mutex_.unlock(); /* - * ---------------------------------------------------------------------------------------------------------------------- - */ + * To return to ros space joint numbers, the efforts are calculated as + * effort[0] equals to left joint effort and + * effort[1] equals to right joint effort + */ + voltage_effort_.setZero(); + voltage_effort_ << Va[1], Va[0]; + // Apply Control Efforts + joints_[0].setCommand(voltage_effort_[0]); + joints_[1].setCommand(voltage_effort_[1]); #ifdef CONTROLLER_OUTPUT_SCREEN ROS_INFO("uref[0]: %f; u[0]: %f",u_ref[0],u[0]); @@ -311,11 +364,50 @@ void DynamicsPIDController::update(const ros::Time& time, if (makeLog == true){ if (k>0) { - fd << k << ","<< duration.toSec() << "," << u_ref[0] << "," << u_ref[1] << "," << nu_ref[0] << "," << nu_ref[1] << "," << u[0] << "," << u[1] << "," - << Effort[0] << "," << Effort[1] << "," << dphi[1] << "," << dphi[0] << std::endl; + // fd << "seq," << "dur," << "pose_ref1," << "pose_ref2," << "pose_ref3," << "pose1," << "pose2," + // << "pose3," << "u_ref1," <<"u_ref2," << "nu_ref1," <<"nu_ref2," << "u1," << "u2," + // << "eff1," << "eff2," << "dphi1," << "dphi2" << std::endl; + fd << k << "," << duration.toSec() << "," << 0 << "," << 0 + << "," << 0 << "," << pose_[0] << "," << pose_[1] << "," << pose_[2] + << "," << u_ref_[0] << "," << u_ref_[1] << "," << nu_ref_[0] << "," << nu_ref_[1] << "," << u[0] << "," << u[1] + << "," << voltage_effort_[1] << "," << voltage_effort_[0] << "," << phi_dot[1] << "," << phi_dot[0] + << std::endl; } } #endif + + odom_.publish(time); + + if(status_publisher_ && status_publisher_->trylock()) + { + odom_.publish(time); + + status_publisher_->msg_.header.stamp=time; + + status_publisher_->msg_.robot_pose.x=pose_[0]; + status_publisher_->msg_.robot_pose.y=pose_[1]; + status_publisher_->msg_.robot_pose.theta=pose_[2]; + + status_publisher_->msg_.time_step=dt.toSec(); + + for(int i=0;i < voltage_effort_.size();i++) + status_publisher_->msg_.command[i]=voltage_effort_[i]; + + for(int i=0;i < u_ref_.size();i++) + status_publisher_->msg_.velocity_set_point[i]=u_ref_[i]; + + for(int i=0;i < u.size();i++) + status_publisher_->msg_.velocity_process_value[i]=u[i]; + + for(int i=0;i < u.size();i++) + status_publisher_->msg_.velocity_error[i]=(u_ref_[i]-u[i]); + + for(int i=0;i < nu_ref_.size();i++) + status_publisher_->msg_.linear_dynamics_command[i]=nu_ref_[i]; + + status_publisher_->unlockAndPublish(); + } + } void DynamicsPIDController::stopping(void) @@ -323,20 +415,77 @@ void DynamicsPIDController::stopping(void) #ifdef DYNAMICS_PID_DEBUG fd.close(); #endif - VelocitySub.shutdown(); + velocity_sub_.shutdown(); } void DynamicsPIDController::commandCB(const geometry_msgs::TwistConstPtr &CommandVel) { - u_ref.setZero(); - u_ref[0] = CommandVel->linear.x; - u_ref[1] = CommandVel->angular.z; + u_ref_.setZero(); + u_ref_[0] = CommandVel->linear.x; + u_ref_[1] = CommandVel->angular.z; #ifdef CONTROLLER_OUTPUT_SCREEN ROS_INFO("u_ref: {%f, %f}", u_ref[0], u_ref[1]); #endif } +void DynamicsPIDController::dynParametersCB(const twil_msgs::robot_dynamicsConstPtr ¶m) +{ + + Dyn_params; + bool update = false; + double max_variance_vl = 1e-5; + // double min_variance_vl = 1e-2; + + if ((sqrt(param->variances[0]*param->variances[0]) < max_variance_vl) )//&& (fabs(param->parameters[0]) > 0)) + { + Dyn_params.setA(param->parameters[0]); + std::cout << "Variance_A: " << param->variances[0] << std::endl; + update = true; + } + if ((sqrt(param->variances[1]*param->variances[1]) < max_variance_vl) )//&& (fabs(param->parameters[1]) > 0)) + { + Dyn_params.setB(param->parameters[1]); + std::cout << "Variance_B: " << param->variances[1] << std::endl; + update = true; + } + if ((sqrt(param->variances[2]*param->variances[2]) < max_variance_vl) )//&& (fabs(param->parameters[2]) > 0)) + { + Dyn_params.setC(param->parameters[2]); + std::cout << "Variance_C: " << param->variances[2] << std::endl; + update = true; + } + + if ((sqrt(param->variances[3]*param->variances[3]) < max_variance_vl) )//&& (fabs(param->parameters[3]) > 0)) + { + Dyn_params.setD(param->parameters[3]); + std::cout << "Variance_D: " << param->variances[3] << std::endl; + update = true; + } + if ((sqrt(param->variances[4]*param->variances[4]) < max_variance_vl) )//&& (fabs(param->parameters[4]) > 0)) + { + Dyn_params.setE(param->parameters[4]*param->parameters[4]); + std::cout << "Variance_E: " << param->variances[4] << std::endl; + update = true; + } + if ((sqrt(param->variances[5]*param->variances[5]) < max_variance_vl) )//&& (fabs(param->parameters[5]) > 0)) + { + Dyn_params.setF(param->parameters[5]); + std::cout << "Variance_F: " << param->variances[5] << std::endl; + update = true; + } + + if (update) { + ROS_INFO("Dynamic_parameters Updated..."); + feedback_linearization_mutex_.lock(); + Mat2_4 Fu_ = feedback_linearization_->computeFu(Dyn_params); + feedback_linearization_->setFu(Fu_); + Eigen::Matrix2d Ginv_ = feedback_linearization_->computeGinv(Dyn_params); + feedback_linearization_->setGinv(Ginv_); + feedback_linearization_mutex_.unlock(); + } +} + } PLUGINLIB_EXPORT_CLASS(twil_controllers::DynamicsPIDController,controller_interface::ControllerBase) diff --git a/twil_controllers/src/joint_group_voltage_controller.cpp b/twil_controllers/src/joint_group_voltage_controller.cpp new file mode 100644 index 0000000..65ec461 --- /dev/null +++ b/twil_controllers/src/joint_group_voltage_controller.cpp @@ -0,0 +1,15 @@ +#include +#include + +namespace twil_controllers +{ +template +void twil_controllers::ForwardJointGroupCommandController::starting(const ros::Time& time) +{ + // Start controller with 0.0 efforts + commands_buffer_.readFromRT()->assign(n_joints_, 0.0); +} + +} + +PLUGINLIB_EXPORT_CLASS(twil_controllers::JointGroupVoltageController,controller_interface::ControllerBase) diff --git a/twil_controllers/src/joint_voltage_controller.cpp b/twil_controllers/src/joint_voltage_controller.cpp new file mode 100644 index 0000000..f49b992 --- /dev/null +++ b/twil_controllers/src/joint_voltage_controller.cpp @@ -0,0 +1,15 @@ +#include +#include + +namespace twil_controllers { + +template + +void twil_controllers::ForwardCommandController::starting(const ros::Time& time) +{ + // Start controller with 0.0 effort + command_buffer_.writeFromNonRT(0.0); +} + +} +PLUGINLIB_EXPORT_CLASS(twil_controllers::JointVoltageController,controller_interface::ControllerBase) diff --git a/twil_controllers/src/nonlinear_backstepping_controller.cpp b/twil_controllers/src/nonlinear_backstepping_controller.cpp index 5e1ed79..8a910b3 100644 --- a/twil_controllers/src/nonlinear_backstepping_controller.cpp +++ b/twil_controllers/src/nonlinear_backstepping_controller.cpp @@ -1 +1,489 @@ -#include "twil_controllers/nonlinear_backstepping_controller.h" +///****************************************************************************** +// Twil Controllers +// Linearizing Backstep Controller + +// Copyright (C) 2017 Tiago G. Alves + +// This program is free software; you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation; either version 2 of the License, or +// (at your option) any later version. + +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + +// You can also obtain a copy of the GNU General Public License +// at . + +//*******************************************************************************/ + +//#include "twil_controllers/nonlinear_backstepping_controller.h" +//#include +//#include +//#include + +////#define URDF +////#define CONTROLLER_OUTPUT_SCREEN + +//#define sqr(x) (x*x) +//#define cub(x) (x*x*x) + +//namespace twil_controllers +//{ + +//NonlinearBacksteppingController::NonlinearBacksteppingController(void) +// : lambda(5), gamma(4) +//{ +// pose_ref.setZero(); +//} + +//NonlinearBacksteppingController::~NonlinearBacksteppingController(void) +//{ +// PositionSub[0].~Subscriber(); +// PositionSub[1].~Subscriber(); +//} + +//bool NonlinearBacksteppingController::init(twil_hardware_interfaces::VoltageJointInterface *robot, ros::NodeHandle &n) +//{ + +// node_ = n; +// robot_ = robot; + +// if(!node_.getParam("/robot_config",robot_ns)) +// { +// ROS_ERROR("Could not find Robot namespace in '/robot_config'."); +// return false; +// } else ROS_INFO("Robot namespace: %s", robot_ns.c_str()); + +// XmlRpc::XmlRpcValue joint_names; +// if(!node_.getParam("joints",joint_names)) +// { +// ROS_ERROR("No 'joints' parameter in controller. (namespace: %s)",node_.getNamespace().c_str()); +// return false; +// } + +// if(joint_names.getType() != XmlRpc::XmlRpcValue::TypeArray) +// { +// ROS_ERROR("The 'joints' parameter is not a struct. (namespace: %s)",node_.getNamespace().c_str()); +// return false; +// } + +// for(int i=0; i < joint_names.size();i++) +// { +// XmlRpc::XmlRpcValue &name_value=joint_names[i]; +// if(name_value.getType() != XmlRpc::XmlRpcValue::TypeString) +// { +// ROS_ERROR("Array of joint names should contain only strings. (namespace: %s)",node_.getNamespace().c_str()); +// return false; +// } + +// hardware_interface::JointHandle j=robot->getHandle((std::string)name_value); +// joints_.push_back(j); +// } + +// // PositionSub[0] = node_.subscribe("cmd_pos",1,&DynamicsPIDBacksteppingController::commandCB,this); +// PositionSub[0] = node_.subscribe("/move_base_simple/goal",1,&NonlinearBacksteppingController::commandCB,this); +// PositionSub[1] = node_.subscribe("cmd_pos2d",1,&NonlinearBacksteppingController::command2dCB,this); + +//#ifdef URDF +// std::string robot_desc_string; +// if(!node_.getParam("/robot_description",robot_desc_string)) +// { +// ROS_ERROR("Could not find '/robot_description'."); +// return false; +// } + +// KDL::Tree tree; +// if (!kdl_parser::treeFromString(robot_desc_string,tree)) +// { +// ROS_ERROR("Failed to construct KDL tree."); +// return false; +// } + +// // get wheelBase from URDF (actually from KDL tree) +// KDL::SegmentMap::const_iterator segmentMapIter=tree.getSegment("left_wheel_support"); +// KDL::Frame leftSupportFrame=segmentMapIter->second.segment.getFrameToTip(); + +// segmentMapIter=tree.getSegment("right_wheel_support"); +// KDL::Frame rightSupportFrame=segmentMapIter->second.segment.getFrameToTip(); + +// WheelRadius.resize(joints_.size()); +// WheelBase=leftSupportFrame(1,3)-rightSupportFrame(1,3); + +// // get wheelRadius from URDF (actually from KDL tree) +// segmentMapIter=tree.getSegment("chassis"); +// KDL::Frame chassisFrame=segmentMapIter->second.segment.getFrameToTip(); + +// segmentMapIter=tree.getSegment("left_wheel"); +// KDL::Joint leftWheelJoint=segmentMapIter->second.segment.getJoint(); +// WheelRadius[0]=chassisFrame(2,3)+leftSupportFrame(2,3)+leftWheelJoint.JointOrigin().z(); + +// segmentMapIter=tree.getSegment("right_wheel"); +// KDL::Joint rightWheelJoint=segmentMapIter->second.segment.getJoint(); +// WheelRadius[1]=chassisFrame(2,3)+rightSupportFrame(2,3)+rightWheelJoint.JointOrigin().z(); + +// std::cout << "Wheel Radius: " << WheelRadius[0] << " - " << WheelRadius[1] << std::endl; +// std::cout << "Wheel base: " << WheelBase << std::endl; + +//#else +// if (node_.getParam("/"+robot_ns+"/robot_baseline",WheelBase)) +// ROS_INFO("Robot baseline: %f", WheelBase); +// else ROS_ERROR("Failed to get param 'robot_baseline'"); + +// if (node_.getParam("/" + robot_ns + "/wheel_radius",WheelRadius[0])){ +// WheelRadius[1] = WheelRadius[0]; +// ROS_INFO("Wheel Radius: %f", WheelRadius[1]); +// } else ROS_ERROR("Failed to get param 'wheel_radius'"); +//#endif + +// double low_level_control_rate = 0.0; +// if(node_.getParam("/" + robot_ns + "/control_loop_rate", low_level_control_rate)) { +// ROS_INFO("Low_level Control Frequency: %fHz", low_level_control_rate); +// } else return false; + +// if (!node_.getParam("/log_date",log_date)){ +// ROS_ERROR("Parameter '/robot_date' not found! The log will be disabled..."); +// log_date=""; +// } + +// /* +// * Dynamics Controller Parameters +// * --------------------------------------------------------------------------------------------------------------------- +// */ + +// { + + +// double k_parameters[6]; + +// std::string param_name[] = {"feedback_linearization/A", +// "feedback_linearization/B", +// "feedback_linearization/C", +// "feedback_linearization/D", +// "feedback_linearization/E", +// "feedback_linearization/F"}; + +// for (int i = 0; i < 6; i++){ +// if (node_.getParam(param_name[i].c_str(),k_parameters[i])) +// ROS_INFO("%s: %f", param_name[i].c_str(), k_parameters[i]); +// else ROS_ERROR("Failed to get param '%s'", param_name[i].c_str()); +// } + +// const double A = k_parameters[0]; +// const double B = k_parameters[1]; +// const double C = k_parameters[2]; +// const double D = k_parameters[3]; +// const double E = k_parameters[4]; +// const double F = k_parameters[5]; + +// Eigen::Matrix2d G; +// G << C, C, F, -F; +// Fu << A, 0, 0, B, 0, D, E, 0; +// std::cout << "Value of constants of F(u): " << Fu << std::endl; + +// feedback_linearization = new StateFeedbackLinearization(G, Eigen::MatrixXd::Identity(2,2)); + +// } + +//#ifdef NONLINEAR_PID_BACKSTEPPING_CONTROLLER_DEBUG +// if (!node_.getParam("take",take)){ +// ROS_ERROR("Execution take '%s/take' not found! The parameter will be assumed as '0'",node_.getNamespace().c_str()); +// take = 0; +// } +//#endif + +// /* +// * Nonlinear Kinematic Controller Parameters +// * --------------------------------------------------------------------------------------------------------------------- +// */ + +// { +// // Starting Odometer +// Eigen::Vector3d pos_ini; + +// if (!node_.getParam("x_ini",pos_ini[0])){ +// ROS_ERROR("Initial pose parameter '%s/x_ini' not found! The parameter will be disabled as '0'",node_.getNamespace().c_str()); +// pos_ini[0] = 0; +// } + +// if (!node_.getParam("y_ini",pos_ini[1])){ +// ROS_ERROR("Initial pose parameter '%s/y_ini' not found! The parameter will be disabled as '0'",node_.getNamespace().c_str()); +// pos_ini[1] = 0; +// } + +// if (!node_.getParam("th_ini",pos_ini[2])){ +// ROS_ERROR("Initial pose parameter '%s/th_ini' not found! The parameter will be disabled as '0'",node_.getNamespace().c_str()); +// pos_ini[2] = 0; +// } + + +// Odometer = new OdometryTool(); +// Odometer->init(node_, robot_ns, pos_ini); + +// // TODO +// // Start odometer in a arbitrary pose + +// // Starting Kinematic Controller + +// double non_smooth_parameters[9]; +// std::string param_name[] = {"nonsmooth_controller/lambda1", +// "nonsmooth_controller/lambda2", +// "nonsmooth_controller/lambda3", +// "nonsmooth_controller/lambda4", +// "nonsmooth_controller/lambda5", +// "nonsmooth_controller/gamma1", +// "nonsmooth_controller/gamma2", +// "nonsmooth_controller/gamma3", +// "nonsmooth_controller/gamma4", +// "nonsmooth_controller/update_ratio"}; + +// for (int i = 0; i < 9; i++){ +// if (node_.getParam(param_name[i].c_str(),non_smooth_parameters[i])) +// ROS_INFO("%s: %f", param_name[i].c_str(), non_smooth_parameters[i]); +// else { +// ROS_ERROR("Failed to get param '%s'", param_name[i].c_str()); +// return false; +// } +// } + +// lambda[0] = non_smooth_parameters[0]; +// lambda[1] = non_smooth_parameters[1]; +// lambda[2] = non_smooth_parameters[2]; +// lambda[3] = non_smooth_parameters[3]; +// lambda[4] = non_smooth_parameters[4]; +// gamma[0] = non_smooth_parameters[5]; +// gamma[1] = non_smooth_parameters[6]; +// gamma[2] = non_smooth_parameters[7]; +// gamma[3] = non_smooth_parameters[8]; +// } + +// return true; +//} + +//void NonlinearBacksteppingController::starting(const ros::Time &) +//{ +// eta.setZero(); +// deta.setZero(); +// nuBar.setZero(); +// Effort.setZero(); +// nu_ref.setZero(); +// pose_ref = Odometer->getPose(); + +//#ifdef NONLINEAR_PID_BACKSTEPPING_CONTROLLER_DEBUG +// if (makeLog == false) +// { +// std::string filename = "none"; + +// if (robot_ns == "mini_twil") +// { +// filename = "/home/talves/Dropbox/Projects/Mini_Twil/ros/exp/" + log_date + "/nonlinear_backstepping_0" + std::to_string(take) + ".txt"; +// ROS_INFO("Output File: %s", filename.c_str()); +// fd.open(filename); +// makeLog = true; +// } +// else if (robot_ns == "twil") +// { +// filename = "/home/talves/Dropbox/Projects/Twil/ros/exp/" + log_date + "/nonlinear_backstepping_0" + std::to_string(take) + ".txt"; +// ROS_INFO("Output File: %s", filename.c_str()); +// fd.open(filename); +// makeLog = true; +// } +// } +//#endif +// ROS_INFO("\nController Started\n"); +//} + +//void NonlinearBacksteppingController::update(const ros::Time &time, const ros::Duration &duration) +//{ + +//#ifdef NONLINEAR_PID_BACKSTEPPING_CONTROLLER_DEBUG +// static int k = 0; +// if (makeLog == true){ +// if (this->fd.is_open()){ +// if (k==0) +// { +// ROS_INFO("Starting the LOG for the '%s_robot'...",robot_ns.c_str()); +// fd << "seq," << "dur," << "pose_ref1," << "pose_ref2," << "pose_ref3," << "pose1," << "pose2," +// << "pose3," << "u_ref1," <<"u_ref2," << "nu_ref1," <<"nu_ref2," << "u1," << "u2," +// << "eff1," << "eff2," << "dphi1," << "dphi2" << std::endl; +// } +// k++; +// } +// } +//#endif + +// /* --------------------------------------------------------------------------------------------------------------------- +// * Nonlinear Pose Controller +// * --------------------------------------------------------------------------------------------------------------------- +// */ + +// Eigen::Vector2d u, joint_ang_vel; + +// // Compute Velocities +// for(int i=0; i < joints_.size(); i++) joint_ang_vel[i]=joints_[i].getVelocity(); + +// /* +// * dphi[0] equals to right joint velocity and +// * dphi[1] equals to left joint velocity (as in the formulation). +// * In ros the left joint is defined as 0 and right joint as 1 +// * to correct the mismatch, a conversion denoted by dphi=[joint_ang_vel[1], joint_ang_vel[0]] is defined +// */ +// Eigen::Vector2d dphi; +// dphi << joint_ang_vel[1], joint_ang_vel[0]; +// u = VelocityTransformation::WheelToBase(dphi,WheelBase,WheelRadius[0]); + +// Odometer->compute(duration,u); +// Eigen::Vector3d pose = Odometer->getPose(); + +// // Change of coordinates +// Eigen::Matrix3d R; +// R << cos(pose_ref[2]), sin(pose_ref[2]), 0.0, +// -sin(pose_ref[2]), cos(pose_ref[2]), 0.0, +// 0.0, 0.0, 1.0; +// Eigen::Vector3d xBar=R*(pose-pose_ref); +// std::cout << "\nxbar: " << xBar << std::endl; + +// // Discontinuous transformation +// double e=sqrt(sqr(xBar[0])+sqr(xBar[1])); +// double psi=atan2(xBar[1],xBar[0]); +// double alpha=xBar[2]-psi; +// ROS_INFO("\ne: %f, psi: %f, alpha: %f", e,psi,alpha); + +// // Backstepping + +// eta[0]=-gamma[0]*e*cos(alpha); + +// if(fabs(alpha ) > 1e-10) +// eta[1]=-gamma[1]*alpha +// -gamma[0]*sin(alpha)*cos(alpha)+gamma[0]*lambda[2]*psi*sin(alpha)/ +// lambda[1]/alpha*cos(alpha); +// else eta[1]=gamma[0]*lambda[2]*psi/lambda[1]; + +// Eigen::Vector2d eb=u-eta; +// std::cout << "\nu: " << u << std::endl; +// std::cout << "\neta: " << eta << std::endl; +// std::cout << "\neb: " << eb << std::endl; + +// if(fabs(e) > 1e-10) nuBar[0]=-gamma[2]*eb[0]-lambda[0]/lambda[3]*cos(alpha) +// +lambda[1]/lambda[3]*alpha*sin(alpha)/e +// -lambda[2]/lambda[3]*psi*sin(alpha)/e; +// else nuBar[0]=-gamma[2]*eb[0]-lambda[0]/lambda[3]*cos(alpha); +// nuBar[1]=-gamma[3]*eb[1]-lambda[1]/lambda[4]*alpha; + +// std::cout << "\nnubar: " << nuBar << std::endl; + +// deta[0]=e*cub(cos(alpha))*sqr(gamma[0])-e*gamma[0]*sin(alpha)*(alpha*gamma[1] +// -(gamma[0]*lambda[2]*cos(alpha)*sin(alpha)*psi)/(alpha*lambda[1])); + +// deta[1]=gamma[1]*(alpha*gamma[1] +// -(gamma[0]*lambda[2]*psi*cos(alpha)*sin(alpha))/(alpha*lambda[1])) +// +gamma[0]*sqr(cos(alpha))*(alpha*gamma[1] +// -(gamma[0]*lambda[2]*psi*cos(alpha)*sin(alpha))/(alpha*lambda[1])) +// -gamma[0]*sqr(sin(alpha))*(alpha*gamma[1] +// -(gamma[0]*lambda[2]*psi*cos(alpha)*sin(alpha))/(alpha*lambda[1])) +// -(lambda[2]*sqr(cos(alpha))*sqr(sin(alpha))*sqr(gamma[0]))/(alpha*lambda[1]) +// -(gamma[0]*lambda[2]*psi*sqr(cos(alpha))*(alpha*gamma[1] +// -(gamma[0]*lambda[2]*psi*cos(alpha)*sin(alpha))/(alpha*lambda[1]))) +// /(alpha*lambda[1]) +// +(gamma[0]*lambda[2]*psi*sqr(sin(alpha))*(alpha*gamma[1] +// -(gamma[0]*lambda[2]*psi*cos(alpha)*sin(alpha))/(alpha*lambda[1]))) +// /(alpha*lambda[1]) +// +(gamma[0]*lambda[2]*psi*cos(alpha)*sin(alpha)*(alpha*gamma[1] +// -(gamma[0]*lambda[2]*psi*cos(alpha)*sin(alpha))/(alpha*lambda[1]))) +// /(sqr(alpha)*lambda[1]); + +// std::cout << "\ndeta: " << deta << std::endl; +// nu_ref=nuBar+deta; +// std::cout << "\nuref: " << nu_ref << std::endl; + +// /* --------------------------------------------------------------------------------------------------------------------- +// * Dynamics Feedback Linearization +// * --------------------------------------------------------------------------------------------------------------------- +// */ + +// Eigen::Vector2d fu; +// Eigen::Vector4d uf; + +// // Feedback Linearization ----------------------------------------------------------------------------------------------- +// uf << u[0], u[1], u[1]*u[0], u[1]*u[1]; +// fu = -Fu*uf; +// std::cout << "\nfu: " << fu << std::endl; +// std::cout << "\nuf: " << uf << std::endl; +// Effort = feedback_linearization->computeInputs(nu_ref,fu); +// // Control Efforts ------------------------------------------------------------------------------------------------------ +// joints_[0].setCommand(Effort(1)); +// joints_[1].setCommand(Effort(0)); + + +//#ifdef CONTROLLER_OUTPUT_SCREEN +// ROS_INFO("uref[0]: %f; u[0]: %f",u_ref[0],u[0]); +// ROS_INFO("uref[1]: %f; u[1]: %f",u_ref[1],u[1]); +// ROS_INFO("Ref: {%f,%f,%f}; Pos: {%f,%f,%f}",pose_ref[0], pose_ref[1], pose_ref[2], pose[0], pose[1], pose[2]); +//#endif + +// Odometer->publish(); + +//#ifdef NONLINEAR_PID_BACKSTEPPING_CONTROLLER_DEBUG +// if (makeLog==true){ +// if (k>0){ +// fd << k << "," << duration.toSec() << "," << pose_ref[0] << "," << pose_ref[1] +// << "," << pose_ref[2] << "," << pose[0] << "," << pose[1] << "," << pose[2] +// << "," << eta[0] << "," << eta[1] << nu_ref[0] << "," << nu_ref[1] << "," << u[0] << "," << u[1] +// << "," << Effort[0] << "," << Effort[1] << "," << dphi[1] << "," << dphi[0] +// << std::endl; +// } +// } +//#endif + +//} + +//void NonlinearBacksteppingController::stopping(const ros::Time &) +//{ +//#ifdef NONLINEAR_PID_BACKSTEPPING_CONTROLLER_DEBUG +// fd.close(); +//#endif +// PositionSub[0].shutdown(); +// PositionSub[1].shutdown(); +//} + +//void NonlinearBacksteppingController::commandCB(const geometry_msgs::PoseStampedConstPtr &command) +//{ +// pose_ref[0] = command->pose.position.x; +// pose_ref[1] = command->pose.position.y; +// tf::Quaternion quat_aux(command->pose.orientation.x, +// command->pose.orientation.y, +// command->pose.orientation.z, +// command->pose.orientation.w); +// tf::Matrix3x3 m(quat_aux); +// double roll, pitch, yaw; +// m.getRPY(roll, pitch, yaw); +// pose_ref[2] = yaw; +//#ifdef CONTROLLER_OUTPUT_SCREEN +// ROS_INFO("Pose_ref: {%f, %f, %f}", pose_ref[0], pose_ref[1], pose_ref[2]); +//#endif +//} + +//void NonlinearBacksteppingController::command2dCB(const geometry_msgs::Pose2DConstPtr &command) +//{ +// // pose_ref[0] = command->x; +// // pose_ref[1] = command->y; +// // pose_ref[2] = command->theta; +// // std::cout << "Pose_ref: " << pose_ref << std::endl; + +// pose_ref[0] = command->x; +// pose_ref[1] = command->y; +// pose_ref[2] = command->theta; + +//#ifdef CONTROLLER_OUTPUT_SCREEN +// ROS_INFO("Pose_ref: {%f, %f, %f}", pose_ref[0], pose_ref[1], pose_ref[2]); +//#endif +//} + +//} +//PLUGINLIB_EXPORT_CLASS(twil_controllers::NonlinearBacksteppingController,controller_interface::ControllerBase) diff --git a/twil_controllers/src/state_feedback_linearization.cpp b/twil_controllers/src/state_feedback_linearization.cpp new file mode 100644 index 0000000..156ef28 --- /dev/null +++ b/twil_controllers/src/state_feedback_linearization.cpp @@ -0,0 +1,199 @@ +/****************************************************************************** + Twil Controllers + State Feedback Linearization + + Copyright (C) 2017 Tiago G. Alves + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program; if not, write to the Free Software + Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + + You can also obtain a copy of the GNU General Public License + at . + +*******************************************************************************/ +#define STATE_FEEDBACK_LINEARIZATION_DEBUG + +#include "twil_controllers/state_feedback_linearization.h" + + +StateFeedbackLinearization::StateFeedbackLinearization(const K_parameters_t params) +{ + // Compute F(u) + Mat2_4 Fu_; + Fu_ << this->computeFu(params); + setFu(Fu_); + + // Compute G(beta,u) + Eigen::Matrix2d G_, Ginv_; + Ginv_ = this->computeGinv(params); + setGinv(Ginv_); + G_ << Ginv_.inverse(); + + // Init H(beta) + H_beta = Eigen::MatrixXd::Identity(2,2); + + // Init Tau_o + Tau_zero.setZero(); + + std::cout << "\n Value of H(beta): " << H_beta << std::endl; + std::cout << "\n Value of G: " << G_ << std::endl; + std::cout << "\n Value of Ginv: " << Ginv << std::endl; + std::cout << "Value of constants of F(u): " << this->Fu << std::endl; + std::cout << "\n Value of Tau0: " << Tau_zero << std::endl; +} + +//StateFeedbackLinearization::StateFeedbackLinearization(const Eigen::Matrix2d &G_, const Eigen::Matrix2d &H_beta_) +//{ +// H_beta = H_beta_; +// Ginv = G_.inverse(); +// Tau_zero.setZero(); +// std::cout << "\n Value of H(beta): " << H_beta << std::endl; +// std::cout << "\n Value of G: " << G_ << std::endl; +// std::cout << "\n Value of Ginv: " << Ginv << std::endl; +// std::cout << "\n Value of Tau0: " << Tau_zero << std::endl; +//} + +Eigen::Vector2d StateFeedbackLinearization::computeInputs(const Eigen::Vector2d &v_, const Eigen::Vector2d &F_Beta_u_) +{ + /** + + * This class follows the standard feedback linearization defined by + * Tau_0 = G^-1(Beta)*( H(Beta)v + f(Beta,u) ) + * + * Input -> Joint Velocities + * v_(0): Linear Acceleration (m/s^2) + * v_(1): Angular Acceleration (rd/s^2) + * + * Return -> Robot Base Velocities + * Tau_zero(0): Right Actuator Effort + * Tau_zero(1): Left Actuator Effort + */ + + v = v_; + F_Beta_u = F_Beta_u_; + // std::cout << "\n Fbeta -v: " << F_Beta_u - v << std::endl; + // std::cout << "\n Fbeta: " << F_Beta_u << std::endl; + Tau_zero = Ginv*(H_beta*v + F_Beta_u); + + // std::cout << "\n Tau: " << Tau_zero << std::endl; + return Tau_zero; +} + +Mat2_4 StateFeedbackLinearization::getFu() const +{ + return Fu; +} + +void StateFeedbackLinearization::setFu(const Mat2_4 &value) +{ + Fu = value; +} + +Mat2_4 StateFeedbackLinearization::computeFu(const K_parameters_t ¶ms) const +{ + Mat2_4 Fu_; + Fu_ << params.A, 0, 0, params.B, 0, params.D, params.E, 0; +#ifdef STATE_FEEDBACK_LINEARIZATION_DEBUG + std::cout << "Value of constants of F(u): " << Fu_ << std::endl; +#endif + return Fu_; +} + +Eigen::Matrix2d StateFeedbackLinearization::getGinv() const +{ + return Ginv; +} + +void StateFeedbackLinearization::setGinv(const Eigen::Matrix2d &value) +{ + Ginv = value; +} + +Eigen::Matrix2d StateFeedbackLinearization::computeGinv(const K_parameters_t ¶ms) const +{ + Eigen::Matrix2d G_, Ginv_; + + G_ << params.C, params.C, params.F, -params.F; + Ginv_ = G_.inverse(); +#ifdef STATE_FEEDBACK_LINEARIZATION_DEBUG + std::cout << "\n Value of G: " << G_ << std::endl; + std::cout << "\n Value of Ginv: " << Ginv << std::endl; +#endif + return Ginv_; +} + +double K_parameters::getA() const +{ + return A; +} + +void K_parameters::setA(double value) +{ + A = value; +} + +double K_parameters::getB() const +{ + return B; +} + +void K_parameters::setB(double value) +{ + B = value; +} + +double K_parameters::getC() const +{ + return C; +} + +void K_parameters::setC(double value) +{ + C = value; +} + +double K_parameters::getD() const +{ + return D; +} + +void K_parameters::setD(double value) +{ + D = value; +} + +double K_parameters::getE() const +{ + return E; +} + +void K_parameters::setE(double value) +{ + E = value; +} + +double K_parameters::getF() const +{ + return F; +} + +void K_parameters::setF(double value) +{ + F = value; +} + +K_parameters::K_parameters() + : A(0), B(0), C(0), D(0), E(0), F(0) {} + + diff --git a/twil_controllers/src/twist_pid_backstepping_controller.cpp b/twil_controllers/src/twist_pid_backstepping_controller.cpp index c83b85c..f491dd5 100644 --- a/twil_controllers/src/twist_pid_backstepping_controller.cpp +++ b/twil_controllers/src/twist_pid_backstepping_controller.cpp @@ -23,10 +23,13 @@ *******************************************************************************/ +#include + #include "twil_controllers/twist_pid_backstepping_controller.h" #include #include #include +#include "twil_controllers/velocity_transformation.h" //#define URDF //#define CONTROLLER_OUTPUT_SCREEN @@ -35,19 +38,19 @@ namespace twil_controllers { TwistPIDBacksteppingController::TwistPIDBacksteppingController() -{ - -} + : WheelRadius(2), odom_(0.0,std::vector(2)){} TwistPIDBacksteppingController::~TwistPIDBacksteppingController() { - PositionSub[0].~Subscriber(); - PositionSub[1].~Subscriber(); - PID[0].~Pid(); - PID[1].~Pid(); + position_sub_[0].~Subscriber(); + position_sub_[1].~Subscriber(); + + pid_[0].~Pid(); + pid_[1].~Pid(); + controller_parameters_sub_.~Subscriber(); } -bool TwistPIDBacksteppingController::init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n) +bool TwistPIDBacksteppingController::init(twil_hardware_interfaces::VoltageJointInterface *robot, ros::NodeHandle &n) { node_ = n; robot_ = robot; @@ -84,9 +87,9 @@ bool TwistPIDBacksteppingController::init(hardware_interface::EffortJointInterfa joints_.push_back(j); } - // VelocitySub = node_.subscribe("cmd_vel",1,&TwistPIDController::commandCB,this); - PositionSub[0] = node_.subscribe("cmd_pos",1,&TwistPIDBacksteppingController::commandCB,this); - PositionSub[1] = node_.subscribe("cmd_pos2d",1,&TwistPIDBacksteppingController::command2dCB,this); + // PositionSub[0] = node_.subscribe("/move_base_simple/goal",1,&TwistPIDBacksteppingController::commandCB,this); + position_sub_[1] = node_.subscribe("cmd_pos2d",1,&TwistPIDBacksteppingController::command2dCB,this); + controller_parameters_sub_ = node_.subscribe("control_param",1,&TwistPIDBacksteppingController::parametersCB,this); #ifdef URDF std::string robot_desc_string; @@ -139,18 +142,27 @@ bool TwistPIDBacksteppingController::init(hardware_interface::EffortJointInterfa } else ROS_ERROR("Failed to get param 'wheel_radius'"); #endif + std::string odom_frame_id="odom"; + node_.param("odom_frame_id",odom_frame_id,odom_frame_id); + + odom_.setParams(WheelBase,WheelRadius); + odom_.publisherConfig(node_); + + status_publisher_.reset(new realtime_tools::RealtimePublisher(node_,"status",1)); + status_publisher_->msg_.header.frame_id=odom_frame_id; + /* * Twist PID Controller Parameters * --------------------------------------------------------------------------------------------------------------------- */ - if (!PID[0].init(ros::NodeHandle(node_, "left_wheel_joint_pid_parameters"))){ + if (!pid_[0].init(ros::NodeHandle(node_, "left_wheel_joint_pid_parameters"))){ ROS_ERROR("MyController could not construct PID controller for joint left_wheel_pid"); return false; } - if (!PID[1].init(ros::NodeHandle(node_, "right_wheel_joint_pid_parameters"))){ + if (!pid_[1].init(ros::NodeHandle(node_, "right_wheel_joint_pid_parameters"))){ ROS_ERROR("MyController could not construct PID controller for joint right_wheel_pid"); return false; } @@ -158,9 +170,9 @@ bool TwistPIDBacksteppingController::init(hardware_interface::EffortJointInterfa double p, i, d, imin, imax; bool awdup; - PID[0].getGains(p,i,d,imax,imin,awdup); + pid_[0].getGains(p,i,d,imax,imin,awdup); ROS_INFO("Left PID: {p: %f, i: %f, d: %f, imax: %f, imin: %f, awdup: %d", p, i, d, imin, imax, awdup); - PID[1].getGains(p,i,d,imax,imin,awdup); + pid_[1].getGains(p,i,d,imax,imin,awdup); ROS_INFO("Right PID: {p: %f, i: %f, d: %f, imax: %f, imin: %f, awdup: %d", p, i, d, imin, imax, awdup); double low_level_control_rate = 0.0; @@ -185,31 +197,23 @@ bool TwistPIDBacksteppingController::init(hardware_interface::EffortJointInterfa * --------------------------------------------------------------------------------------------------------------------- */ - // Starting Odometer - Eigen::Vector3d pos_ini; + // Configure the Odometer Initial Pose - if (!node_.getParam("x_ini",pos_ini[0])){ + if (!node_.getParam("x_ini",pose_[0])){ ROS_ERROR("Initial pose parameter '%s/x_ini' not found! The parameter will be disabled as '0'",node_.getNamespace().c_str()); - pos_ini[0] = 0; + pose_[0] = 0; } - if (!node_.getParam("y_ini",pos_ini[1])){ + if (!node_.getParam("y_ini",pose_[1])){ ROS_ERROR("Initial pose parameter '%s/y_ini' not found! The parameter will be disabled as '0'",node_.getNamespace().c_str()); - pos_ini[1] = 0; + pose_[1] = 0; } - if (!node_.getParam("th_ini",pos_ini[2])){ + if (!node_.getParam("th_ini",pose_[2])){ ROS_ERROR("Initial pose parameter '%s/th_ini' not found! The parameter will be disabled as '0'",node_.getNamespace().c_str()); - pos_ini[2] = 0; + pose_[2] = 0; } - - Odometer = new OdometryTool(); - Odometer->init(node_, robot_ns, pos_ini); - - // TODO - // Start odometer in a arbitrary pose - // Starting Kinematic Controller double non_smooth_parameters[6]; @@ -236,20 +240,50 @@ bool TwistPIDBacksteppingController::init(hardware_interface::EffortJointInterfa gamma[1] = non_smooth_parameters[4]; update_ratio = non_smooth_parameters[5]; - Kinematics = new NonSmoothControl(lambda,gamma); + std::vector tolerances; + if (node_.getParam("tolerances",tolerances)) + { ROS_INFO("Tolerances: [%f, %f]", tolerances[0], tolerances[1]); + nonsmooth_control_ = new NonSmoothControl(lambda,gamma,tolerances[0],tolerances[1]); + } else { + ROS_ERROR("Failed to get param 'tolerances'"); + nonsmooth_control_ = new NonSmoothControl(lambda,gamma); + } ROS_INFO("%s: %f", "Kinematic Controller Frequency: ", low_level_control_rate/update_ratio); + node_.param("time_step",time_step_,0.01); + return true; } -void TwistPIDBacksteppingController::starting(const ros::Time &) +void TwistPIDBacksteppingController::starting(const ros::Time &time) { - dphi_ref.setZero(); - dphi.setZero(); - Effort.setZero(); - u_ref.setZero(); - pose_ref = Odometer->getPose(); + phi_dot_ref_.setZero(); + phi_dot_.setZero(); + u_ref_.setZero(); + voltage_effort_.setZero(); + pose_ref_ = pose_; + odom_.setPose(pose_); + lastSamplingTime_ = time; + + for(unsigned int i=0;i < joints_.size();i++) + { + phi_[i]=joints_[i].getPosition(); + } + + struct sched_param param; + if(!node_.getParam("priority",param.sched_priority)) + { + ROS_WARN("No 'priority' configured for controller %s. Using highest possible priority.",node_.getNamespace().c_str()); + param.sched_priority=sched_get_priority_max(SCHED_FIFO); + } + if(sched_setscheduler(0,SCHED_FIFO,¶m) == -1) + { + ROS_WARN("Failed to set real-time scheduler."); + return; + } + if(mlockall(MCL_CURRENT|MCL_FUTURE) == -1) + ROS_WARN("Failed to lock memory."); #ifdef TWIST_PID_BACKSTEPPING_CONTROLLER_DEBUG if (makeLog == false) @@ -258,14 +292,14 @@ void TwistPIDBacksteppingController::starting(const ros::Time &) if (robot_ns == "mini_twil") { - filename = "/home/talves/Dropbox/Mini_Twil/ros/exp/" + log_date + "/backstepping_pid_0" + std::to_string(take) + ".txt"; + filename = "/home/talves/Dropbox/Projects/Mini_Twil/ros/exp/" + log_date + "/backstepping_pid_0" + std::to_string(take) + ".txt"; ROS_INFO("Output File: %s", filename.c_str()); fd.open(filename); makeLog = true; } else if (robot_ns == "twil") { - filename = "/home/talves/Dropbox/Twil/ros/exp/" + log_date + "/backstepping_pid_0" + std::to_string(take) + ".txt"; + filename = "/home/talves/Dropbox/Projects/Twil/ros/exp/" + log_date + "/backstepping_pid_0" + std::to_string(take) + ".txt"; ROS_INFO("Output File: %s", filename.c_str()); fd.open(filename); makeLog = true; @@ -294,37 +328,54 @@ void TwistPIDBacksteppingController::update(const ros::Time &time, const ros::Du } #endif + ros::Duration dt=time-lastSamplingTime_; + + // if(fabs(dt.toSec()-time_step_) > time_step_/20) { + // ROS_INFO("Real-time problem..."); + // return; + // } + lastSamplingTime_=time; + + // Incremental encoders sense angular displacement and + // not velocity + // phi[0] is the left wheel angular displacement + // phi[1] is the right wheel angular displacement + + // phi_dot_[0] is the left wheel angular rate + // phi_dot_[1] is the right wheel angular rate + + Eigen::Vector2d deltaPhi=-phi_; + for(unsigned int i=0;i < joints_.size();i++) + { + phi_[i]=joints_[i].getPosition(); + } + deltaPhi+=phi_; + phi_dot_ = deltaPhi/dt.toSec(); + + odom_.update(deltaPhi,dt); + odom_.getPose(pose_); + /* --------------------------------------------------------------------------------------------------------------------- * Nonlinear Pose Controller * --------------------------------------------------------------------------------------------------------------------- */ - Eigen::Vector2d error, joint_ang_vel; - - for (int i=0; i<2; i++) joint_ang_vel[i]=joints_[i].getVelocity(); - - /* - * dphi[0] equals to right joint velocity and - * dphi[1] equals to left joint velocity (as in the formulation). - * In ros the left joint is defined as 0 and right joint as 1 - * to correct the mismatch, a conversion denoted by dphi=[joint_ang_vel[1], joint_ang_vel[0]] is defined - */ - dphi << joint_ang_vel[1], joint_ang_vel[0]; - Eigen::Vector2d u = VelocityTransformation::WheelToBase(dphi,WheelBase,WheelRadius[0]); - - Odometer->compute(duration,u); - Eigen::Vector3d pose = Odometer->getPose(); - - if (++kinematic_cycle == update_ratio){ + if (++kinematic_cycle == update_ratio) + { kinematic_cycle = 0; #ifdef CONTROLLER_OUTPUT_SCREEN ROS_INFO("Updating kinematic controller"); #endif // if (!Kinematics->StopCondition(pose,pose_ref,10e-3,1e-2)){ - u_ref = Kinematics->compute(pose,pose_ref); // Here will be generated the linear and angular velocities setpoints - dphi_ref = VelocityTransformation::BaseToWheel(u_ref,WheelBase,WheelRadius[0]); - // } else dphi_ref.setZero(); + u_ref_ = nonsmooth_control_->compute(pose_,pose_ref_); // Here will be generated the linear and angular velocities setpoints + + // The BaseToWheel Transformations consider the velocities as: + // Wheel_velocities[0]: right wheel angular rate + // Wheel_velocities[1]: left wheel angular rate + Eigen::Vector2d Wheel_velocities = VelocityTransformation::BaseToWheel(u_ref_,WheelBase,WheelRadius[0]); + phi_dot_ref_ << Wheel_velocities[1], Wheel_velocities[0]; + // } else phi_dot_ref.setZero(); } /* --------------------------------------------------------------------------------------------------------------------- @@ -332,20 +383,21 @@ void TwistPIDBacksteppingController::update(const ros::Time &time, const ros::Du * --------------------------------------------------------------------------------------------------------------------- */ - error[1] = dphi_ref[1]-dphi[1]; // Here will be computed left velocity error - error[0] = dphi_ref[0]-dphi[0]; // Here will be computed right velocity error + Eigen::Vector2d error, u; - /* - * To return to ros space joint numbers, the efforts are calculated as - * effort[0] equals to left joint effort and - * effort[1] equals to right joint effort - */ + // Compute Velocities + odom_.getVelocity(u); + + error[0] = phi_dot_ref_[0]-phi_dot_[0]; // Here will be computed left velocity error + error[1] = phi_dot_ref_[1]-phi_dot_[1]; // Here will be computed right velocity error - Effort[0] = PID[0].computeCommand(error[1],duration); // Here will be generated left wheel control effort - Effort[1] = PID[1].computeCommand(error[0],duration); // Here will be generated right wheel control effort - // Control Efforts ------------------------------------------------------------------------------------------------------ - joints_[0].setCommand(Effort[0]); // Here wil be applied the left control effort - joints_[1].setCommand(Effort[1]); // Here wil be applied the right control effort + // Compute Control Actions + voltage_effort_[0] = pid_[0].computeCommand(error[0],duration); // Here will be generated left wheel control effort + voltage_effort_[1] = pid_[1].computeCommand(error[1],duration); // Here will be generated right wheel control effort + + // Apply Control Efforts + joints_[0].setCommand(voltage_effort_[0]); // Here wil be applied the left control effort + joints_[1].setCommand(voltage_effort_[1]); // Here wil be applied the right control effortelRadius[0]); #ifdef CONTROLLER_OUTPUT_SCREEN ROS_INFO("uref[0]: %f; u[0]: %f",u_ref[0],u[0]); @@ -353,38 +405,104 @@ void TwistPIDBacksteppingController::update(const ros::Time &time, const ros::Du ROS_INFO("Ref: {%f,%f,%f}; Pos: {%f,%f,%f}",pose_ref[0], pose_ref[1], pose_ref[2], pose[0], pose[1], pose[2]); #endif - - - Odometer->publish(); - #ifdef TWIST_PID_BACKSTEPPING_CONTROLLER_DEBUG if (makeLog==true){ if (k>0){ - fd << k << "," << duration.toSec() << "," << pose_ref[0] << "," << pose_ref[1] - << "," << pose_ref[2] << "," << pose[0] << "," << pose[1] << "," << pose[2] - << "," << u_ref[0] << "," << u_ref[1] << "," << 0 << "," << 0 << "," << u[0] << "," << u[1] - << "," << Effort[0] << "," << Effort[1] << "," << dphi[1] << "," << dphi[0] + // fd << "seq," << "dur," << "pose_ref1," << "pose_ref2," << "pose_ref3," << "pose1," << "pose2," + // << "pose3," << "u_ref1," <<"u_ref2," << "nu_ref1," <<"nu_ref2," << "u1," << "u2," + // << "eff1," << "eff2," << "dphi1," << "dphi2" << std::endl; + fd << k << "," << duration.toSec() << "," << pose_ref_[0] << "," << pose_ref_[1] + << "," << pose_ref_[2] << "," << pose_[0] << "," << pose_[1] << "," << pose_[2] + << "," << u_ref_[0] << "," << u_ref_[1] << "," << 0 << "," << 0 << "," << u[0] << "," << u[1] + << "," << voltage_effort_[1] << "," << voltage_effort_[0] << "," << phi_dot_[1] << "," << phi_dot_[0] << std::endl; } } #endif + + odom_.publish(time); + + if(status_publisher_ && status_publisher_->trylock()) + { + status_publisher_->msg_.header.stamp=time; + + status_publisher_->msg_.set_point.x=pose_ref_[0]; + status_publisher_->msg_.set_point.y=pose_ref_[1]; + status_publisher_->msg_.set_point.theta=pose_ref_[2]; + + status_publisher_->msg_.process_value.x=pose_[0]; + status_publisher_->msg_.process_value.y=pose_[1]; + status_publisher_->msg_.process_value.theta=pose_[2]; + + status_publisher_->msg_.process_value_dot.x=u[0]*cos(pose_[2]); + status_publisher_->msg_.process_value_dot.y=u[0]*sin(pose_[2]); + status_publisher_->msg_.process_value_dot.theta=u[1]; + + status_publisher_->msg_.error.x=pose_ref_[0]-pose_[0]; + status_publisher_->msg_.error.y=pose_ref_[1]-pose_[1]; + status_publisher_->msg_.error.theta=pose_ref_[2]-pose_[2]; + + status_publisher_->msg_.time_step=dt.toSec(); + + for(int i=0;i < voltage_effort_.size();i++) + status_publisher_->msg_.command[i]=voltage_effort_[i]; + + Eigen::Vector3d lb_value = nonsmooth_control_->getLamb(); + for(int i=0;i < lb_value.size();i++) + status_publisher_->msg_.lambda[i]=lb_value[i]; + + Eigen::Vector2d gm_value = nonsmooth_control_->getGam(); + for(int i=0;i < gm_value.size();i++) + status_publisher_->msg_.gamma[i]=gm_value[i]; + + status_publisher_->msg_.polar_error.range=nonsmooth_control_->getE(); + status_publisher_->msg_.polar_error.angle=nonsmooth_control_->getPsi(); + status_publisher_->msg_.polar_error.orientation=nonsmooth_control_->getAlpha(); + + for(int i=0;i < u_ref_.size();i++) + status_publisher_->msg_.backstep_set_point[i]=phi_dot_ref_[i]; + + for(int i=0;i < u.size();i++) + status_publisher_->msg_.backstep_process_value[i]=phi_dot_[i]; + + Eigen::Vector2d eb=phi_dot_ref_ - phi_dot_; + + for(int i=0;i < eb.size();i++) + status_publisher_->msg_.backstep_error[i]=eb[i]; + + status_publisher_->unlockAndPublish(); + } + + //--------------------------------------------------------------// + // Evaluation of PID errors + //--------------------------------------------------------------// +// double iterm1; +// double dterm1; +// double pterm1; +// double iterm2; +// double dterm2; +// double pterm2; +// pid_[0].getCurrentPIDErrors(&pterm1,&iterm1,&dterm1); +// pid_[1].getCurrentPIDErrors(&pterm2,&iterm2,&dterm2); +// ROS_INFO("Left Errors [p, i, d]: %f, %f, %f - Right Errors [p, i, d]: %f, %f, %f", pterm1, iterm1, dterm1, pterm2, iterm2, dterm2); + //--------------------------------------------------------------// } -void TwistPIDBacksteppingController::stopping(const ros::Time &) +void TwistPIDBacksteppingController::stopping(const ros::Time &time) { #ifdef TWIST_PID_BACKSTEPPING_CONTROLLER_DEBUG fd.close(); #endif - PositionSub[0].shutdown(); - PositionSub[1].shutdown(); + position_sub_[0].shutdown(); + position_sub_[1].shutdown(); } void TwistPIDBacksteppingController::commandCB(const geometry_msgs::PoseStampedConstPtr &command) { - pose_ref[0] = command->pose.position.x; - pose_ref[1] = command->pose.position.y; + pose_ref_[0] = command->pose.position.x; + pose_ref_[1] = command->pose.position.y; tf::Quaternion quat_aux(command->pose.orientation.x, command->pose.orientation.y, command->pose.orientation.z, @@ -392,30 +510,39 @@ void TwistPIDBacksteppingController::commandCB(const geometry_msgs::PoseStampedC tf::Matrix3x3 m(quat_aux); double roll, pitch, yaw; m.getRPY(roll, pitch, yaw); - pose_ref[2] = yaw; + pose_ref_[2] = yaw; #ifdef CONTROLLER_OUTPUT_SCREEN ROS_INFO("Pose_ref: {%f, %f, %f}", pose_ref[0], pose_ref[1], pose_ref[2]); #endif } void TwistPIDBacksteppingController::command2dCB(const geometry_msgs::Pose2DConstPtr &command) -{ - // Eigen::Vector3d pose_vector_aux; - // pose_vector_aux[0] = command->x; - // pose_vector_aux[1] = command->y; - // pose_vector_aux[2] = command->theta; - // geometry_msgs::PoseWithCovariance pose_aux_ = conv_vector_to_pose(pose_vector_aux); - // pose_ref = conv_pose_to_vector(pose_aux_); - - pose_ref[0] = command->x; - pose_ref[1] = command->y; - pose_ref[2] = command->theta; +{ + pose_ref_[0] = command->x; + pose_ref_[1] = command->y; + pose_ref_[2] = command->theta; #ifdef CONTROLLER_OUTPUT_SCREEN ROS_INFO("Pose_ref: {%f, %f, %f}", pose_ref[0], pose_ref[1], pose_ref[2]); #endif } +void TwistPIDBacksteppingController::parametersCB(const twil_msgs::non_smooth_parametersConstPtr ¶m) +{ + ROS_INFO("\nUpdate in Nonsmooth Controller parameters..."); + Eigen::Vector3d lb_par; + Eigen::Vector2d gm_par; + Eigen::Vector2d eps_par; + + lb_par << param->lambda[0], param->lambda[1], param->lambda[2]; + gm_par << param->gamma[0], param->gamma[1]; + eps_par << param->eps[0], param->eps[1]; + + nonsmooth_control_->setLamb(lb_par); + nonsmooth_control_->setGam(gm_par); + nonsmooth_control_->setEps(eps_par); +} + } PLUGINLIB_EXPORT_CLASS(twil_controllers::TwistPIDBacksteppingController,controller_interface::ControllerBase) diff --git a/twil_controllers/src/twist_pid_controller.cpp b/twil_controllers/src/twist_pid_controller.cpp index 0502004..8947f26 100644 --- a/twil_controllers/src/twist_pid_controller.cpp +++ b/twil_controllers/src/twist_pid_controller.cpp @@ -23,9 +23,12 @@ *******************************************************************************/ +#include + #include "twil_controllers/twist_pid_controller.h" #include #include +#include "twil_controllers/velocity_transformation.h" //#define URDF //#define CONTROLLER_OUTPUT_SCREEN @@ -34,26 +37,26 @@ namespace twil_controllers { TwistPIDController::TwistPIDController(void) - : WheelRadius(2) {} + : WheelRadius(2), odom_(0.0,std::vector(2)){} TwistPIDController::~TwistPIDController() -{ - VelocitySub.~Subscriber(); +{ + velocity_sub_.~Subscriber(); - PID[0].~Pid(); - PID[0].~Pid(); + pid_[0].~Pid(); + pid_[0].~Pid(); } -bool TwistPIDController::init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n) +bool TwistPIDController::init(twil_hardware_interfaces::VoltageJointInterface *robot, ros::NodeHandle &n) { node_ = n; robot_ = robot; - if(!node_.getParam("/robot_config",robot_ns)) + if(!node_.getParam("/robot_config",robot_ns_)) { ROS_ERROR("Could not find Robot namespace in '/robot_config'."); return false; - } else ROS_INFO("Robot namespace: %s", robot_ns.c_str()); + } else ROS_INFO("Robot namespace: %s", robot_ns_.c_str()); XmlRpc::XmlRpcValue joint_names; if(!node_.getParam("joints",joint_names)) @@ -81,7 +84,7 @@ bool TwistPIDController::init(hardware_interface::EffortJointInterface *robot, r joints_.push_back(j); } - VelocitySub = node_.subscribe("cmd_vel",1,&TwistPIDController::commandCB,this); + velocity_sub_ = node_.subscribe("cmd_vel",1,&TwistPIDController::commandCB,this); #ifdef URDF std::string robot_desc_string; @@ -124,27 +127,36 @@ bool TwistPIDController::init(hardware_interface::EffortJointInterface *robot, r std::cout << "Wheel base: " << WheelBase << std::endl; #else - if (node_.getParam("/"+robot_ns+"/robot_baseline",WheelBase)) + if (node_.getParam("/"+robot_ns_+"/robot_baseline",WheelBase)) ROS_INFO("Robot baseline: %f", WheelBase); else ROS_ERROR("Failed to get param 'robot_baseline'"); - if (node_.getParam("/" + robot_ns + "/wheel_radius",WheelRadius[0])){ + if (node_.getParam("/" + robot_ns_ + "/wheel_radius",WheelRadius[0])){ WheelRadius[1] = WheelRadius[0]; ROS_INFO("Wheel Radius: %f", WheelRadius[1]); } else ROS_ERROR("Failed to get param 'wheel_radius'"); -#endif +#endif + + std::string odom_frame_id="odom"; + node_.param("odom_frame_id",odom_frame_id,odom_frame_id); + + odom_.setParams(WheelBase,WheelRadius); + odom_.publisherConfig(node_); + + status_publisher_.reset(new realtime_tools::RealtimePublisher(node_,"status",1)); + status_publisher_->msg_.header.frame_id=odom_frame_id; /* * Twist PID Controller Parameters * --------------------------------------------------------------------------------------------------------------------- */ - if (!PID[0].init(ros::NodeHandle(node_, "left_wheel_joint_pid_parameters"))){ + if (!pid_[0].init(ros::NodeHandle(node_, "left_wheel_joint_pid_parameters"))){ ROS_ERROR("MyController could not construct PID controller for joint left_wheel_pid"); return false; } - if (!PID[1].init(ros::NodeHandle(node_, "right_wheel_joint_pid_parameters"))){ + if (!pid_[1].init(ros::NodeHandle(node_, "right_wheel_joint_pid_parameters"))){ ROS_ERROR("MyController could not construct PID controller for joint right_wheel_pid"); return false; } @@ -152,9 +164,9 @@ bool TwistPIDController::init(hardware_interface::EffortJointInterface *robot, r double p, i, d, imin, imax; bool awdup; - PID[0].getGains(p,i,d,imax,imin,awdup); + pid_[0].getGains(p,i,d,imax,imin,awdup); ROS_INFO("Left PID: {p: %f, i: %f, d: %f, imax: %f, imin: %f, awdup: %d", p, i, d, imin, imax, awdup); - PID[1].getGains(p,i,d,imax,imin,awdup); + pid_[1].getGains(p,i,d,imax,imin,awdup); ROS_INFO("Right PID: {p: %f, i: %f, d: %f, imax: %f, imin: %f, awdup: %d", p, i, d, imin, imax, awdup); if (!node_.getParam("/log_date",log_date)){ @@ -169,31 +181,55 @@ bool TwistPIDController::init(hardware_interface::EffortJointInterface *robot, r } #endif + node_.param("time_step",time_step_,0.01); + return true; } void TwistPIDController::starting(const ros::Time &time) { - dphi_ref.setZero(); - dphi.setZero(); - Effort.setZero(); - u_ref.setZero(); + phi_dot_.setZero(); + phi_dot_ref_.setZero(); + u_ref_.setZero(); + voltage_effort_.setZero(); + pose_.setZero(); + odom_.setPose(pose_); + lastSamplingTime_ = time; + + for(unsigned int i=0;i < joints_.size();i++) + { + phi_[i]=joints_[i].getPosition(); + } + + struct sched_param param; + if(!node_.getParam("priority",param.sched_priority)) + { + ROS_WARN("No 'priority' configured for controller %s. Using highest possible priority.",node_.getNamespace().c_str()); + param.sched_priority=sched_get_priority_max(SCHED_FIFO); + } + if(sched_setscheduler(0,SCHED_FIFO,¶m) == -1) + { + ROS_WARN("Failed to set real-time scheduler."); + return; + } + if(mlockall(MCL_CURRENT|MCL_FUTURE) == -1) + ROS_WARN("Failed to lock memory."); #ifdef TWIST_PID_DEBUG if (makeLog == false) { std::string filename = "none"; - if (robot_ns == "mini_twil") + if (robot_ns_ == "mini_twil") { - filename = "/home/talves/Dropbox/Mini_Twil/ros/exp/" + log_date + "/twist_pid_0" + std::to_string(take) + ".txt"; + filename = "/home/talves/Dropbox/Projects/Mini_Twil/ros/exp/" + log_date + "/twist_pid_0" + std::to_string(take) + ".txt"; ROS_INFO("Output File: %s", filename.c_str()); fd.open(filename); makeLog = true; } - else if (robot_ns == "twil") + else if (robot_ns_ == "twil") { - filename = "/home/talves/Dropbox/Twil/ros/exp/" + log_date + "/twist_pid_01.txt" + std::to_string(take) + ".txt"; + filename = "/home/talves/Dropbox/Projects/Twil/ros/exp/" + log_date + "/twist_pid_0" + std::to_string(take) + ".txt"; ROS_INFO("Output File: %s", filename.c_str()); fd.open(filename); makeLog = true; @@ -210,47 +246,62 @@ void TwistPIDController::update(const ros::Time &time, const ros::Duration &dura if (this->fd.is_open()){ if (k==0) { - ROS_INFO("Starting the LOG for the '%s_robot'...",robot_ns.c_str()); - fd << "seq," << "dur," << "u_ref1," <<"u_ref2," << "nu_ref1," <<"nu_ref2,"<< "u1," << "u2," << "eff1," << "eff2," << "dphi1," << "dphi2" << std::endl; + ROS_INFO("Starting the LOG for the '%s_robot'...",robot_ns_.c_str()); + fd << "seq," << "dur," << "pose_ref1," << "pose_ref2," << "pose_ref3," << "pose1," << "pose2," + << "pose3," << "u_ref1," <<"u_ref2," << "nu_ref1," <<"nu_ref2," << "u1," << "u2," + << "eff1," << "eff2," << "dphi1," << "dphi2" << std::endl; } k++; } } #endif + ros::Duration dt=time-lastSamplingTime_; + + // if(fabs(dt.toSec()-time_step_) > time_step_/20) { + // ROS_INFO("Real-time problem..."); + // return; + // } + lastSamplingTime_=time; + + // Incremental encoders sense angular displacement and + // not velocity + // phi[0] is the left wheel angular displacement + // phi[1] is the right wheel angular displacement + + // phi_dot[0] is the left wheel angular rate + // phi_dot[1] is the right wheel angular rate + + Eigen::Vector2d phi_dot, deltaPhi=-phi_; + for(unsigned int i=0;i < joints_.size();i++) + { + phi_[i]=joints_[i].getPosition(); + } + deltaPhi+=phi_; + phi_dot = deltaPhi/dt.toSec(); + + odom_.update(deltaPhi,dt); + odom_.getPose(pose_); + /* --------------------------------------------------------------------------------------------------------------------- * Velocity Controller * --------------------------------------------------------------------------------------------------------------------- */ - Eigen::Vector2d error, joint_ang_vel; + Eigen::Vector2d error, u; // Compute Velocities - for(int i=0; i < joints_.size(); i++) joint_ang_vel[i]=joints_[i].getVelocity(); + odom_.getVelocity(u); - /* - * dphi[0] equals to right joint velocity and - * dphi[1] equals to left joint velocity (as in the formulation). - * In ros the left joint is defined as 0 and right joint as 1 - * to correct the mismatch, a conversion denoted by dphi=[joint_ang_vel[1], joint_ang_vel[0]] is defined - */ - dphi << joint_ang_vel[1], joint_ang_vel[0]; - - error[1] = dphi_ref[1]-dphi[1]; // Here will be computed left velocity error - error[0] = dphi_ref[0]-dphi[0]; // Here will be computed right velocity error - - /* - * To return to ros space joint numbers, the efforts are calculated as - * effort[0] equals to left joint effort and - * effort[1] equals to right joint effort - */ + error[0] = phi_dot_ref_[0]-phi_dot[0]; // Here will be computed left velocity error + error[1] = phi_dot_ref_[1]-phi_dot[1]; // Here will be computed right velocity error - Effort[0] = PID[0].computeCommand(error[1],duration); // Here will be generated left wheel control effort - Effort[1] = PID[1].computeCommand(error[0],duration); // Here will be generated right wheel control effort + // Compute Control Actions + voltage_effort_[0] = pid_[0].computeCommand(error[0],duration); // Here will be generated left wheel control effort + voltage_effort_[1] = pid_[1].computeCommand(error[1],duration); // Here will be generated right wheel control effort - joints_[0].setCommand(Effort[0]); // Here wil be applied the left control effort - joints_[1].setCommand(Effort[1]); // Here wil be applied the right control effort - - Eigen::Vector2d u = VelocityTransformation::WheelToBase(dphi,WheelBase,WheelRadius[0]); + // Apply Control Efforts + joints_[0].setCommand(voltage_effort_[0]); // Here wil be applied the left control effort + joints_[1].setCommand(voltage_effort_[1]); // Here wil be applied the right control effortelRadius[0]); #ifdef CONTROLLER_OUTPUT_SCREEN ROS_INFO("uref[0]: %f; u[0]: %f",u_ref[0],u[0]); @@ -261,11 +312,48 @@ void TwistPIDController::update(const ros::Time &time, const ros::Duration &dura #ifdef TWIST_PID_DEBUG if (makeLog==true){ if (k>0){ - fd << k << ","<< duration.toSec() << "," << u_ref[0] << "," << u_ref[1] << "," << 0 << "," << 0 << "," << u[0] << "," << u[1] << "," - << Effort[0] << "," << Effort[1] << "," << dphi[1] << "," << dphi[0] << std::endl; + // fd << "seq," << "dur," << "pose_ref1," << "pose_ref2," << "pose_ref3," << "pose1," << "pose2," + // << "pose3," << "u_ref1," <<"u_ref2," << "nu_ref1," <<"nu_ref2," << "u1," << "u2," + // << "eff1," << "eff2," << "dphi1," << "dphi2" << std::endl; + fd << k << "," << duration.toSec() << "," << 0 << "," << 0 + << "," << 0 << "," << pose_[0] << "," << pose_[1] << "," << pose_[2] + << "," << u_ref_[0] << "," << u_ref_[1] << "," << 0 << "," << 0 << "," << u[0] << "," << u[1] + << "," << voltage_effort_[1] << "," << voltage_effort_[0] << "," << phi_dot[1] << "," << phi_dot[0] + << std::endl; } } #endif + + if(status_publisher_ && status_publisher_->trylock()) + { + odom_.publish(time); + + status_publisher_->msg_.header.stamp=time; + + status_publisher_->msg_.robot_pose.x=pose_[0]; + status_publisher_->msg_.robot_pose.y=pose_[1]; + status_publisher_->msg_.robot_pose.theta=pose_[2]; + + status_publisher_->msg_.time_step=dt.toSec(); + + for(int i=0;i < voltage_effort_.size();i++) + status_publisher_->msg_.command[i]=voltage_effort_[i]; + + for(int i=0;i < u_ref_.size();i++) + status_publisher_->msg_.velocity_set_point[i]=u_ref_[i]; + + for(int i=0;i < u.size();i++) + status_publisher_->msg_.velocity_process_value[i]=u[i]; + + for(int i=0;i < u.size();i++) + status_publisher_->msg_.velocity_error[i]=(u_ref_[i]-u[i]); + + // for(int i=0;i < nu_ref.size();i++) + // status_publisher_->msg_.linear_dynamics_command[i]=nu_ref[i]; + + status_publisher_->unlockAndPublish(); + } + } void TwistPIDController::stopping() @@ -273,16 +361,26 @@ void TwistPIDController::stopping() #ifdef TWIST_PID_DEBUG fd.close(); #endif - VelocitySub.shutdown(); + velocity_sub_.shutdown(); } void TwistPIDController::commandCB(const geometry_msgs::TwistConstPtr &CommandVel) { - u_ref.setZero(); - u_ref[0] = CommandVel->linear.x; - u_ref[1] = CommandVel->angular.z; + u_ref_.setZero(); + u_ref_[0] = CommandVel->linear.x; + u_ref_[1] = CommandVel->angular.z; + + // The BaseToWheel Transformations consider the velocities as: + // Wheel_velocities[0]: right wheel angular rate + // Wheel_velocities[1]: left wheel angular rate + Eigen::Vector2d Wheel_velocities = VelocityTransformation::BaseToWheel(u_ref_,WheelBase,WheelRadius[0]); + /* + * To return to ros space joint numbers, the velocities are calculated as + * phi_dot_ref[0] equals to left joint effort and + * phi_dot_ref[1] equals to right joint effort + */ - dphi_ref = VelocityTransformation::BaseToWheel(u_ref,WheelBase,WheelRadius[0]); + phi_dot_ref_ << Wheel_velocities[1], Wheel_velocities[0]; #ifdef CONTROLLER_OUTPUT_SCREEN ROS_INFO("u_ref: {%f, %f}", u_ref[0], u_ref[1]); #endif diff --git a/twil_controllers/twil_controllers_plugins.xml b/twil_controllers/twil_controllers_plugins.xml index 836f348..4d96c94 100644 --- a/twil_controllers/twil_controllers_plugins.xml +++ b/twil_controllers/twil_controllers_plugins.xml @@ -30,9 +30,27 @@ - + + + Blá-blá-blá... + + + + + + Blá-blá-blá... + + + + + + Blá-blá-blá... + + + + - Blá-blá-blá... + Blá-blá-blá... diff --git a/twil_hardware/CMakeLists.txt b/twil_hardware/CMakeLists.txt new file mode 100644 index 0000000..b8c5554 --- /dev/null +++ b/twil_hardware/CMakeLists.txt @@ -0,0 +1,88 @@ +cmake_minimum_required(VERSION 2.8.3) +project(twil_hardware) + +add_definitions(-std=c++11) + +find_package(catkin REQUIRED COMPONENTS + controller_interface + controller_manager + std_msgs + hardware_interface + joint_state_controller + roscpp + rospy + twil_hardware_interfaces +) +find_package(cmake_modules REQUIRED) + + +catkin_package( + INCLUDE_DIRS include + LIBRARIES ${PROJECT_NAME}_lib + CATKIN_DEPENDS controller_interface controller_manager hardware_interface joint_state_controller roscpp rospy + DEPENDS aic_lib +) + +########### +## Build ## +########### + + +SET (AIC_LIB_ROOT_DIR $ENV{HOME}/twil_ws/src/ufrgs_aic/aic_lib) + + +# header files +include_directories( + ${AIC_LIB_ROOT_DIR}/include +) + +# Find source files +file(GLOB SOURCES ${AIC_LIB_ROOT_DIR}/src/*.cpp) + +# Create shared library +add_library(aic_lib ${SOURCES}) + + +## Specify additional locations of header files +## Your package locations should be listed before other locations +# include_directories(include) +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${AIC_LIB_ROOT_DIR}/include +) + +add_library(${PROJECT_NAME}_lib + src/${PROJECT_NAME}.cpp + ) + +# add_executable(${PROJECT_NAME}_node src/twil_hw.cpp) + target_link_libraries(${PROJECT_NAME}_lib + ${catkin_LIBRARIES} + aic_lib + ) + +#add_executable(${PROJECT_NAME}_node src/${PROJECT_NAME}_node.cpp) +#target_link_libraries(twil_hw_node ${catkin_LIBRARIES} aic_lib twil_hw_lib) + +add_executable(twil_control_loop_node src/twil_control_loop_node.cpp) +target_link_libraries(twil_control_loop_node ${catkin_LIBRARIES} aic_lib twil_hardware_lib) + + +############# +## Install ## +############# + +## Mark executables and/or libraries for installation +install(TARGETS twil_hardware_lib aic_lib twil_control_loop_node + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +## Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/ ${AIC_LIB_ROOT_DIR}/include + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) diff --git a/twil_hardware/include/twil_hardware/twil_hardware.h b/twil_hardware/include/twil_hardware/twil_hardware.h new file mode 100644 index 0000000..2297343 --- /dev/null +++ b/twil_hardware/include/twil_hardware/twil_hardware.h @@ -0,0 +1,49 @@ +#ifndef TWIL_ROBOT_HW_H +#define TWIL_ROBOT_HW_H + +#include +#include +#include + +#include "aic.h" +#include "twil_hardware_interfaces/joint_command_extended_interface.h" + +namespace twil_hardware +{ + +class TwilRobotHW : public hardware_interface::RobotHW +{ +public: + TwilRobotHW(); + ~TwilRobotHW(); + bool init(ros::NodeHandle &root_nh, ros::NodeHandle &robot_hw_nh); + void read(const ros::Time& time, const ros::Duration& period); + void write(const ros::Time& time, const ros::Duration& period); + +private: + ros::NodeHandle node_; + hardware_interface::JointStateInterface joint_state_interface; + twil_hardware_interfaces::VoltageJointInterface voltage_interface; + + double cmd[2]; + double prev_cmd[2]; + + // joint variables + double pos[2]; + double vel[2]; + double eff[2]; + + double act_nom_voltage; // Max Voltage applied to Motors + double bat_nom_voltage; // Nominal Battery Voltage + double pos_dead_zone[2]; + double neg_dead_zone[2]; + bool dead_zone_compensation[2]; + + aic *joint_hw[2]; + aic_displacement_msg_t joint_sensor[2]; + aic_comm_config_t connection_parameters[2]; + + +}; +} +#endif // TWIL_ROBOT_HW_H diff --git a/twil_hardware/package.xml b/twil_hardware/package.xml new file mode 100644 index 0000000..d75c95a --- /dev/null +++ b/twil_hardware/package.xml @@ -0,0 +1,65 @@ + + + twil_hardware + 0.0.0 + The twil_hw package + + + + + talves + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + controller_interface + controller_manager + hardware_interface + joint_state_controller + twil_hardware_interfaces + + roscpp + rospy + controller_interface + controller_manager + hardware_interface + twil_hardware_interfaces + joint_state_controller + roscpp + rospy + + + + + + + + diff --git a/twil_hardware/scripts/export_motor_test.sh b/twil_hardware/scripts/export_motor_test.sh new file mode 100755 index 0000000..bbc13b5 --- /dev/null +++ b/twil_hardware/scripts/export_motor_test.sh @@ -0,0 +1,21 @@ +#!/bin/bash + +echo "USAGE: ./export_motor_test -f file.bag -t topic_root -o output_root" + +while getopts f:t:o: option +do + case "${option}" + in + f) FILE=${OPTARG};; + t) TOPIC_ROOT=${OPTARG};; + o) OUTPUT=${OPTARG};; + esac +done + +set -x +echo "Joint_States_File:" $OUTPUT"_joint_states.dat" +rostopic echo -b $FILE -p /$TOPIC_ROOT/joint_states > $OUTPUT"_joint_states.dat" +echo "Left_Motor_CMD_File:" $OUTPUT"_left_wheel.dat" +rostopic echo -b $FILE -p /$TOPIC_ROOT/left_wheel_joint_effort_controller/command > $OUTPUT"_left_wheel.dat" +echo "Right_Motor_CMD_File:" $OUTPUT"_right_wheel.dat" +rostopic echo -b $FILE -p /$TOPIC_ROOT/right_wheel_joint_effort_controller/command > $OUTPUT"_right_wheel.dat" diff --git a/twil_hardware/src/twil_control_loop_node.cpp b/twil_hardware/src/twil_control_loop_node.cpp new file mode 100644 index 0000000..3d7aff7 --- /dev/null +++ b/twil_hardware/src/twil_control_loop_node.cpp @@ -0,0 +1,116 @@ +#include +#include +#include +#include +#include +#include "twil_hardware/twil_hardware.h" + +//#define CONTROL_TIME_DEBUG +//#define SAMPLING_TIME_DEBUG +#define LOOP_TIME_DEBUG + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "twil_control_loop_node"); + ros::NodeHandle nh; + std::string robot_ns; + + if(!nh.getParam("/robot_config",robot_ns)) + { + ROS_ERROR("Could not find Robot namespace in '/robot_config'."); + return false; + } + + double control_rate = 0.0; + if(nh.getParam("control_loop_rate", control_rate)) { + ROS_INFO("Control Frequency: %fHz", control_rate); + } else { + ROS_ERROR("Failed to get param '/%s/control_loop_rate'!\n Used default value = 10Hz", robot_ns.c_str()); + control_rate = 10.0; + } + + ros::Duration dt; + +#ifdef SAMPLING_TIME_DEBUG + boost::scoped_ptr< realtime_tools::RealtimePublisher >sampling_time_pub; + sampling_time_pub.reset(new realtime_tools::RealtimePublisher(nh,"sampling_time",1)); +#endif + +#ifdef CONTROL_TIME_DEBUG + boost::scoped_ptr< realtime_tools::RealtimePublisher >control_time_pub; + control_time_pub.reset(new realtime_tools::RealtimePublisher(nh,"control_computation_time",1)); +#endif + +#ifdef LOOP_TIME_DEBUG + boost::scoped_ptr< realtime_tools::RealtimePublisher >loop_time_pub; + loop_time_pub.reset(new realtime_tools::RealtimePublisher(nh,"control_loop_time",1)); +#endif + + twil_hardware::TwilRobotHW robot_hw; + ROS_INFO("Starting the Robot Hardware"); + robot_hw.init(nh, nh); + controller_manager::ControllerManager cm(&robot_hw); + ros::AsyncSpinner spinner(1); spinner.start(); + + ROS_INFO("Starting the Control Loop"); + ros::Rate loop_rate(control_rate); + + ros::Time last_time = ros::Time::now(), current_time; + while (ros::ok()) + { + // Updates Sample Time informations + current_time = ros::Time::now(); + dt = current_time - last_time; + last_time = current_time; + +#ifdef SAMPLING_TIME_DEBUG + ros::Time sampling_tick = ros::Time::now(); +#endif + + robot_hw.read(current_time,dt); + +#ifdef SAMPLING_TIME_DEBUG + ros::Duration sampling_dt = ros::Time::now() - sampling_tick; +#endif + +#ifdef CONTROL_TIME_DEBUG + ros::Time control_tick = ros::Time::now(); +#endif + + cm.update(current_time, dt); + +#ifdef CONTROL_TIME_DEBUG + ros::Duration control_dt = ros::Time::now() - control_tick; +#endif + + robot_hw.write(current_time,dt); + +#ifdef LOOP_TIME_DEBUG + if (loop_time_pub->trylock()) { + loop_time_pub->msg_.data = dt; + loop_time_pub->unlockAndPublish(); + } +#endif + +#ifdef SAMPLING_TIME_DEBUG + if (sampling_time_pub->trylock()) { + sampling_time_pub->msg_.data = sampling_dt; + sampling_time_pub->unlockAndPublish(); + } +#endif + +#ifdef CONTROL_TIME_DEBUG + if (control_time_pub->trylock()) { + control_time_pub->msg_.data = control_dt; + control_time_pub->unlockAndPublish(); + } +#endif + + loop_rate.sleep(); + } + + spinner.stop(); + robot_hw.~TwilRobotHW(); + + return 0; +} diff --git a/twil_hardware/src/twil_hardware.cpp b/twil_hardware/src/twil_hardware.cpp new file mode 100644 index 0000000..85778c6 --- /dev/null +++ b/twil_hardware/src/twil_hardware.cpp @@ -0,0 +1,200 @@ +#include "twil_hardware/twil_hardware.h" + +//#define AIC_HW_DEBUG +//#define NULL_HW + +namespace twil_hardware +{ + +TwilRobotHW::TwilRobotHW() +{ + for (int i = 0; i < 2; i++) + { + cmd[i] = 0; + prev_cmd[i] = -1000; + eff[i] = 0; + pos[i] = 0; + vel[i] = 0; + dead_zone_compensation[i] = 0; + } +} + +TwilRobotHW::~TwilRobotHW() +{ + voltage_interface.~HardwareInterface(); + joint_state_interface.~HardwareInterface(); + +#ifndef NULL_HW +#ifdef AIC_HW_DEBUG + for (int i = 0; i < 1; i++) +#else + for (int i = 0; i < 2; i++) +#endif + { + joint_hw[i]->~aic(); + } +#endif +} + +bool TwilRobotHW::init(ros::NodeHandle &root_nh, ros::NodeHandle &robot_hw_nh) { + + + node_ = robot_hw_nh; + std::string joint_side[] = {"left_wheel_joint/","right_wheel_joint/"}; + +#ifdef AIC_HW_DEBUG + for (int i = 0; i < 1; i++) +#else + for (int i = 0; i < 2; i++) +#endif + { + if (node_.getParam(joint_side[i]+"aic_com_device",connection_parameters[i].aic_comm_device)) + { + ROS_INFO("%scom_device: %d", joint_side[i].c_str(), (connection_parameters[i].aic_comm_device)); + if (connection_parameters[i].aic_comm_device == rs232) + { + ROS_INFO("RS-232 Hardware Connection!"); + if (node_.getParam(joint_side[i]+"serial_port",connection_parameters[i].aic_serial_port)) + ROS_INFO("%sserial_port: %s", joint_side[i].c_str(), (connection_parameters[i].aic_serial_port).c_str()); + else ROS_ERROR("Failed to get param '%sserial_port'", joint_side[i].c_str()); + + } else if (connection_parameters[i].aic_comm_device == socketcan) + { + ROS_INFO("CAN-Bus Hardware Connection!"); + // if (nh.getParam(joint_side[i]+"can_iface",connection_parameters[i].aic_can_iface)) + // ROS_INFO("%scan_iface: %d", joint_side[i].c_str(), (connection_parameters[i].aic_can_iface)); + // else ROS_ERROR("Failed to get param '%scan_iface'", joint_side[i].c_str()); + + if (node_.getParam(joint_side[i]+"can_id_number",connection_parameters[i].aic_can_id_number)) + ROS_INFO("%s_canid_number: %d", joint_side[i].c_str(), (connection_parameters[i].aic_can_id_number)); + else ROS_ERROR("Failed to get param '%scan_id_number'", joint_side[i].c_str()); + } + else ROS_ERROR("ERROR IN CONNECTION PARAMETERS for '%s'!", joint_side[i].c_str()); +#ifndef NULL_HW + joint_hw[i] = new aic(connection_parameters[i]); + usleep(500); +#endif + + } + else ROS_ERROR("Failed to get param '%saic_com_device'", joint_side[i].c_str()); + + if (node_.getParam(joint_side[i]+"dead_zone_compensation",dead_zone_compensation[i])) + { + ROS_INFO("Dead Zone Comp. for '%s': '%i'", joint_side[i].c_str(), dead_zone_compensation[i]); + if (dead_zone_compensation[i] == true) + { + if (node_.getParam(joint_side[i]+"neg_dead_zone",neg_dead_zone[i])) + ROS_INFO("%snegative_dead_zone: %f", joint_side[i].c_str(), (neg_dead_zone[i])); + else ROS_ERROR("Failed to get param '%snegative_dead_zone'", joint_side[i].c_str()); + + if (node_.getParam(joint_side[i]+"pos_dead_zone",pos_dead_zone[i])) + ROS_INFO("%spositive_dead_zone: %f", joint_side[i].c_str(), (pos_dead_zone[i])); + else ROS_ERROR("Failed to get param '%spositive_dead_zone'", joint_side[i].c_str()); + } + + } + else ROS_ERROR("Failed to get param '%sdead_zone_compensation'", joint_side[i].c_str()); +#ifndef NULL_HW + if (joint_hw[i]->status_ok() == false) { + ROS_ERROR("AIC %d NOT CONNECTED!", i+1); + return false; + } +#endif + } + + if (node_.getParam("motor_nominal_voltage",act_nom_voltage)) + ROS_INFO("Actuator Nominal Voltage: %f", act_nom_voltage); + else { + ROS_ERROR("Failed to get param 'motor_nominal_voltage'"); + return false; + } + + if (node_.getParam("battery_nominal_voltage",bat_nom_voltage)) + ROS_INFO("Battery Nominal Voltage: %f", bat_nom_voltage); + else { + ROS_ERROR("Failed to get param 'battery_nominal_voltage'"); + return false; + } + + // connect and register the joint state interface + hardware_interface::JointStateHandle state_handle_left("left_wheel_joint", &pos[0], &vel[0], &eff[0]); + joint_state_interface.registerHandle(state_handle_left); + + hardware_interface::JointStateHandle state_handle_right("right_wheel_joint", &pos[1], &vel[1], &eff[1]); + joint_state_interface.registerHandle(state_handle_right); + + registerInterface(&joint_state_interface); + + + // connect and register the joint voltage interface + hardware_interface::JointHandle eff_handle_left(joint_state_interface.getHandle("left_wheel_joint"), &cmd[0]); + voltage_interface.registerHandle(eff_handle_left); + + hardware_interface::JointHandle eff_handle_right(joint_state_interface.getHandle("right_wheel_joint"), &cmd[1]); + voltage_interface.registerHandle(eff_handle_right); + + registerInterface(&voltage_interface); + ROS_INFO("\nRobot Hardware Registered! \n"); + return true; +} + +void TwilRobotHW::read(const ros::Time &time, const ros::Duration &period) +{ +#ifndef NULL_HW + joint_sensor[0] = joint_hw[0]->read_displacement_sensors(); +#ifndef AIC_HW_DEBUG + joint_sensor[1] = joint_hw[1]->read_displacement_sensors(); +#endif + pos[0] += joint_sensor[0].joint_displacement; + pos[1] += joint_sensor[1].joint_displacement; + vel[0] = joint_sensor[0].joint_displacement/period.toSec(); + vel[1] = joint_sensor[1].joint_displacement/period.toSec(); +#endif +} + +void TwilRobotHW::write(const ros::Time &time, const ros::Duration &period) +{ + double Va[2]; // Armature voltage without dead zone compensation + double Va_comp[2]; // Armature voltage after dead zone compensation + double Vref = act_nom_voltage; // Actuator nominal voltage for calculation + +#ifndef NULL_HW + // Actuator effort scale adjust + Va[0] = cmd[0]*(act_nom_voltage/bat_nom_voltage); + Va[1] = cmd[1]*(act_nom_voltage/bat_nom_voltage); + + +#ifdef AIC_HW_DEBUG + for (int i = 0; i < 1; i++) { +#else + for (int i = 0; i < 2; i++) { +#endif + + if(dead_zone_compensation[i] == true) + { + + if (cmd[i] > 0){ + Va_comp[i] = (Va[i]+pos_dead_zone[i])*(Vref/(Vref+abs(pos_dead_zone[i]))); + } else if (cmd[i] < 0) { + // User Va[i] plus neg_dead_zone[i] because the parameter is loaded with sign + Va_comp[i] = (Va[i]+neg_dead_zone[i])*(Vref/(Vref+abs(neg_dead_zone[i]))); + } else Va_comp[i] = 0; + + if (abs(Va_comp[i]) < 0.1) Va_comp[i] = 0; // Used to reduce jerk motion near 0V. + + } else Va_comp[i] = Va[i]; + + // Overvoltage protection to the actuators + if (Va_comp[i] > act_nom_voltage) Va_comp[i] = act_nom_voltage; + else if (Va_comp[i] < -act_nom_voltage) Va_comp[i] = -act_nom_voltage; + + // Apply effort to the joint actuator + joint_hw[i]->set_motor_voltage(Va_comp[i]); + } + eff[0] = cmd[0]; + eff[1] = cmd[1]; +#endif + +} + +} diff --git a/twil_hardware_interfaces/CMakeLists.txt b/twil_hardware_interfaces/CMakeLists.txt new file mode 100644 index 0000000..3e545fd --- /dev/null +++ b/twil_hardware_interfaces/CMakeLists.txt @@ -0,0 +1,199 @@ +cmake_minimum_required(VERSION 2.8.3) +project(twil_hardware_interfaces) + +## Compile as C++11, supported in ROS Kinetic and newer +# add_compile_options(-std=c++11) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + hardware_interface + roscpp + rospy +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a run_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# std_msgs # Or other packages containing msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a run_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( + INCLUDE_DIRS include +# LIBRARIES twil_hardware_interfaces +# CATKIN_DEPENDS hardware_interfaces roscpp rospy +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/twil_hardware_interfaces.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +# add_executable(${PROJECT_NAME}_node src/twil_hardware_interfaces_node.cpp) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(${PROJECT_NAME}_node +# ${catkin_LIBRARIES} +# ) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables and/or libraries for installation +# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_twil_hardware_interfaces.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/twil_hardware_interfaces/include/twil_hardware_interfaces/actuator_command_extended_interface.h b/twil_hardware_interfaces/include/twil_hardware_interfaces/actuator_command_extended_interface.h new file mode 100644 index 0000000..ba80292 --- /dev/null +++ b/twil_hardware_interfaces/include/twil_hardware_interfaces/actuator_command_extended_interface.h @@ -0,0 +1,19 @@ +#ifndef HARDWARE_INTERFACE_ACTUATOR_COMMAND_EXTENDED_INTERFACE_H +#define HARDWARE_INTERFACE_ACTUATOR_COMMAND_EXTENDED_INTERFACE_H + +#include "hardware_interface/actuator_command_interface.h" + +namespace twil_hardware_interfaces { + +/// \ref ActuatorCommandInterface for commanding voltage-based actuators. +class VoltageActuatorInterface : public hardware_interface::ActuatorCommandInterface {}; + +/// \ref ActuatorCommandInterface for commanding current-based actuators. +class CurrentActuatorInterface : public hardware_interface::ActuatorCommandInterface {}; + +/// \ref ActuatorCommandInterface for commanding duty_cycle-based actuators. +class DutyCycleActuatorInterface : public hardware_interface::ActuatorCommandInterface {}; + +} + +#endif diff --git a/twil_hardware_interfaces/include/twil_hardware_interfaces/joint_command_extended_interface.h b/twil_hardware_interfaces/include/twil_hardware_interfaces/joint_command_extended_interface.h new file mode 100644 index 0000000..5bd6814 --- /dev/null +++ b/twil_hardware_interfaces/include/twil_hardware_interfaces/joint_command_extended_interface.h @@ -0,0 +1,19 @@ +#ifndef HARDWARE_INTERFACE_JOINT_COMMAND_EXTENDED_INTERFACE_H +#define HARDWARE_INTERFACE_JOINT_COMMAND_EXTENDED_INTERFACE_H + +#include "hardware_interface/joint_command_interface.h" + +namespace twil_hardware_interfaces { + +/// \ref JointCommandInterface for commanding voltage-based joints. +class VoltageJointInterface : public hardware_interface::JointCommandInterface {}; + +/// \ref JointCommandInterface for commanding current-based joints. +class CurrentJointInterface : public hardware_interface::JointCommandInterface {}; + +/// \ref JointCommandInterface for commanding duty_cycle-based joints. +class DutyCycleJointInterface : public hardware_interface::JointCommandInterface {}; + +} + +#endif diff --git a/twil_hardware_interfaces/package.xml b/twil_hardware_interfaces/package.xml new file mode 100644 index 0000000..149cd99 --- /dev/null +++ b/twil_hardware_interfaces/package.xml @@ -0,0 +1,68 @@ + + + twil_hardware_interfaces + 0.0.0 + The twil_hardware_interfaces package + + + + + talves + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + hardware_interface + roscpp + rospy + hardware_interface + roscpp + rospy + hardware_interface + roscpp + rospy + + + + + + + + diff --git a/twil_ident/CMakeLists.txt b/twil_ident/CMakeLists.txt new file mode 100644 index 0000000..a5a78b9 --- /dev/null +++ b/twil_ident/CMakeLists.txt @@ -0,0 +1,170 @@ +cmake_minimum_required(VERSION 2.8.3) +project(twil_ident) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + roscpp + kdl_parser + twil_msgs +) + +find_package(cmake_modules REQUIRED) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) +find_package(Eigen REQUIRED) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependencies might have been +## pulled in transitively but can be declared for certainty nonetheless: +## * add a build_depend tag for "message_generation" +## * add a run_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# std_msgs # Or other packages containing msgs +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if you package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( + INCLUDE_DIRS include +# LIBRARIES twil_ident + CATKIN_DEPENDS twil_msgs + DEPENDS eigen + twil_msgs +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( + include + ${catkin_INCLUDE_DIRS} +# TODO: Check names of system library include directories (eigen) + ${Eigen_INCLUDE_DIRS} +) + +## Declare a cpp library +# add_library(twil_ident +# src/${PROJECT_NAME}/twil_ident.cpp +# ) + +## Declare a cpp executable +add_executable(ident src/ident.cpp) + +## Add cmake target dependencies of the executable/library +## as an example, message headers may need to be generated before nodes +add_dependencies(ident ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +target_link_libraries(ident + ${catkin_LIBRARIES} + ${eigen_LIBRARIES} +) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables and/or libraries for installation +install(TARGETS ident +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_twil_ident.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/twil_ident/launch/gazebo.launch b/twil_ident/launch/gazebo.launch new file mode 100644 index 0000000..a18f7f6 --- /dev/null +++ b/twil_ident/launch/gazebo.launch @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + + diff --git a/twil_ident/launch/ident.launch b/twil_ident/launch/ident.launch new file mode 100644 index 0000000..e939edf --- /dev/null +++ b/twil_ident/launch/ident.launch @@ -0,0 +1,5 @@ + + + + + diff --git a/twil_ident/package.xml b/twil_ident/package.xml new file mode 100644 index 0000000..4f54a3c --- /dev/null +++ b/twil_ident/package.xml @@ -0,0 +1,60 @@ + + + twil_ident + 2.0.0 + The twil_ident package + + + + + Walter Fetter Lages + + + + + + GPLv3 + + + + + + + + + + + + + Walter Fetter Lages + + + + + + + + + + + + + + catkin + twil_msgs + eigen + kdl_parser + twil_msgs + eigen + kdl_parser + + + + + + + + + + + diff --git a/twil_ident/src/ident.cpp b/twil_ident/src/ident.cpp new file mode 100644 index 0000000..46530ac --- /dev/null +++ b/twil_ident/src/ident.cpp @@ -0,0 +1,297 @@ +/****************************************************************************** + ROS twil_indet Package + Twil Dynamics Model + Copyright (C) 2014..2017 Walter Fetter Lages + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see + . + +*******************************************************************************/ + +#include + +#include + +#include +#include +#include + +#include +#include "twil_msgs/robot_dynamics.h" + +class Prbs +{ + int index; + unsigned int nd; + unsigned char *sh; + + public: + + Prbs(unsigned int n=10,unsigned int seed=0); + ~Prbs(void); + + void seed(unsigned int s) { index=s % nd; }; + + operator int(void); +}; + + +Prbs::Prbs(unsigned int n,unsigned int seed) +{ + nd=n; + index=seed % nd; + sh=new unsigned char[nd]; + for(unsigned int i=0; i < nd;i++) sh[i]=1; + for(unsigned int i=0; i < 98;i++) (int) *this; // call operator int() to exercise + +} + +Prbs::~Prbs(void) +{ +// delete[] sh; +} + +Prbs::operator int(void) +{ + unsigned char s=sh[nd-1]+sh[nd-2]; + if(s > 1) s=0; + for(int j=nd-2;j >= 0;j--) sh[j+1]=sh[j]; + sh[0]=s; + return sh[index]; +} + + +class Ident +{ + public: + Ident(ros::NodeHandle node); + ~Ident(void); + void setCommand(void); + + private: + ros::NodeHandle node_; + + ros::Subscriber jointStatesSubscriber; + ros::Publisher dynParamPublisher; + ros::Publisher leftWheelCommandPublisher; + ros::Publisher rightWheelCommandPublisher; + + const int nJoints; + + Eigen::VectorXd u; + Eigen::VectorXd thetaEst1; + Eigen::VectorXd thetaEst2; + Eigen::MatrixXd P1; + Eigen::MatrixXd P2; + + std::vector prbs; + int iter; + + ros::Time lastTime; + + std::vector WheelRadius; + double WheelBase; + std::string robot_ns; + + void jointStatesCB(const sensor_msgs::JointState::ConstPtr &jointStates); + void resetCovariance(void); +}; + + +Ident::Ident(ros::NodeHandle node): + nJoints(2),u(nJoints),thetaEst1(3),thetaEst2(3),P1(3,3),P2(3,3),prbs(nJoints),WheelRadius(2) +{ + node_=node; + + std::string robot_desc_string; + if(!node_.getParam("/robot_description",robot_desc_string)) + { + ROS_ERROR("Could not find '/robot_description'."); + } + +// KDL::Tree tree; +// if (!kdl_parser::treeFromString(robot_desc_string,tree)) +// { +// ROS_ERROR("Failed to construct KDL tree."); +// } + +// // get wheelBase from URDF (actually from KDL tree) +// KDL::SegmentMap::const_iterator segmentMapIter=tree.getSegment("left_wheel_support"); +// KDL::Frame leftSupportFrame=segmentMapIter->second.segment.getFrameToTip(); + +// segmentMapIter=tree.getSegment("right_wheel_support"); +// KDL::Frame rightSupportFrame=segmentMapIter->second.segment.getFrameToTip(); + +// wheelRadius.resize(nJoints); +// wheelBase=leftSupportFrame(1,3)-rightSupportFrame(1,3); + + // get wheelRadius from URDF (actually from KDL tree) +// segmentMapIter=tree.getSegment("chassis"); +// KDL::Frame chassisFrame=segmentMapIter->second.segment.getFrameToTip(); + +// segmentMapIter=tree.getSegment("left_wheel"); +// KDL::Joint leftWheelJoint=segmentMapIter->second.segment.getJoint(); +// wheelRadius[0]=chassisFrame(2,3)+leftSupportFrame(2,3)+leftWheelJoint.JointOrigin().z(); + +// segmentMapIter=tree.getSegment("right_wheel"); +// KDL::Joint rightWheelJoint=segmentMapIter->second.segment.getJoint(); +// wheelRadius[1]=chassisFrame(2,3)+rightSupportFrame(2,3)+rightWheelJoint.JointOrigin().z(); + + // Parameter configuration + if(!node_.getParam("/robot_config",robot_ns)) + { + ROS_ERROR("Could not find Robot namespace in '/robot_config'."); + exit(-1); + } else ROS_INFO("Robot namespace: %s", robot_ns.c_str()); + + if (node_.getParam("/"+robot_ns+"/robot_baseline",WheelBase)) + ROS_INFO("Robot baseline: %f", WheelBase); + else ROS_ERROR("Failed to get param 'robot_baseline'"); + + if (node_.getParam("/" + robot_ns + "/wheel_radius",WheelRadius[0])){ + WheelRadius[1] = WheelRadius[0]; + ROS_INFO("Wheel Radius: %f", WheelRadius[1]); + } else ROS_ERROR("Failed to get param 'wheel_radius'"); + + jointStatesSubscriber=node_.subscribe("joint_states",1000,&Ident::jointStatesCB,this); +// dynParamPublisher=node_.advertise("ident/dynamic_parameters",1000); + dynParamPublisher=node_.advertise("ident/dynamic_parameters",1000); + leftWheelCommandPublisher=node_.advertise("ident/left_wheel_command",1000); + rightWheelCommandPublisher=node_.advertise("ident/right_wheel_command",1000); + + u.setZero(); + thetaEst1.setZero(); + thetaEst2.setZero(); + resetCovariance(); + + lastTime=ros::Time::now(); +} + +Ident::~Ident(void) +{ + jointStatesSubscriber.shutdown(); +} + +void Ident::resetCovariance(void) +{ + P1.setIdentity(); + P1*=1; + P2.setIdentity(); + P2*=1; + iter=0; +} + +void Ident::jointStatesCB(const sensor_msgs::JointState::ConstPtr &jointStates) +{ + ros::Duration dt=jointStates->header.stamp-lastTime; + lastTime=jointStates->header.stamp; + + Eigen::VectorXd y=-u; //y(k+1)=(u(k+1)-u(k))/dt + + Eigen::VectorXd Phi1(3); + Eigen::VectorXd Phi2(3); + Phi1[0] = u[0]; + Phi1[1] = u[1]*u[1]; + Phi2[0] = u[1]; + Phi2[0] = u[0]*u[1]; + + Eigen::VectorXd torque(2); + + // u(k+1) + // jointStates->velocity[0] is left wheel + // jointStates->velocity[1] is right wheel + u[0]=(jointStates->velocity[1]*WheelRadius[1]+jointStates->velocity[0]*WheelRadius[0])/2; + u[1]=(jointStates->velocity[1]*WheelRadius[1]-jointStates->velocity[0]*WheelRadius[0])/WheelBase; + + for(int i=0;i < nJoints;i++) + torque[i]=jointStates->effort[i]; // torque(k) + + y+=u; + y/=dt.toSec(); + + Phi1[2]=torque[1]+torque[0]; + Phi2[2]=torque[1]-torque[0]; + + double yEst1=Phi1.transpose()*thetaEst1; + Eigen::VectorXd K1=P1*Phi1/(1+Phi1.transpose()*P1*Phi1); + thetaEst1+=K1*(y[0]-yEst1); + P1-=K1*Phi1.transpose()*P1; + + double yEst2=Phi2.transpose()*thetaEst2; + Eigen::VectorXd K2=P2*Phi2/(1+Phi2.transpose()*P2*Phi2); + thetaEst2+=K2*(y[1]-yEst2); + P2-=K2*Phi2.transpose()*P2; + + twil_msgs::robot_dynamics dynParam; + dynParam.header.stamp = ros::Time::now(); + + for (int i = 0; i<3; i++) { + dynParam.parameters[i] = thetaEst1[i]; + dynParam.variances[i] = P1(i,i); + } + + for (int i = 0; i<3; i++) { + dynParam.parameters[i+3] = thetaEst2[i]; + dynParam.variances[i+3] = P2(i,i); + } + + +// std_msgs::Float64MultiArray dynParam; +// dynParam.layout.dim.push_back(std_msgs::MultiArrayDimension()); +// dynParam.layout.dim[0].label="K5 K6 K7 K8 P55 P66 P77 P88"; +// dynParam.layout.dim[0].size=nJoints*4; +// dynParam.layout.dim[0].stride=1; +// dynParam.layout.data_offset=0; +// for(int i=0;i < nJoints;i++) +// { +// dynParam.data.push_back(thetaEst1[i]); +// dynParam.data.push_back(thetaEst2[i]); +// } +// for(int i=0;i < nJoints;i++) +// { +// dynParam.data.push_back(P1(i,i)); +// dynParam.data.push_back(P2(i,i)); +// } + dynParamPublisher.publish(dynParam); + +// if(iter++ > 2048) resetCovariance(); +} + +void Ident::setCommand(void) +{ + std_msgs::Float64 leftCommand; + std_msgs::Float64 rightCommand; + leftCommand.data=10.0*prbs[0]-5.0; + rightCommand.data=10.0*prbs[1]-5.0; + leftWheelCommandPublisher.publish(leftCommand); + rightWheelCommandPublisher.publish(rightCommand); +} + +int main(int argc,char* argv[]) +{ + ros::init(argc,argv,"twil_ident"); + ros::NodeHandle node; + + Ident ident(node); + + ros::Rate loop(100); + while(ros::ok()) + { + ident.setCommand(); + + ros::spinOnce(); + loop.sleep(); + } + return 0; +} diff --git a/twil_msgs/CMakeLists.txt b/twil_msgs/CMakeLists.txt new file mode 100644 index 0000000..1cf4980 --- /dev/null +++ b/twil_msgs/CMakeLists.txt @@ -0,0 +1,105 @@ +cmake_minimum_required(VERSION 2.8.3) +project(twil_msgs) + +## Compile as C++11, supported in ROS Kinetic and newer +# add_compile_options(-std=c++11) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + geometry_msgs + message_generation + message_runtime + nav_msgs + roscpp + std_msgs + trajectory_msgs +) + +## Generate messages in the 'msg' folder +add_message_files( + FILES + non_smooth_parameters.msg + PosePolar.msg + PoseControllerStatus.msg + VelocityControllerStatus.msg + robot_dynamics.msg + ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here + generate_messages( + DEPENDENCIES + geometry_msgs + nav_msgs + std_msgs + trajectory_msgs + ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a run_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES twil_msgs +# CATKIN_DEPENDS geometry_msgs message_generation message_runtime nav_msgs roscpp std_msgs trajectory_msgs +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( +# include + ${catkin_INCLUDE_DIRS} +) + + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE + ) diff --git a/twil_msgs/msg/PoseControllerStatus.msg b/twil_msgs/msg/PoseControllerStatus.msg new file mode 100644 index 0000000..3012416 --- /dev/null +++ b/twil_msgs/msg/PoseControllerStatus.msg @@ -0,0 +1,16 @@ +Header header +geometry_msgs/Pose2D set_point +geometry_msgs/Pose2D process_value +geometry_msgs/Pose2D process_value_dot +geometry_msgs/Pose2D error +float64 time_step +float64[2] command +float64[5] lambda +float64[4] gamma +PosePolar polar_error +float64[2] backstep_set_point +float64[2] backstep_set_point_dot +float64[2] backstep_process_value +float64[2] backstep_error +float64[2] backstep_command +float64[2] linear_dynamics_command diff --git a/twil_msgs/msg/PosePolar.msg b/twil_msgs/msg/PosePolar.msg new file mode 100644 index 0000000..14a00fa --- /dev/null +++ b/twil_msgs/msg/PosePolar.msg @@ -0,0 +1,3 @@ +float64 range +float64 angle +float64 orientation diff --git a/twil_msgs/msg/VelocityControllerStatus.msg b/twil_msgs/msg/VelocityControllerStatus.msg new file mode 100644 index 0000000..4272ebb --- /dev/null +++ b/twil_msgs/msg/VelocityControllerStatus.msg @@ -0,0 +1,8 @@ +Header header +geometry_msgs/Pose2D robot_pose +float64 time_step +float64[2] velocity_set_point +float64[2] velocity_process_value +float64[2] velocity_error +float64[2] command +float64[2] linear_dynamics_command diff --git a/twil_msgs/msg/non_smooth_parameters.msg b/twil_msgs/msg/non_smooth_parameters.msg new file mode 100644 index 0000000..8450162 --- /dev/null +++ b/twil_msgs/msg/non_smooth_parameters.msg @@ -0,0 +1,3 @@ +float64[3] lambda +float64[2] gamma +float64[2] eps diff --git a/twil_msgs/msg/robot_dynamics.msg b/twil_msgs/msg/robot_dynamics.msg new file mode 100644 index 0000000..81fdb8d --- /dev/null +++ b/twil_msgs/msg/robot_dynamics.msg @@ -0,0 +1,4 @@ +Header header +float64[6] parameters +float64[6] variances +float64[2] error diff --git a/twil_msgs/package.xml b/twil_msgs/package.xml new file mode 100644 index 0000000..b9cef77 --- /dev/null +++ b/twil_msgs/package.xml @@ -0,0 +1,76 @@ + + + twil_msgs + 0.0.0 + The twil_msgs package + + + + + talves + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + geometry_msgs + message_generation + nav_msgs + roscpp + std_msgs + trajectory_msgs + geometry_msgs + nav_msgs + roscpp + std_msgs + trajectory_msgs + geometry_msgs + message_runtime + nav_msgs + roscpp + std_msgs + trajectory_msgs + + + + + + + + diff --git a/twil_trajectories/CMakeLists.txt b/twil_trajectories/CMakeLists.txt index 3c2670f..a2b3671 100644 --- a/twil_trajectories/CMakeLists.txt +++ b/twil_trajectories/CMakeLists.txt @@ -159,8 +159,8 @@ install(TARGETS ${PROJECT_NAME} ## Mark cpp header files for installation install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE ) ## Mark other files for installation (e.g. launch and bag files, etc.) diff --git a/twil_trajectories/src/eight_trajectory.cpp b/twil_trajectories/src/eight_trajectory.cpp index 1be794f..3756678 100644 --- a/twil_trajectories/src/eight_trajectory.cpp +++ b/twil_trajectories/src/eight_trajectory.cpp @@ -18,7 +18,7 @@ class EightTrajectory ros::NodeHandle node_; const EightPath *path; - ros::Publisher commandPublisher,commandPublisher2; + ros::Publisher commandPublisher1,commandPublisher2, commandPublisher3; }; @@ -48,15 +48,16 @@ EightTrajectory::EightTrajectory(ros::NodeHandle node) if (node_.getParam("robot_trajectory_path/loop_rate", loop_rate)) ROS_INFO("Got param 'loop_rate': %f", loop_rate); else ROS_ERROR("Failed to get param '%s/loop_rate'",node_.getNamespace().c_str()); - commandPublisher=node_.advertise("/"+robot_config+"/twist_pid_backstepping_controller/cmd_pos2d",1); + commandPublisher1=node_.advertise("/"+robot_config+"/twist_pid_backstepping_controller/cmd_pos2d",1); commandPublisher2=node_.advertise("/"+robot_config+"/dynamics_pid_backstepping_controller/cmd_pos2d",1); - + commandPublisher3=node_.advertise("/"+robot_config+"/dynamics_mrac_backstepping_controller/cmd_pos2d",1); + path=new EightPath(pc,r,w); } EightTrajectory::~EightTrajectory(void) { - commandPublisher.shutdown(); + commandPublisher1.shutdown(); delete path; } @@ -68,8 +69,9 @@ void EightTrajectory::setCommand(ros::Duration t) command.x=p[0]; command.y=p[1]; command.theta=p[2]; - commandPublisher.publish(command); + commandPublisher1.publish(command); commandPublisher2.publish(command); + commandPublisher3.publish(command); } int main(int argc,char* argv[])