From: Walter Fetter Lages Date: Sun, 6 May 2018 03:50:37 +0000 (-0300) Subject: 2018-03-01 X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=11e628300485e19323abd5e7544499af8483429f;p=twil.git 2018-03-01 --- 11e628300485e19323abd5e7544499af8483429f diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..c1edc29 --- /dev/null +++ b/.gitignore @@ -0,0 +1,51 @@ +devel/ +logs/ +build/ +bin/ +lib/ +msg_gen/ +srv_gen/ +msg/*Action.msg +msg/*ActionFeedback.msg +msg/*ActionGoal.msg +msg/*ActionResult.msg +msg/*Feedback.msg +msg/*Goal.msg +msg/*Result.msg +msg/_*.py +build_isolated/ +devel_isolated/ + +# Generated by dynamic reconfigure +*.cfgc +/cfg/cpp/ +/cfg/*.py + +# Ignore generated docs +#*.dox +*.wikidoc + +# eclipse stuff +.project +.cproject + +# qcreator stuff +CMakeLists.txt.user + +srv/_*.py +*.pcd +*.pyc +qtcreator-* +*.user + +/planning/cfg +/planning/docs +/planning/src + +*~ + +# Emacs +.#* + +# Catkin custom files +CATKIN_IGNORE diff --git a/twil/CMakeLists.txt b/twil/CMakeLists.txt new file mode 100644 index 0000000..ccd3cdb --- /dev/null +++ b/twil/CMakeLists.txt @@ -0,0 +1,4 @@ +cmake_minimum_required(VERSION 2.8.3) +project(twil) +find_package(catkin REQUIRED) +catkin_metapackage() diff --git a/twil/package.xml b/twil/package.xml new file mode 100644 index 0000000..9724f87 --- /dev/null +++ b/twil/package.xml @@ -0,0 +1,59 @@ + + + twil + 2.0.0 + The twil package + + + + + Walter Fetter Lages + + + + + + GPLv3 + + + + + + + http://www.ece.ufrgs.br/~fetter/twil + + + + + + Walter Fetter Lages + + + + + + + + + + + + + catkin + + twil_description + twil_bringup + twil_controllers + twil_hw + + + + + + + + + + + + diff --git a/twil_bringup/CMakeLists.txt b/twil_bringup/CMakeLists.txt new file mode 100644 index 0000000..2a7e313 --- /dev/null +++ b/twil_bringup/CMakeLists.txt @@ -0,0 +1,74 @@ +cmake_minimum_required(VERSION 2.8.3) +project(twil_bringup) + +add_definitions(-std=c++11) + +## Find catkin macros and libraries +find_package(catkin REQUIRED COMPONENTS + controller_interface + controller_manager + hardware_interface + joint_state_controller + roscpp + rospy + std_msgs + sensor_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 specific configuration ## +################################### +catkin_package( + INCLUDE_DIRS include + LIBRARIES twil_bringup + CATKIN_DEPENDS controller_interface controller_manager hardware_interface joint_state_controller roscpp rospy std_msgs + DEPENDS system_lib + ) + +########### +## Build ## +########### + +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${twil_hw_INCLUDE_DIRS} + ${AIC_LIB_ROOT_DIR}/include + ) + +add_executable(twil_joystick_node src/joy.cpp) +target_link_libraries(twil_joystick_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 twil_joystick_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 +# ) diff --git a/twil_bringup/config/controllers/mini_twil_dynamics_pid_backstepping_control.yaml b/twil_bringup/config/controllers/mini_twil_dynamics_pid_backstepping_control.yaml new file mode 100644 index 0000000..6070cec --- /dev/null +++ b/twil_bringup/config/controllers/mini_twil_dynamics_pid_backstepping_control.yaml @@ -0,0 +1,47 @@ +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: -7.969445e+00 + B: -1.876616e-02 + C: 3.454858e-01 + D: -9.405481e+00 + E: -5.987630e+00 + F: 3.472006e+00 + + 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 + + take: 0 + + nonsmooth_controller: + lambda1: 0 + lambda2: 1 + lambda3: 0.5 + gamma1: 1 + gamma2: 1 + update_ratio: 1 + + x_ini: 0.0 + y_ini: 0.0 + th_ini: 0.0 diff --git a/twil_bringup/config/controllers/mini_twil_dynamics_pid_control.yaml b/twil_bringup/config/controllers/mini_twil_dynamics_pid_control.yaml new file mode 100644 index 0000000..a4b4084 --- /dev/null +++ b/twil_bringup/config/controllers/mini_twil_dynamics_pid_control.yaml @@ -0,0 +1,37 @@ +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: -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 + + linear_velocity_pid_parameters: + p: 16 + i: 64 + d: 0.0 + i_clamp: 12000 + antiwindup: false + + angular_velocity_pid_parameters: + p: 16 + i: 64 + d: 0.0 + i_clamp: 12000 + antiwindup: false + + + diff --git a/twil_bringup/config/controllers/mini_twil_effort_control.yaml b/twil_bringup/config/controllers/mini_twil_effort_control.yaml new file mode 100644 index 0000000..4f77130 --- /dev/null +++ b/twil_bringup/config/controllers/mini_twil_effort_control.yaml @@ -0,0 +1,12 @@ +mini_twil: + joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 25 + + left_wheel_joint_effort_controller: + type: effort_controllers/JointEffortController + joint: left_wheel_joint + + right_wheel_joint_effort_controller: + type: effort_controllers/JointEffortController + joint: right_wheel_joint diff --git a/twil_bringup/config/controllers/mini_twil_joint_velocity_control.yaml b/twil_bringup/config/controllers/mini_twil_joint_velocity_control.yaml new file mode 100644 index 0000000..68491d1 --- /dev/null +++ b/twil_bringup/config/controllers/mini_twil_joint_velocity_control.yaml @@ -0,0 +1,27 @@ +mini_twil: + + joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 25 + + left_wheel_joint_velocity_controller: + type: effort_controllers/JointVelocityController + joint: left_wheel_joint + pid: + p: 0.5476848 + i: 4.457272 + d: 0.0 + i_clamp: 12000 + antiwindup: False + + right_wheel_joint_velocity_controller: + type: effort_controllers/JointVelocityController + joint: right_wheel_joint + pid: + p: 0.5147369 + i: 4.295548 + d: 0.0 + i_clamp: 12000 + antiwindup: False + + diff --git a/twil_bringup/config/controllers/mini_twil_linearizing_control.yaml b/twil_bringup/config/controllers/mini_twil_linearizing_control.yaml new file mode 100644 index 0000000..604ec8f --- /dev/null +++ b/twil_bringup/config/controllers/mini_twil_linearizing_control.yaml @@ -0,0 +1,21 @@ +mini_twil: + joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 25 + + cart_linearizing_controller: + type: twil_controllers/CartLinearizingController + joints: + - left_wheel_joint + - right_wheel_joint + + velocity_filter_taps: 5 + + 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 + diff --git a/twil_bringup/config/controllers/mini_twil_twist_pid_backstepping_control.yaml b/twil_bringup/config/controllers/mini_twil_twist_pid_backstepping_control.yaml new file mode 100644 index 0000000..29563b2 --- /dev/null +++ b/twil_bringup/config/controllers/mini_twil_twist_pid_backstepping_control.yaml @@ -0,0 +1,41 @@ +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: 2.455561e-01 + i: 1.814526e+00 + d: 0 + i_clamp: 12000 + antiwindup: true + + right_wheel_joint_pid_parameters: + p: 1.751695e-01 + i: 1.553672e+00 + d: 0 + i_clamp: 12000 + antiwindup: False + + velocity_filter_taps: 3 + take: 0 + + + nonsmooth_controller: + lambda1: 1000 + lambda2: 10 + lambda3: 0.1 + gamma1: 0.8 + gamma2: 0.8 + update_ratio: 1 + + x_ini: 0.0 + y_ini: 0.0 + th_ini: 0.0 diff --git a/twil_bringup/config/controllers/mini_twil_twist_pid_control.yaml b/twil_bringup/config/controllers/mini_twil_twist_pid_control.yaml new file mode 100644 index 0000000..c12f774 --- /dev/null +++ b/twil_bringup/config/controllers/mini_twil_twist_pid_control.yaml @@ -0,0 +1,26 @@ +mini_twil: + joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 25 + + twist_pid_controller: + type: twil_controllers/TwistPIDController + joints: + - left_wheel_joint + - right_wheel_joint + + velocity_filter_taps: 2 + + left_wheel_joint_pid_parameters: + p: 2.455561e-01 + i: 1.814526e+00 + d: 0 + i_clamp: 12000 + antiwindup: True + + right_wheel_joint_pid_parameters: + p: 1.751695e-01 + i: 1.553672e+00 + d: 0 + i_clamp: 12000 + antiwindup: True diff --git a/twil_bringup/config/controllers/twil_dynamics_pid_backstepping_control.yaml b/twil_bringup/config/controllers/twil_dynamics_pid_backstepping_control.yaml new file mode 100644 index 0000000..9724795 --- /dev/null +++ b/twil_bringup/config/controllers/twil_dynamics_pid_backstepping_control.yaml @@ -0,0 +1,47 @@ +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: -14.778419353034346 + B: 0.009650740074675 + C: 0.133440027500232 + D: -9.731786200206576 + E: -13.056880796631793 + F: 0.666386266954281 + + 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: 0 + + nonsmooth_controller: + lambda1: 1000 + lambda2: 10 + lambda3: 0.00001 + gamma1: 1 + gamma2: 1 + update_ratio: 1 + + x_ini: 0.0 + y_ini: 0.0 + th_ini: 0 diff --git a/twil_bringup/config/controllers/twil_dynamics_pid_control.yaml b/twil_bringup/config/controllers/twil_dynamics_pid_control.yaml new file mode 100644 index 0000000..3be6e84 --- /dev/null +++ b/twil_bringup/config/controllers/twil_dynamics_pid_control.yaml @@ -0,0 +1,37 @@ +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: -14.778419353034346 + B: 0.009650740074675 + C: 0.133440027500232 + D: -9.731786200206576 + E: -13.056880796631793 + F: 0.666386266954281 + + velocity_filter_taps: 10 + + linear_velocity_pid_parameters: + p: 16 + i: 64 + d: 0.0 + i_clamp: 120000 + antiwindup: True + + angular_velocity_pid_parameters: + p: 16 + i: 64 + d: 0.0 + i_clamp: 120000 + antiwindup: True + + + diff --git a/twil_bringup/config/controllers/twil_effort_control.yaml b/twil_bringup/config/controllers/twil_effort_control.yaml new file mode 100644 index 0000000..ffb0b43 --- /dev/null +++ b/twil_bringup/config/controllers/twil_effort_control.yaml @@ -0,0 +1,12 @@ +twil: + joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 100 + + left_wheel_joint_effort_controller: + type: effort_controllers/JointEffortController + joint: left_wheel_joint + + right_wheel_joint_effort_controller: + type: effort_controllers/JointEffortController + joint: right_wheel_joint diff --git a/twil_bringup/config/controllers/twil_joint_velocity_control.yaml b/twil_bringup/config/controllers/twil_joint_velocity_control.yaml new file mode 100644 index 0000000..1ec7199 --- /dev/null +++ b/twil_bringup/config/controllers/twil_joint_velocity_control.yaml @@ -0,0 +1,27 @@ +twil: + + joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 100 + + left_wheel_joint_velocity_controller: + type: effort_controllers/JointVelocityController + joint: left_wheel_joint + pid: + p: 4.622505e-01 + i: 6.617994e+01 + d: 0 + i_clamp: 120000 + antiwindup: false + + right_wheel_joint_velocity_controller: + type: effort_controllers/JointVelocityController + joint: right_wheel_joint + pid: + p: 1.158977e+00 + i: 7.266055e+01 + d: 0 + i_clamp: 120000 + antiwindup: false + + diff --git a/twil_bringup/config/controllers/twil_linearizing_control.yaml b/twil_bringup/config/controllers/twil_linearizing_control.yaml new file mode 100644 index 0000000..16d8ec7 --- /dev/null +++ b/twil_bringup/config/controllers/twil_linearizing_control.yaml @@ -0,0 +1,20 @@ +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 + + velocity_filter_taps: 5 + + feedback_linearization: + A: -14.778419353034346 + B: 0.009650740074675 + C: 0.133440027500232 + D: -9.731786200206576 + E: -13.056880796631793 + F: 0.666386266954281 diff --git a/twil_bringup/config/controllers/twil_twist_pid_backstepping_control.yaml b/twil_bringup/config/controllers/twil_twist_pid_backstepping_control.yaml new file mode 100644 index 0000000..9d5b20d --- /dev/null +++ b/twil_bringup/config/controllers/twil_twist_pid_backstepping_control.yaml @@ -0,0 +1,39 @@ +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: 4.622505e-01 + i: 6.617994e+01 + d: 0 + i_clamp: 120000 + antiwindup: false + + right_wheel_joint_pid_parameters: + p: 1.158977e+00 + i: 7.266055e+01 + d: 0 + i_clamp: 1000 + antiwindup: false + + take: 1 + + nonsmooth_controller: + lambda1: 1000 + lambda2: 10 + lambda3: 0.00001 + gamma1: 1 + gamma2: 1 + update_ratio: 1 + + x_ini: 0.0 + y_ini: 0.0 + th_ini: 0.0 diff --git a/twil_bringup/config/controllers/twil_twist_pid_control.yaml b/twil_bringup/config/controllers/twil_twist_pid_control.yaml new file mode 100644 index 0000000..bd48c43 --- /dev/null +++ b/twil_bringup/config/controllers/twil_twist_pid_control.yaml @@ -0,0 +1,27 @@ +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 + + velocity_filter_taps: 1 + + + left_wheel_joint_pid_parameters: + p: 4.622505e-01 + i: 6.617994e+01 + d: 0 + i_clamp: 120000 + antiwindup: False + + right_wheel_joint_pid_parameters: + p: 1.158977e+00 + i: 7.266055e+01 + d: 0 + i_clamp: 120000 + antiwindup: False diff --git a/twil_bringup/config/electronics/mini_twil_hw.yaml b/twil_bringup/config/electronics/mini_twil_hw.yaml new file mode 100644 index 0000000..d8465fd --- /dev/null +++ b/twil_bringup/config/electronics/mini_twil_hw.yaml @@ -0,0 +1,21 @@ +mini_twil: + + left_wheel_joint: + aic_com_device: 2 + can_bus_number: 0 + can_iface: 0 + can_id_number: 2 + dead_zone_compensation: False + neg_dead_zone: 0 + pos_dead_zone: 0 + + 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 + + control_loop_rate: 25 diff --git a/twil_bringup/config/electronics/twil_hw.yaml b/twil_bringup/config/electronics/twil_hw.yaml new file mode 100644 index 0000000..f034707 --- /dev/null +++ b/twil_bringup/config/electronics/twil_hw.yaml @@ -0,0 +1,20 @@ +twil: + + left_wheel_joint: + + aic_com_device: 1 + serial_port: /dev/ttyUSB0 + dead_zone_compensation: False + neg_dead_zone: -4.9 + 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 + + control_loop_rate: 100 + diff --git a/twil_bringup/config/mechanics/mini_twil_specs.yaml b/twil_bringup/config/mechanics/mini_twil_specs.yaml new file mode 100644 index 0000000..3b22b69 --- /dev/null +++ b/twil_bringup/config/mechanics/mini_twil_specs.yaml @@ -0,0 +1,4 @@ +mini_twil: + + wheel_radius: 0.0325 + robot_baseline: 0.198 diff --git a/twil_bringup/config/mechanics/twil_specs.yaml b/twil_bringup/config/mechanics/twil_specs.yaml new file mode 100644 index 0000000..1866cf6 --- /dev/null +++ b/twil_bringup/config/mechanics/twil_specs.yaml @@ -0,0 +1,4 @@ +twil: + + wheel_radius: 0.075 + robot_baseline: 0.285 diff --git a/twil_bringup/include/twil_bringup/joy.h b/twil_bringup/include/twil_bringup/joy.h new file mode 100644 index 0000000..2ddad45 --- /dev/null +++ b/twil_bringup/include/twil_bringup/joy.h @@ -0,0 +1,24 @@ +#ifndef JOY_H +#define JOY_H + +#include +#include + +class JoyHandle +{ +public: + JoyHandle(ros::NodeHandle nh); + +private: + void joyCallback(const sensor_msgs::Joy::ConstPtr& joy); + + ros::NodeHandle nh_; + + int linear_, angular_; + double l_scale_, a_scale_; + ros::Publisher vel_pub1,vel_pub2,vel_pub3,vel_pub4; + ros::Subscriber joy_sub_; + +}; + +#endif diff --git a/twil_bringup/launch/controllers/dynamics_pid_backstepping_controller.launch b/twil_bringup/launch/controllers/dynamics_pid_backstepping_controller.launch new file mode 100644 index 0000000..2028903 --- /dev/null +++ b/twil_bringup/launch/controllers/dynamics_pid_backstepping_controller.launch @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/twil_bringup/launch/controllers/dynamics_pid_controller.launch b/twil_bringup/launch/controllers/dynamics_pid_controller.launch new file mode 100644 index 0000000..30176da --- /dev/null +++ b/twil_bringup/launch/controllers/dynamics_pid_controller.launch @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/twil_bringup/launch/controllers/joint_effort_controller.launch b/twil_bringup/launch/controllers/joint_effort_controller.launch new file mode 100644 index 0000000..754b916 --- /dev/null +++ b/twil_bringup/launch/controllers/joint_effort_controller.launch @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/twil_bringup/launch/controllers/joint_velocity_controller.launch b/twil_bringup/launch/controllers/joint_velocity_controller.launch new file mode 100644 index 0000000..3de11de --- /dev/null +++ b/twil_bringup/launch/controllers/joint_velocity_controller.launch @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/twil_bringup/launch/controllers/linearizing_controller.launch b/twil_bringup/launch/controllers/linearizing_controller.launch new file mode 100644 index 0000000..bb1e5f8 --- /dev/null +++ b/twil_bringup/launch/controllers/linearizing_controller.launch @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/twil_bringup/launch/controllers/twist_pid_backstepping_controller.launch b/twil_bringup/launch/controllers/twist_pid_backstepping_controller.launch new file mode 100644 index 0000000..37dd661 --- /dev/null +++ b/twil_bringup/launch/controllers/twist_pid_backstepping_controller.launch @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + + + + + + + + diff --git a/twil_bringup/launch/controllers/twist_pid_controller.launch b/twil_bringup/launch/controllers/twist_pid_controller.launch new file mode 100644 index 0000000..1bdc7e6 --- /dev/null +++ b/twil_bringup/launch/controllers/twist_pid_controller.launch @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/twil_bringup/launch/joystick.launch b/twil_bringup/launch/joystick.launch new file mode 100644 index 0000000..dc772b3 --- /dev/null +++ b/twil_bringup/launch/joystick.launch @@ -0,0 +1,30 @@ + + + + + + + + + + + + + + + + + + + + + +--> + + + + + diff --git a/twil_bringup/launch/navigation/move_base.launch b/twil_bringup/launch/navigation/move_base.launch new file mode 100644 index 0000000..8079ec8 --- /dev/null +++ b/twil_bringup/launch/navigation/move_base.launch @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + diff --git a/twil_bringup/launch/robot_hw.launch b/twil_bringup/launch/robot_hw.launch new file mode 100644 index 0000000..01a5a1f --- /dev/null +++ b/twil_bringup/launch/robot_hw.launch @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/twil_bringup/launch/trajectory/mini_twil_circle_trajectory.launch b/twil_bringup/launch/trajectory/mini_twil_circle_trajectory.launch new file mode 100644 index 0000000..a1b197b --- /dev/null +++ b/twil_bringup/launch/trajectory/mini_twil_circle_trajectory.launch @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/twil_bringup/launch/trajectory/mini_twil_eight_trajectory.launch b/twil_bringup/launch/trajectory/mini_twil_eight_trajectory.launch new file mode 100644 index 0000000..9bbc561 --- /dev/null +++ b/twil_bringup/launch/trajectory/mini_twil_eight_trajectory.launch @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/twil_bringup/launch/trajectory/twil_circle_trajectory.launch b/twil_bringup/launch/trajectory/twil_circle_trajectory.launch new file mode 100644 index 0000000..31babb9 --- /dev/null +++ b/twil_bringup/launch/trajectory/twil_circle_trajectory.launch @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/twil_bringup/launch/trajectory/twil_eight_trajectory.launch b/twil_bringup/launch/trajectory/twil_eight_trajectory.launch new file mode 100644 index 0000000..1f14b76 --- /dev/null +++ b/twil_bringup/launch/trajectory/twil_eight_trajectory.launch @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + diff --git a/twil_bringup/package.xml b/twil_bringup/package.xml new file mode 100644 index 0000000..2c60d71 --- /dev/null +++ b/twil_bringup/package.xml @@ -0,0 +1,72 @@ + + + twil_bringup + 0.0.0 + The twil_bringup package + + + + + talves + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + controller_interface + controller_manager + hardware_interface + joint_state_controller + roscpp + rospy + std_msgs + twil_hw + sensor_msgs + trajectory_msgs + geometry_msgs + + controller_interface + controller_manager + hardware_interface + joint_state_controller + roscpp + rospy + std_msgs + twil_hw + sensor_msgs + trajectory_msgs + geometry_msgs + + + + + + + diff --git a/twil_bringup/src/joy.cpp b/twil_bringup/src/joy.cpp new file mode 100644 index 0000000..ad86efd --- /dev/null +++ b/twil_bringup/src/joy.cpp @@ -0,0 +1,50 @@ +#include +#include +#include +#include +#include +#include +#include "twil_bringup/joy.h" +#include + +JoyHandle::JoyHandle(ros::NodeHandle nh): + linear_(1), + angular_(2), + nh_(nh) +{ + nh_.param("axis_linear", linear_, linear_); + nh_.param("axis_angular", angular_, angular_); + nh_.param("scale_angular", a_scale_, a_scale_); + nh_.param("scale_linear", l_scale_, l_scale_); + + vel_pub1 = nh_.advertise("/twil/dynamics_pid_controller/cmd_vel", 1); + vel_pub2 = nh_.advertise("/twil/twist_pid_controller/cmd_vel", 1); + vel_pub3 = nh_.advertise("/mini_twil/dynamics_pid_controller/cmd_vel", 1); + vel_pub4 = nh_.advertise("/mini_twil/twist_pid_controller/cmd_vel", 1); + + + joy_sub_ = nh_.subscribe("joy", 1, &JoyHandle::joyCallback, this); +} + +void JoyHandle::joyCallback(const sensor_msgs::Joy::ConstPtr& joy) +{ + + // Twil + geometry_msgs::Twist twist; + twist.angular.z = a_scale_*joy->axes[angular_]; + twist.linear.x = l_scale_*joy->axes[linear_]; + vel_pub1.publish(twist); + vel_pub2.publish(twist); + vel_pub3.publish(twist); + vel_pub4.publish(twist); + std::cout << "Joystick SP [v, w]: [" << twist.linear.x << ", " << twist.angular.z << "]"<< std::endl; +} + + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "joy_node_handle"); + ros::NodeHandle nh; + JoyHandle Node(nh); + ros::spin(); +} diff --git a/twil_controllers/CMakeLists.txt b/twil_controllers/CMakeLists.txt new file mode 100644 index 0000000..a0fe050 --- /dev/null +++ b/twil_controllers/CMakeLists.txt @@ -0,0 +1,186 @@ +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 + ) + +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 ## +################################################ + +## 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 ${PROJECT_NAME} + CATKIN_DEPENDS controller_interface control_toolbox + # DEPENDS system_lib + 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} + ) + +## 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) + +## 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) + +## Specify libraries to link a library or executable target against +target_link_libraries(${PROJECT_NAME} + ${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} + ) + +## 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 twistd cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_twil_controllers.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/twil_controllers/README b/twil_controllers/README new file mode 100644 index 0000000..acd2807 --- /dev/null +++ b/twil_controllers/README @@ -0,0 +1,11 @@ +To publish with joint_effort_controller: + +rostopic pub /twil/left_wheel_joint_effort_controller/command std_msgs/Float64 "0.0" + +To publish with cart_linearizing_controller: + +rostopic pub /twil/cart_linearizing_controller/command std_msgs/Float64MultiArray "data: [0.1, 0.0]" + +To get time and pose: + +rostopic echo -p /gazebo/model_states | awk '{FS=","; printf("%g %g %g %g\n",$1/1e9,$11,$12,2*atan2(sqrt($14^2+$15^2+$16^2),$17))}' diff --git a/twil_controllers/include/twil_controllers/cart_linearizing_controller.h b/twil_controllers/include/twil_controllers/cart_linearizing_controller.h new file mode 100644 index 0000000..79c5a80 --- /dev/null +++ b/twil_controllers/include/twil_controllers/cart_linearizing_controller.h @@ -0,0 +1,84 @@ +/****************************************************************************** + Twil Controllers + Linearizing Controller + + Copyright (C) 2017 Tiago G. Alves + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program; if not, write to the Free Software + Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + + You can also obtain a copy of the GNU General Public License + at . + +*******************************************************************************/ + +#ifndef TWIL_CONTROLLERS_CART_LINEARIZING_CONTROLLER_H +#define TWIL_CONTROLLERS_CART_LINEARIZING_CONTROLLER_H + +#include +#include +#include +#include +#include +#include "Eigen/Dense" + +#include "state_feedback_linearization.h" +#include "velocity_transformation.h" + +#define CART_LIN_DEBUG + +#ifdef CART_LIN_DEBUG +#include +#endif + +namespace twil_controllers +{ +class CartLinearizingController: public controller_interface::Controller +{ +public: + CartLinearizingController(void); + ~CartLinearizingController(void); + + bool init(hardware_interface::EffortJointInterface *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); + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + +private: + ros::NodeHandle node_; + hardware_interface::EffortJointInterface *robot_; + std::vector joints_; + StateFeedbackLinearization * feedback_linearization; + std::string robot_ns; + + ros::Subscriber sub_command_; + + Eigen::Vector2d Effort; + Eigen::Vector2d nu_ref; + Mat2_4 Fu; + +#ifdef CART_LIN_DEBUG + std::ofstream fd; + bool makeLog; + std::string log_date; + int take; +#endif + + std::vector WheelRadius; + double WheelBase; +}; +} +#endif diff --git a/twil_controllers/include/twil_controllers/dynamics_pid_backstepping_controller.h b/twil_controllers/include/twil_controllers/dynamics_pid_backstepping_controller.h new file mode 100644 index 0000000..c969434 --- /dev/null +++ b/twil_controllers/include/twil_controllers/dynamics_pid_backstepping_controller.h @@ -0,0 +1,104 @@ +/****************************************************************************** + Twil Controllers + Linearizing Backstep Controller + + Copyright (C) 2017 Tiago G. Alves + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program; if not, write to the Free Software + Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + + You can also obtain a copy of the GNU General Public License + at . + +*******************************************************************************/ + +#ifndef DYNAMICS_PID_BACKSTEPPING_CONTROLLER_H +#define DYNAMICS_PID_BACKSTEPPING_CONTROLLER_H + +#include +#include +#include +#include +#include +#include +#include "twil_controllers/odometry_tool.h" +#include "twil_controllers/velocity_transformation.h" +#include "non_smooth_control.h" +#include "state_feedback_linearization.h" + +#define DYNAMICS_PID_BACKSTEPPING_CONTROLLER_DEBUG + +#ifdef DYNAMICS_PID_BACKSTEPPING_CONTROLLER_DEBUG +#include +#endif + + +namespace twil_controllers{ + +class DynamicsPIDBacksteppingController: public controller_interface::Controller +{ + +public: + DynamicsPIDBacksteppingController(); + ~DynamicsPIDBacksteppingController(); + bool init(hardware_interface::EffortJointInterface *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]; + control_toolbox::Pid PID[2]; + ros::NodeHandle node_; + hardware_interface::EffortJointInterface *robot_; + std::vector joints_; + std::string robot_ns; + + /* Dynamics PID Controller Variables */ + StateFeedbackLinearization * feedback_linearization; + Eigen::Vector2d Effort; + Eigen::Vector2d nu_ref; + Eigen::Vector2d u_ref; + Mat2_4 Fu; + + /* Nonlinear Kinematic Controller Variables */ + Eigen::Vector3d lambda; + Eigen::Vector2d gamma; + Eigen::Vector3d pose_ref; + OdometryTool * Odometer; + NonSmoothControl * Kinematics; + int update_ratio; + + + /* Model Variables */ + Eigen::Vector2d WheelRadius; + double WheelBase; + +#ifdef DYNAMICS_PID_BACKSTEPPING_CONTROLLER_DEBUG + std::ofstream fd; + bool makeLog; + std::string log_date; + int take; +#endif + +}; + +} + +#endif diff --git a/twil_controllers/include/twil_controllers/dynamics_pid_controller.h b/twil_controllers/include/twil_controllers/dynamics_pid_controller.h new file mode 100644 index 0000000..d70de47 --- /dev/null +++ b/twil_controllers/include/twil_controllers/dynamics_pid_controller.h @@ -0,0 +1,92 @@ +/****************************************************************************** + Twil Controllers + Dynamics PID Controller + + Copyright (C) 2017 Tiago G. Alves + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program; if not, write to the Free Software + Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + + You can also obtain a copy of the GNU General Public License + at . + +*******************************************************************************/ + +#ifndef DYNAMICS_PID_CONTROLLER_H +#define DYNAMICS_PID_CONTROLLER_H + +#include +#include +#include +#include +#include +#include +#include "velocity_transformation.h" +#include "state_feedback_linearization.h" + +#define DYNAMICS_PID_DEBUG + +#ifdef DYNAMICS_PID_DEBUG +#include +#endif + +namespace twil_controllers +{ +class DynamicsPIDController: public controller_interface::Controller +{ + +public: + DynamicsPIDController(void); + ~DynamicsPIDController(void); + + bool init(hardware_interface::EffortJointInterface *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); + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + + private: + + /* ROS Variables and Structures */ + ros::Subscriber VelocitySub; + control_toolbox::Pid PID[2]; + ros::NodeHandle node_; + hardware_interface::EffortJointInterface *robot_; + std::vector joints_; + std::string robot_ns; + + /* Dynamics Controller Variables */ + StateFeedbackLinearization * feedback_linearization; + Eigen::Vector2d Effort; + Eigen::Vector2d nu_ref; + Eigen::Vector2d u_ref; + + Mat2_4 Fu; + + + +#ifdef DYNAMICS_PID_DEBUG + std::ofstream fd; + bool makeLog; + std::string log_date; + int take; +#endif + + std::vector WheelRadius; + double WheelBase; +}; +} +#endif diff --git a/twil_controllers/include/twil_controllers/non_smooth_control.h b/twil_controllers/include/twil_controllers/non_smooth_control.h new file mode 100644 index 0000000..631c2a0 --- /dev/null +++ b/twil_controllers/include/twil_controllers/non_smooth_control.h @@ -0,0 +1,122 @@ +/****************************************************************************** + Twil Controllers + Nonlinear Kinematic Controller + + Copyright (C) 2017 Tiago G. Alves + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program; if not, write to the Free Software + Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + + You can also obtain a copy of the GNU General Public License + at . + +*******************************************************************************/ + +#ifndef NON_SMOOTH_CONTROL_H +#define NON_SMOOTH_CONTROL_H + +#include "Eigen/Dense" +#include +#include + +//#define NONSMOOTH_CONTROLLER_OUTPUT_SCREEN +class NonSmoothControl +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + NonSmoothControl(const Eigen::Vector3d &lambda, const Eigen::Vector2d &gamma) + : lamb(lambda), gam(gamma) { + std::cout << "Parametros [lb, gm]: "< 1e-6) ? + (gam[0]*(lamb[2]/lamb[1])*cos(alpha)*(sin(alpha)/alpha)*psi) : (gam[0]*(lamb[2]/lamb[1])*cos(alpha)*(1)*psi); + } + + if (std::isnan(u_r[0])) { + u_r[0] = 0; + std::cout << "u_r[0] generated a nan-value..." << std::endl; + } + + if (std::isnan(u_r[1])) { + u_r[1] = 0; + std::cout << "u_r[1] generated a nan-value..." << std::endl; + } + +#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; +#endif + return u_r; + } + + bool StopCondition(const Eigen::Vector3d &xc, const Eigen::Vector3d &x_r, double eps){ + return StopCondition(xc, x_r, eps, eps); + } + + 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 err_heading = pow((xc[2]-x_r[2]),2); + if(sqrt(err_heading)>=eps_pos) return false; + + return true; + } + +private: + + const Eigen::Vector3d coordinate_change(const Eigen::Vector3d &x, const Eigen::Vector3d &x_r){ + Eigen::Matrix3d rot; + rot << cos(x_r[2]), sin(x_r[2]), 0, + -sin(x_r[2]), cos(x_r[2]), 0, + 0, 0, 1; + Eigen::Vector3d x_bar = rot*(x-x_r); + +#ifdef NONSMOOTH_CONTROLLER_OUTPUT_SCREEN + // std::cout << "Rotation Matrix: " << rot < + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program; if not, write to the Free Software + Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + + You can also obtain a copy of the GNU General Public License + at . + +*******************************************************************************/ + +#ifndef ODOMETRY_TOOL_H +#define ODOMETRY_TOOL_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "twil_controllers/pose_tool.h" +#include + +namespace twil_controllers +{ +class OdometryTool +{ +public: + OdometryTool(); + ~OdometryTool(); + bool publish(); + bool init(ros::NodeHandle n, std::string topic_root, const Eigen::Vector3d &initial_state); + bool compute(ros::Duration duration, const Eigen::Vector2d &u); + Eigen::Vector3d getPose(); + void shutdown(); + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + +private: + + ros::NodeHandle Node; + geometry_msgs::PoseWithCovariance pose; + geometry_msgs::Pose2D pose2d; + Eigen::Vector2d u; + ros::Duration odometer_cycle; + ros::Time odometer_compute_time; + + boost::shared_ptr > tf_odom_pub; + boost::shared_ptr< realtime_tools::RealtimePublisher >odom_pub; +// boost::shared_ptr< realtime_tools::RealtimePublisher >pose_pub; +}; + + +} +#endif // ODOMETRY_H diff --git a/twil_controllers/include/twil_controllers/pose_tool.h b/twil_controllers/include/twil_controllers/pose_tool.h new file mode 100644 index 0000000..156dcd3 --- /dev/null +++ b/twil_controllers/include/twil_controllers/pose_tool.h @@ -0,0 +1,13 @@ +#ifndef POSE_TOOL_H +#define POSE_TOOL_H + +#include +#include +#include +#include + +//Eigen::Vector3d conv_pose_to_vector(geometry_msgs::PoseWithCovariance pose_); +//Eigen::Vector3d conv_pose2d_to_vector(geometry_msgs::Pose2D pose_); +//geometry_msgs::PoseWithCovariance conv_vector_to_pose(Eigen::Vector3d pose_); +//geometry_msgs::Pose2D conv_vector_to_pose2d(Eigen::Vector3d pose_); +#endif diff --git a/twil_controllers/include/twil_controllers/state_feedback_linearization.h b/twil_controllers/include/twil_controllers/state_feedback_linearization.h new file mode 100644 index 0000000..627aad5 --- /dev/null +++ b/twil_controllers/include/twil_controllers/state_feedback_linearization.h @@ -0,0 +1,89 @@ +/****************************************************************************** + Twil Controllers + State Feedback Linearization + + Copyright (C) 2017 Tiago G. Alves + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program; if not, write to the Free Software + Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + + You can also obtain a copy of the GNU General Public License + at . + +*******************************************************************************/ + + +#ifndef STATE_FEEDBACK_LINEARIZATION_H +#define STATE_FEEDBACK_LINEARIZATION_H + +#include +#include +#include "Eigen/Dense" + +typedef Eigen::Matrix Mat2_4; + +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; + } + + 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; + } + + ~StateFeedbackLinearization() {} + +private: + Eigen::Matrix2d Ginv; + Eigen::Matrix2d H_beta; + Eigen::Vector2d F_Beta_u; + Eigen::Vector2d v; + Eigen::Vector2d Tau_zero; +}; + +#endif diff --git a/twil_controllers/include/twil_controllers/twist_pid_backstepping_controller.h b/twil_controllers/include/twil_controllers/twist_pid_backstepping_controller.h new file mode 100644 index 0000000..bea6ac3 --- /dev/null +++ b/twil_controllers/include/twil_controllers/twist_pid_backstepping_controller.h @@ -0,0 +1,105 @@ +/****************************************************************************** + Twil Controllers + PID Backstepping Controller + + Copyright (C) 2017 Tiago G. Alves + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program; if not, write to the Free Software + Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + + You can also obtain a copy of the GNU General Public License + at . + +*******************************************************************************/ + +#ifndef TWIST_PID_BACKSTEPPING_CONTROLLER_H +#define TWIST_PID_BACKSTEPPING_CONTROLLER_H + +#include +#include +#include +#include +#include +#include +#include +#include "twil_controllers/odometry_tool.h" +#include "twil_controllers/pose_tool.h" +#include "twil_controllers/velocity_transformation.h" +#include "non_smooth_control.h" + + + +#define TWIST_PID_BACKSTEPPING_CONTROLLER_DEBUG + +#ifdef TWIST_PID_BACKSTEPPING_CONTROLLER_DEBUG +#include +#endif + +namespace twil_controllers{ + +class TwistPIDBacksteppingController: public controller_interface::Controller +{ + +public: + TwistPIDBacksteppingController(); + ~TwistPIDBacksteppingController(); + bool init(hardware_interface::EffortJointInterface *, ros::NodeHandle &); + 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]; + control_toolbox::Pid PID[2]; + ros::NodeHandle node_; + hardware_interface::EffortJointInterface *robot_; + std::vector joints_; + std::string robot_ns; + + + /* Twist PID Controller Variables */ + Eigen::Vector2d dphi_ref, dphi, Effort; + Eigen::Vector2d u_ref; + + /* Nonlinear Kinematic Controller Variables */ + Eigen::Vector3d lambda; + Eigen::Vector2d gamma; + Eigen::Vector3d pose_ref; + OdometryTool * Odometer; + NonSmoothControl * Kinematics; + int update_ratio; + + + /* Model Variables */ + Eigen::Vector2d WheelRadius; + double WheelBase; + +#ifdef TWIST_PID_BACKSTEPPING_CONTROLLER_DEBUG + std::ofstream fd; + bool makeLog; + std::string log_date; + int take; +#endif + +}; + +} + +#endif + diff --git a/twil_controllers/include/twil_controllers/twist_pid_controller.h b/twil_controllers/include/twil_controllers/twist_pid_controller.h new file mode 100644 index 0000000..a3d840d --- /dev/null +++ b/twil_controllers/include/twil_controllers/twist_pid_controller.h @@ -0,0 +1,90 @@ +/****************************************************************************** + Twil Controllers + Twist PID Controller + + Copyright (C) 2017 Tiago G. Alves + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program; if not, write to the Free Software + Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + + You can also obtain a copy of the GNU General Public License + at . + +*******************************************************************************/ + +#ifndef DYNAMICS_CONTROLLER_ +#define DYNAMICS_CONTROLLER_H + +#include +#include +#include +#include +#include +#include +#include "velocity_transformation.h" +#include "state_feedback_linearization.h" + +#define TWIST_PID_DEBUG + +#ifdef TWIST_PID_DEBUG +#include +#endif + +typedef Eigen::Matrix Mat2_4; + +namespace twil_controllers +{ +class TwistPIDController: public controller_interface::Controller +{ +public: + TwistPIDController(void); + ~TwistPIDController(void); + + bool init(hardware_interface::EffortJointInterface *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); + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + private: + + /* ROS Variables and Structures */ + ros::Subscriber VelocitySub; + control_toolbox::Pid PID[2]; + ros::NodeHandle node_; + hardware_interface::EffortJointInterface *robot_; + std::vector joints_; + std::string robot_ns; + + /* Velocity Controller Variables */ + Eigen::Vector2d dphi_ref, dphi, Effort; + Eigen::Vector2d u_ref; + + /* Model Variables */ + Eigen::Vector2d WheelRadius; + double WheelBase; + +#ifdef TWIST_PID_DEBUG + std::ofstream fd; + bool makeLog; + std::string log_date; + int take; +#endif + + +}; +} +#endif // DYNAMICS_CONTROLLER_H + diff --git a/twil_controllers/include/twil_controllers/velocity_transformation.h b/twil_controllers/include/twil_controllers/velocity_transformation.h new file mode 100644 index 0000000..a9ea53b --- /dev/null +++ b/twil_controllers/include/twil_controllers/velocity_transformation.h @@ -0,0 +1,91 @@ +/****************************************************************************** + Twil Controllers + Velocity Transformations + + Copyright (C) 2017 Tiago G. Alves + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program; if not, write to the Free Software + Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + + You can also obtain a copy of the GNU General Public License + at . + +*******************************************************************************/ + + +#ifndef VELOCITY_TRANSFORMATION_H +#define VELOCITY_TRANSFORMATION_H + +#include +#include +#include + +class VelocityTransformation +{ +public: + VelocityTransformation(); + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + static Eigen::Vector2d WheelToBase(const Eigen::Vector2d &JointVelocities, + double WheelBase, + double WheelRadius) + { + /** + * Input -> Joint Velocities + * dphi(0): Right Wheel + * dphi(1): Left Wheel + * + * Return -> Robot Base Velocities + * u(0): Linear Velocity (m/s) + * u(1): Angular Velocity (rd/s) + */ + + double b = WheelBase/2; // To maintain compatibility with Barros' formulation. + double c = WheelRadius/(2*b); + Eigen::Vector2d u, dphi; + u.setZero(); + dphi.setZero(); + dphi = JointVelocities; + u[0] = WheelRadius*(dphi[0]+dphi[1])/2; + u[1] = c*(dphi[0]-dphi[1]); + return u; + } + + static Eigen::Vector2d BaseToWheel(const Eigen::Vector2d &BaseVelocity, + double WheelBase, + double WheelRadius) + { + /** + * Input -> Base Velocities + * u(0): Linear Velocity (m/s) + * u(1): Angular Velocity (rd/s) + * + * Return -> Joint Velocities + * dphi(0): Right Wheel + * dphi(1): Left Wheel + */ + + double b = WheelBase/2; // To maintain compatibility with Barros' formulation. + double c = WheelRadius/(2*b); + Eigen::Vector2d dphi, u; + u.setZero(); + dphi.setZero(); + u = BaseVelocity; + dphi[0] = ( u[0]/WheelRadius )+( u[1]/(2*c) ); + dphi[1] = ( u[0]/WheelRadius )-( u[1]/(2*c) ); + return dphi; + } +}; + +#endif diff --git a/twil_controllers/package.xml b/twil_controllers/package.xml new file mode 100644 index 0000000..d644f67 --- /dev/null +++ b/twil_controllers/package.xml @@ -0,0 +1,77 @@ + + + twil_controllers + 2.0.0 + The twil_controllers package + + + + + Walter Fetter Lages + + + + + + GPLv3 + + + + + + + + + + + + + Walter Fetter Lages + + + + + + + + + + + + + + catkin + + controller_interface + effort_controllers + kdl_parser + orocos_kdl + twil_gazebo_ros_control + control_toolbox + + + cmake_modules + + + controller_interface + controller_manager + effort_controllers + joint_state_controller + kdl_parser + orocos_kdl + twil_gazebo_ros_control + control_toolbox + + cmake_modules + + + + + + + + + + + + diff --git a/twil_controllers/scripts/pub_array.sh b/twil_controllers/scripts/pub_array.sh new file mode 100755 index 0000000..5bc4ba5 --- /dev/null +++ b/twil_controllers/scripts/pub_array.sh @@ -0,0 +1 @@ +rostopic pub /twil/cart_linearizing_controller/command std_msgs/Float64MultiArray '{data: [0.01, 0.00]}' diff --git a/twil_controllers/src/cart_linearizing_controller.cpp b/twil_controllers/src/cart_linearizing_controller.cpp new file mode 100644 index 0000000..8f3acfc --- /dev/null +++ b/twil_controllers/src/cart_linearizing_controller.cpp @@ -0,0 +1,274 @@ +/****************************************************************************** + Twil Controllers + Linearizing Controller + + Copyright (C) 2017 Tiago G. Alves + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program; if not, write to the Free Software + Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + + You can also obtain a copy of the GNU General Public License + at . + +*******************************************************************************/ + +#include +#include +#include + +//#define URDF +//#define CONTROLLER_OUTPUT_SCREEN + +namespace twil_controllers +{ +CartLinearizingController::CartLinearizingController(void): + WheelRadius(2){} + +CartLinearizingController::~CartLinearizingController(void) { + sub_command_.~Subscriber(); + feedback_linearization->~StateFeedbackLinearization(); +} + +bool CartLinearizingController::init(hardware_interface::EffortJointInterface *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); + } + sub_command_ = node_.subscribe("command",1000,&CartLinearizingController::commandCB,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); + std::string full_param_name; + + // 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 + + /* + * Dynamics Controller Parameters + * --------------------------------------------------------------------------------------------------------------------- + */ + + if (!node_.getParam("/log_date",log_date)){ + ROS_ERROR("Parameter '%s/log_date' not found! The log will be disabled...",node_.getNamespace().c_str()); + log_date=""; + } + +#ifdef CART_LIN_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 + + 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 (n.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)); + return true; +} + +void CartLinearizingController::starting(const ros::Time& time) +{ + nu_ref.setZero(); + Effort.setZero(); + +#ifdef CART_LIN_DEBUG + if (makeLog == false) + { + std::string filename = "none"; + + if (robot_ns == "mini_twil") + { + filename = "/home/talves/Dropbox/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"; + ROS_INFO("Output File: %s", filename.c_str());; + fd.open(filename); + makeLog = true; + } + } +#endif +} + +void CartLinearizingController::update(const ros::Time& time, + const ros::Duration& duration) +{ +#ifdef CART_LIN_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; + } + k++; + } + } +#endif + + Eigen::Vector2d u, fu, joint_ang_vel; + Eigen::Vector4d uf; + + // 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]; + + u = VelocityTransformation::WheelToBase(dphi,WheelBase,WheelRadius[0]); + + // Compute linearization + uf << u[0], u[1], u[1]*u[0], u[1]*u[1]; + fu = -Fu*uf; + Effort = feedback_linearization->computeInputs(nu_ref,fu); + // Apply Control Efforts + joints_[0].setCommand(Effort[1]); + joints_[1].setCommand(Effort[0]); + +#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; + } + } +#endif +} + +void CartLinearizingController::stopping() +{ + +#ifdef CART_LIN_DEBUG + fd.close(); +#endif + 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]; + } +#ifdef CONTROLLER_OUTPUT_SCREEN + ROS_INFO("nu_ref: {%f, %f}", nu_ref[0], nu_ref[1]); +#endif +} + +} +PLUGINLIB_EXPORT_CLASS(twil_controllers::CartLinearizingController,controller_interface::ControllerBase) diff --git a/twil_controllers/src/dynamics_pid_backstepping_controller.cpp b/twil_controllers/src/dynamics_pid_backstepping_controller.cpp new file mode 100644 index 0000000..67a8020 --- /dev/null +++ b/twil_controllers/src/dynamics_pid_backstepping_controller.cpp @@ -0,0 +1,474 @@ +/****************************************************************************** + Twil Controllers + Linearizing Backstep Controller + + Copyright (C) 2017 Tiago G. Alves + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program; if not, write to the Free Software + Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + + You can also obtain a copy of the GNU General Public License + at . + +*******************************************************************************/ + +#include "twil_controllers/dynamics_pid_backstepping_controller.h" +#include +#include +#include + +//#define URDF +//#define CONTROLLER_OUTPUT_SCREEN + +namespace twil_controllers +{ + +DynamicsPIDBacksteppingController::DynamicsPIDBacksteppingController() +{ + +} + +DynamicsPIDBacksteppingController::~DynamicsPIDBacksteppingController() +{ + PositionSub[0].~Subscriber(); + PositionSub[1].~Subscriber(); + PID[0].~Pid(); + PID[1].~Pid(); +} + +bool DynamicsPIDBacksteppingController::init(hardware_interface::EffortJointInterface *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,&DynamicsPIDBacksteppingController::commandCB,this); + PositionSub[1] = node_.subscribe("cmd_pos2d",1,&DynamicsPIDBacksteppingController::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 + + + /* + * Velocity PID Controller 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"))){ + ROS_ERROR("This class could not construct PID controller for angular velocity"); + return false; + } + + double p, i, d, imin, imax; + bool 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); + 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; + 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 DYNAMICS_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[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); + } + ROS_INFO("%s: %f", "Kinematic Controller Frequency: ", low_level_control_rate/update_ratio); + + return true; +} + +void DynamicsPIDBacksteppingController::starting(const ros::Time &) +{ + Effort.setZero(); + nu_ref.setZero(); + u_ref.setZero(); + pose_ref = Odometer->getPose(); + +#ifdef DYNAMICS_PID_BACKSTEPPING_CONTROLLER_DEBUG + if (makeLog == false) + { + std::string filename = "none"; + + if (robot_ns == "mini_twil") + { + filename = "/home/talves/Dropbox/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"; + ROS_INFO("Output File: %s", filename.c_str()); + fd.open(filename); + makeLog = true; + } + } +#endif +} + +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; + 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; + 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]); + } + last_vel[0]=joint_ang_vel[0]; + last_vel[1]=joint_ang_vel[1]; + + u = VelocityTransformation::WheelToBase(dphi,WheelBase,WheelRadius[0]); + + Odometer->compute(duration,u); + Eigen::Vector3d pose = Odometer->getPose(); + + 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 + // } else u_ref.setZero(); + } + + /* --------------------------------------------------------------------------------------------------------------------- + * Velocity Controller + * --------------------------------------------------------------------------------------------------------------------- + */ + + 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) + + /* --------------------------------------------------------------------------------------------------------------------- + * 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; + 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 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] + << std::endl; + } + } +#endif + +} + +void DynamicsPIDBacksteppingController::stopping(const ros::Time &) +{ +#ifdef DYNAMICS_PID_BACKSTEPPING_CONTROLLER_DEBUG + fd.close(); +#endif + PositionSub[0].shutdown(); + PositionSub[1].shutdown(); +} + +void DynamicsPIDBacksteppingController::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 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; + +#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::DynamicsPIDBacksteppingController,controller_interface::ControllerBase) diff --git a/twil_controllers/src/dynamics_pid_controller.cpp b/twil_controllers/src/dynamics_pid_controller.cpp new file mode 100644 index 0000000..71007ae --- /dev/null +++ b/twil_controllers/src/dynamics_pid_controller.cpp @@ -0,0 +1,342 @@ +/****************************************************************************** + Twil Controllers + Dynamics PID Controller + + Copyright (C) 2017 Tiago G. Alves + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program; if not, write to the Free Software + Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + + You can also obtain a copy of the GNU General Public License + at . + +*******************************************************************************/ + +#include +#include +#include + +//#define URDF +//#define CONTROLLER_OUTPUT_SCREEN + +namespace twil_controllers +{ +DynamicsPIDController::DynamicsPIDController(void): + WheelRadius(2){} + +DynamicsPIDController::~DynamicsPIDController(void) { + VelocitySub.~Subscriber(); + feedback_linearization->~StateFeedbackLinearization(); + PID[0].~Pid(); + PID[1].~Pid(); +} + +bool DynamicsPIDController::init(hardware_interface::EffortJointInterface *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); + } + + VelocitySub = node_.subscribe("cmd_vel",1,&DynamicsPIDController::commandCB,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 + + + + /* + * Velocity PID Controller 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"))){ + ROS_ERROR("This class could not construct PID controller for angular velocity"); + return false; + } + + double p, i, d, imin, imax; + bool 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); + 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)){ + ROS_ERROR("Parameter '/log_date' not found! The log will be disabled..."); + log_date=""; + } + +#ifdef TWIST_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; + } +#endif + + /* + * 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)); + return true; +} + +void DynamicsPIDController::starting(const ros::Time& time) +{ + nu_ref.setZero(); + u_ref.setZero(); + Effort.setZero(); + +#ifdef DYNAMICS_PID_DEBUG + if (makeLog == false) + { + std::string filename = "none"; + + if (robot_ns == "mini_twil") + { + filename = "/home/talves/Dropbox/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") + { + filename = "/home/talves/Dropbox/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; + } + } +#endif +} + +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; + } + 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(); + + /* + * 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]); + + + + // 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) + /* + * ---------------------------------------------------------------------------------------------------------------------- + */ + + + /* --------------------------------------------------------------------------------------------------------------------- + * 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; + 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]); +#endif + +#ifdef DYNAMICS_PID_DEBUG + 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; + } + } +#endif +} + +void DynamicsPIDController::stopping(void) +{ +#ifdef DYNAMICS_PID_DEBUG + fd.close(); +#endif + VelocitySub.shutdown(); +} + + + +void DynamicsPIDController::commandCB(const geometry_msgs::TwistConstPtr &CommandVel) +{ + 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 +} + +} +PLUGINLIB_EXPORT_CLASS(twil_controllers::DynamicsPIDController,controller_interface::ControllerBase) diff --git a/twil_controllers/src/nonlinear_backstepping_controller.cpp b/twil_controllers/src/nonlinear_backstepping_controller.cpp new file mode 100644 index 0000000..5e1ed79 --- /dev/null +++ b/twil_controllers/src/nonlinear_backstepping_controller.cpp @@ -0,0 +1 @@ +#include "twil_controllers/nonlinear_backstepping_controller.h" diff --git a/twil_controllers/src/odometry_tool.cpp b/twil_controllers/src/odometry_tool.cpp new file mode 100644 index 0000000..5b1739d --- /dev/null +++ b/twil_controllers/src/odometry_tool.cpp @@ -0,0 +1,130 @@ +/****************************************************************************** + Twil Controllers + Odometry Tool + + Copyright (C) 2017 Tiago G. Alves + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program; if not, write to the Free Software + Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + + You can also obtain a copy of the GNU General Public License + at . + +*******************************************************************************/ + + +#include "twil_controllers/odometry_tool.h" +#include +#include +#include + +namespace twil_controllers +{ +OdometryTool::OdometryTool() +{ +} + +OdometryTool::~OdometryTool() +{ + odom_pub->~RealtimePublisher(); + tf_odom_pub->~RealtimePublisher(); +} + +bool OdometryTool::compute(ros::Duration duration, const Eigen::Vector2d &u_) +{ + u = u_; + odometer_compute_time = ros::Time::now(); + odometer_cycle = duration; + + Eigen::Vector3d dp; + Eigen::MatrixXd B_x(3,2); + + double th = this->pose2d.theta + (u[1]*duration.toSec()/2.0); // Compute the heading angle for Runge-Kutta Integration + B_x << cos(th), 0.0, sin(th), 0.0, 0.0, 1.0; // Compute the B(x) matrix + dp = B_x*u*duration.toSec(); // Compute the displacement + + // Update the pose + this->pose2d.x += dp[0]; + this->pose2d.y += dp[1]; + this->pose2d.theta += dp[2]; + return true; + +} + +Eigen::Vector3d OdometryTool::getPose() +{ + Eigen::Vector3d pose_atu; + pose_atu << this->pose2d.x, this->pose2d.y, this->pose2d.theta; + return pose_atu; +} + +bool OdometryTool::publish() +{ + bool ret = false; + + const geometry_msgs::Quaternion orientation_from_quat(tf::createQuaternionMsgFromYaw(pose2d.theta)); + + if(odom_pub->trylock()) + { + odom_pub->msg_.header.stamp = odometer_compute_time; + odom_pub->msg_.pose.pose.position.x = this->pose2d.x; + odom_pub->msg_.pose.pose.position.y = this->pose2d.y; + odom_pub->msg_.pose.pose.orientation = orientation_from_quat; + odom_pub->msg_.twist.twist.linear.x = u[0]; + odom_pub->msg_.twist.twist.angular.z = u[1]; + odom_pub->unlockAndPublish(); + ret = true; + } else ret = false; + + if(tf_odom_pub->trylock()){ + geometry_msgs::TransformStamped& odom_frame = tf_odom_pub->msg_.transforms[0]; + odom_frame.header.stamp = odometer_compute_time; + odom_frame.transform.translation.x = this->pose2d.x; + odom_frame.transform.translation.y = this->pose2d.y; + odom_frame.transform.rotation = orientation_from_quat; + tf_odom_pub->unlockAndPublish(); + ret = true; + } else ret = false; + + return ret; +} + +bool OdometryTool::init(ros::NodeHandle n, std::string topic_root, const Eigen::Vector3d &initial_state) +{ + Node = n; + + this->pose2d.x = initial_state[0]; + this->pose2d.y = initial_state[1]; + this->pose2d.theta = initial_state[2]; + + odom_pub.reset(new realtime_tools::RealtimePublisher(Node, "/odom", 4)); + odom_pub->msg_.header.frame_id = "odom"; + odom_pub->msg_.child_frame_id = "twil_origin"; + odom_pub->msg_.pose.pose.position.z = 0; + odom_pub->msg_.twist.twist.linear.y = 0; + odom_pub->msg_.twist.twist.linear.z = 0; + odom_pub->msg_.twist.twist.angular.x = 0; + odom_pub->msg_.twist.twist.angular.y = 0; + + tf_odom_pub.reset(new realtime_tools::RealtimePublisher(Node, "/tf", 100)); + tf_odom_pub->msg_.transforms.resize(1); + tf_odom_pub->msg_.transforms[0].transform.translation.z = 0.0; + tf_odom_pub->msg_.transforms[0].child_frame_id = "twil_origin"; + tf_odom_pub->msg_.transforms[0].header.frame_id = "odom"; + + return true; + +} + +} diff --git a/twil_controllers/src/pose_tool.cpp b/twil_controllers/src/pose_tool.cpp new file mode 100644 index 0000000..75e1a89 --- /dev/null +++ b/twil_controllers/src/pose_tool.cpp @@ -0,0 +1,45 @@ +#include "twil_controllers/pose_tool.h" +#include + +//Eigen::Vector3d conv_pose_to_vector(geometry_msgs::PoseWithCovariance pose_) +//{ +// Eigen::Vector3d actual_pose; + +// tf::Quaternion quat_aux(pose_.pose.orientation.x, pose_.pose.orientation.y, pose_.pose.orientation.z, pose_.pose.orientation.w); +// tf::Matrix3x3 m(quat_aux); +// double roll, pitch, yaw; +// m.getRPY(roll, pitch, yaw); + +// actual_pose[0] = pose_.pose.position.x; +// actual_pose[1] = pose_.pose.position.y; +// actual_pose[2] = yaw; +// return actual_pose; +//} + +//Eigen::Vector3d conv_pose2d_to_vector(geometry_msgs::Pose2D pose_) +//{ +// Eigen::Vector3d pose_vector; +// pose_vector[0]=pose_.x; +// pose_vector[1]=pose_.y; +// pose_vector[2]=pose_.theta; +// return pose_vector; +//} + +//geometry_msgs::PoseWithCovariance conv_vector_to_pose(Eigen::Vector3d pose_) +//{ +// geometry_msgs::PoseWithCovariance pose_conv; +// pose_conv.pose.position.x = pose_[0]; +// pose_conv.pose.position.y = pose_[1]; +// pose_conv.pose.orientation = tf::createQuaternionMsgFromYaw(pose_[2]); +// return pose_conv; +//} + + +//geometry_msgs::Pose2D conv_vector_to_pose2d(Eigen::Vector3d pose_){ +// geometry_msgs::Pose2D pose2d; +// pose2d.x = pose_[0]; +// pose2d.y = pose_[1]; +// pose2d.theta = pose_[2]; +//} + + diff --git a/twil_controllers/src/twist_pid_backstepping_controller.cpp b/twil_controllers/src/twist_pid_backstepping_controller.cpp new file mode 100644 index 0000000..c83b85c --- /dev/null +++ b/twil_controllers/src/twist_pid_backstepping_controller.cpp @@ -0,0 +1,421 @@ +/****************************************************************************** + Twil Controllers + PID Backstepping Controller + + Copyright (C) 2017 Tiago G. Alves + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program; if not, write to the Free Software + Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + + You can also obtain a copy of the GNU General Public License + at . + +*******************************************************************************/ + +#include "twil_controllers/twist_pid_backstepping_controller.h" +#include +#include +#include + +//#define URDF +//#define CONTROLLER_OUTPUT_SCREEN + +namespace twil_controllers +{ + +TwistPIDBacksteppingController::TwistPIDBacksteppingController() +{ + +} + +TwistPIDBacksteppingController::~TwistPIDBacksteppingController() +{ + PositionSub[0].~Subscriber(); + PositionSub[1].~Subscriber(); + PID[0].~Pid(); + PID[1].~Pid(); +} + +bool TwistPIDBacksteppingController::init(hardware_interface::EffortJointInterface *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); + } + + // 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); + +#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 + + + /* + * Twist PID Controller 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"))){ + 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); + 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); + 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; + 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=""; + } + +#ifdef TWIST_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[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); + + ROS_INFO("%s: %f", "Kinematic Controller Frequency: ", low_level_control_rate/update_ratio); + + return true; +} + +void TwistPIDBacksteppingController::starting(const ros::Time &) +{ + dphi_ref.setZero(); + dphi.setZero(); + Effort.setZero(); + u_ref.setZero(); + pose_ref = Odometer->getPose(); + +#ifdef TWIST_PID_BACKSTEPPING_CONTROLLER_DEBUG + if (makeLog == false) + { + std::string filename = "none"; + + if (robot_ns == "mini_twil") + { + filename = "/home/talves/Dropbox/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"; + ROS_INFO("Output File: %s", filename.c_str()); + fd.open(filename); + makeLog = true; + } + } +#endif +} + +void TwistPIDBacksteppingController::update(const ros::Time &time, const ros::Duration &duration) +{ + static int kinematic_cycle = 0; + +#ifdef TWIST_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 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){ + 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(); + } + + /* --------------------------------------------------------------------------------------------------------------------- + * Velocity Controller + * --------------------------------------------------------------------------------------------------------------------- + */ + + 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 + */ + + 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 + +#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 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] + << std::endl; + } + } +#endif +} + +void TwistPIDBacksteppingController::stopping(const ros::Time &) +{ +#ifdef TWIST_PID_BACKSTEPPING_CONTROLLER_DEBUG + fd.close(); +#endif + PositionSub[0].shutdown(); + PositionSub[1].shutdown(); + +} + + +void TwistPIDBacksteppingController::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 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; + +#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::TwistPIDBacksteppingController,controller_interface::ControllerBase) + diff --git a/twil_controllers/src/twist_pid_controller.cpp b/twil_controllers/src/twist_pid_controller.cpp new file mode 100644 index 0000000..0502004 --- /dev/null +++ b/twil_controllers/src/twist_pid_controller.cpp @@ -0,0 +1,292 @@ +/****************************************************************************** + Twil Controllers + Twist PID Controller + + Copyright (C) 2017 Tiago G. Alves + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program; if not, write to the Free Software + Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + + You can also obtain a copy of the GNU General Public License + at . + +*******************************************************************************/ + +#include "twil_controllers/twist_pid_controller.h" +#include +#include + +//#define URDF +//#define CONTROLLER_OUTPUT_SCREEN + + +namespace twil_controllers +{ +TwistPIDController::TwistPIDController(void) + : WheelRadius(2) {} + +TwistPIDController::~TwistPIDController() +{ + VelocitySub.~Subscriber(); + + PID[0].~Pid(); + PID[0].~Pid(); +} + +bool TwistPIDController::init(hardware_interface::EffortJointInterface *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); + } + + VelocitySub = node_.subscribe("cmd_vel",1,&TwistPIDController::commandCB,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 + + /* + * Twist PID Controller 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"))){ + 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); + 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); + 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)){ + ROS_ERROR("Parameter '/log_date' not found! The log will be disabled..."); + log_date=""; + } + +#ifdef TWIST_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; + } +#endif + + return true; +} + +void TwistPIDController::starting(const ros::Time &time) +{ + dphi_ref.setZero(); + dphi.setZero(); + Effort.setZero(); + u_ref.setZero(); + +#ifdef TWIST_PID_DEBUG + if (makeLog == false) + { + std::string filename = "none"; + + if (robot_ns == "mini_twil") + { + filename = "/home/talves/Dropbox/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") + { + filename = "/home/talves/Dropbox/Twil/ros/exp/" + log_date + "/twist_pid_01.txt" + std::to_string(take) + ".txt"; + ROS_INFO("Output File: %s", filename.c_str()); + fd.open(filename); + makeLog = true; + } + } +#endif +} + +void TwistPIDController::update(const ros::Time &time, const ros::Duration &duration) +{ +#ifdef TWIST_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; + } + k++; + } + } +#endif + + /* --------------------------------------------------------------------------------------------------------------------- + * Velocity Controller + * --------------------------------------------------------------------------------------------------------------------- + */ + Eigen::Vector2d error, 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 + */ + 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 + */ + + 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 + + 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]); + +#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]); +#endif + + +#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; + } + } +#endif +} + +void TwistPIDController::stopping() +{ +#ifdef TWIST_PID_DEBUG + fd.close(); +#endif + VelocitySub.shutdown(); +} + +void TwistPIDController::commandCB(const geometry_msgs::TwistConstPtr &CommandVel) +{ + u_ref.setZero(); + u_ref[0] = CommandVel->linear.x; + u_ref[1] = CommandVel->angular.z; + + dphi_ref = VelocityTransformation::BaseToWheel(u_ref,WheelBase,WheelRadius[0]); +#ifdef CONTROLLER_OUTPUT_SCREEN + ROS_INFO("u_ref: {%f, %f}", u_ref[0], u_ref[1]); +#endif +} +} + +PLUGINLIB_EXPORT_CLASS(twil_controllers::TwistPIDController, controller_interface::ControllerBase) diff --git a/twil_controllers/twil_controllers_plugins.xml b/twil_controllers/twil_controllers_plugins.xml new file mode 100644 index 0000000..836f348 --- /dev/null +++ b/twil_controllers/twil_controllers_plugins.xml @@ -0,0 +1,39 @@ + + + + + Blá-blá-blá... + + + + + + Blá-blá-blá... + + + + + + Blá-blá-blá... + + + + + + Blá-blá-blá... + + + + + + Blá-blá-blá... + + + + + + Blá-blá-blá... + + + + diff --git a/twil_description/CMakeLists.txt b/twil_description/CMakeLists.txt new file mode 100644 index 0000000..b28a957 --- /dev/null +++ b/twil_description/CMakeLists.txt @@ -0,0 +1,156 @@ +cmake_minimum_required(VERSION 2.8.3) +project(twil_description) + +## 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) + +## 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 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_description +# CATKIN_DEPENDS other_catkin_pkg +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +# include_directories(include) + +## Declare a cpp library +# add_library(twil_description +# src/${PROJECT_NAME}/twil_description.cpp +# ) + +## Declare a cpp executable +# add_executable(twil_description_node src/twil_description_node.cpp) + +## Add cmake target dependencies of the executable/library +## as an example, message headers may need to be generated before nodes +# add_dependencies(twil_description_node twil_description_generate_messages_cpp) + +## Specify libraries to link a library or executable target against +# target_link_libraries(twil_description_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 twil_description twil_description_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_description.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/twil_description/bkp/twil_description.tar.gz b/twil_description/bkp/twil_description.tar.gz new file mode 100644 index 0000000..9ec1b75 Binary files /dev/null and b/twil_description/bkp/twil_description.tar.gz differ diff --git a/twil_description/meshes/battery_bosch_12v.stl b/twil_description/meshes/battery_bosch_12v.stl new file mode 100644 index 0000000..a8fa985 Binary files /dev/null and b/twil_description/meshes/battery_bosch_12v.stl differ diff --git a/twil_description/meshes/castor_base.stl b/twil_description/meshes/castor_base.stl new file mode 100644 index 0000000..d7cc923 Binary files /dev/null and b/twil_description/meshes/castor_base.stl differ diff --git a/twil_description/meshes/castor_support.stl b/twil_description/meshes/castor_support.stl new file mode 100644 index 0000000..4cac77b Binary files /dev/null and b/twil_description/meshes/castor_support.stl differ diff --git a/twil_description/meshes/castor_wheel.stl b/twil_description/meshes/castor_wheel.stl new file mode 100644 index 0000000..718450c Binary files /dev/null and b/twil_description/meshes/castor_wheel.stl differ diff --git a/twil_description/meshes/chassis.stl b/twil_description/meshes/chassis.stl new file mode 100644 index 0000000..57c9257 Binary files /dev/null and b/twil_description/meshes/chassis.stl differ diff --git a/twil_description/meshes/cpu.stl b/twil_description/meshes/cpu.stl new file mode 100644 index 0000000..3e77b3d Binary files /dev/null and b/twil_description/meshes/cpu.stl differ diff --git a/twil_description/meshes/fixed_wheel.stl b/twil_description/meshes/fixed_wheel.stl new file mode 100644 index 0000000..ca0f19f Binary files /dev/null and b/twil_description/meshes/fixed_wheel.stl differ diff --git a/twil_description/meshes/fixed_wheel_support.stl b/twil_description/meshes/fixed_wheel_support.stl new file mode 100644 index 0000000..e9500ca Binary files /dev/null and b/twil_description/meshes/fixed_wheel_support.stl differ diff --git a/twil_description/meshes/left_wheel_support.stl b/twil_description/meshes/left_wheel_support.stl new file mode 100644 index 0000000..e9500ca Binary files /dev/null and b/twil_description/meshes/left_wheel_support.stl differ diff --git a/twil_description/meshes/right_wheel_support.stl b/twil_description/meshes/right_wheel_support.stl new file mode 100644 index 0000000..20054ff Binary files /dev/null and b/twil_description/meshes/right_wheel_support.stl differ diff --git a/twil_description/package.xml b/twil_description/package.xml new file mode 100644 index 0000000..ae98f35 --- /dev/null +++ b/twil_description/package.xml @@ -0,0 +1,54 @@ + + + twil_description + 2.0.0 + The twil_description package + + + + + Walter Fetter Lages + + + + + + GPLv3 + + + + + + + + + + + + + Walter Fetter Lages + + + + + + + + + + + + + + catkin + + + + + + + + + + + diff --git a/twil_description/xacro/battery_bosch_12v.urdf.xacro b/twil_description/xacro/battery_bosch_12v.urdf.xacro new file mode 100644 index 0000000..289089b --- /dev/null +++ b/twil_description/xacro/battery_bosch_12v.urdf.xacro @@ -0,0 +1,42 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/FlatBlack + + + + + diff --git a/twil_description/xacro/castor_base.urdf.xacro b/twil_description/xacro/castor_base.urdf.xacro new file mode 100644 index 0000000..9271976 --- /dev/null +++ b/twil_description/xacro/castor_base.urdf.xacro @@ -0,0 +1,44 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/White + + + + + + diff --git a/twil_description/xacro/castor_support.urdf.xacro b/twil_description/xacro/castor_support.urdf.xacro new file mode 100644 index 0000000..b5c86d0 --- /dev/null +++ b/twil_description/xacro/castor_support.urdf.xacro @@ -0,0 +1,42 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Grey + + + + + diff --git a/twil_description/xacro/castor_wheel.urdf.xacro b/twil_description/xacro/castor_wheel.urdf.xacro new file mode 100644 index 0000000..21172a2 --- /dev/null +++ b/twil_description/xacro/castor_wheel.urdf.xacro @@ -0,0 +1,43 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Grey + + + + + diff --git a/twil_description/xacro/chassis.urdf.xacro b/twil_description/xacro/chassis.urdf.xacro new file mode 100644 index 0000000..dc9e36c --- /dev/null +++ b/twil_description/xacro/chassis.urdf.xacro @@ -0,0 +1,43 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Gold + + + + + diff --git a/twil_description/xacro/cpu.urdf.xacro b/twil_description/xacro/cpu.urdf.xacro new file mode 100644 index 0000000..dcc4a58 --- /dev/null +++ b/twil_description/xacro/cpu.urdf.xacro @@ -0,0 +1,42 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Grey + + + + + diff --git a/twil_description/xacro/eurocard.urdf.xacro b/twil_description/xacro/eurocard.urdf.xacro new file mode 100644 index 0000000..b28b461 --- /dev/null +++ b/twil_description/xacro/eurocard.urdf.xacro @@ -0,0 +1,47 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Green + + + + + diff --git a/twil_description/xacro/fixed_wheel.urdf.xacro b/twil_description/xacro/fixed_wheel.urdf.xacro new file mode 100644 index 0000000..91bd915 --- /dev/null +++ b/twil_description/xacro/fixed_wheel.urdf.xacro @@ -0,0 +1,52 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission" + + EffortJointInterface + + EffortJointInterface + 1 + + + + Gazebo/FlatBlack + + + + + diff --git a/twil_description/xacro/fixed_wheel_support.urdf.xacro b/twil_description/xacro/fixed_wheel_support.urdf.xacro new file mode 100644 index 0000000..1346e56 --- /dev/null +++ b/twil_description/xacro/fixed_wheel_support.urdf.xacro @@ -0,0 +1,42 @@ + + + + + + + + + + + + + + + + + + + /> + + + + + + + + + + + + + + + + + + Gazebo/White + + + + + diff --git a/twil_description/xacro/twil.urdf.xacro b/twil_description/xacro/twil.urdf.xacro new file mode 100644 index 0000000..ebc8381 --- /dev/null +++ b/twil_description/xacro/twil.urdf.xacro @@ -0,0 +1,113 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + /twil + + + + + + + + 0.001 + + + + + diff --git a/twil_description/xacro/twil_real.urdf.xacro b/twil_description/xacro/twil_real.urdf.xacro new file mode 100644 index 0000000..97e9bb7 --- /dev/null +++ b/twil_description/xacro/twil_real.urdf.xacro @@ -0,0 +1,98 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/twil_hw/CMakeLists.txt b/twil_hw/CMakeLists.txt new file mode 100644 index 0000000..5f616dc --- /dev/null +++ b/twil_hw/CMakeLists.txt @@ -0,0 +1,85 @@ +cmake_minimum_required(VERSION 2.8.3) +project(twil_hw) + +add_definitions(-std=c++11) + +find_package(catkin REQUIRED COMPONENTS + controller_interface + controller_manager + std_msgs + hardware_interface + joint_state_controller + roscpp + rospy +) +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} + include/${PROJECT_NAME} + ${AIC_LIB_ROOT_DIR}/include +) + +add_library(${PROJECT_NAME}_lib + src/twil_hw.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) + + +############# +## Install ## +############# + +## Mark executables and/or libraries for installation +install(TARGETS twil_hw_lib aic_lib twil_hw_node + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +## Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/ ${AIC_LIB_ROOT_DIR}/include + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) diff --git a/twil_hw/include/twil_hw/twil_hw.h b/twil_hw/include/twil_hw/twil_hw.h new file mode 100644 index 0000000..0a3ad93 --- /dev/null +++ b/twil_hw/include/twil_hw/twil_hw.h @@ -0,0 +1,47 @@ +#ifndef TWIL_HW_H +#define TWIL_HW_H + +#include +#include +#include +#include +#include + +#include "aic.h" +#include + +namespace twil_hw +{ + +class twil_hw : public hardware_interface::RobotHW +{ +public: + twil_hw(); + ~twil_hw(); + 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: + hardware_interface::JointStateInterface jnt_state_interface; + hardware_interface::EffortJointInterface effort_joint_interface; + + double cmd[2]; + double prev_cmd[2]; + double pos[2]; + double vel[2]; + double eff[2]; + double pos_dead_zone[2]; + double neg_dead_zone[2]; + bool dead_zone_compensation[2]; + + aic *wheel_joint[2]; + aic_displacement_msg_t displacement_status[2]; + aic_comm_config_t connection_parameters[2]; + ros::NodeHandle nh; + + +}; +} +#endif // TWIL_HW_H diff --git a/twil_hw/package.xml b/twil_hw/package.xml new file mode 100644 index 0000000..ebe47c4 --- /dev/null +++ b/twil_hw/package.xml @@ -0,0 +1,62 @@ + + + twil_hw + 0.0.0 + The twil_hw package + + + + + talves + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + controller_interface + controller_manager + hardware_interface + joint_state_controller + roscpp + rospy + controller_interface + controller_manager + hardware_interface + joint_state_controller + roscpp + rospy + + + + + + + + diff --git a/twil_hw/scripts/export_motor_test.sh b/twil_hw/scripts/export_motor_test.sh new file mode 100755 index 0000000..bbc13b5 --- /dev/null +++ b/twil_hw/scripts/export_motor_test.sh @@ -0,0 +1,21 @@ +#!/bin/bash + +echo "USAGE: ./export_motor_test -f file.bag -t topic_root -o output_root" + +while getopts f:t:o: option +do + case "${option}" + in + f) FILE=${OPTARG};; + t) TOPIC_ROOT=${OPTARG};; + o) OUTPUT=${OPTARG};; + esac +done + +set -x +echo "Joint_States_File:" $OUTPUT"_joint_states.dat" +rostopic echo -b $FILE -p /$TOPIC_ROOT/joint_states > $OUTPUT"_joint_states.dat" +echo "Left_Motor_CMD_File:" $OUTPUT"_left_wheel.dat" +rostopic echo -b $FILE -p /$TOPIC_ROOT/left_wheel_joint_effort_controller/command > $OUTPUT"_left_wheel.dat" +echo "Right_Motor_CMD_File:" $OUTPUT"_right_wheel.dat" +rostopic echo -b $FILE -p /$TOPIC_ROOT/right_wheel_joint_effort_controller/command > $OUTPUT"_right_wheel.dat" diff --git a/twil_hw/src/twil_hw.cpp b/twil_hw/src/twil_hw.cpp new file mode 100644 index 0000000..8d6273b --- /dev/null +++ b/twil_hw/src/twil_hw.cpp @@ -0,0 +1,181 @@ +#include "twil_hw.h" + +//#define AIC_HW_DEBUG +//#define NULL_HW + +namespace twil_hw +{ + +twil_hw::twil_hw() +{ + cmd[0] = 0; + cmd[1] = 0; + prev_cmd[0] = -1000; + prev_cmd[1] = -1000; + // prev_cmd[0] = 0; + // prev_cmd[1] = 0; + eff[0] = 0; + eff[1] = 0; + pos[0] = 0; + pos[1] = 0; + vel[0] = 0; + vel[1] = 0; + dead_zone_compensation[0] = 0; + dead_zone_compensation[1] = 0; +} + +twil_hw::~twil_hw() +{ + effort_joint_interface.~HardwareInterface(); + jnt_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 + { + wheel_joint[i]->~aic(); + } +#endif +} + +bool twil_hw::init(ros::NodeHandle &root_nh, ros::NodeHandle &robot_hw_nh) { + + + nh = 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 (nh.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("SERIAL CONNECTION!\n"); + if (nh.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) + { + // 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 (nh.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 + wheel_joint[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 (nh.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 (nh.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 (nh.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 (wheel_joint[i]->status_ok() == false) { + ROS_ERROR("AIC %d NOT CONNECTED!", i+1); + return false; + } +#endif + } + + // connect and register the joint state interface + hardware_interface::JointStateHandle state_handle_left("left_wheel_joint", &pos[0], &vel[0], &eff[0]); + jnt_state_interface.registerHandle(state_handle_left); + hardware_interface::JointStateHandle state_handle_right("right_wheel_joint", &pos[1], &vel[1], &eff[1]); + jnt_state_interface.registerHandle(state_handle_right); + registerInterface(&jnt_state_interface); + + + // connect and register the joint position interface + hardware_interface::JointHandle eff_handle_left(jnt_state_interface.getHandle("left_wheel_joint"), &cmd[0]); + effort_joint_interface.registerHandle(eff_handle_left); + hardware_interface::JointHandle eff_handle_right(jnt_state_interface.getHandle("right_wheel_joint"), &cmd[1]); + effort_joint_interface.registerHandle(eff_handle_right); + registerInterface(&effort_joint_interface); + return true; +} + +void twil_hw::read(const ros::Time &time, const ros::Duration &period) +{ +#ifndef NULL_HW + displacement_status[0] = wheel_joint[0]->read_displacement_sensors(); +#ifndef AIC_HW_DEBUG + displacement_status[1] = wheel_joint[1]->read_displacement_sensors(); +#endif + pos[0] += displacement_status[0].joint_displacement; + pos[1] += displacement_status[1].joint_displacement; + vel[0] = displacement_status[0].joint_displacement/period.toSec(); + vel[1] = displacement_status[1].joint_displacement/period.toSec(); +#endif +} + +void twil_hw::write(const ros::Time &time, const ros::Duration &period) +{ + double Va[2]; // Armature Voltage Without dead zone compensation + double Vmotor[2]; // Armature Voltage after dead zone compensation + double Vref = 12.00; // Voltage Reference for duty cycle + double Vbat = 12.00; // + + Va[0] = cmd[0]*(Vbat/Vref); + Va[1] = cmd[1]*(Vbat/Vref); + +#ifndef NULL_HW + +#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){ + Vmotor[i] = (Va[i]+pos_dead_zone[i])*(Vref/(Vref+abs(pos_dead_zone[i]))); + } else if (cmd[i] < 0) { + Vmotor[i] = (Va[i]+neg_dead_zone[i])*(Vref/(Vref+abs(neg_dead_zone[i]))); + } else Vmotor[i] = 0; + + if (abs(Vmotor[i]) < 0.2) Vmotor[i] = 0; // Used to reduce jerk motion near 0V. + + } else Vmotor[i] = Va[i]; + + if (Vmotor[i] > Vref) Vmotor[i] = Vref; + else if (Vmotor[i] < -Vref) Vmotor[i] = -Vref; + + wheel_joint[i]->set_motor_voltage(Vmotor[i]); +#endif + } + + eff[0] = cmd[0]; + eff[1] = cmd[1]; +} + +} diff --git a/twil_hw/src/twil_hw_node.cpp b/twil_hw/src/twil_hw_node.cpp new file mode 100644 index 0000000..f2642c2 --- /dev/null +++ b/twil_hw/src/twil_hw_node.cpp @@ -0,0 +1,68 @@ +#include +#include +#include +#include +#include +#include +#include "twil_hw/twil_hw.h" + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "twil_hw_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; + } else ROS_INFO("->\nRobot namespace: %s\n->", robot_ns.c_str()); + + double control_rate = 0.0; + if(nh.getParam("control_loop_rate", control_rate)) { + ROS_INFO("Twil_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; + std_msgs::Duration msg; + boost::scoped_ptr< realtime_tools::RealtimePublisher >ts_pub; + ts_pub.reset(new realtime_tools::RealtimePublisher(nh,"sample_time",1)); + + twil_hw::twil_hw robot; + robot.init(nh, nh); + ROS_INFO("Starting the Hardware and Control Loop Manager!"); + controller_manager::ControllerManager cm(&robot); + ros::AsyncSpinner spinner(1); spinner.start(); + + 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; + + // ros::Time read_tick = ros::Time::now(); + robot.read(current_time,dt); + // ros::Duration toc_read = ros::Time::now() - read_tick; + cm.update(current_time, dt); + robot.write(current_time,dt); + // msg.data = toc_read; + + if (ts_pub->trylock()) { + ts_pub->msg_.data = dt; + ts_pub->unlockAndPublish(); + } + loop_rate.sleep(); + } + + spinner.stop(); + robot.~twil_hw(); + + return 0; +} diff --git a/twil_trajectories/CMakeLists.txt b/twil_trajectories/CMakeLists.txt new file mode 100644 index 0000000..3c2670f --- /dev/null +++ b/twil_trajectories/CMakeLists.txt @@ -0,0 +1,184 @@ +cmake_minimum_required(VERSION 2.8.3) +project(twil_trajectories) + +## 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) + +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 ${PROJECT_NAME} +# CATKIN_DEPENDS other_catkin_pkg + DEPENDS eigen +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +# include_directories(include) +# TODO: Check names of system library include directories (eigen) +include_directories( + include ${catkin_INCLUDE_DIRS} + ${Eigen_INCLUDE_DIRS} +) + +## Declare a cpp library +add_library(${PROJECT_NAME} + src/circle_path.cpp + src/eight_path.cpp +) + +## Declare a cpp executable +add_executable(eight_trajectory src/eight_trajectory.cpp) +add_executable(circle_trajectory src/circle_trajectory.cpp) +add_executable(pose2d_stamp src/pose2d_stamp.cpp) + +## Add cmake target dependencies of the executable/library +## as an example, message headers may need to be generated before nodes +# add_dependencies(twil_trajectories_node twil_trajectories_generate_messages_cpp) + +## Specify libraries to link a library or executable target against +target_link_libraries(${PROJECT_NAME} + ${catkin_LIBRARIES} + ${eigen_LIBRARIES} +) + +target_link_libraries(eight_trajectory + ${catkin_LIBRARIES} + ${eigen_LIBRARIES} + ${PROJECT_NAME} +) + +target_link_libraries(circle_trajectory + ${catkin_LIBRARIES} + ${eigen_LIBRARIES} + ${PROJECT_NAME} +) + +target_link_libraries(pose2d_stamp + ${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} + 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_trajectories.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/twil_trajectories/include/twil_trajectories/circle_path.h b/twil_trajectories/include/twil_trajectories/circle_path.h new file mode 100644 index 0000000..4c16412 --- /dev/null +++ b/twil_trajectories/include/twil_trajectories/circle_path.h @@ -0,0 +1,56 @@ +#ifndef CIRCLE_PATH_H +#define CIRCLE_PATH_H + +#include + +/** Circle path +* @author Walter Fetter Lages +*/ +class CirclePath +{ + Eigen::Vector2d pc_; + double phi0_; + double r_; + double w_; + + public: + + /** Build a circle path. + * @param pc circle center point. + * @param phi0 initial phase. + * @param r circle radius. + * @param w angular velocity. + */ + CirclePath(const Eigen::Vector2d &pc,double phi0,double r,double w); + + /** Build a circle path. + * @param p0 starting pose. + * @param r circle radius. + * @param w angular velocity. + */ + CirclePath(const Eigen::Vector3d &p0,double r,double w); + + /** Build a circle path. + * @param pc circle center point. + * @param p0 starting point. + * @param w angular velocity. + */ + CirclePath(const Eigen::Vector2d &pc,Eigen::Vector2d &p0,double w); + + /** Destroy a circle path. + */ + ~CirclePath(void) { }; + + /** Get path point. + * @param t path time. + * @return path point. + */ + Eigen::Vector3d point(double t) const; + + /** Get path steering controls. + * @param t path time. + * @return steering controls. + */ + Eigen::Vector2d steering(double t) const; +}; +#endif diff --git a/twil_trajectories/include/twil_trajectories/eight_path.h b/twil_trajectories/include/twil_trajectories/eight_path.h new file mode 100644 index 0000000..b3f5af2 --- /dev/null +++ b/twil_trajectories/include/twil_trajectories/eight_path.h @@ -0,0 +1,40 @@ +#ifndef EIGHT_PATH_H +#define EIGHT_PATH_H + +#include + +/** 8 path +* @author Walter Fetter Lages +*/ +class EightPath +{ + CirclePath c1_; + CirclePath c2_; + double period_; + + public: + + /** Build an 8 path. + * @param pc center point. + * @param r radius. + * @param w angular velocity. + */ + EightPath(const Eigen::Vector2d &pc,double r,double w); + + /** Destroy an 8 path. + */ + ~EightPath(void) { }; + + /** Get path point. + * @param t path time. + * @return path point. + */ + Eigen::Vector3d point(double t) const; + + /** Get path steering controls. + * @param t path time. + * @return steering controls. + */ + Eigen::Vector2d steering(double t) const; +}; +#endif diff --git a/twil_trajectories/package.xml b/twil_trajectories/package.xml new file mode 100644 index 0000000..8545852 --- /dev/null +++ b/twil_trajectories/package.xml @@ -0,0 +1,58 @@ + + + twil_trajectories + 2.0.0 + The twil_trajectories package + + + + + Walter Fetter Lages + + + + + + GPLv3 + + + + + + + + + + + + + Walter Fetter Lages + + + + + + + + + + + + + + catkin + eigen + geometry_msgs + + eigen + geometry_msgs + + + + + + + + + + diff --git a/twil_trajectories/src/circle_path.cpp b/twil_trajectories/src/circle_path.cpp new file mode 100644 index 0000000..8c7c805 --- /dev/null +++ b/twil_trajectories/src/circle_path.cpp @@ -0,0 +1,51 @@ +#include + +#define sqr(x) (x*x) +#define sgn(x) ((x == 0.0)? 0.0:(x/fabs(x))) + +CirclePath::CirclePath(const Eigen::Vector2d &pc,double phi0,double r,double w) +{ + pc_=pc; + phi0_=phi0; + r_=r; + w_=w; +} + +CirclePath::CirclePath(const Eigen::Vector3d &p0,double r,double w) +{ + phi0_=p0[2]-sgn(w)*M_PI_2; + pc_[0]=p0[0]-r*cos(phi0_); + pc_[1]=p0[1]-r*sin(phi0_); + r_=r; + w_=w; +} + +CirclePath::CirclePath(const Eigen::Vector2d &pc,Eigen::Vector2d &p0,double w) +{ + pc_=pc; + w_=w; + phi0_=atan2(p0[1]-pc[1],p0[0]-pc[0]); + r_=sqrt(sqr(p0[1]-pc[1])+sqr(p0[0]-pc[0])); +} + +Eigen::Vector3d CirclePath::point(double t) const +{ + double wt=w_*t; + Eigen::Vector3d p; + + p[0]=pc_[0]+r_*cos(wt+phi0_); + p[1]=pc_[1]+r_*sin(wt+phi0_); + p[2]=wt+phi0_+sgn(w_)*M_PI_2; + + return p; +} + +Eigen::Vector2d CirclePath::steering(double t) const +{ + Eigen::Vector2d e(2); + + e[0]=w_*r_; + e[1]=w_; + + return e; +} diff --git a/twil_trajectories/src/circle_trajectory.cpp b/twil_trajectories/src/circle_trajectory.cpp new file mode 100644 index 0000000..1c42f8f --- /dev/null +++ b/twil_trajectories/src/circle_trajectory.cpp @@ -0,0 +1,100 @@ +#include +#include + +#include + + +double rate; +class CircleTrajectory +{ +public: + CircleTrajectory(ros::NodeHandle node); + ~CircleTrajectory(void); + void setCommand(ros::Duration t); + double loop_rate; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + private: + ros::NodeHandle node_; + const CirclePath *path; + + ros::Publisher commandPublisher,commandPublisher2; +}; + + +CircleTrajectory::CircleTrajectory(ros::NodeHandle node) +{ + + node_=node; + Eigen::Vector2d pc; + double w; + double r; + double phi0; + + std::string robot_config; + + if (node_.getParam("/robot_config", robot_config)) ROS_INFO("Got param 'robot_config': %s", robot_config.c_str()); + else ROS_ERROR("Failed to get param '/robot_config'"); + + if (node_.getParam("robot_trajectory_path/pc_x", pc[0])) ROS_INFO("Got param 'pc_x': %f", pc[0]); + else ROS_ERROR("Failed to get param '%s/pc_x'",node_.getNamespace().c_str()); + + if (node_.getParam("robot_trajectory_path/pc_y", pc[1])) ROS_INFO("Got param 'pc_y': %f", pc[1]); + else ROS_ERROR("Failed to get param '%s/pc_y'",node_.getNamespace().c_str()); + + if (node_.getParam("robot_trajectory_path/phi0", phi0)) ROS_INFO("Got param 'phi0': %f", phi0); + else ROS_ERROR("Failed to get param '%s/phi0'",node_.getNamespace().c_str()); + + if (node_.getParam("robot_trajectory_path/r", r)) ROS_INFO("Got param 'r': %f", r); + else ROS_ERROR("Failed to get param '%s/r'",node_.getNamespace().c_str()); + + if (node_.getParam("robot_trajectory_path/w", w)) ROS_INFO("Got param 'w': %f", w); + else ROS_ERROR("Failed to get param '%s/w'",node_.getNamespace().c_str()); + + if (node_.getParam("robot_trajectory_path/loop_rate", loop_rate)) ROS_INFO("Got param 'loop_rate': %f", loop_rate); + else ROS_ERROR("Failed to get param '%s/loop_rate'",node_.getNamespace().c_str()); + + commandPublisher=node_.advertise("/"+robot_config+"/twist_pid_backstepping_controller/cmd_pos2d",1); + commandPublisher2=node_.advertise("/"+robot_config+"/dynamics_pid_backstepping_controller/cmd_pos2d",1); + + path=new CirclePath(pc,phi0,r,w); +} + +CircleTrajectory::~CircleTrajectory(void) +{ + commandPublisher.shutdown(); + delete path; +} + +void CircleTrajectory::setCommand(ros::Duration t) +{ + Eigen::Vector3d p=path->point(t.toSec()); + + geometry_msgs::Pose2D command; + command.x=p[0]; + command.y=p[1]; + command.theta=p[2]; + commandPublisher.publish(command); + commandPublisher2.publish(command); +} + +int main(int argc,char* argv[]) +{ + + ros::init(argc,argv,"circle_trajectory"); + ros::NodeHandle node; + + CircleTrajectory CircleTrajectory(node); + + ros::Rate loop(CircleTrajectory.loop_rate); + + ros::Time t0=ros::Time::now(); + while(ros::ok()) + { + CircleTrajectory.setCommand(ros::Time::now()-t0); + + ros::spinOnce(); + loop.sleep(); + } + return 0; +} diff --git a/twil_trajectories/src/eight_path.cpp b/twil_trajectories/src/eight_path.cpp new file mode 100644 index 0000000..3a7603f --- /dev/null +++ b/twil_trajectories/src/eight_path.cpp @@ -0,0 +1,26 @@ +#include + +#define sgn(x) ((x == 0.0)? 0.0:(x/fabs(x))) + +EightPath::EightPath(const Eigen::Vector2d &pc,double r,double w): +c1_(Eigen::Vector2d(pc[0],pc[1]+r),-M_PI_2*sgn(w),r,w), +c2_(Eigen::Vector2d(pc[0],pc[1]-r),2.5*M_PI*sgn(w),r,-w) +{ + period_=4*M_PI/w; +}; + +Eigen::Vector3d EightPath::point(double t) const +{ + double tc=fmod(t,period_/2.0); + + if(int(t/(period_/2.0)) % 2 == 0) return c1_.point(tc); + else return c2_.point(tc); +} + +Eigen::Vector2d EightPath::steering(double t) const +{ + double tc=fmod(t,period_/2.0); + + if(int(t/(period_/2.0)) % 2 == 0) return c1_.steering(tc); + else return c2_.steering(tc); +} diff --git a/twil_trajectories/src/eight_trajectory.cpp b/twil_trajectories/src/eight_trajectory.cpp new file mode 100644 index 0000000..1be794f --- /dev/null +++ b/twil_trajectories/src/eight_trajectory.cpp @@ -0,0 +1,94 @@ +#include +#include + +#include + + +double rate; +class EightTrajectory +{ + public: + EightTrajectory(ros::NodeHandle node); + ~EightTrajectory(void); + void setCommand(ros::Duration t); + double loop_rate; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + private: + ros::NodeHandle node_; + const EightPath *path; + + ros::Publisher commandPublisher,commandPublisher2; +}; + + +EightTrajectory::EightTrajectory(ros::NodeHandle node) +{ + node_=node; + Eigen::Vector2d pc; + double w; + double r; + std::string robot_config; + + if (node_.getParam("/robot_config", robot_config)) ROS_INFO("Got param 'robot_config': %s", robot_config.c_str()); + else ROS_ERROR("Failed to get param '/robot_config'"); + + if (node_.getParam("robot_trajectory_path/pc_x", pc[0])) ROS_INFO("Got param 'pc_x': %f", pc[0]); + else ROS_ERROR("Failed to get param '%s/pc_x'",node_.getNamespace().c_str()); + + if (node_.getParam("robot_trajectory_path/pc_y", pc[1])) ROS_INFO("Got param 'pc_y': %f", pc[1]); + else ROS_ERROR("Failed to get param '%s/pc_y'",node_.getNamespace().c_str()); + + if (node_.getParam("robot_trajectory_path/r", r)) ROS_INFO("Got param 'r': %f", r); + else ROS_ERROR("Failed to get param '%s/r'",node_.getNamespace().c_str()); + + if (node_.getParam("robot_trajectory_path/w", w)) ROS_INFO("Got param 'w': %f", w); + else ROS_ERROR("Failed to get param '%s/w'",node_.getNamespace().c_str()); + + if (node_.getParam("robot_trajectory_path/loop_rate", loop_rate)) ROS_INFO("Got param 'loop_rate': %f", loop_rate); + else ROS_ERROR("Failed to get param '%s/loop_rate'",node_.getNamespace().c_str()); + + commandPublisher=node_.advertise("/"+robot_config+"/twist_pid_backstepping_controller/cmd_pos2d",1); + commandPublisher2=node_.advertise("/"+robot_config+"/dynamics_pid_backstepping_controller/cmd_pos2d",1); + + path=new EightPath(pc,r,w); +} + +EightTrajectory::~EightTrajectory(void) +{ + commandPublisher.shutdown(); + delete path; +} + +void EightTrajectory::setCommand(ros::Duration t) +{ + Eigen::Vector3d p=path->point(t.toSec()); + + geometry_msgs::Pose2D command; + command.x=p[0]; + command.y=p[1]; + command.theta=p[2]; + commandPublisher.publish(command); + commandPublisher2.publish(command); +} + +int main(int argc,char* argv[]) +{ + + ros::init(argc,argv,"eight_trajectory"); + ros::NodeHandle node; + + EightTrajectory eightTrajectory(node); + + ros::Rate loop(eightTrajectory.loop_rate); + + ros::Time t0=ros::Time::now(); + while(ros::ok()) + { + eightTrajectory.setCommand(ros::Time::now()-t0); + + ros::spinOnce(); + loop.sleep(); + } + return 0; +} diff --git a/twil_trajectories/src/pose2d_stamp.cpp b/twil_trajectories/src/pose2d_stamp.cpp new file mode 100644 index 0000000..84e0164 --- /dev/null +++ b/twil_trajectories/src/pose2d_stamp.cpp @@ -0,0 +1,75 @@ +#include + +#include + +#include +#include + +class Pose2DStamp +{ + public: + Pose2DStamp(ros::NodeHandle node); + ~Pose2DStamp(void); + + private: + ros::NodeHandle node_; + + ros::Subscriber poseSubscriber; + ros::Publisher posePublisher; + + int seq; + std::string frame_id; + + void poseCB(const geometry_msgs::Pose2D::ConstPtr &pose); +}; + + +Pose2DStamp::Pose2DStamp(ros::NodeHandle node) +{ + node_=node; + + poseSubscriber=node_.subscribe("command",1000,&Pose2DStamp::poseCB,this); + posePublisher=node_.advertise("command_stamped",1000); + + seq=0; + + ros::param::get("~frame_id",frame_id); +} + +Pose2DStamp::~Pose2DStamp(void) +{ + poseSubscriber.shutdown(); + posePublisher.shutdown(); +} + +void Pose2DStamp::poseCB(const geometry_msgs::Pose2D::ConstPtr &pose) +{ + geometry_msgs::PoseStamped stamped; + stamped.header.stamp=ros::Time::now(); + stamped.header.frame_id=frame_id; + stamped.pose.position.x=pose->x; + stamped.pose.position.y=pose->y; + stamped.pose.position.z=0; + stamped.pose.orientation.x=0; + stamped.pose.orientation.y=0; + stamped.pose.orientation.z=sin(pose->theta/2.0); + stamped.pose.orientation.w=cos(pose->theta/2.0); + posePublisher.publish(stamped); +} + +int main(int argc,char* argv[]) +{ + ros::init(argc,argv,"pose2d_stamp"); + ros::NodeHandle node; + + Pose2DStamp pose2DStamp(node); + + ros::Rate loop(100); + ros::Time t0=ros::Time::now(); + while(ros::ok()) + { + ros::spinOnce(); + loop.sleep(); + } + return 0; +}