<!-- <test_depend>gtest</test_depend> -->
<buildtool_depend>catkin</buildtool_depend>
- <run_depend>twil_description</run_depend>
<run_depend>twil_bringup</run_depend>
<run_depend>twil_controllers</run_depend>
- <run_depend>twil_hw</run_depend>
+ <run_depend>twil_msgs</run_depend>
+ <run_depend>twil_description</run_depend>
+ <run_depend>twil_hardware</run_depend>
<!-- The export tag contains other, unspecified, tags -->
## Find catkin macros and libraries
find_package(catkin REQUIRED COMPONENTS
- controller_interface
- controller_manager
- hardware_interface
- joint_state_controller
roscpp
rospy
std_msgs
trajectory_msgs
geometry_msgs
)
-find_package(twil_hw REQUIRED)
## System dependencies are found with CMake's conventions
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
)
include_directories(
include
${catkin_INCLUDE_DIRS}
- ${twil_hw_INCLUDE_DIRS}
- ${AIC_LIB_ROOT_DIR}/include
)
add_executable(twil_joystick_node src/joy.cpp)
)
## 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
+ )
--- /dev/null
+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]
--- /dev/null
+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]
--- /dev/null
+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
+
+
+
--- /dev/null
+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
+
+
--- /dev/null
+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
+
--- /dev/null
+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
--- /dev/null
+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]
--- /dev/null
+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
--- /dev/null
+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
--- /dev/null
+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]
--- /dev/null
+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]
--- /dev/null
+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
+
+
+
--- /dev/null
+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
+
+
--- /dev/null
+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
--- /dev/null
+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]
--- /dev/null
+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
--- /dev/null
+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
--- /dev/null
+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]
- 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]
- 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
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
- 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
--- /dev/null
+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
- 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]
- 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
--- /dev/null
+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
--- /dev/null
+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]
- 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
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]
- 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
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
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
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
- 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]
- 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
--- /dev/null
+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
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
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
--- /dev/null
+<?xml version="1.0" encoding="UTF-8"?>
+
+<launch>
+ <!-- Environment Variables -->
+ <arg name="Robot_Name" value="$(env robot_config)" />
+ <arg name="Remote_Hardware" value="$(env remote_hardware)"/>
+
+ <!-- Robot Mechanics -->
+ <include file="$(find twil_bringup)/launch/robot_mechanics.launch">
+ </include>
+
+ <!-- Robot HW Launcher -->
+ <group unless="$(arg Remote_Hardware)">
+ <include file="$(find twil_bringup)/launch/robot_hw.launch">
+ </include>
+ </group>
+
+ <!-- Controller Parameters -->
+ <rosparam file="$(find twil_bringup)/config/controllers/$(arg Robot_Name)_dynamics_mrac_backstepping_control.yaml" command="load"/>
+
+ <!-- Robot Control Node -->
+ <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
+ output="screen" ns="$(arg Robot_Name)" args="dynamics_mrac_backstepping_controller joint_state_controller">
+ </node>
+
+ <!-- State Publisher Node -->
+ <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" ns="$(arg Robot_Name)" />
+
+ <!-- Dynamic Parameters Estimator -->
+ <node name="online_dynamics_parameter_adapter" pkg="twil_controllers" type="adaptive_dynamics_identification" ns="$(arg Robot_Name)"/>
+
+</launch>
--- /dev/null
+<?xml version="1.0" encoding="UTF-8"?>
+
+<launch>
+ <!-- Environment Variables -->
+ <arg name="Robot_Name" value="$(env robot_config)" />
+ <arg name="Remote_Hardware" value="$(env remote_hardware)"/>
+
+ <!-- Robot Mechanics -->
+ <include file="$(find twil_bringup)/launch/robot_mechanics.launch">
+ </include>
+
+ <!-- Robot HW Launcher -->
+ <group unless="$(arg Remote_Hardware)">
+ <include file="$(find twil_bringup)/launch/robot_hw.launch">
+ </include>
+ </group>
+
+ <!-- Controller Parameters -->
+ <rosparam file="$(find twil_bringup)/config/controllers/$(arg Robot_Name)_dynamics_pid_backstepping_control.yaml" command="load"/>
+
+ <!-- Robot Control Node -->
+ <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
+ output="screen" ns="$(arg Robot_Name)" args="dynamics_pid_backstepping_controller joint_state_controller">
+ </node>
+
+ <!-- State Publisher Node -->
+ <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" ns="$(arg Robot_Name)" />
+
+ <!-- Dynamic Parameters Estimator -->
+ <node name="online_dynamics_parameter_adapter" pkg="twil_controllers" type="adaptive_dynamics_identification" ns="$(arg Robot_Name)"/>
+
+</launch>
--- /dev/null
+<?xml version="1.0" encoding="UTF-8"?>
+
+<launch>
+ <!-- Environment Variables -->
+ <arg name="Robot_Name" value="$(env robot_config)" />
+ <arg name="Remote_Hardware" value="$(env remote_hardware)"/>
+
+ <!-- Robot Mechanics -->
+ <include file="$(find twil_bringup)/launch/robot_mechanics.launch">
+ </include>
+
+ <!-- Robot HW Launcher -->
+ <group unless="$(arg Remote_Hardware)">
+ <include file="$(find twil_bringup)/launch/robot_hw.launch">
+ </include>
+ </group>
+
+ <!-- Controller Parameters -->
+ <rosparam file="$(find twil_bringup)/config/controllers/$(arg Robot_Name)_dynamics_pid_control.yaml" command="load"/>
+
+ <!-- Robot Control Node -->
+ <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
+ output="screen" ns="$(arg Robot_Name)" args="dynamics_pid_controller joint_state_controller">
+ </node>
+
+ <!-- State Publisher Node -->
+ <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" ns="$(arg Robot_Name)"/>
+
+ <!-- State Publisher Node -->
+<!-- <node name="online_dynamics_parameter_adapter" pkg="twil_controllers" type="adaptive_dynamics_identification" ns="$(arg Robot_Name)" output="screen"/>-->
+ <node name="ident" pkg="twil_ident" type="ident" output="screen" ns="$(arg Robot_Name)">
+<!-- <remap from="ident/dynamic_parameters" to="nonsmooth_backstep_controller/dynamic_parameters"/>-->
+ </node>
+
+</launch>
--- /dev/null
+<?xml version="1.0" encoding="UTF-8"?>
+
+<launch>
+ <!-- Environment Variables -->
+ <arg name="Robot_Name" value="$(env robot_config)" />
+ <arg name="Remote_Hardware" value="$(env remote_hardware)"/>
+
+ <!-- Robot Mechanics -->
+ <include file="$(find twil_bringup)/launch/robot_mechanics.launch">
+ </include>
+
+ <!-- Robot HW Launcher -->
+ <group unless="$(arg Remote_Hardware)">
+ <include file="$(find twil_bringup)/launch/robot_hw.launch">
+ </include>
+ </group>
+
+ <!-- Controller Parameters -->
+ <rosparam file="$(find twil_bringup)/config/controllers/$(arg Robot_Name)_dynamics_mrac_backstepping_control.yaml" command="load"/>
+
+ <!-- Robot Control Node -->
+ <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
+ output="screen" ns="$(arg Robot_Name)" args="dynamics_mrac_backstepping_controller joint_state_controller">
+ </node>
+
+ <!-- State Publisher Node -->
+ <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" ns="$(arg Robot_Name)" />
+
+</launch>
<launch>
<!-- Environment Variables -->
<arg name="Robot_Name" value="$(env robot_config)" />
+ <arg name="Remote_Hardware" value="$(env remote_hardware)"/>
- <!-- Robot HW Launcher -->
- <include file="$(find twil_bringup)/launch/robot_hw.launch">
+ <!-- Robot Mechanics -->
+ <include file="$(find twil_bringup)/launch/robot_mechanics.launch">
</include>
+ <!-- Robot HW Launcher -->
+ <group unless="$(arg Remote_Hardware)">
+ <include file="$(find twil_bringup)/launch/robot_hw.launch">
+ </include>
+ </group>
+
<!-- Controller Parameters -->
<rosparam file="$(find twil_bringup)/config/controllers/$(arg Robot_Name)_dynamics_pid_backstepping_control.yaml" command="load"/>
output="screen" ns="$(arg Robot_Name)" args="dynamics_pid_backstepping_controller joint_state_controller">
</node>
- <!-- State Publisher Node -->
+ <!-- State Publisher Node -->
<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" ns="$(arg Robot_Name)" />
</launch>
<launch>
<!-- Environment Variables -->
<arg name="Robot_Name" value="$(env robot_config)" />
+ <arg name="Remote_Hardware" value="$(env remote_hardware)"/>
- <!-- Robot HW Launcher -->
- <include file="$(find twil_bringup)/launch/robot_hw.launch">
+ <!-- Robot Mechanics -->
+ <include file="$(find twil_bringup)/launch/robot_mechanics.launch">
</include>
+ <!-- Robot HW Launcher -->
+ <group unless="$(arg Remote_Hardware)">
+ <include file="$(find twil_bringup)/launch/robot_hw.launch">
+ </include>
+ </group>
+
<!-- Controller Parameters -->
<rosparam file="$(find twil_bringup)/config/controllers/$(arg Robot_Name)_dynamics_pid_control.yaml" command="load"/>
<launch>
<!-- Environment Variables -->
<arg name="Robot_Name" value="$(env robot_config)" />
+ <arg name="Remote_Hardware" value="$(env remote_hardware)"/>
- <!-- Robot HW Launcher -->
- <include file="$(find twil_bringup)/launch/robot_hw.launch">
+ <!-- Robot Mechanics -->
+ <include file="$(find twil_bringup)/launch/robot_mechanics.launch">
</include>
+ <!-- Robot HW Launcher -->
+ <group unless="$(arg Remote_Hardware)">
+ <include file="$(find twil_bringup)/launch/robot_hw.launch">
+ </include>
+ </group>
+
<!-- Controller Parameters -->
<rosparam file="$(find twil_bringup)/config/controllers/$(arg Robot_Name)_joint_velocity_control.yaml" command="load"/>
--- /dev/null
+<?xml version="1.0" encoding="UTF-8"?>
+
+<launch>
+ <!-- Environment Variables -->
+ <arg name="Robot_Name" value="$(env robot_config)" />
+ <arg name="Remote_Hardware" value="$(env remote_hardware)"/>
+
+ <!-- Robot Mechanics -->
+ <include file="$(find twil_bringup)/launch/robot_mechanics.launch">
+ </include>
+
+ <!-- Robot HW Launcher -->
+ <group unless="$(arg Remote_Hardware)">
+ <include file="$(find twil_bringup)/launch/robot_hw.launch">
+ </include>
+ </group>
+
+ <!-- Controller Parameters -->
+ <rosparam file="$(find twil_bringup)/config/controllers/$(arg Robot_Name)_voltage_control.yaml" command="load"/>
+
+ <!-- Robot Control Node -->
+ <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
+ output="screen" ns="$(arg Robot_Name)" args="left_wheel_joint_voltage_controller right_wheel_joint_voltage_controller joint_state_controller">
+ </node>
+
+ <!-- State Publisher Node -->
+ <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" ns="$(arg Robot_Name)"/>
+
+</launch>
<launch>
<!-- Environment Variables -->
<arg name="Robot_Name" value="$(env robot_config)" />
+ <arg name="Remote_Hardware" value="$(env remote_hardware)"/>
- <!-- Robot HW Launcher -->
- <include file="$(find twil_bringup)/launch/robot_hw.launch">
+ <!-- Robot Mechanics -->
+ <include file="$(find twil_bringup)/launch/robot_mechanics.launch">
</include>
+ <!-- Robot HW Launcher -->
+ <group unless="$(arg Remote_Hardware)">
+ <include file="$(find twil_bringup)/launch/robot_hw.launch">
+ </include>
+ </group>
+
<!-- Controller Parameters -->
<rosparam file="$(find twil_bringup)/config/controllers/$(arg Robot_Name)_linearizing_control.yaml" command="load"/>
--- /dev/null
+<?xml version="1.0" encoding="UTF-8"?>
+
+<launch>
+ <!-- Environment Variables -->
+ <arg name="Robot_Name" value="$(env robot_config)" />
+ <arg name="Remote_Hardware" value="$(env remote_hardware)"/>
+
+ <!-- Robot Mechanics -->
+ <include file="$(find twil_bringup)/launch/robot_mechanics.launch">
+ </include>
+
+ <!-- Robot HW Launcher -->
+ <group unless="$(arg Remote_Hardware)">
+ <include file="$(find twil_bringup)/launch/robot_hw.launch">
+ </include>
+ </group>
+
+ <!-- Controller Parameters -->
+ <rosparam file="$(find twil_bringup)/config/controllers/$(arg Robot_Name)_nonlinear_backstepping_control.yaml" command="load"/>
+
+ <!-- Robot Control Node -->
+ <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
+ output="screen" ns="$(arg Robot_Name)" args="nonlinear_backstepping_controller joint_state_controller">
+ </node>
+
+ <!-- State Publisher Node -->
+ <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" ns="$(arg Robot_Name)" />
+
+</launch>
<launch>
<!-- Environment Variables -->
<arg name="Robot_Name" value="$(env robot_config)" />
+ <arg name="Remote_Hardware" value="$(env remote_hardware)"/>
- <!-- Robot HW Launcher -->
- <include file="$(find twil_bringup)/launch/robot_hw.launch">
+ <!-- Robot Mechanics -->
+ <include file="$(find twil_bringup)/launch/robot_mechanics.launch">
</include>
+ <!-- Robot HW Launcher -->
+ <group unless="$(arg Remote_Hardware)">
+ <include file="$(find twil_bringup)/launch/robot_hw.launch">
+ </include>
+ </group>
+
<!-- Controller Parameters -->
<rosparam file="$(find twil_bringup)/config/controllers/$(arg Robot_Name)_twist_pid_backstepping_control.yaml" command="load"/>
<launch>
<!-- Environment Variables -->
<arg name="Robot_Name" value="$(env robot_config)" />
+ <arg name="Remote_Hardware" value="$(env remote_hardware)"/>
- <!-- Robot HW Launcher -->
- <include file="$(find twil_bringup)/launch/robot_hw.launch">
+ <!-- Robot Mechanics -->
+ <include file="$(find twil_bringup)/launch/robot_mechanics.launch">
</include>
+ <!-- Robot HW Launcher -->
+ <group unless="$(arg Remote_Hardware)">
+ <include file="$(find twil_bringup)/launch/robot_hw.launch">
+ </include>
+ </group>
+
<!-- Controller Parameters -->
<rosparam file="$(find twil_bringup)/config/controllers/$(arg Robot_Name)_twist_pid_control.yaml" command="load"/>
<param name="robot_config" value="$(arg Robot_Name)"/>
<param name="log_date" value="$(arg Log_Date)"/>
- <!-- Model Parameters -->
- <param name="robot_description" command="$(find xacro)/xacro.py '$(find twil_description)/xacro/twil_real.urdf.xacro'" />
- <!-- Mechanical Parameters -->
- <rosparam file="$(find twil_bringup)/config/mechanics/$(arg Robot_Name)_specs.yaml" command="load"/>
<!-- Electronic Parameters -->
<rosparam file="$(find twil_bringup)/config/electronics/$(arg Robot_Name)_hw.yaml" command="load"/>
<!-- Robot Control Node -->
- <node name="twil_hw_node" ns="$(arg Robot_Name)" pkg="twil_hw" type="twil_hw_node" output="screen" />
+ <node name="control_loop_node" ns="$(arg Robot_Name)" pkg="twil_hardware" type="twil_control_loop_node" output="screen" />
</launch>
--- /dev/null
+<?xml version="1.0" encoding="UTF-8"?>
+
+<launch>
+ <!-- Environment Variables -->
+ <arg name="Robot_Name" value="$(env robot_config)" />
+
+ <!-- Model Parameters -->
+ <param name="robot_description" command="$(find xacro)/xacro.py '$(find twil_description)/xacro/twil_real.urdf.xacro'" />
+ <!-- Mechanical Parameters -->
+ <rosparam file="$(find twil_bringup)/config/mechanics/$(arg Robot_Name)_specs.yaml" command="load"/>
+</launch>
<param name="pc_x" value="0.0"/>
<param name="pc_y" value="0.0"/>
<param name="phi0" value="0.0"/>
- <param name="r" value="0.25"/>
- <param name="w" value="0.8"/>
- <param name="loop_rate" value="20"/>
+ <param name="r" value="0.4"/>
+ <param name="w" value="0.3"/>
+ <param name="loop_rate" value="25"/>
</node>
</launch>
<param name="pc_y" value="0.0"/>
<param name="phi0" value="0.0"/>
<param name="r" value="0.15"/>
- <param name="w" value="0.6"/>
- <param name="loop_rate" value="20"/>
+ <param name="w" value="0.7"/>
+ <param name="loop_rate" value="25"/>
</node>
</launch>
<param name="pc_x" value="0.0"/>
<param name="pc_y" value="0.0"/>
<param name="phi0" value="0.0"/>
- <param name="r" value="0.3"/>
+ <param name="r" value="0.2"/>
<param name="w" value="0.4"/>
- <param name="loop_rate" value="20"/>
+ <param name="loop_rate" value="100"/>
</node>
</launch>
<param name="pc_x" value="0.0"/>
<param name="pc_y" value="0.0"/>
<param name="phi0" value="0.0"/>
- <param name="r" value="0.3"/>
- <param name="w" value="0.4"/>
- <param name="loop_rate" value="20"/>
+ <param name="r" value="0.5"/>
+ <param name="w" value="0.2"/>
+ <param name="loop_rate" value="25"/>
</node>
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<buildtool_depend>catkin</buildtool_depend>
- <build_depend>controller_interface</build_depend>
- <build_depend>controller_manager</build_depend>
- <build_depend>hardware_interface</build_depend>
- <build_depend>joint_state_controller</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend>
<build_depend>std_msgs</build_depend>
- <build_depend>twil_hw</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>trajectory_msgs</build_depend>
<build_depend>geometry_msgs</build_depend>
- <run_depend>controller_interface</run_depend>
- <run_depend>controller_manager</run_depend>
- <run_depend>hardware_interface</run_depend>
- <run_depend>joint_state_controller</run_depend>
<run_depend>roscpp</run_depend>
<run_depend>rospy</run_depend>
<run_depend>std_msgs</run_depend>
- <run_depend>twil_hw</run_depend>
<run_depend>sensor_msgs</run_depend>
<run_depend>trajectory_msgs</run_depend>
<run_depend>geometry_msgs</run_depend>
-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 ##
################################################
## Generate messages in the 'msg' folder
# add_message_files(
-# FILES
-# Message1.msg
-# Message2.msg
-# )
+# FILES
+# )
## Generate services in the 'srv' folder
# add_service_files(
## 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 ##
## 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
# 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)
#define TWIL_CONTROLLERS_CART_LINEARIZING_CONTROLLER_H
#include <ros/ros.h>
-#include <hardware_interface/joint_command_interface.h>
+//#include <hardware_interface/joint_command_interface.h>
#include <controller_interface/controller.h>
#include <std_msgs/Float64MultiArray.h>
+#include <geometry_msgs/Accel.h>
#include <sensor_msgs/JointState.h>
#include "Eigen/Dense"
#include "state_feedback_linearization.h"
-#include "velocity_transformation.h"
+#include <arc_odometry/diff_odometry.h>
+#include "twil_hardware_interfaces/joint_command_extended_interface.h"
#define CART_LIN_DEBUG
namespace twil_controllers
{
-class CartLinearizingController: public controller_interface::Controller<hardware_interface::EffortJointInterface>
+class CartLinearizingController: public controller_interface::Controller<twil_hardware_interfaces::VoltageJointInterface>
{
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<hardware_interface::JointHandle> 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<double> 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;
int take;
#endif
- std::vector<double> WheelRadius;
- double WheelBase;
+
};
}
#endif
--- /dev/null
+/******************************************************************************
+ Twil Controllers
+ Linearizing MRAC Backstep Controller
+
+ Copyright (C) 2017 Tiago G. Alves <talves@ece.ufrgs.br>
+
+ 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 <http://www.gnu.org/licenses>.
+
+*******************************************************************************/
+
+#ifndef DYNAMICS_MRAC_BACKSTEPPING_CONTROLLER_H
+#define DYNAMICS_MRAC_BACKSTEPPING_CONTROLLER_H
+
+#include <ros/ros.h>
+//#include <hardware_interface/joint_command_interface.h>
+#include <controller_interface/controller.h>
+
+#include <geometry_msgs/Pose2D.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <boost/thread/mutex.hpp>
+
+#include "non_smooth_control.h"
+#include "state_feedback_linearization.h"
+#include <arc_odometry/diff_odometry.h>
+
+#include <boost/scoped_ptr.hpp>
+#include <realtime_tools/realtime_publisher.h>
+#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 <fstream>
+#endif
+
+
+namespace twil_controllers{
+
+class DynamicsMRACBacksteppingController: public controller_interface::Controller<twil_hardware_interfaces::VoltageJointInterface>
+{
+
+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<hardware_interface::JointHandle> 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<double,2> 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<double> 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<realtime_tools::RealtimePublisher <twil_msgs::PoseControllerStatus> > status_publisher_ ;
+
+#ifdef DYNAMICS_MRAC_BACKSTEPPING_CONTROLLER_DEBUG
+ std::ofstream fd;
+ bool makeLog;
+ std::string log_date;
+ int take;
+#endif
+
+};
+
+}
+
+#endif
+
#define DYNAMICS_PID_BACKSTEPPING_CONTROLLER_H
#include <ros/ros.h>
-#include <hardware_interface/joint_command_interface.h>
+//#include <hardware_interface/joint_command_interface.h>
#include <controller_interface/controller.h>
#include <control_toolbox/pid.h>
#include <geometry_msgs/Pose2D.h>
#include <geometry_msgs/PoseStamped.h>
-#include "twil_controllers/odometry_tool.h"
-#include "twil_controllers/velocity_transformation.h"
+#include <boost/thread/mutex.hpp>
+
#include "non_smooth_control.h"
#include "state_feedback_linearization.h"
+#include <arc_odometry/diff_odometry.h>
+
+#include <boost/scoped_ptr.hpp>
+#include <realtime_tools/realtime_publisher.h>
+#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
namespace twil_controllers{
-class DynamicsPIDBacksteppingController: public controller_interface::Controller<hardware_interface::EffortJointInterface>
+class DynamicsPIDBacksteppingController: public controller_interface::Controller<twil_hardware_interfaces::VoltageJointInterface>
{
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<hardware_interface::JointHandle> 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<double> 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<realtime_tools::RealtimePublisher <twil_msgs::PoseControllerStatus> > status_publisher_ ;
+
#ifdef DYNAMICS_PID_BACKSTEPPING_CONTROLLER_DEBUG
std::ofstream fd;
bool makeLog;
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
-#include <hardware_interface/joint_command_interface.h>
+//#include <hardware_interface/joint_command_interface.h>
#include <controller_interface/controller.h>
#include <control_toolbox/pid.h>
#include <Eigen/Dense>
-#include "velocity_transformation.h"
+#include <boost/thread/mutex.hpp>
+
#include "state_feedback_linearization.h"
+#include <arc_odometry/diff_odometry.h>
+
+#include <boost/scoped_ptr.hpp>
+#include <realtime_tools/realtime_publisher.h>
+#include "twil_msgs/VelocityControllerStatus.h"
+#include "twil_msgs/robot_dynamics.h"
+#include "twil_hardware_interfaces/joint_command_extended_interface.h"
#define DYNAMICS_PID_DEBUG
namespace twil_controllers
{
-class DynamicsPIDController: public controller_interface::Controller<hardware_interface::EffortJointInterface>
+class DynamicsPIDController: public controller_interface::Controller<twil_hardware_interfaces::VoltageJointInterface>
{
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<hardware_interface::JointHandle> 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<double> 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<realtime_tools::RealtimePublisher <twil_msgs::VelocityControllerStatus> > status_publisher_ ;
#ifdef DYNAMICS_PID_DEBUG
std::ofstream fd;
int take;
#endif
- std::vector<double> WheelRadius;
- double WheelBase;
+
};
}
#endif
--- /dev/null
+#ifndef FORWARD_COMMAND_CONTROLLER_FORWARD_COMMAND_CONTROLLER_H
+#define FORWARD_COMMAND_CONTROLLER_FORWARD_COMMAND_CONTROLLER_H
+
+#include <ros/node_handle.h>
+#include <hardware_interface/joint_command_interface.h>
+#include <controller_interface/controller.h>
+#include <std_msgs/Float64.h>
+#include <realtime_tools/realtime_buffer.h>
+
+
+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 T>
+class ForwardCommandController: public controller_interface::Controller<T>
+{
+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<std_msgs::Float64>("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<double> command_buffer_;
+
+private:
+ ros::Subscriber sub_command_;
+ void commandCB(const std_msgs::Float64ConstPtr& msg) {command_buffer_.writeFromNonRT(msg->data);}
+};
+
+}
+
+#endif
--- /dev/null
+#ifndef FORWARD_COMMAND_CONTROLLER_FORWARD_JOINT_GROUP_COMMAND_CONTROLLER_H
+#define FORWARD_COMMAND_CONTROLLER_FORWARD_JOINT_GROUP_COMMAND_CONTROLLER_H
+
+#include <vector>
+#include <string>
+
+#include <ros/node_handle.h>
+#include <hardware_interface/joint_command_interface.h>
+#include <controller_interface/controller.h>
+#include <std_msgs/Float64MultiArray.h>
+#include <realtime_tools/realtime_buffer.h>
+
+
+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 T>
+class ForwardJointGroupCommandController: public controller_interface::Controller<T>
+{
+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; i<n_joints_; i++)
+ {
+ try
+ {
+ joints_.push_back(hw->getHandle(joint_names_[i]));
+ }
+ catch (const hardware_interface::HardwareInterfaceException& e)
+ {
+ ROS_ERROR_STREAM("Exception thrown: " << e.what());
+ return false;
+ }
+ }
+
+ commands_buffer_.writeFromNonRT(std::vector<double>(n_joints_, 0.0));
+
+ sub_command_ = n.subscribe<std_msgs::Float64MultiArray>("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<double> & commands = *commands_buffer_.readFromRT();
+ for(unsigned int i=0; i<n_joints_; i++)
+ { joints_[i].setCommand(commands[i]); }
+ }
+
+ std::vector< std::string > joint_names_;
+ std::vector< hardware_interface::JointHandle > joints_;
+ realtime_tools::RealtimeBuffer<std::vector<double> > 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
--- /dev/null
+#ifndef EFFORT_CONTROLLERS_JOINT_GROUP_VOLTAGE_CONTROLLER_H
+#define EFFORT_CONTROLLERS_JOINT_GROUP_VOLTAGE_CONTROLLER_H
+
+#include <twil_controllers/forward_joint_group_command_controller.h>
+#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<twil_hardware_interfaces::VoltageJointInterface>
+ JointGroupVoltageController;
+}
+
+#endif
--- /dev/null
+#ifndef EFFORT_CONTROLLERS_JOINT_VOLTAGE_CONTROLLER_H
+#define EFFORT_CONTROLLERS_JOINT_VOLTAGE_CONTROLLER_H
+
+#include <twil_controllers/forward_command_controller.h>
+#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<twil_hardware_interfaces::VoltageJointInterface>
+ JointVoltageController;
+}
+
+#endif
#include <cstdio>
#include <cmath>
-//#define NONSMOOTH_CONTROLLER_OUTPUT_SCREEN
+#include <fstream>
+
+#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]: "<<lambda << gamma << std::endl;
+ eps << 5e-3, M_PI/180;
+ }
+
+ NonSmoothControl(const Eigen::Vector3d &lambda, const Eigen::Vector2d &gamma, const double &linear_tol, const double &angular_tol)
+ : lamb(lambda), gam(gamma)
+ #ifdef NONSMOOTH_CONTROLLER_OUTPUT_SCREEN
+ , fd("~/output_fetter.txt")
+ #endif
+ {
std::cout << "Parametros [lb, gm]: "<<lambda << gamma << std::endl;
+ std::cout << "Tolerâncias [lin, ang]: "<<linear_tol << " - " << angular_tol << std::endl;
+ eps << linear_tol, angular_tol;
}
- ~NonSmoothControl();
+ ~NonSmoothControl(){
+ fd.close();
+ }
Eigen::Vector2d compute(const Eigen::Vector3d &xc, const Eigen::Vector3d &x_r)
{
-
- double e=0, psi=0, alpha=0;
+ e=0; psi=0; alpha=0;
Eigen::Vector2d u_r;
u_r.setZero();
- if (x_r != xc){
- Eigen::Vector3d xc_bar = coordinate_change(xc, x_r);
+ Eigen::Vector3d xc_bar = coordinate_change(xc, x_r);
- e = sqrt( pow(xc_bar[0],2) + pow(xc_bar[1],2) );
- psi = atan2(xc_bar[1],xc_bar[0]);
- alpha = xc_bar[2]-psi;
+ e = sqrt( (xc_bar[0]*xc_bar[0]) + (xc_bar[1]*xc_bar[1]) );
+ psi = atan2(xc_bar[1],xc_bar[0]);
+ alpha = xc_bar[2]-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 << e << "\t" << alpha << "\t" << psi << "\t";
- // Avoid singularity
- u_r[1] += (fabs(alpha) > 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:
Eigen::Vector3d lamb;
Eigen::Vector2d gam;
+ Eigen::Vector2d eps;
+ double e;
+ double psi;
+ double alpha;
+
+#ifdef NONSMOOTH_CONTROLLER_OUTPUT_SCREEN
+ std::ofstream fd;
+#endif
};
+/******************************************************************************
+ Twil Controllers
+ Nonlinear Backstep Controller
+
+ Copyright (C) 2017 Tiago G. Alves <talves@ece.ufrgs.br>
+
+ 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 <http://www.gnu.org/licenses>.
+
+*******************************************************************************/
+
#ifndef NONLINEAR_BACKSTEPPING_CONTROLLER_H
#define NONLINEAR_BACKSTEPPING_CONTROLLER_H
+#include <ros/ros.h>
+//#include <hardware_interface/joint_command_interface.h>
+#include <controller_interface/controller.h>
+#include <control_toolbox/pid.h>
+#include <geometry_msgs/Pose2D.h>
+#include <geometry_msgs/PoseStamped.h>
+#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 <fstream>
+#endif
+
+
+namespace twil_controllers{
+
+class NonlinearBacksteppingController: public controller_interface::Controller<twil_hardware_interfaces::VoltageJointInterface>
+{
+
+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<hardware_interface::JointHandle> 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<double> lambda;
+ std::vector<double> 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
typedef Eigen::Matrix<double,2,4> 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
+
+
+
#define TWIST_PID_BACKSTEPPING_CONTROLLER_H
#include <ros/ros.h>
-#include <hardware_interface/joint_command_interface.h>
+//#include <hardware_interface/joint_command_interface.h>
#include <controller_interface/controller.h>
#include <control_toolbox/pid.h>
#include <Eigen/Dense>
#include <geometry_msgs/Pose2D.h>
#include <geometry_msgs/PoseStamped.h>
-#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 <arc_odometry/diff_odometry.h>
+#include <boost/scoped_ptr.hpp>
+#include <realtime_tools/realtime_publisher.h>
+#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
namespace twil_controllers{
-class TwistPIDBacksteppingController: public controller_interface::Controller<hardware_interface::EffortJointInterface>
+class TwistPIDBacksteppingController: public controller_interface::Controller<twil_hardware_interfaces::VoltageJointInterface>
{
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<hardware_interface::JointHandle> 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<double> 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<realtime_tools::RealtimePublisher <twil_msgs::PoseControllerStatus> > status_publisher_ ;
+
#ifdef TWIST_PID_BACKSTEPPING_CONTROLLER_DEBUG
std::ofstream fd;
bool makeLog;
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
-#include <hardware_interface/joint_command_interface.h>
+//#include <hardware_interface/joint_command_interface.h>
#include <controller_interface/controller.h>
#include <control_toolbox/pid.h>
#include <Eigen/Dense>
-#include "velocity_transformation.h"
+
#include "state_feedback_linearization.h"
+#include <arc_odometry/diff_odometry.h>
+
+#include <boost/scoped_ptr.hpp>
+#include <realtime_tools/realtime_publisher.h>
+#include "twil_msgs/VelocityControllerStatus.h"
+#include "twil_hardware_interfaces/joint_command_extended_interface.h"
#define TWIST_PID_DEBUG
#include <fstream>
#endif
-typedef Eigen::Matrix<double,2,4> Mat2_4;
-
namespace twil_controllers
{
-class TwistPIDController: public controller_interface::Controller<hardware_interface::EffortJointInterface>
+class TwistPIDController: public controller_interface::Controller<twil_hardware_interfaces::VoltageJointInterface>
{
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);
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<hardware_interface::JointHandle> 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<double> 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<realtime_tools::RealtimePublisher <twil_msgs::VelocityControllerStatus> > status_publisher_ ;
+
#ifdef TWIST_PID_DEBUG
std::ofstream fd;
bool makeLog;
- std::string log_date;
+ std::string log_date;
int take;
#endif
<version>2.0.0</version>
<description>The twil_controllers package</description>
- <!-- One maintainer tag required, multiple allowed, one person per tag -->
- <!-- Example: -->
- <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="fetter@ece.ufrgs.br">Walter Fetter Lages</maintainer>
-
- <!-- One license tag required, multiple allowed, one license per tag -->
- <!-- Commonly used license strings: -->
- <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>GPLv3</license>
-
- <!-- Url tags are optional, but mutiple are allowed, one per tag -->
- <!-- Optional attribute type can be: website, bugtracker, or repository -->
- <!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/twil_controllers</url> -->
-
- <!-- Author tags are optional, mutiple are allowed, one per tag -->
- <!-- Authors do not have to be maintianers, but could be -->
- <!-- Example: -->
- <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<author email="fetter@ece.ufrgs.br">Walter Fetter Lages</author>
-
- <!-- The *_depend tags are used to specify dependencies -->
- <!-- Dependencies can be catkin packages or system dependencies -->
- <!-- Examples: -->
- <!-- Use build_depend for packages you need at compile time: -->
- <!-- <build_depend>message_generation</build_depend> -->
- <!-- Use buildtool_depend for build tool packages: -->
- <!-- <buildtool_depend>catkin</buildtool_depend> -->
- <!-- Use run_depend for packages you need at runtime: -->
- <!-- <run_depend>message_runtime</run_depend> -->
- <!-- Use test_depend for packages you need only for testing: -->
- <!-- <test_depend>gtest</test_depend> -->
<buildtool_depend>catkin</buildtool_depend>
+ <build_depend>twil_msgs</build_depend>
<build_depend>controller_interface</build_depend>
<build_depend>effort_controllers</build_depend>
<build_depend>kdl_parser</build_depend>
- <build_depend>orocos_kdl</build_depend>
- <build_depend>twil_gazebo_ros_control</build_depend>
- <build_depend>control_toolbox</build_depend>
-
-
- <build_depend>cmake_modules</build_depend>
-
-
+ <build_depend>control_toolbox</build_depend>
+ <build_depend>Eigen</build_depend>
+ <build_depend>geometry_msgs</build_depend>
+ <build_depend>nav_msgs</build_depend>
+ <build_depend>roscpp</build_depend>
+ <build_depend>tf</build_depend>
+ <build_depend>arc_odometry</build_depend>
+ <build_depend>twil_hardware_interfaces</build_depend>
+
+ <run_depend>twil_msgs</run_depend>
<run_depend>controller_interface</run_depend>
<run_depend>controller_manager</run_depend>
<run_depend>effort_controllers</run_depend>
<run_depend>joint_state_controller</run_depend>
<run_depend>kdl_parser</run_depend>
- <run_depend>orocos_kdl</run_depend>
- <run_depend>twil_gazebo_ros_control</run_depend>
<run_depend>control_toolbox</run_depend>
-
<run_depend>cmake_modules</run_depend>
+ <run_depend>Eigen</run_depend>
+ <run_depend>geometry_msgs</run_depend>
+ <run_depend>nav_msgs</run_depend>
+ <run_depend>roscpp</run_depend>
+ <run_depend>tf</run_depend>
+ <run_depend>arc_odometry</run_depend>
+ <run_depend>twil_hardware_interfaces</run_depend>
-
- <!-- The export tag contains other, unspecified, tags -->
<export>
- <!-- You can specify that this package is a metapackage here: -->
- <!-- <metapackage/> -->
-
- <!-- Other tools can request additional information be placed here -->
<controller_interface plugin="${prefix}/twil_controllers_plugins.xml"/>
</export>
</package>
--- /dev/null
+#!/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]}'
--- /dev/null
+#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
--- /dev/null
+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
--- /dev/null
+#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
--- /dev/null
+#include "ros/ros.h"
+
+#include <Eigen/Dense>
+
+#include "arc_odometry/diff_odometry.h"
+#include "twil_msgs/robot_dynamics.h"
+
+#include "sensor_msgs/JointState.h"
+
+#include <cmath>
+
+//#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<double> 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<double>(2))
+{
+ // Publishers and Subscribers
+ dynParamPublisher=node_.advertise<twil_msgs::robot_dynamics>("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;
+}
*******************************************************************************/
+#include <sys/mman.h>
+
#include <twil_controllers/cart_linearizing_controller.h>
#include <pluginlib/class_list_macros.h>
#include <kdl_parser/kdl_parser.hpp>
namespace twil_controllers
{
-CartLinearizingController::CartLinearizingController(void):
- WheelRadius(2){}
+CartLinearizingController::CartLinearizingController(void)
+ : WheelRadius(2), odom_(0.0,std::vector<double>(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))
{
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;
} else ROS_ERROR("Failed to get param 'wheel_radius'");
#endif
+ odom_.setParams(WheelBase,WheelRadius);
+ odom_.publisherConfig(node_);
+
/*
* Dynamics Controller Parameters
* ---------------------------------------------------------------------------------------------------------------------
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)
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;
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()
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
}
--- /dev/null
+/******************************************************************************
+ Twil Controllers
+ Linearizing MRAC Backstep Controller
+
+ Copyright (C) 2017 Tiago G. Alves <talves@ece.ufrgs.br>
+
+ 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 <http://www.gnu.org/licenses>.
+
+*******************************************************************************/
+
+#include <sys/mman.h>
+
+#include "twil_controllers/dynamics_mrac_backstepping_controller.h"
+#include <pluginlib/class_list_macros.h>
+#include <kdl_parser/kdl_parser.hpp>
+#include <tf/tf.h>
+
+//#define URDF
+//#define CONTROLLER_OUTPUT_SCREEN
+
+namespace twil_controllers
+{
+
+DynamicsMRACBacksteppingController::DynamicsMRACBacksteppingController()
+ : WheelRadius(2), odom_(0.0,std::vector<double>(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<twil_msgs::PoseControllerStatus>(node_,"status",1));
+ status_publisher_->msg_.header.frame_id=odom_frame_id;
+
+ /*
+ * Velocity MRAC Controller Parameters
+ * ---------------------------------------------------------------------------------------------------------------------
+ */
+
+ std::vector<double> AlphaVec;
+ if(!node_.getParam("Alpha",AlphaVec))
+ {
+ ROS_ERROR("No 'Alpha' in controller %s.",node_.getNamespace().c_str());
+ return false;
+ }
+ Alpha_.diagonal()=Eigen::Map<Eigen::Vector2d>(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<double> 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)
+
*******************************************************************************/
+#include <sys/mman.h>
+
#include "twil_controllers/dynamics_pid_backstepping_controller.h"
#include <pluginlib/class_list_macros.h>
#include <kdl_parser/kdl_parser.hpp>
{
DynamicsPIDBacksteppingController::DynamicsPIDBacksteppingController()
-{
-
-}
+ : WheelRadius(2), odom_(0.0,std::vector<double>(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;
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;
} 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<twil_msgs::PoseControllerStatus>(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;
}
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;
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
* ---------------------------------------------------------------------------------------------------------------------
*/
- {
- // 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<double> 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)
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;
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;
}
#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();
}
* ---------------------------------------------------------------------------------------------------------------------
*/
- 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]);
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 &)
#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,
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 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)
*******************************************************************************/
+#include <sys/mman.h>
+
#include <twil_controllers/dynamics_pid_controller.h>
#include <pluginlib/class_list_macros.h>
#include <kdl_parser/kdl_parser.hpp>
namespace twil_controllers
{
-DynamicsPIDController::DynamicsPIDController(void):
- WheelRadius(2){}
+DynamicsPIDController::DynamicsPIDController(void)
+ : WheelRadius(2), odom_(0.0,std::vector<double>(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))
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;
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<twil_msgs::VelocityControllerStatus>(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;
}
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)){
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;
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;
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]);
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)
#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)
--- /dev/null
+#include <twil_controllers/joint_group_voltage_controller.h>
+#include <pluginlib/class_list_macros.h>
+
+namespace twil_controllers
+{
+template <class T>
+void twil_controllers::ForwardJointGroupCommandController<T>::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)
--- /dev/null
+#include <twil_controllers/joint_voltage_controller.h>
+#include <pluginlib/class_list_macros.h>
+
+namespace twil_controllers {
+
+template <class T>
+
+void twil_controllers::ForwardCommandController<T>::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)
-#include "twil_controllers/nonlinear_backstepping_controller.h"
+///******************************************************************************
+// Twil Controllers
+// Linearizing Backstep Controller
+
+// Copyright (C) 2017 Tiago G. Alves <talves@ece.ufrgs.br>
+
+// 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 <http://www.gnu.org/licenses>.
+
+//*******************************************************************************/
+
+//#include "twil_controllers/nonlinear_backstepping_controller.h"
+//#include <pluginlib/class_list_macros.h>
+//#include <kdl_parser/kdl_parser.hpp>
+//#include <tf/tf.h>
+
+////#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)
--- /dev/null
+/******************************************************************************
+ Twil Controllers
+ State Feedback Linearization
+
+ Copyright (C) 2017 Tiago G. Alves <talves@ece.ufrgs.br>
+
+ 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 <http://www.gnu.org/licenses>.
+
+*******************************************************************************/
+#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) {}
+
+
*******************************************************************************/
+#include <sys/mman.h>
+
#include "twil_controllers/twist_pid_backstepping_controller.h"
#include <pluginlib/class_list_macros.h>
#include <kdl_parser/kdl_parser.hpp>
#include <tf/tf.h>
+#include "twil_controllers/velocity_transformation.h"
//#define URDF
//#define CONTROLLER_OUTPUT_SCREEN
{
TwistPIDBacksteppingController::TwistPIDBacksteppingController()
-{
-
-}
+ : WheelRadius(2), odom_(0.0,std::vector<double>(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;
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;
} 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<twil_msgs::PoseControllerStatus>(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;
}
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;
* ---------------------------------------------------------------------------------------------------------------------
*/
- // 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];
gamma[1] = non_smooth_parameters[4];
update_ratio = non_smooth_parameters[5];
- Kinematics = new NonSmoothControl(lambda,gamma);
+ std::vector<double> 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)
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;
}
#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();
}
/* ---------------------------------------------------------------------------------------------------------------------
* ---------------------------------------------------------------------------------------------------------------------
*/
- 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]);
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,
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)
*******************************************************************************/
+#include <sys/mman.h>
+
#include "twil_controllers/twist_pid_controller.h"
#include <pluginlib/class_list_macros.h>
#include <kdl_parser/kdl_parser.hpp>
+#include "twil_controllers/velocity_transformation.h"
//#define URDF
//#define CONTROLLER_OUTPUT_SCREEN
namespace twil_controllers
{
TwistPIDController::TwistPIDController(void)
- : WheelRadius(2) {}
+ : WheelRadius(2), odom_(0.0,std::vector<double>(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))
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;
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<twil_msgs::VelocityControllerStatus>(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;
}
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)){
}
#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;
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]);
#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()
#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
</description>
</class>
-<class name="twil_controllers/NonlinearBacksteppingController" type="twil_controllers::NonlinearBacksteppingController" base_class_type="controller_interface::ControllerBase">
+ <class name="twil_controllers/DynamicsMRACBacksteppingController" type="twil_controllers::DynamicsMRACBacksteppingController" base_class_type="controller_interface::ControllerBase">
+ <description>
+ Blá-blá-blá...
+ </description>
+ </class>
+
+ <class name="twil_controllers/NonlinearBacksteppingController" type="twil_controllers::NonlinearBacksteppingController" base_class_type="controller_interface::ControllerBase">
+ <description>
+ Blá-blá-blá...
+ </description>
+ </class>
+
+<class name="twil_controllers/JointVoltageController" type="twil_controllers::JointVoltageController" base_class_type="controller_interface::ControllerBase">
+ <description>
+ Blá-blá-blá...
+ </description>
+</class>
+
+<class name="twil_controllers/JointGroupVoltageController" type="twil_controllers::JointGroupVoltageController" base_class_type="controller_interface::ControllerBase">
<description>
- Blá-blá-blá...
+ Blá-blá-blá...
</description>
</class>
--- /dev/null
+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
+)
--- /dev/null
+#ifndef TWIL_ROBOT_HW_H
+#define TWIL_ROBOT_HW_H
+
+#include <ros/ros.h>
+#include <hardware_interface/joint_state_interface.h>
+#include <hardware_interface/robot_hw.h>
+
+#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
--- /dev/null
+<?xml version="1.0"?>
+<package>
+ <name>twil_hardware</name>
+ <version>0.0.0</version>
+ <description>The twil_hw package</description>
+
+ <!-- One maintainer tag required, multiple allowed, one person per tag -->
+ <!-- Example: -->
+ <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+ <maintainer email="talves@todo.todo">talves</maintainer>
+
+
+ <!-- One license tag required, multiple allowed, one license per tag -->
+ <!-- Commonly used license strings: -->
+ <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+ <license>TODO</license>
+
+
+ <!-- Url tags are optional, but multiple are allowed, one per tag -->
+ <!-- Optional attribute type can be: website, bugtracker, or repository -->
+ <!-- Example: -->
+ <!-- <url type="website">http://wiki.ros.org/twil_hw</url> -->
+
+
+ <!-- Author tags are optional, multiple are allowed, one per tag -->
+ <!-- Authors do not have to be maintainers, but could be -->
+ <!-- Example: -->
+ <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+ <!-- The *_depend tags are used to specify dependencies -->
+ <!-- Dependencies can be catkin packages or system dependencies -->
+ <!-- Examples: -->
+ <!-- Use build_depend for packages you need at compile time: -->
+ <!-- <build_depend>message_generation</build_depend> -->
+ <!-- Use buildtool_depend for build tool packages: -->
+ <!-- <buildtool_depend>catkin</buildtool_depend> -->
+ <!-- Use run_depend for packages you need at runtime: -->
+ <!-- <run_depend>message_runtime</run_depend> -->
+ <!-- Use test_depend for packages you need only for testing: -->
+ <!-- <test_depend>gtest</test_depend> -->
+ <buildtool_depend>catkin</buildtool_depend>
+ <build_depend>controller_interface</build_depend>
+ <build_depend>controller_manager</build_depend>
+ <build_depend>hardware_interface</build_depend>
+ <build_depend>joint_state_controller</build_depend>
+ <build_depend>twil_hardware_interfaces</build_depend>
+
+ <build_depend>roscpp</build_depend>
+ <build_depend>rospy</build_depend>
+ <run_depend>controller_interface</run_depend>
+ <run_depend>controller_manager</run_depend>
+ <run_depend>hardware_interface</run_depend>
+ <run_depend>twil_hardware_interfaces</run_depend>
+ <run_depend>joint_state_controller</run_depend>
+ <run_depend>roscpp</run_depend>
+ <run_depend>rospy</run_depend>
+
+
+ <!-- The export tag contains other, unspecified, tags -->
+ <export>
+ <!-- Other tools can request additional information be placed here -->
+
+ </export>
+</package>
--- /dev/null
+#!/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"
--- /dev/null
+#include <ros/ros.h>
+#include <std_msgs/Duration.h>
+#include <controller_manager/controller_manager.h>
+#include <realtime_tools/realtime_publisher.h>
+#include <boost/scoped_ptr.hpp>
+#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<std_msgs::Duration> >sampling_time_pub;
+ sampling_time_pub.reset(new realtime_tools::RealtimePublisher<std_msgs::Duration>(nh,"sampling_time",1));
+#endif
+
+#ifdef CONTROL_TIME_DEBUG
+ boost::scoped_ptr< realtime_tools::RealtimePublisher<std_msgs::Duration> >control_time_pub;
+ control_time_pub.reset(new realtime_tools::RealtimePublisher<std_msgs::Duration>(nh,"control_computation_time",1));
+#endif
+
+#ifdef LOOP_TIME_DEBUG
+ boost::scoped_ptr< realtime_tools::RealtimePublisher<std_msgs::Duration> >loop_time_pub;
+ loop_time_pub.reset(new realtime_tools::RealtimePublisher<std_msgs::Duration>(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;
+}
--- /dev/null
+#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
+
+}
+
+}
--- /dev/null
+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)
--- /dev/null
+#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
--- /dev/null
+#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
--- /dev/null
+<?xml version="1.0"?>
+<package format="2">
+ <name>twil_hardware_interfaces</name>
+ <version>0.0.0</version>
+ <description>The twil_hardware_interfaces package</description>
+
+ <!-- One maintainer tag required, multiple allowed, one person per tag -->
+ <!-- Example: -->
+ <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+ <maintainer email="talves@todo.todo">talves</maintainer>
+
+
+ <!-- One license tag required, multiple allowed, one license per tag -->
+ <!-- Commonly used license strings: -->
+ <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+ <license>TODO</license>
+
+
+ <!-- Url tags are optional, but multiple are allowed, one per tag -->
+ <!-- Optional attribute type can be: website, bugtracker, or repository -->
+ <!-- Example: -->
+ <!-- <url type="website">http://wiki.ros.org/twil_hardware_interfaces</url> -->
+
+
+ <!-- Author tags are optional, multiple are allowed, one per tag -->
+ <!-- Authors do not have to be maintainers, but could be -->
+ <!-- Example: -->
+ <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+ <!-- The *depend tags are used to specify dependencies -->
+ <!-- Dependencies can be catkin packages or system dependencies -->
+ <!-- Examples: -->
+ <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+ <!-- <depend>roscpp</depend> -->
+ <!-- Note that this is equivalent to the following: -->
+ <!-- <build_depend>roscpp</build_depend> -->
+ <!-- <exec_depend>roscpp</exec_depend> -->
+ <!-- Use build_depend for packages you need at compile time: -->
+ <!-- <build_depend>message_generation</build_depend> -->
+ <!-- Use build_export_depend for packages you need in order to build against this package: -->
+ <!-- <build_export_depend>message_generation</build_export_depend> -->
+ <!-- Use buildtool_depend for build tool packages: -->
+ <!-- <buildtool_depend>catkin</buildtool_depend> -->
+ <!-- Use exec_depend for packages you need at runtime: -->
+ <!-- <exec_depend>message_runtime</exec_depend> -->
+ <!-- Use test_depend for packages you need only for testing: -->
+ <!-- <test_depend>gtest</test_depend> -->
+ <!-- Use doc_depend for packages you need only for building documentation: -->
+ <!-- <doc_depend>doxygen</doc_depend> -->
+ <buildtool_depend>catkin</buildtool_depend>
+ <build_depend>hardware_interface</build_depend>
+ <build_depend>roscpp</build_depend>
+ <build_depend>rospy</build_depend>
+ <build_export_depend>hardware_interface</build_export_depend>
+ <build_export_depend>roscpp</build_export_depend>
+ <build_export_depend>rospy</build_export_depend>
+ <exec_depend>hardware_interface</exec_depend>
+ <exec_depend>roscpp</exec_depend>
+ <exec_depend>rospy</exec_depend>
+
+
+ <!-- The export tag contains other, unspecified, tags -->
+ <export>
+ <!-- Other tools can request additional information be placed here -->
+
+ </export>
+</package>
--- /dev/null
+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)
--- /dev/null
+<launch>
+ <arg name="paused" default="true"/>
+ <arg name="headless" default="false"/>
+ <arg name="use_sim_time" default="true"/>
+
+ <remap from="/left_wheel_joint_effort_controller/command" to="/left_wheel_command"/>
+ <remap from="/right_wheel_joint_effort_controller/command" to="/right_wheel_command"/>
+
+ <remap from="/ident/left_wheel_command" to="/left_wheel_command"/>
+ <remap from="/ident/right_wheel_command" to="/right_wheel_command"/>
+
+ <include file="$(find twil_description)/launch/gazebo.launch" >
+ <arg name="paused" value="$(arg paused)"/>
+ <arg name="headless" value="$(arg headless)"/>
+ <arg name="use_sim_time" value="$(arg use_sim_time)"/>
+ </include>
+
+ <include file="$(find twil_ident)/launch/ident.launch"/>
+</launch>
--- /dev/null
+<launch>
+ <include file="$(find twil_controllers)/launch/joint_effort.launch" />
+
+ <node name="ident" pkg="twil_ident" type="ident" output="screen" />
+</launch>
--- /dev/null
+<?xml version="1.0"?>
+<package>
+ <name>twil_ident</name>
+ <version>2.0.0</version>
+ <description>The twil_ident package</description>
+
+ <!-- One maintainer tag required, multiple allowed, one person per tag -->
+ <!-- Example: -->
+ <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+ <maintainer email="fetter@ece.ufrgs.br">Walter Fetter Lages</maintainer>
+
+
+ <!-- One license tag required, multiple allowed, one license per tag -->
+ <!-- Commonly used license strings: -->
+ <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+ <license>GPLv3</license>
+
+
+ <!-- Url tags are optional, but mutiple are allowed, one per tag -->
+ <!-- Optional attribute type can be: website, bugtracker, or repository -->
+ <!-- Example: -->
+ <!-- <url type="website">http://wiki.ros.org/twil_ident</url> -->
+
+
+ <!-- Author tags are optional, mutiple are allowed, one per tag -->
+ <!-- Authors do not have to be maintianers, but could be -->
+ <!-- Example: -->
+ <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+ <author email="fetter@ece.ufrgs.br">Walter Fetter Lages</author>
+
+
+ <!-- The *_depend tags are used to specify dependencies -->
+ <!-- Dependencies can be catkin packages or system dependencies -->
+ <!-- Examples: -->
+ <!-- Use build_depend for packages you need at compile time: -->
+ <!-- <build_depend>message_generation</build_depend> -->
+ <!-- Use buildtool_depend for build tool packages: -->
+ <!-- <buildtool_depend>catkin</buildtool_depend> -->
+ <!-- Use run_depend for packages you need at runtime: -->
+ <!-- <run_depend>message_runtime</run_depend> -->
+ <!-- Use test_depend for packages you need only for testing: -->
+ <!-- <test_depend>gtest</test_depend> -->
+ <buildtool_depend>catkin</buildtool_depend>
+ <build_depend>twil_msgs</build_depend>
+ <build_depend>eigen</build_depend>
+ <build_depend>kdl_parser</build_depend>
+ <run_depend>twil_msgs</run_depend>
+ <run_depend>eigen</run_depend>
+ <run_depend>kdl_parser</run_depend>
+
+
+ <!-- The export tag contains other, unspecified, tags -->
+ <export>
+ <!-- You can specify that this package is a metapackage here: -->
+ <!-- <metapackage/> -->
+
+ <!-- Other tools can request additional information be placed here -->
+
+ </export>
+</package>
--- /dev/null
+/******************************************************************************
+ ROS twil_indet Package
+ Twil Dynamics Model
+ Copyright (C) 2014..2017 Walter Fetter Lages <w.fetter@ieee.org>
+
+ 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
+ <http://www.gnu.org/licenses/>.
+
+*******************************************************************************/
+
+#include <ros/ros.h>
+
+#include <Eigen/Dense>
+
+#include <std_msgs/Float64.h>
+#include <std_msgs/Float64MultiArray.h>
+#include <sensor_msgs/JointState.h>
+
+#include <kdl_parser/kdl_parser.hpp>
+#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> prbs;
+ int iter;
+
+ ros::Time lastTime;
+
+ std::vector<double> 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<std_msgs::Float64MultiArray>("ident/dynamic_parameters",1000);
+ dynParamPublisher=node_.advertise<twil_msgs::robot_dynamics>("ident/dynamic_parameters",1000);
+ leftWheelCommandPublisher=node_.advertise<std_msgs::Float64>("ident/left_wheel_command",1000);
+ rightWheelCommandPublisher=node_.advertise<std_msgs::Float64>("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;
+}
--- /dev/null
+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
+ )
--- /dev/null
+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
--- /dev/null
+float64 range
+float64 angle
+float64 orientation
--- /dev/null
+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
--- /dev/null
+float64[3] lambda
+float64[2] gamma
+float64[2] eps
--- /dev/null
+Header header
+float64[6] parameters
+float64[6] variances
+float64[2] error
--- /dev/null
+<?xml version="1.0"?>
+<package format="2">
+ <name>twil_msgs</name>
+ <version>0.0.0</version>
+ <description>The twil_msgs package</description>
+
+ <!-- One maintainer tag required, multiple allowed, one person per tag -->
+ <!-- Example: -->
+ <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+ <maintainer email="talves@todo.todo">talves</maintainer>
+
+
+ <!-- One license tag required, multiple allowed, one license per tag -->
+ <!-- Commonly used license strings: -->
+ <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+ <license>TODO</license>
+
+
+ <!-- Url tags are optional, but multiple are allowed, one per tag -->
+ <!-- Optional attribute type can be: website, bugtracker, or repository -->
+ <!-- Example: -->
+ <!-- <url type="website">http://wiki.ros.org/twil_msgs</url> -->
+
+
+ <!-- Author tags are optional, multiple are allowed, one per tag -->
+ <!-- Authors do not have to be maintainers, but could be -->
+ <!-- Example: -->
+ <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+ <!-- The *depend tags are used to specify dependencies -->
+ <!-- Dependencies can be catkin packages or system dependencies -->
+ <!-- Examples: -->
+ <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+ <!-- <depend>roscpp</depend> -->
+ <!-- Note that this is equivalent to the following: -->
+ <!-- <build_depend>roscpp</build_depend> -->
+ <!-- <exec_depend>roscpp</exec_depend> -->
+ <!-- Use build_depend for packages you need at compile time: -->
+ <!-- <build_depend>message_generation</build_depend> -->
+ <!-- Use build_export_depend for packages you need in order to build against this package: -->
+ <!-- <build_export_depend>message_generation</build_export_depend> -->
+ <!-- Use buildtool_depend for build tool packages: -->
+ <!-- <buildtool_depend>catkin</buildtool_depend> -->
+ <!-- Use exec_depend for packages you need at runtime: -->
+ <!-- <exec_depend>message_runtime</exec_depend> -->
+ <!-- Use test_depend for packages you need only for testing: -->
+ <!-- <test_depend>gtest</test_depend> -->
+ <!-- Use doc_depend for packages you need only for building documentation: -->
+ <!-- <doc_depend>doxygen</doc_depend> -->
+ <buildtool_depend>catkin</buildtool_depend>
+ <build_depend>geometry_msgs</build_depend>
+ <build_depend>message_generation</build_depend>
+ <build_depend>nav_msgs</build_depend>
+ <build_depend>roscpp</build_depend>
+ <build_depend>std_msgs</build_depend>
+ <build_depend>trajectory_msgs</build_depend>
+ <build_export_depend>geometry_msgs</build_export_depend>
+ <build_export_depend>nav_msgs</build_export_depend>
+ <build_export_depend>roscpp</build_export_depend>
+ <build_export_depend>std_msgs</build_export_depend>
+ <build_export_depend>trajectory_msgs</build_export_depend>
+ <exec_depend>geometry_msgs</exec_depend>
+ <exec_depend>message_runtime</exec_depend>
+ <exec_depend>nav_msgs</exec_depend>
+ <exec_depend>roscpp</exec_depend>
+ <exec_depend>std_msgs</exec_depend>
+ <exec_depend>trajectory_msgs</exec_depend>
+
+
+ <!-- The export tag contains other, unspecified, tags -->
+ <export>
+ <!-- Other tools can request additional information be placed here -->
+
+ </export>
+</package>
## 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.)
ros::NodeHandle node_;
const EightPath *path;
- ros::Publisher commandPublisher,commandPublisher2;
+ ros::Publisher commandPublisher1,commandPublisher2, commandPublisher3;
};
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<geometry_msgs::Pose2D>("/"+robot_config+"/twist_pid_backstepping_controller/cmd_pos2d",1);
+ commandPublisher1=node_.advertise<geometry_msgs::Pose2D>("/"+robot_config+"/twist_pid_backstepping_controller/cmd_pos2d",1);
commandPublisher2=node_.advertise<geometry_msgs::Pose2D>("/"+robot_config+"/dynamics_pid_backstepping_controller/cmd_pos2d",1);
-
+ commandPublisher3=node_.advertise<geometry_msgs::Pose2D>("/"+robot_config+"/dynamics_mrac_backstepping_controller/cmd_pos2d",1);
+
path=new EightPath(pc,r,w);
}
EightTrajectory::~EightTrajectory(void)
{
- commandPublisher.shutdown();
+ commandPublisher1.shutdown();
delete path;
}
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[])
projects / twil.git / commitdiff