--- /dev/null
+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
--- /dev/null
+cmake_minimum_required(VERSION 2.8.3)
+project(twil)
+find_package(catkin REQUIRED)
+catkin_metapackage()
--- /dev/null
+<?xml version="1.0"?>
+<package>
+ <name>twil</name>
+ <version>2.0.0</version>
+ <description>The twil package</description>
+
+ <!-- One maintainer tag required, multiple allowed, one person per tag -->
+ <!-- Example: -->
+ <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+ <maintainer email="talves@ece.ufrgs.br">Walter Fetter Lages</maintainer>
+
+
+ <!-- One license tag required, multiple allowed, one license per tag -->
+ <!-- Commonly used license strings: -->
+ <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+ <license>GPLv3</license>
+
+
+ <!-- Url tags are optional, but mutiple are allowed, one per tag -->
+ <!-- Optional attribute type can be: website, bugtracker, or repository -->
+ <!-- Example: -->
+ <!-- <url type="website">http://wiki.ros.org/twil</url> -->
+ <url type="website">http://www.ece.ufrgs.br/~fetter/twil</url>
+
+ <!-- Author tags are optional, mutiple are allowed, one per tag -->
+ <!-- Authors do not have to be maintianers, but could be -->
+ <!-- Example: -->
+ <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+ <author email="fetter@ece.ufrgs.br">Walter Fetter Lages</author>
+
+ <!-- The *_depend tags are used to specify dependencies -->
+ <!-- Dependencies can be catkin packages or system dependencies -->
+ <!-- Examples: -->
+ <!-- Use build_depend for packages you need at compile time: -->
+ <!-- <build_depend>message_generation</build_depend> -->
+ <!-- Use buildtool_depend for build tool packages: -->
+ <!-- <buildtool_depend>catkin</buildtool_depend> -->
+ <!-- Use run_depend for packages you need at runtime: -->
+ <!-- <run_depend>message_runtime</run_depend> -->
+ <!-- Use test_depend for packages you need only for testing: -->
+ <!-- <test_depend>gtest</test_depend> -->
+ <buildtool_depend>catkin</buildtool_depend>
+
+ <run_depend>twil_description</run_depend>
+ <run_depend>twil_bringup</run_depend>
+ <run_depend>twil_controllers</run_depend>
+ <run_depend>twil_hw</run_depend>
+
+
+ <!-- The export tag contains other, unspecified, tags -->
+ <export>
+ <!-- You can specify that this package is a metapackage here: -->
+ <!-- <metapackage/> -->
+ <metapackage/>
+
+ <!-- Other tools can request additional information be placed here -->
+
+ </export>
+</package>
--- /dev/null
+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
+# )
--- /dev/null
+mini_twil:
+
+ joint_state_controller:
+ type: joint_state_controller/JointStateController
+ publish_rate: 25
+
+ dynamics_pid_backstepping_controller:
+ type: twil_controllers/DynamicsPIDBacksteppingController
+ joints:
+ - left_wheel_joint
+ - right_wheel_joint
+
+ feedback_linearization:
+ A: -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
--- /dev/null
+mini_twil:
+ joint_state_controller:
+ type: joint_state_controller/JointStateController
+ publish_rate: 25
+
+ dynamics_pid_controller:
+ type: twil_controllers/DynamicsPIDController
+ joints:
+ - left_wheel_joint
+ - right_wheel_joint
+
+ feedback_linearization:
+ A: -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
+
+
+
--- /dev/null
+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
--- /dev/null
+mini_twil:
+
+ joint_state_controller:
+ type: joint_state_controller/JointStateController
+ publish_rate: 25
+
+ left_wheel_joint_velocity_controller:
+ type: effort_controllers/JointVelocityController
+ joint: left_wheel_joint
+ pid:
+ p: 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
+
+
--- /dev/null
+mini_twil:
+ joint_state_controller:
+ type: joint_state_controller/JointStateController
+ publish_rate: 25
+
+ cart_linearizing_controller:
+ type: twil_controllers/CartLinearizingController
+ joints:
+ - left_wheel_joint
+ - right_wheel_joint
+
+ 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
+
--- /dev/null
+mini_twil:
+
+ joint_state_controller:
+ type: joint_state_controller/JointStateController
+ publish_rate: 25
+
+ twist_pid_backstepping_controller:
+ type: twil_controllers/TwistPIDBacksteppingController
+ joints:
+ - left_wheel_joint
+ - right_wheel_joint
+
+ left_wheel_joint_pid_parameters:
+ p: 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
--- /dev/null
+mini_twil:
+ joint_state_controller:
+ type: joint_state_controller/JointStateController
+ publish_rate: 25
+
+ twist_pid_controller:
+ type: twil_controllers/TwistPIDController
+ joints:
+ - left_wheel_joint
+ - right_wheel_joint
+
+ 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
--- /dev/null
+twil:
+
+ joint_state_controller:
+ type: joint_state_controller/JointStateController
+ publish_rate: 100
+
+ dynamics_pid_backstepping_controller:
+ type: twil_controllers/DynamicsPIDBacksteppingController
+ joints:
+ - left_wheel_joint
+ - right_wheel_joint
+
+ feedback_linearization:
+ A: -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
--- /dev/null
+twil:
+ joint_state_controller:
+ type: joint_state_controller/JointStateController
+ publish_rate: 100
+
+ dynamics_pid_controller:
+ type: twil_controllers/DynamicsPIDController
+ joints:
+ - left_wheel_joint
+ - right_wheel_joint
+
+ feedback_linearization:
+ A: -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
+
+
+
--- /dev/null
+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
--- /dev/null
+twil:
+
+ joint_state_controller:
+ type: joint_state_controller/JointStateController
+ publish_rate: 100
+
+ left_wheel_joint_velocity_controller:
+ type: effort_controllers/JointVelocityController
+ joint: left_wheel_joint
+ pid:
+ p: 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
+
+
--- /dev/null
+twil:
+ joint_state_controller:
+ type: joint_state_controller/JointStateController
+ publish_rate: 100
+
+ cart_linearizing_controller:
+ type: twil_controllers/CartLinearizingController
+ joints:
+ - left_wheel_joint
+ - right_wheel_joint
+
+ velocity_filter_taps: 5
+
+ feedback_linearization:
+ A: -14.778419353034346
+ B: 0.009650740074675
+ C: 0.133440027500232
+ D: -9.731786200206576
+ E: -13.056880796631793
+ F: 0.666386266954281
--- /dev/null
+twil:
+
+ joint_state_controller:
+ type: joint_state_controller/JointStateController
+ publish_rate: 100
+
+ twist_pid_backstepping_controller:
+ type: twil_controllers/TwistPIDBacksteppingController
+ joints:
+ - left_wheel_joint
+ - right_wheel_joint
+
+ left_wheel_joint_pid_parameters:
+ p: 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
--- /dev/null
+twil:
+ joint_state_controller:
+ type: joint_state_controller/JointStateController
+ publish_rate: 100
+
+ twist_pid_controller:
+ type: twil_controllers/TwistPIDController
+ joints:
+ - left_wheel_joint
+ - right_wheel_joint
+
+ 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
--- /dev/null
+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
--- /dev/null
+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
+
--- /dev/null
+mini_twil:
+
+ wheel_radius: 0.0325
+ robot_baseline: 0.198
--- /dev/null
+twil:
+
+ wheel_radius: 0.075
+ robot_baseline: 0.285
--- /dev/null
+#ifndef JOY_H
+#define JOY_H
+
+#include <cstdio>
+#include <iostream>
+
+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
--- /dev/null
+<?xml version="1.0" encoding="UTF-8"?>
+
+<launch>
+ <!-- Environment Variables -->
+ <arg name="Robot_Name" value="$(env robot_config)" />
+
+ <!-- Robot HW Launcher -->
+ <include file="$(find twil_bringup)/launch/robot_hw.launch">
+ </include>
+
+ <!-- Controller Parameters -->
+ <rosparam file="$(find twil_bringup)/config/controllers/$(arg Robot_Name)_dynamics_pid_backstepping_control.yaml" command="load"/>
+
+ <!-- Robot Control Node -->
+ <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
+ output="screen" ns="$(arg Robot_Name)" args="dynamics_pid_backstepping_controller joint_state_controller">
+ </node>
+
+ <!-- State Publisher Node -->
+ <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" ns="$(arg Robot_Name)" />
+
+</launch>
--- /dev/null
+<?xml version="1.0" encoding="UTF-8"?>
+
+<launch>
+ <!-- Environment Variables -->
+ <arg name="Robot_Name" value="$(env robot_config)" />
+
+ <!-- Robot HW Launcher -->
+ <include file="$(find twil_bringup)/launch/robot_hw.launch">
+ </include>
+
+ <!-- Controller Parameters -->
+ <rosparam file="$(find twil_bringup)/config/controllers/$(arg Robot_Name)_dynamics_pid_control.yaml" command="load"/>
+
+ <!-- Robot Control Node -->
+ <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
+ output="screen" ns="$(arg Robot_Name)" args="dynamics_pid_controller joint_state_controller">
+ </node>
+
+ <!-- State Publisher Node -->
+ <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" ns="$(arg Robot_Name)"/>
+
+</launch>
--- /dev/null
+<?xml version="1.0" encoding="UTF-8"?>
+
+<launch>
+ <!-- Environment Variables -->
+ <arg name="Robot_Name" value="$(env robot_config)" />
+
+ <!-- Robot HW Launcher -->
+ <include file="$(find twil_bringup)/launch/robot_hw.launch">
+ </include>
+
+ <!-- Controller Parameters -->
+ <rosparam file="$(find twil_bringup)/config/controllers/$(arg Robot_Name)_effort_control.yaml" command="load"/>
+
+ <!-- Robot Control Node -->
+ <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
+ output="screen" ns="$(arg Robot_Name)" args="left_wheel_joint_effort_controller right_wheel_joint_effort_controller joint_state_controller">
+ </node>
+
+ <!-- State Publisher Node -->
+ <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" ns="$(arg Robot_Name)"/>
+
+</launch>
--- /dev/null
+<?xml version="1.0" encoding="UTF-8"?>
+
+<launch>
+ <!-- Environment Variables -->
+ <arg name="Robot_Name" value="$(env robot_config)" />
+
+ <!-- Robot HW Launcher -->
+ <include file="$(find twil_bringup)/launch/robot_hw.launch">
+ </include>
+
+ <!-- Controller Parameters -->
+ <rosparam file="$(find twil_bringup)/config/controllers/$(arg Robot_Name)_joint_velocity_control.yaml" command="load"/>
+
+ <!-- Robot Control Node -->
+ <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
+ output="screen" ns="$(arg Robot_Name)" args="left_wheel_joint_velocity_controller right_wheel_joint_velocity_controller joint_state_controller">
+ </node>
+
+ <!-- State Publisher Node -->
+ <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" ns="$(arg Robot_Name)"/>
+
+</launch>
--- /dev/null
+<?xml version="1.0" encoding="UTF-8"?>
+
+<launch>
+ <!-- Environment Variables -->
+ <arg name="Robot_Name" value="$(env robot_config)" />
+
+ <!-- Robot HW Launcher -->
+ <include file="$(find twil_bringup)/launch/robot_hw.launch">
+ </include>
+
+ <!-- Controller Parameters -->
+ <rosparam file="$(find twil_bringup)/config/controllers/$(arg Robot_Name)_linearizing_control.yaml" command="load"/>
+
+ <!-- Robot Control Node -->
+ <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
+ output="screen" ns="$(arg Robot_Name)" args="cart_linearizing_controller joint_state_controller">
+ </node>
+
+ <!-- State Publisher Node -->
+ <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" ns="$(arg Robot_Name)"/>
+
+</launch>
--- /dev/null
+<?xml version="1.0" encoding="UTF-8"?>
+
+<launch>
+ <!-- Environment Variables -->
+ <arg name="Robot_Name" value="$(env robot_config)" />
+
+ <!-- Robot HW Launcher -->
+ <include file="$(find twil_bringup)/launch/robot_hw.launch">
+ </include>
+
+ <!-- Controller Parameters -->
+ <rosparam file="$(find twil_bringup)/config/controllers/$(arg Robot_Name)_twist_pid_backstepping_control.yaml" command="load"/>
+
+ <!-- Robot Control Node -->
+ <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
+ output="screen" ns="$(arg Robot_Name)" args="twist_pid_backstepping_controller joint_state_controller">
+ </node>
+
+ <!-- State Publisher Node -->
+ <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" ns="$(arg Robot_Name)">
+ </node>
+
+
+</launch>
--- /dev/null
+<?xml version="1.0" encoding="UTF-8"?>
+
+<launch>
+ <!-- Environment Variables -->
+ <arg name="Robot_Name" value="$(env robot_config)" />
+
+ <!-- Robot HW Launcher -->
+ <include file="$(find twil_bringup)/launch/robot_hw.launch">
+ </include>
+
+ <!-- Controller Parameters -->
+ <rosparam file="$(find twil_bringup)/config/controllers/$(arg Robot_Name)_twist_pid_control.yaml" command="load"/>
+
+ <!-- Robot Control Node -->
+ <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
+ output="screen" ns="$(arg Robot_Name)" args="twist_pid_controller joint_state_controller">
+ </node>
+
+ <!-- State Publisher Node -->
+ <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" ns="$(arg Robot_Name)"/>
+
+</launch>
--- /dev/null
+<?xml version="1.0" encoding="UTF-8" standalone="no"?>
+<launch>
+<!-- Turtlesim Node-->
+
+ <arg name="Robot_Name" value="$(env robot_config)" />
+
+ <!-- joy node -->
+ <node respawn="true" pkg="joy" type="joy_node" name="joy" output="screen">
+ <param name="dev" type="string" value="/dev/input/js0" />
+ <!-- <param name="deadzone" value="0.12" /> -->
+ </node>
+
+ <!-- Axes -->
+
+<!-- <group if="$(arg Robot_Name)==twil">-->
+ <param name="axis_linear" value="1" type="int"/>
+ <param name="axis_angular" value="0" type="int"/>
+ <param name="scale_linear" value="0.2" type="double"/>
+ <param name="scale_angular" value="1.5" type="double"/>
+<!-- </group>-->
+
+<!-- <group if="$(arg Robot_Name)==mini_twil">-->-->
+<!-- <param name="axis_linear" value="1" type="int"/>
+ <param name="axis_angular" value="0" type="int"/>
+ <param name="scale_linear" value="0.8" type="double"/>
+ <param name="scale_angular" value="6.0" type="double"/>-->
+<!-- </group> -->
+
+<node pkg="twil_bringup" type="twil_joystick_node" name="twil_joystick_node" output="screen"/>
+</launch>
--- /dev/null
+<?xml version="1.0" encoding="UTF-8"?>
+
+<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen" clear_params="true">
+ <param name="controller_frequency" value="10.0" />
+ <param name="controller_patience" value="15.0" />
+ <param name="planner_frequency" value="2.0" />
+ <param name="clearing_rotation_allowed" value="false" />
+ <param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS" />
+ <rosparam file="$(find course_p3at_navigation)/sg_costmap_common_params_p3at.yaml" command="load" ns="global_costmap" />
+ <rosparam file="$(find course_p3at_navigation)/sg_costmap_common_params_p3at.yaml" command="load" ns="local_costmap" />
+ <rosparam file="$(find course_p3at_navigation)/sg_local_costmap_params.yaml" command="load"/>
+ <rosparam file="$(find course_p3at_navigation)/sg_global_costmap_params.yaml" command="load"/>
+ <rosparam file="$(find course_p3at_navigation)/dwa_planner_ros.yaml" command="load" />
+</node>
--- /dev/null
+<?xml version="1.0" encoding="UTF-8"?>
+
+<launch>
+ <!-- Environment Variables -->
+ <arg name="Robot_Name" value="$(env robot_config)" />
+ <arg name="Log_Date" value="$(env robot_date)" />
+
+ <!-- Robot Log Parameters -->
+ <param name="robot_config" value="$(arg Robot_Name)"/>
+ <param name="log_date" value="$(arg Log_Date)"/>
+
+ <!-- Model Parameters -->
+ <param name="robot_description" command="$(find xacro)/xacro.py '$(find twil_description)/xacro/twil_real.urdf.xacro'" />
+ <!-- Mechanical Parameters -->
+ <rosparam file="$(find twil_bringup)/config/mechanics/$(arg Robot_Name)_specs.yaml" command="load"/>
+ <!-- Electronic Parameters -->
+ <rosparam file="$(find twil_bringup)/config/electronics/$(arg Robot_Name)_hw.yaml" command="load"/>
+
+ <!-- Robot Control Node -->
+ <node name="twil_hw_node" ns="$(arg Robot_Name)" pkg="twil_hw" type="twil_hw_node" output="screen" />
+</launch>
--- /dev/null
+<?xml version="1.0" encoding="UTF-8"?>
+
+<launch>
+ <!-- Environment Variables -->
+ <arg name="Robot_Name" value="$(env robot_config)" />
+
+ <!-- Trajectory Publisher Node -->
+ <node name="robot_trajectory_path" pkg="twil_trajectories" type="circle_trajectory" ns="$(arg Robot_Name)">
+ <param name="pc_x" value="0.0"/>
+ <param name="pc_y" value="0.0"/>
+ <param name="phi0" value="0.0"/>
+ <param name="r" value="0.25"/>
+ <param name="w" value="0.8"/>
+ <param name="loop_rate" value="20"/>
+ </node>
+
+</launch>
--- /dev/null
+<?xml version="1.0" encoding="UTF-8"?>
+
+<launch>
+ <!-- Environment Variables -->
+ <arg name="Robot_Name" value="$(env robot_config)" />
+
+ <!-- Trajectory Publisher Node -->
+ <node name="robot_trajectory_path" pkg="twil_trajectories" type="eight_trajectory" ns="$(arg Robot_Name)">
+ <param name="pc_x" value="0.0"/>
+ <param name="pc_y" value="0.0"/>
+ <param name="phi0" value="0.0"/>
+ <param name="r" value="0.15"/>
+ <param name="w" value="0.6"/>
+ <param name="loop_rate" value="20"/>
+ </node>
+
+</launch>
--- /dev/null
+<?xml version="1.0" encoding="UTF-8"?>
+
+<launch>
+ <!-- Environment Variables -->
+ <arg name="Robot_Name" value="$(env robot_config)" />
+
+ <!-- Trajectory Publisher Node -->
+ <node name="robot_trajectory_path" pkg="twil_trajectories" type="circle_trajectory" ns="$(arg Robot_Name)">
+ <param name="pc_x" value="0.0"/>
+ <param name="pc_y" value="0.0"/>
+ <param name="phi0" value="0.0"/>
+ <param name="r" value="0.3"/>
+ <param name="w" value="0.4"/>
+ <param name="loop_rate" value="20"/>
+ </node>
+
+</launch>
--- /dev/null
+<?xml version="1.0" encoding="UTF-8"?>
+
+<launch>
+ <!-- Environment Variables -->
+ <arg name="Robot_Name" value="$(env robot_config)" />
+
+ <!-- Trajectory Publisher Node -->
+ <node name="robot_trajectory_path" pkg="twil_trajectories" type="eight_trajectory" ns="$(arg Robot_Name)">
+ <param name="pc_x" value="0.0"/>
+ <param name="pc_y" value="0.0"/>
+ <param name="phi0" value="0.0"/>
+ <param name="r" value="0.3"/>
+ <param name="w" value="0.4"/>
+ <param name="loop_rate" value="20"/>
+
+ </node>
+
+</launch>
--- /dev/null
+<?xml version="1.0"?>
+<package>
+ <name>twil_bringup</name>
+ <version>0.0.0</version>
+ <description>The twil_bringup package</description>
+
+ <!-- One maintainer tag required, multiple allowed, one person per tag -->
+ <!-- Example: -->
+ <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+ <maintainer email="talves@todo.todo">talves</maintainer>
+
+
+ <!-- One license tag required, multiple allowed, one license per tag -->
+ <!-- Commonly used license strings: -->
+ <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+ <license>TODO</license>
+
+
+ <!-- Url tags are optional, but multiple are allowed, one per tag -->
+ <!-- Optional attribute type can be: website, bugtracker, or repository -->
+ <!-- Example: -->
+ <!-- <url type="website">http://wiki.ros.org/twil_bringup</url> -->
+
+
+ <!-- Author tags are optional, multiple are allowed, one per tag -->
+ <!-- Authors do not have to be maintainers, but could be -->
+ <!-- Example: -->
+ <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+ <!-- The *_depend tags are used to specify dependencies -->
+ <!-- Dependencies can be catkin packages or system dependencies -->
+ <!-- Examples: -->
+ <!-- Use build_depend for packages you need at compile time: -->
+ <!-- <build_depend>message_generation</build_depend> -->
+ <!-- Use buildtool_depend for build tool packages: -->
+ <!-- <buildtool_depend>catkin</buildtool_depend> -->
+ <!-- Use run_depend for packages you need at runtime: -->
+ <!-- <run_depend>message_runtime</run_depend> -->
+ <!-- Use test_depend for packages you need only for testing: -->
+ <!-- <test_depend>gtest</test_depend> -->
+ <buildtool_depend>catkin</buildtool_depend>
+ <build_depend>controller_interface</build_depend>
+ <build_depend>controller_manager</build_depend>
+ <build_depend>hardware_interface</build_depend>
+ <build_depend>joint_state_controller</build_depend>
+ <build_depend>roscpp</build_depend>
+ <build_depend>rospy</build_depend>
+ <build_depend>std_msgs</build_depend>
+ <build_depend>twil_hw</build_depend>
+ <build_depend>sensor_msgs</build_depend>
+ <build_depend>trajectory_msgs</build_depend>
+ <build_depend>geometry_msgs</build_depend>
+
+ <run_depend>controller_interface</run_depend>
+ <run_depend>controller_manager</run_depend>
+ <run_depend>hardware_interface</run_depend>
+ <run_depend>joint_state_controller</run_depend>
+ <run_depend>roscpp</run_depend>
+ <run_depend>rospy</run_depend>
+ <run_depend>std_msgs</run_depend>
+ <run_depend>twil_hw</run_depend>
+ <run_depend>sensor_msgs</run_depend>
+ <run_depend>trajectory_msgs</run_depend>
+ <run_depend>geometry_msgs</run_depend>
+
+ <!-- The export tag contains other, unspecified, tags -->
+ <export>
+ <!-- Other tools can request additional information be placed here -->
+
+ </export>
+</package>
--- /dev/null
+#include <ros/ros.h>
+#include <geometry_msgs/Twist.h>
+#include <geometry_msgs/Pose.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <sensor_msgs/Joy.h>
+#include <std_msgs/Float64.h>
+#include "twil_bringup/joy.h"
+#include <ros/console.h>
+
+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<geometry_msgs::Twist>("/twil/dynamics_pid_controller/cmd_vel", 1);
+ vel_pub2 = nh_.advertise<geometry_msgs::Twist>("/twil/twist_pid_controller/cmd_vel", 1);
+ vel_pub3 = nh_.advertise<geometry_msgs::Twist>("/mini_twil/dynamics_pid_controller/cmd_vel", 1);
+ vel_pub4 = nh_.advertise<geometry_msgs::Twist>("/mini_twil/twist_pid_controller/cmd_vel", 1);
+
+
+ joy_sub_ = nh_.subscribe<sensor_msgs::Joy>("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();
+}
--- /dev/null
+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)
--- /dev/null
+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))}'
--- /dev/null
+/******************************************************************************
+ Twil Controllers
+ Linearizing Controller
+
+ Copyright (C) 2017 Tiago G. Alves <talves@ece.ufrgs.br>
+
+ This program is free software; you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation; either version 2 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program; if not, write to the Free Software
+ Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+ You can also obtain a copy of the GNU General Public License
+ at <http://www.gnu.org/licenses>.
+
+*******************************************************************************/
+
+#ifndef TWIL_CONTROLLERS_CART_LINEARIZING_CONTROLLER_H
+#define TWIL_CONTROLLERS_CART_LINEARIZING_CONTROLLER_H
+
+#include <ros/ros.h>
+#include <hardware_interface/joint_command_interface.h>
+#include <controller_interface/controller.h>
+#include <std_msgs/Float64MultiArray.h>
+#include <sensor_msgs/JointState.h>
+#include "Eigen/Dense"
+
+#include "state_feedback_linearization.h"
+#include "velocity_transformation.h"
+
+#define CART_LIN_DEBUG
+
+#ifdef CART_LIN_DEBUG
+#include <fstream>
+#endif
+
+namespace twil_controllers
+{
+class CartLinearizingController: public controller_interface::Controller<hardware_interface::EffortJointInterface>
+{
+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<hardware_interface::JointHandle> 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<double> WheelRadius;
+ double WheelBase;
+};
+}
+#endif
--- /dev/null
+/******************************************************************************
+ Twil Controllers
+ Linearizing Backstep Controller
+
+ Copyright (C) 2017 Tiago G. Alves <talves@ece.ufrgs.br>
+
+ This program is free software; you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation; either version 2 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program; if not, write to the Free Software
+ Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+ You can also obtain a copy of the GNU General Public License
+ at <http://www.gnu.org/licenses>.
+
+*******************************************************************************/
+
+#ifndef DYNAMICS_PID_BACKSTEPPING_CONTROLLER_H
+#define DYNAMICS_PID_BACKSTEPPING_CONTROLLER_H
+
+#include <ros/ros.h>
+#include <hardware_interface/joint_command_interface.h>
+#include <controller_interface/controller.h>
+#include <control_toolbox/pid.h>
+#include <geometry_msgs/Pose2D.h>
+#include <geometry_msgs/PoseStamped.h>
+#include "twil_controllers/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 <fstream>
+#endif
+
+
+namespace twil_controllers{
+
+class DynamicsPIDBacksteppingController: public controller_interface::Controller<hardware_interface::EffortJointInterface>
+{
+
+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<hardware_interface::JointHandle> joints_;
+ std::string robot_ns;
+
+ /* Dynamics PID Controller Variables */
+ StateFeedbackLinearization * feedback_linearization;
+ Eigen::Vector2d Effort;
+ Eigen::Vector2d nu_ref;
+ Eigen::Vector2d u_ref;
+ Mat2_4 Fu;
+
+ /* 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
--- /dev/null
+/******************************************************************************
+ Twil Controllers
+ Dynamics PID Controller
+
+ Copyright (C) 2017 Tiago G. Alves <talves@ece.ufrgs.br>
+
+ This program is free software; you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation; either version 2 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program; if not, write to the Free Software
+ Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+ You can also obtain a copy of the GNU General Public License
+ at <http://www.gnu.org/licenses>.
+
+*******************************************************************************/
+
+#ifndef DYNAMICS_PID_CONTROLLER_H
+#define DYNAMICS_PID_CONTROLLER_H
+
+#include <ros/ros.h>
+#include <geometry_msgs/Twist.h>
+#include <hardware_interface/joint_command_interface.h>
+#include <controller_interface/controller.h>
+#include <control_toolbox/pid.h>
+#include <Eigen/Dense>
+#include "velocity_transformation.h"
+#include "state_feedback_linearization.h"
+
+#define DYNAMICS_PID_DEBUG
+
+#ifdef DYNAMICS_PID_DEBUG
+#include <fstream>
+#endif
+
+namespace twil_controllers
+{
+class DynamicsPIDController: public controller_interface::Controller<hardware_interface::EffortJointInterface>
+{
+
+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<hardware_interface::JointHandle> 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<double> WheelRadius;
+ double WheelBase;
+};
+}
+#endif
--- /dev/null
+/******************************************************************************
+ Twil Controllers
+ Nonlinear Kinematic Controller
+
+ Copyright (C) 2017 Tiago G. Alves <talves@ece.ufrgs.br>
+
+ This program is free software; you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation; either version 2 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program; if not, write to the Free Software
+ Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+ You can also obtain a copy of the GNU General Public License
+ at <http://www.gnu.org/licenses>.
+
+*******************************************************************************/
+
+#ifndef NON_SMOOTH_CONTROL_H
+#define NON_SMOOTH_CONTROL_H
+
+#include "Eigen/Dense"
+#include <cstdio>
+#include <cmath>
+
+//#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]: "<<lambda << gamma << std::endl;
+ }
+
+ ~NonSmoothControl();
+
+ Eigen::Vector2d compute(const Eigen::Vector3d &xc, const Eigen::Vector3d &x_r)
+ {
+
+ double e=0, psi=0, alpha=0;
+ Eigen::Vector2d u_r;
+ u_r.setZero();
+
+ if (x_r != xc){
+ Eigen::Vector3d xc_bar = coordinate_change(xc, x_r);
+
+ e = sqrt( pow(xc_bar[0],2) + pow(xc_bar[1],2) );
+ psi = atan2(xc_bar[1],xc_bar[0]);
+ alpha = xc_bar[2]-psi;
+
+ // Compute velocity references
+ u_r[0] = -gam[0]*e*cos(alpha);
+ u_r[1] = -(gam[1]*alpha) -(gam[0]*sin(alpha)*cos(alpha));
+
+ // Avoid singularity
+ u_r[1] += (fabs(alpha) > 1e-6) ?
+ (gam[0]*(lamb[2]/lamb[1])*cos(alpha)*(sin(alpha)/alpha)*psi) : (gam[0]*(lamb[2]/lamb[1])*cos(alpha)*(1)*psi);
+ }
+
+ 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 <<std::endl;
+ // std::cout << "X: " << x << " - X_R: " << x_r << " - X_bar: " << x_bar << std::endl;
+#endif
+ return x_bar;
+ }
+
+ Eigen::Vector3d lamb;
+ Eigen::Vector2d gam;
+
+};
+
+#endif
--- /dev/null
+#ifndef NONLINEAR_BACKSTEPPING_CONTROLLER_H
+#define NONLINEAR_BACKSTEPPING_CONTROLLER_H
+
+
+#endif
--- /dev/null
+/******************************************************************************
+ Twil Controllers
+ Odometry Tool
+
+ Copyright (C) 2017 Tiago G. Alves <talves@ece.ufrgs.br>
+
+ This program is free software; you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation; either version 2 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program; if not, write to the Free Software
+ Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+ You can also obtain a copy of the GNU General Public License
+ at <http://www.gnu.org/licenses>.
+
+*******************************************************************************/
+
+#ifndef ODOMETRY_TOOL_H
+#define ODOMETRY_TOOL_H
+
+#include <ros/ros.h>
+#include <cstring>
+#include <ros/time.h>
+#include <Eigen/Dense>
+#include <cmath>
+#include <realtime_tools/realtime_publisher.h>
+#include <geometry_msgs/Pose2D.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <geometry_msgs/PoseWithCovariance.h>
+#include <geometry_msgs/PoseWithCovarianceStamped.h>
+#include <nav_msgs/Odometry.h>
+#include "twil_controllers/pose_tool.h"
+#include <tf/tfMessage.h>
+
+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<realtime_tools::RealtimePublisher<tf::tfMessage> > tf_odom_pub;
+ boost::shared_ptr< realtime_tools::RealtimePublisher<nav_msgs::Odometry> >odom_pub;
+// boost::shared_ptr< realtime_tools::RealtimePublisher<geometry_msgs::PoseWithCovarianceStamped> >pose_pub;
+};
+
+
+}
+#endif // ODOMETRY_H
--- /dev/null
+#ifndef POSE_TOOL_H
+#define POSE_TOOL_H
+
+#include <Eigen/Dense>
+#include <cmath>
+#include <geometry_msgs/PoseWithCovariance.h>
+#include <geometry_msgs/Pose2D.h>
+
+//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
--- /dev/null
+/******************************************************************************
+ Twil Controllers
+ State Feedback Linearization
+
+ Copyright (C) 2017 Tiago G. Alves <talves@ece.ufrgs.br>
+
+ This program is free software; you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation; either version 2 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program; if not, write to the Free Software
+ Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+ You can also obtain a copy of the GNU General Public License
+ at <http://www.gnu.org/licenses>.
+
+*******************************************************************************/
+
+
+#ifndef STATE_FEEDBACK_LINEARIZATION_H
+#define STATE_FEEDBACK_LINEARIZATION_H
+
+#include <cstdio>
+#include <iostream>
+#include "Eigen/Dense"
+
+typedef Eigen::Matrix<double,2,4> 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
--- /dev/null
+/******************************************************************************
+ Twil Controllers
+ PID Backstepping Controller
+
+ Copyright (C) 2017 Tiago G. Alves <talves@ece.ufrgs.br>
+
+ This program is free software; you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation; either version 2 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program; if not, write to the Free Software
+ Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+ You can also obtain a copy of the GNU General Public License
+ at <http://www.gnu.org/licenses>.
+
+*******************************************************************************/
+
+#ifndef TWIST_PID_BACKSTEPPING_CONTROLLER_H
+#define TWIST_PID_BACKSTEPPING_CONTROLLER_H
+
+#include <ros/ros.h>
+#include <hardware_interface/joint_command_interface.h>
+#include <controller_interface/controller.h>
+#include <control_toolbox/pid.h>
+#include <Eigen/Dense>
+#include <geometry_msgs/Pose2D.h>
+#include <geometry_msgs/PoseStamped.h>
+#include "twil_controllers/odometry_tool.h"
+#include "twil_controllers/pose_tool.h"
+#include "twil_controllers/velocity_transformation.h"
+#include "non_smooth_control.h"
+
+
+
+#define TWIST_PID_BACKSTEPPING_CONTROLLER_DEBUG
+
+#ifdef TWIST_PID_BACKSTEPPING_CONTROLLER_DEBUG
+#include <fstream>
+#endif
+
+namespace twil_controllers{
+
+class TwistPIDBacksteppingController: public controller_interface::Controller<hardware_interface::EffortJointInterface>
+{
+
+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<hardware_interface::JointHandle> 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
+
--- /dev/null
+/******************************************************************************
+ Twil Controllers
+ Twist PID Controller
+
+ Copyright (C) 2017 Tiago G. Alves <talves@ece.ufrgs.br>
+
+ This program is free software; you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation; either version 2 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program; if not, write to the Free Software
+ Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+ You can also obtain a copy of the GNU General Public License
+ at <http://www.gnu.org/licenses>.
+
+*******************************************************************************/
+
+#ifndef DYNAMICS_CONTROLLER_
+#define DYNAMICS_CONTROLLER_H
+
+#include <ros/ros.h>
+#include <geometry_msgs/Twist.h>
+#include <hardware_interface/joint_command_interface.h>
+#include <controller_interface/controller.h>
+#include <control_toolbox/pid.h>
+#include <Eigen/Dense>
+#include "velocity_transformation.h"
+#include "state_feedback_linearization.h"
+
+#define TWIST_PID_DEBUG
+
+#ifdef TWIST_PID_DEBUG
+#include <fstream>
+#endif
+
+typedef Eigen::Matrix<double,2,4> Mat2_4;
+
+namespace twil_controllers
+{
+class TwistPIDController: public controller_interface::Controller<hardware_interface::EffortJointInterface>
+{
+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<hardware_interface::JointHandle> 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
+
--- /dev/null
+/******************************************************************************
+ Twil Controllers
+ Velocity Transformations
+
+ Copyright (C) 2017 Tiago G. Alves <talves@ece.ufrgs.br>
+
+ This program is free software; you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation; either version 2 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program; if not, write to the Free Software
+ Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+ You can also obtain a copy of the GNU General Public License
+ at <http://www.gnu.org/licenses>.
+
+*******************************************************************************/
+
+
+#ifndef VELOCITY_TRANSFORMATION_H
+#define VELOCITY_TRANSFORMATION_H
+
+#include <cmath>
+#include <cstdio>
+#include <Eigen/Dense>
+
+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
--- /dev/null
+<?xml version="1.0"?>
+<package>
+ <name>twil_controllers</name>
+ <version>2.0.0</version>
+ <description>The twil_controllers package</description>
+
+ <!-- One maintainer tag required, multiple allowed, one person per tag -->
+ <!-- Example: -->
+ <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+ <maintainer email="fetter@ece.ufrgs.br">Walter Fetter Lages</maintainer>
+
+
+ <!-- One license tag required, multiple allowed, one license per tag -->
+ <!-- Commonly used license strings: -->
+ <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+ <license>GPLv3</license>
+
+
+ <!-- Url tags are optional, but mutiple are allowed, one per tag -->
+ <!-- Optional attribute type can be: website, bugtracker, or repository -->
+ <!-- Example: -->
+ <!-- <url type="website">http://wiki.ros.org/twil_controllers</url> -->
+
+
+ <!-- Author tags are optional, mutiple are allowed, one per tag -->
+ <!-- Authors do not have to be maintianers, but could be -->
+ <!-- Example: -->
+ <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+ <author email="fetter@ece.ufrgs.br">Walter Fetter Lages</author>
+
+
+ <!-- The *_depend tags are used to specify dependencies -->
+ <!-- Dependencies can be catkin packages or system dependencies -->
+ <!-- Examples: -->
+ <!-- Use build_depend for packages you need at compile time: -->
+ <!-- <build_depend>message_generation</build_depend> -->
+ <!-- Use buildtool_depend for build tool packages: -->
+ <!-- <buildtool_depend>catkin</buildtool_depend> -->
+ <!-- Use run_depend for packages you need at runtime: -->
+ <!-- <run_depend>message_runtime</run_depend> -->
+ <!-- Use test_depend for packages you need only for testing: -->
+ <!-- <test_depend>gtest</test_depend> -->
+ <buildtool_depend>catkin</buildtool_depend>
+
+ <build_depend>controller_interface</build_depend>
+ <build_depend>effort_controllers</build_depend>
+ <build_depend>kdl_parser</build_depend>
+ <build_depend>orocos_kdl</build_depend>
+ <build_depend>twil_gazebo_ros_control</build_depend>
+ <build_depend>control_toolbox</build_depend>
+
+
+ <build_depend>cmake_modules</build_depend>
+
+
+ <run_depend>controller_interface</run_depend>
+ <run_depend>controller_manager</run_depend>
+ <run_depend>effort_controllers</run_depend>
+ <run_depend>joint_state_controller</run_depend>
+ <run_depend>kdl_parser</run_depend>
+ <run_depend>orocos_kdl</run_depend>
+ <run_depend>twil_gazebo_ros_control</run_depend>
+ <run_depend>control_toolbox</run_depend>
+
+ <run_depend>cmake_modules</run_depend>
+
+
+
+ <!-- The export tag contains other, unspecified, tags -->
+ <export>
+ <!-- You can specify that this package is a metapackage here: -->
+ <!-- <metapackage/> -->
+
+ <!-- Other tools can request additional information be placed here -->
+ <controller_interface plugin="${prefix}/twil_controllers_plugins.xml"/>
+ </export>
+</package>
--- /dev/null
+rostopic pub /twil/cart_linearizing_controller/command std_msgs/Float64MultiArray '{data: [0.01, 0.00]}'
--- /dev/null
+/******************************************************************************
+ Twil Controllers
+ Linearizing Controller
+
+ Copyright (C) 2017 Tiago G. Alves <talves@ece.ufrgs.br>
+
+ This program is free software; you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation; either version 2 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program; if not, write to the Free Software
+ Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+ You can also obtain a copy of the GNU General Public License
+ at <http://www.gnu.org/licenses>.
+
+*******************************************************************************/
+
+#include <twil_controllers/cart_linearizing_controller.h>
+#include <pluginlib/class_list_macros.h>
+#include <kdl_parser/kdl_parser.hpp>
+
+//#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)
--- /dev/null
+/******************************************************************************
+ Twil Controllers
+ Linearizing Backstep Controller
+
+ Copyright (C) 2017 Tiago G. Alves <talves@ece.ufrgs.br>
+
+ This program is free software; you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation; either version 2 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program; if not, write to the Free Software
+ Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+ You can also obtain a copy of the GNU General Public License
+ at <http://www.gnu.org/licenses>.
+
+*******************************************************************************/
+
+#include "twil_controllers/dynamics_pid_backstepping_controller.h"
+#include <pluginlib/class_list_macros.h>
+#include <kdl_parser/kdl_parser.hpp>
+#include <tf/tf.h>
+
+//#define URDF
+//#define CONTROLLER_OUTPUT_SCREEN
+
+namespace twil_controllers
+{
+
+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)
--- /dev/null
+/******************************************************************************
+ Twil Controllers
+ Dynamics PID Controller
+
+ Copyright (C) 2017 Tiago G. Alves <talves@ece.ufrgs.br>
+
+ This program is free software; you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation; either version 2 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program; if not, write to the Free Software
+ Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+ You can also obtain a copy of the GNU General Public License
+ at <http://www.gnu.org/licenses>.
+
+*******************************************************************************/
+
+#include <twil_controllers/dynamics_pid_controller.h>
+#include <pluginlib/class_list_macros.h>
+#include <kdl_parser/kdl_parser.hpp>
+
+//#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)
--- /dev/null
+#include "twil_controllers/nonlinear_backstepping_controller.h"
--- /dev/null
+/******************************************************************************
+ Twil Controllers
+ Odometry Tool
+
+ Copyright (C) 2017 Tiago G. Alves <talves@ece.ufrgs.br>
+
+ This program is free software; you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation; either version 2 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program; if not, write to the Free Software
+ Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+ You can also obtain a copy of the GNU General Public License
+ at <http://www.gnu.org/licenses>.
+
+*******************************************************************************/
+
+
+#include "twil_controllers/odometry_tool.h"
+#include <tf/transform_broadcaster.h>
+#include <tf/tf.h>
+#include <geometry_msgs/Quaternion.h>
+
+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<nav_msgs::Odometry>(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<tf::tfMessage>(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;
+
+}
+
+}
--- /dev/null
+#include "twil_controllers/pose_tool.h"
+#include <tf/transform_broadcaster.h>
+
+//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];
+//}
+
+
--- /dev/null
+/******************************************************************************
+ Twil Controllers
+ PID Backstepping Controller
+
+ Copyright (C) 2017 Tiago G. Alves <talves@ece.ufrgs.br>
+
+ This program is free software; you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation; either version 2 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program; if not, write to the Free Software
+ Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+ You can also obtain a copy of the GNU General Public License
+ at <http://www.gnu.org/licenses>.
+
+*******************************************************************************/
+
+#include "twil_controllers/twist_pid_backstepping_controller.h"
+#include <pluginlib/class_list_macros.h>
+#include <kdl_parser/kdl_parser.hpp>
+#include <tf/tf.h>
+
+//#define URDF
+//#define CONTROLLER_OUTPUT_SCREEN
+
+namespace twil_controllers
+{
+
+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)
+
--- /dev/null
+/******************************************************************************
+ Twil Controllers
+ Twist PID Controller
+
+ Copyright (C) 2017 Tiago G. Alves <talves@ece.ufrgs.br>
+
+ This program is free software; you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation; either version 2 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program; if not, write to the Free Software
+ Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+ You can also obtain a copy of the GNU General Public License
+ at <http://www.gnu.org/licenses>.
+
+*******************************************************************************/
+
+#include "twil_controllers/twist_pid_controller.h"
+#include <pluginlib/class_list_macros.h>
+#include <kdl_parser/kdl_parser.hpp>
+
+//#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)
--- /dev/null
+<library path="lib/libtwil_controllers">
+
+ <class name="twil_controllers/CartLinearizingController" type="twil_controllers::CartLinearizingController" base_class_type="controller_interface::ControllerBase">
+ <description>
+ Blá-blá-blá...
+ </description>
+ </class>
+
+ <class name="twil_controllers/DynamicsPIDController" type="twil_controllers::DynamicsPIDController" base_class_type="controller_interface::ControllerBase">
+ <description>
+ Blá-blá-blá...
+ </description>
+ </class>
+
+ <class name="twil_controllers/TwistPIDController" type="twil_controllers::TwistPIDController" base_class_type="controller_interface::ControllerBase">
+ <description>
+ Blá-blá-blá...
+ </description>
+ </class>
+
+ <class name="twil_controllers/TwistPIDBacksteppingController" type="twil_controllers::TwistPIDBacksteppingController" base_class_type="controller_interface::ControllerBase">
+ <description>
+ Blá-blá-blá...
+ </description>
+ </class>
+
+ <class name="twil_controllers/DynamicsPIDBacksteppingController" type="twil_controllers::DynamicsPIDBacksteppingController" base_class_type="controller_interface::ControllerBase">
+ <description>
+ Blá-blá-blá...
+ </description>
+ </class>
+
+<class name="twil_controllers/NonlinearBacksteppingController" type="twil_controllers::NonlinearBacksteppingController" base_class_type="controller_interface::ControllerBase">
+ <description>
+ Blá-blá-blá...
+ </description>
+</class>
+
+</library>
--- /dev/null
+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)
--- /dev/null
+<?xml version="1.0"?>
+<package>
+ <name>twil_description</name>
+ <version>2.0.0</version>
+ <description>The twil_description package</description>
+
+ <!-- One maintainer tag required, multiple allowed, one person per tag -->
+ <!-- Example: -->
+ <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+ <maintainer email="feter@ece.ufrgs.br">Walter Fetter Lages</maintainer>
+
+
+ <!-- One license tag required, multiple allowed, one license per tag -->
+ <!-- Commonly used license strings: -->
+ <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+ <license>GPLv3</license>
+
+
+ <!-- Url tags are optional, but mutiple are allowed, one per tag -->
+ <!-- Optional attribute type can be: website, bugtracker, or repository -->
+ <!-- Example: -->
+ <!-- <url type="website">http://wiki.ros.org/twil_description</url> -->
+
+
+ <!-- Author tags are optional, mutiple are allowed, one per tag -->
+ <!-- Authors do not have to be maintianers, but could be -->
+ <!-- Example: -->
+ <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+ <author email="feter@ece.ufrgs.br">Walter Fetter Lages</author>
+
+
+ <!-- The *_depend tags are used to specify dependencies -->
+ <!-- Dependencies can be catkin packages or system dependencies -->
+ <!-- Examples: -->
+ <!-- Use build_depend for packages you need at compile time: -->
+ <!-- <build_depend>message_generation</build_depend> -->
+ <!-- Use buildtool_depend for build tool packages: -->
+ <!-- <buildtool_depend>catkin</buildtool_depend> -->
+ <!-- Use run_depend for packages you need at runtime: -->
+ <!-- <run_depend>message_runtime</run_depend> -->
+ <!-- Use test_depend for packages you need only for testing: -->
+ <!-- <test_depend>gtest</test_depend> -->
+ <buildtool_depend>catkin</buildtool_depend>
+
+
+ <!-- The export tag contains other, unspecified, tags -->
+ <export>
+ <!-- You can specify that this package is a metapackage here: -->
+ <!-- <metapackage/> -->
+
+ <!-- Other tools can request additional information be placed here -->
+
+ </export>
+</package>
--- /dev/null
+<?xml version="1.0" ?>
+
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+ <xacro:macro name="battery_bosch_12v" params="name parent *origin">
+
+ <link name="${name}">
+ <inertial>
+ <origin xyz="-2.856E-07 0.0012183 -0.001734" rpy="0 0 0" />
+ <mass value="15" />
+ <inertia ixx="0.0730739985187771" ixy="1.98707075079408E-07" ixz="-2.54224589859575E-07" iyy="0.107634158285368" iyz="0.00135890388070998" izz="0.106246326145496" />
+ </inertial>
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://twil_description/meshes/battery_bosch_12v.stl" />
+ </geometry>
+ <material name="">
+ <color rgba="0.2 0.2 0.2 1" />
+ </material>
+ </visual>
+ <collision>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://twil_description/meshes/battery_bosch_12v.stl" />
+ </geometry>
+ </collision>
+ </link>
+
+ <joint name="${name}" type="fixed">
+ <xacro:insert_block name="origin" />
+ <parent link="${parent}"/>
+ <child link="${name}" />
+ </joint>
+
+ <gazebo reference="${name}">
+ <material>Gazebo/FlatBlack</material>
+ </gazebo>
+
+ </xacro:macro>
+
+</robot>
--- /dev/null
+<?xml version="1.0" ?>
+
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+ <xacro:macro name="castor_base" params="name parent *origin">
+
+ <link name="${name}">
+ <inertial>
+ <origin xyz="-0.018396 -6.7757E-19 0.049957" rpy="0 0 0" />
+ <mass value="0.41627" />
+ <inertia ixx="0.00064128" ixy="-1.9687E-21" ixz="0.00019015" iyy="0.00067742" iyz="-1.3315E-20" izz="0.00018951" />
+ </inertial>
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://twil_description/meshes/castor_base.stl" />
+ </geometry>
+ <material name="">
+ <color rgba="0.64706 0.61961 0.58431 1" />
+ </material>
+ </visual>
+ <collision>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://twil_description/meshes/castor_base.stl" />
+ </geometry>
+ </collision>
+ </link>
+
+ <joint name="${name}_joint" type="continuous">
+ <xacro:insert_block name="origin" />
+ <parent link="${parent}" />
+ <child link="${name}" />
+ <axis xyz="0 0 1" />
+ </joint>
+
+ <gazebo reference="${name}">
+ <material>Gazebo/White</material>
+ </gazebo>
+
+
+ </xacro:macro>
+
+</robot>
--- /dev/null
+<?xml version="1.0" ?>
+
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+ <xacro:macro name="castor_support" params="name parent *origin">
+
+ <link name="${name}">
+ <inertial>
+ <origin xyz="0 1.4861E-17 -0.025271" rpy="0 0 0" />
+ <mass value="1.1237" />
+ <inertia ixx="0.00090319" ixy="-1.2436E-19" ixz="1.4703E-20" iyy="0.00079089" iyz="6.5874E-21" izz="0.00075135" />
+ </inertial>
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://twil_description/meshes/castor_support.stl" />
+ </geometry>
+ <material name="">
+ <color rgba="0.64706 0.61961 0.58431 1" />
+ </material>
+ </visual>
+ <collision>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://twil_description/meshes/castor_support.stl" />
+ </geometry>
+ </collision>
+ </link>
+
+ <joint name="${name}" type="fixed">
+ <xacro:insert_block name="origin" />
+ <parent link="${parent}"/>
+ <child link="${name}" />
+ </joint>
+
+ <gazebo reference="${name}">
+ <material>Gazebo/Grey</material>
+ </gazebo>
+
+ </xacro:macro>
+
+</robot>
--- /dev/null
+<?xml version="1.0" ?>
+
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+ <xacro:macro name="castor_wheel" params="name parent *origin">
+
+ <link name="${name}">
+ <inertial>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <mass value="0.11047" />
+ <inertia ixx="4.8046E-05" ixy="0" ixz="0" iyy="8.3809E-05" iyz="0" izz="4.8046E-05" />
+ </inertial>
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://twil_description/meshes/castor_wheel.stl" />
+ </geometry>
+ <material name="">
+ <color rgba="0.75294 0.75294 0.75294 1" />
+ </material>
+ </visual>
+ <collision>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://twil_description/meshes/castor_wheel.stl" />
+ </geometry>
+ </collision>
+ </link>
+
+ <joint name="${name}_joint" type="continuous">
+ <xacro:insert_block name="origin" />
+ <parent link="${parent}" />
+ <child link="${name}" />
+ <axis xyz="0 1 0" />
+ </joint>
+
+ <gazebo reference="${name}">
+ <material>Gazebo/Grey</material>
+ </gazebo>
+
+ </xacro:macro>
+
+</robot>
--- /dev/null
+<?xml version="1.0" ?>
+
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+ <xacro:macro name="chassis" params="name parent *origin">
+
+ <link name="${name}">
+ <inertial>
+ <origin
+ xyz="6.3955E-06 -2.1963E-17 0.27338" rpy="0 0 0" />
+ <mass value="6.4923" />
+ <inertia ixx="0.67525" ixy="0.0014553" ixz="-0.00017525" iyy="0.69058" iyz="-6.3289E-18" izz="0.28611" />
+ </inertial>
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://twil_description/meshes/chassis.stl" />
+ </geometry>
+ <material name="">
+ <color rgba="0.75294 0.75294 0.75294 1" />
+ </material>
+ </visual>
+ <collision>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://twil_description/meshes/chassis.stl" />
+ </geometry>
+ </collision>
+ </link>
+
+ <joint name="${name}_joint" type="fixed">
+ <xacro:insert_block name="origin" />
+ <parent link="${parent}"/>
+ <child link="${name}"/>
+ </joint>
+
+ <gazebo reference="${name}">
+ <material>Gazebo/Gold</material>
+ </gazebo>
+
+ </xacro:macro>
+
+</robot>
--- /dev/null
+<?xml version="1.0" ?>
+
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+ <xacro:macro name="cpu" params="name parent *origin">
+
+ <link name="${name}">
+ <inertial>
+ <origin xyz="8.9093E-05 2.5829E-06 0.00012597" rpy="0 0 0" />
+ <mass value="11" />
+ <inertia ixx="0.126605876415377" ixy="5.44490292261203E-06" ixz="0.000338979478568545" iyy="0.238484767338505" iyz="9.68721363707629E-07" izz="0.153116603514266" />
+ </inertial>
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://twil_description/meshes/cpu.stl" />
+ </geometry>
+ <material name="">
+ <color rgba="0.79216 0.81961 0.93333 1" />
+ </material>
+ </visual>
+ <collision>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://twil_description/meshes/cpu.stl" />
+ </geometry>
+ </collision>
+ </link>
+
+ <joint name="${name}_joint" type="fixed">
+ <xacro:insert_block name="origin" />
+ <parent link="${parent}"/>
+ <child link="${name}" />
+ </joint>
+
+ <gazebo reference="${name}">
+ <material>Gazebo/Grey</material>
+ </gazebo>
+
+ </xacro:macro>
+
+</robot>
--- /dev/null
+<?xml version="1.0" ?>
+
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+ <xacro:property name="EUROCARD_M" value="0.1" />
+ <xacro:property name="EUROCARD_W" value="0.16" />
+ <xacro:property name="EUROCARD_L" value="0.0016" />
+ <xacro:property name="EUROCARD_H" value="0.1" />
+
+ <xacro:macro name="eurocard" params="name parent *origin">
+
+ <link name="${name}">
+ <inertial>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <mass value="${EUROCARD_M}" />
+ <inertia ixx="${EUROCARD_M/12*(EUROCARD_H*EUROCARD_H+EUROCARD_L*EUROCARD_L)}" ixy="0.0" ixz="0.0" iyy="${EUROCARD_M/12*(EUROCARD_W*EUROCARD_W+EUROCARD_H*EUROCARD_H)}" iyz="0.0" izz="${EUROCARD_M/12*(EUROCARD_L*EUROCARD_L+EUROCARD_W*EUROCARD_W)}" />
+ </inertial>
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <box size="${EUROCARD_W} ${EUROCARD_L} ${EUROCARD_H}" />
+ </geometry>
+ <material name="">
+ <color rgba="0.0 0.7 0.0 1" />
+ </material>
+ </visual>
+ <collision>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <box size="${EUROCARD_W} ${EUROCARD_L} ${EUROCARD_H}" />
+ </geometry>
+ </collision>
+ </link>
+
+ <joint name="${name}" type="fixed">
+ <xacro:insert_block name="origin" />
+ <parent link="${parent}"/>
+ <child link="${name}" />
+ </joint>
+
+ <gazebo reference="${name}">
+ <material>Gazebo/Green</material>
+ </gazebo>
+
+ </xacro:macro>
+
+</robot>
--- /dev/null
+<?xml version="1.0" ?>
+
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+ <xacro:macro name="fixed_wheel" params="name parent *origin">
+
+ <link name="${name}">
+ <inertial>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <mass value="0.4935" />
+ <inertia ixx="0.0007853" ixy="0" ixz="0" iyy="0.001527" iyz="0" izz="0.0007853" />
+ </inertial>
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://twil_description/meshes/fixed_wheel.stl" />
+ </geometry>
+ <material name="">
+ <color rgba="0.29412 0.29412 0.29412 1" />
+ </material>
+ </visual>
+ <collision>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://twil_description/meshes/fixed_wheel.stl" />
+ </geometry>
+ </collision>
+ </link>
+
+ <joint name="${name}_joint" type="continuous">
+ <xacro:insert_block name="origin" />
+ <parent link="${parent}" />
+ <child link="${name}" />
+ <axis xyz="0 1 0" />
+ </joint>
+
+ <transmission name="${name}_transmission">
+ <type>transmission_interface/SimpleTransmission"</type>
+ <joint name="${name}_joint">
+ <hardwareInterface>EffortJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="${name}_motor"><hardwareInterface>EffortJointInterface</hardwareInterface>
+ <mechanicalReduction>1</mechanicalReduction></actuator>
+ </transmission>
+
+ <gazebo reference="${name}">
+ <material>Gazebo/FlatBlack</material>
+ </gazebo>
+
+ </xacro:macro>
+
+</robot>
--- /dev/null
+<?xml version="1.0" ?>
+
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+ <xacro:macro name="fixed_wheel_support" params="name parent *origin">
+
+ <link name="${name}">
+ <inertial>
+ <origin xyz="-0.027091 0.049354 -0.038294" rpy="0 0 0" />
+ <mass value="2.6913" />
+ <inertia ixx="0.004937" ixy="-0.00032401" ixz="-0.0013024" iyy="0.0052545" iyz="0.00022821" izz="0.0022004" />
+ </inertial>
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://twil_description/meshes/fixed_wheel_support.stl" />
+ </geometry>
+ <material name="">
+ <color rgba="0.75294 0.75294 0.75294 1" />/>
+ </material>
+ </visual>
+ <collision>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://twil_description/meshes/fixed_wheel_support.stl" />
+ </geometry>
+ </collision>
+ </link>
+
+ <joint name="${name}" type="fixed">
+ <xacro:insert_block name="origin" />
+ <parent link="${parent}"/>
+ <child link="${name}" />
+ </joint>
+
+ <gazebo reference="${name}">
+ <material>Gazebo/White</material>
+ </gazebo>
+
+ </xacro:macro>
+
+</robot>
--- /dev/null
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro" name="twil">
+
+ <property name="M_PI" value="3.1415926535897931" />
+
+ <xacro:include filename="$(find twil_description)/xacro/chassis.urdf.xacro" />
+ <xacro:include filename="$(find twil_description)/xacro/fixed_wheel_support.urdf.xacro" />
+ <xacro:include filename="$(find twil_description)/xacro/fixed_wheel.urdf.xacro" />
+ <xacro:include filename="$(find twil_description)/xacro/castor_support.urdf.xacro" />
+ <xacro:include filename="$(find twil_description)/xacro/castor_base.urdf.xacro" />
+ <xacro:include filename="$(find twil_description)/xacro/castor_wheel.urdf.xacro" />
+ <xacro:include filename="$(find twil_description)/xacro/battery_bosch_12v.urdf.xacro" />
+ <xacro:include filename="$(find twil_description)/xacro/cpu.urdf.xacro" />
+ <xacro:include filename="$(find twil_description)/xacro/eurocard.urdf.xacro" />
+
+ <link name="twil_origin" />
+
+ <xacro:chassis name="chassis" parent="twil_origin">
+ <origin xyz="0 0 0.172" rpy="0 0 0"/>
+ </xacro:chassis>
+
+ <xacro:fixed_wheel_support name="right_wheel_support" parent="chassis">
+ <origin xyz="0 -0.161 -0.002" rpy="0 0 ${M_PI}" />
+ </xacro:fixed_wheel_support>
+
+ <xacro:fixed_wheel name="right_wheel" parent="right_wheel_support">
+ <origin xyz="0 0 -0.095" rpy="0 0 ${-M_PI}" />
+ </xacro:fixed_wheel>
+
+ <xacro:fixed_wheel_support name="left_wheel_support" parent="chassis">
+ <origin xyz="0 0.161 -0.002" rpy="0 0 0" />
+ </xacro:fixed_wheel_support>
+
+ <xacro:fixed_wheel name="left_wheel" parent="left_wheel_support">
+ <origin xyz="0 0.0 -0.095" rpy="0 0 0" />
+ </xacro:fixed_wheel>
+
+ <xacro:castor_support name="castor_support" parent="chassis">
+ <origin xyz="-0.2 0 -0.002" rpy="0 0 0" />
+ </xacro:castor_support>
+
+ <xacro:castor_base name="castor_base" parent="castor_support">
+ <origin xyz="0 0 -0.1325" rpy="0 0 0" />
+ </xacro:castor_base>
+
+ <xacro:castor_wheel name="castor_wheel" parent="castor_base">
+ <origin xyz="-0.04 0 0" rpy="0 0 0" />
+ </xacro:castor_wheel>
+
+ <xacro:castor_support name="front_castor_support" parent="chassis">
+ <origin xyz="0.2 0 -0.002" rpy="0 0 0" />
+ </xacro:castor_support>
+
+ <xacro:castor_base name="front_castor_base" parent="front_castor_support">
+ <origin xyz="0 0 -0.1325" rpy="0 0 0" />
+ </xacro:castor_base>
+
+ <xacro:castor_wheel name="front_castor_wheel" parent="front_castor_base">
+ <origin xyz="-0.04 0 0" rpy="0 0 0" />
+ </xacro:castor_wheel>
+
+ <!--xacro:battery_bosch_12v name="battery" parent="chassis">
+ <origin xyz="-0.15 0.0 ${0.0015+0.0875}" rpy="0.0 0.0 ${M_PI/2}" />
+ </xacro:battery_bosch_12v-->
+
+ <xacro:battery_bosch_12v name="battery" parent="chassis">
+ <origin xyz="0.0 0.0 ${0.0015+0.0875}" rpy="0.0 0.0 ${M_PI/2}" />
+ </xacro:battery_bosch_12v>
+
+ <xacro:eurocard name="fan" parent="chassis">
+ <origin xyz="0 0.2 ${0.0015+0.3+0.003+0.1/2}" rpy="0.0 0.0 0.0" />
+ </xacro:eurocard>
+
+ <xacro:eurocard name="motor_driver" parent="chassis">
+ <origin xyz="0 0.1 ${0.0015+0.3+0.003+0.1/2}" rpy="0.0 0.0 0.0" />
+ </xacro:eurocard>
+
+ <xacro:eurocard name="stepper_driver" parent="chassis">
+ <origin xyz="0 0.0 ${0.0015+0.3+0.003+0.1/2}" rpy="0.0 0.0 0.0" />
+ </xacro:eurocard>
+
+ <xacro:eurocard name="power_supply" parent="chassis">
+ <origin xyz="0 -0.1 ${0.0015+0.3+0.003+0.1/2}" rpy="0.0 0.0 0.0" />
+ </xacro:eurocard>
+
+ <xacro:cpu name="cpu" parent="chassis">
+ <origin xyz="0 0.0 ${0.0015+0.3+0.003+0.127+0.003+0.17}" rpy="0.0 0.0 0.0" />
+ </xacro:cpu>
+
+ <link name="chassis_top" />
+
+ <joint name="chassis_top_joint" type="fixed">
+ <parent link="chassis"/>
+ <child link="chassis_top" />
+ <origin xyz="0.0 0.0 0.787" rpy="0.0 0.0 0.0" />
+ </joint>
+
+ <gazebo>
+ <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so" >
+ <robotNamespace>/twil</robotNamespace>
+
+ <!-- Custom plugin -->
+ <!-- robotSimType>twil_gazebo_ros_control/TwilRobotHWSim</robotSimType -->
+
+ <!-- Default plugin -->
+ <!-- robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType -->
+
+ <controlPeriod>0.001</controlPeriod>
+ </plugin>
+ </gazebo>
+
+
+</robot>
--- /dev/null
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro" name="twil">
+
+ <property name="M_PI" value="3.1415926535897931" />
+
+ <xacro:include filename="$(find twil_description)/xacro/chassis.urdf.xacro" />
+ <xacro:include filename="$(find twil_description)/xacro/fixed_wheel_support.urdf.xacro" />
+ <xacro:include filename="$(find twil_description)/xacro/fixed_wheel.urdf.xacro" />
+ <xacro:include filename="$(find twil_description)/xacro/castor_support.urdf.xacro" />
+ <xacro:include filename="$(find twil_description)/xacro/castor_base.urdf.xacro" />
+ <xacro:include filename="$(find twil_description)/xacro/castor_wheel.urdf.xacro" />
+ <xacro:include filename="$(find twil_description)/xacro/battery_bosch_12v.urdf.xacro" />
+ <xacro:include filename="$(find twil_description)/xacro/cpu.urdf.xacro" />
+ <xacro:include filename="$(find twil_description)/xacro/eurocard.urdf.xacro" />
+
+ <link name="twil_origin" />
+
+ <xacro:chassis name="chassis" parent="twil_origin">
+ <origin xyz="0 0 0.172" rpy="0 0 0"/>
+ </xacro:chassis>
+
+ <xacro:fixed_wheel_support name="right_wheel_support" parent="chassis">
+ <origin xyz="0 -0.161 -0.002" rpy="0 0 ${M_PI}" />
+ </xacro:fixed_wheel_support>
+
+ <xacro:fixed_wheel name="right_wheel" parent="right_wheel_support">
+ <origin xyz="0 0 -0.095" rpy="0 0 ${-M_PI}" />
+ </xacro:fixed_wheel>
+
+ <xacro:fixed_wheel_support name="left_wheel_support" parent="chassis">
+ <origin xyz="0 0.161 -0.002" rpy="0 0 0" />
+ </xacro:fixed_wheel_support>
+
+ <xacro:fixed_wheel name="left_wheel" parent="left_wheel_support">
+ <origin xyz="0 0.0 -0.095" rpy="0 0 0" />
+ </xacro:fixed_wheel>
+
+ <xacro:castor_support name="castor_support" parent="chassis">
+ <origin xyz="-0.2 0 -0.002" rpy="0 0 0" />
+ </xacro:castor_support>
+
+ <xacro:castor_base name="castor_base" parent="castor_support">
+ <origin xyz="0 0 -0.1325" rpy="0 0 0" />
+ </xacro:castor_base>
+
+ <xacro:castor_wheel name="castor_wheel" parent="castor_base">
+ <origin xyz="-0.04 0 0" rpy="0 0 0" />
+ </xacro:castor_wheel>
+
+ <xacro:castor_support name="front_castor_support" parent="chassis">
+ <origin xyz="0.2 0 -0.002" rpy="0 0 0" />
+ </xacro:castor_support>
+
+ <xacro:castor_base name="front_castor_base" parent="front_castor_support">
+ <origin xyz="0 0 -0.1325" rpy="0 0 0" />
+ </xacro:castor_base>
+
+ <xacro:castor_wheel name="front_castor_wheel" parent="front_castor_base">
+ <origin xyz="-0.04 0 0" rpy="0 0 0" />
+ </xacro:castor_wheel>
+
+ <!--xacro:battery_bosch_12v name="battery" parent="chassis">
+ <origin xyz="-0.15 0.0 ${0.0015+0.0875}" rpy="0.0 0.0 ${M_PI/2}" />
+ </xacro:battery_bosch_12v-->
+
+ <xacro:battery_bosch_12v name="battery" parent="chassis">
+ <origin xyz="0.0 0.0 ${0.0015+0.0875}" rpy="0.0 0.0 ${M_PI/2}" />
+ </xacro:battery_bosch_12v>
+
+ <xacro:eurocard name="fan" parent="chassis">
+ <origin xyz="0 0.2 ${0.0015+0.3+0.003+0.1/2}" rpy="0.0 0.0 0.0" />
+ </xacro:eurocard>
+
+ <xacro:eurocard name="motor_driver" parent="chassis">
+ <origin xyz="0 0.1 ${0.0015+0.3+0.003+0.1/2}" rpy="0.0 0.0 0.0" />
+ </xacro:eurocard>
+
+ <xacro:eurocard name="stepper_driver" parent="chassis">
+ <origin xyz="0 0.0 ${0.0015+0.3+0.003+0.1/2}" rpy="0.0 0.0 0.0" />
+ </xacro:eurocard>
+
+ <xacro:eurocard name="power_supply" parent="chassis">
+ <origin xyz="0 -0.1 ${0.0015+0.3+0.003+0.1/2}" rpy="0.0 0.0 0.0" />
+ </xacro:eurocard>
+
+ <xacro:cpu name="cpu" parent="chassis">
+ <origin xyz="0 0.0 ${0.0015+0.3+0.003+0.127+0.003+0.17}" rpy="0.0 0.0 0.0" />
+ </xacro:cpu>
+
+ <link name="chassis_top" />
+
+ <joint name="chassis_top_joint" type="fixed">
+ <parent link="chassis"/>
+ <child link="chassis_top" />
+ <origin xyz="0.0 0.0 0.787" rpy="0.0 0.0 0.0" />
+ </joint>
+
+ </robot>
--- /dev/null
+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
+)
--- /dev/null
+#ifndef TWIL_HW_H
+#define TWIL_HW_H
+
+#include <hardware_interface/joint_command_interface.h>
+#include <hardware_interface/joint_state_interface.h>
+#include <hardware_interface/internal/interface_manager.h>
+#include <hardware_interface/robot_hw.h>
+#include <pluginlib/class_loader.h>
+
+#include "aic.h"
+#include <ros/ros.h>
+
+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
--- /dev/null
+<?xml version="1.0"?>
+<package>
+ <name>twil_hw</name>
+ <version>0.0.0</version>
+ <description>The twil_hw package</description>
+
+ <!-- One maintainer tag required, multiple allowed, one person per tag -->
+ <!-- Example: -->
+ <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+ <maintainer email="talves@todo.todo">talves</maintainer>
+
+
+ <!-- One license tag required, multiple allowed, one license per tag -->
+ <!-- Commonly used license strings: -->
+ <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+ <license>TODO</license>
+
+
+ <!-- Url tags are optional, but multiple are allowed, one per tag -->
+ <!-- Optional attribute type can be: website, bugtracker, or repository -->
+ <!-- Example: -->
+ <!-- <url type="website">http://wiki.ros.org/twil_hw</url> -->
+
+
+ <!-- Author tags are optional, multiple are allowed, one per tag -->
+ <!-- Authors do not have to be maintainers, but could be -->
+ <!-- Example: -->
+ <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+ <!-- The *_depend tags are used to specify dependencies -->
+ <!-- Dependencies can be catkin packages or system dependencies -->
+ <!-- Examples: -->
+ <!-- Use build_depend for packages you need at compile time: -->
+ <!-- <build_depend>message_generation</build_depend> -->
+ <!-- Use buildtool_depend for build tool packages: -->
+ <!-- <buildtool_depend>catkin</buildtool_depend> -->
+ <!-- Use run_depend for packages you need at runtime: -->
+ <!-- <run_depend>message_runtime</run_depend> -->
+ <!-- Use test_depend for packages you need only for testing: -->
+ <!-- <test_depend>gtest</test_depend> -->
+ <buildtool_depend>catkin</buildtool_depend>
+ <build_depend>controller_interface</build_depend>
+ <build_depend>controller_manager</build_depend>
+ <build_depend>hardware_interface</build_depend>
+ <build_depend>joint_state_controller</build_depend>
+ <build_depend>roscpp</build_depend>
+ <build_depend>rospy</build_depend>
+ <run_depend>controller_interface</run_depend>
+ <run_depend>controller_manager</run_depend>
+ <run_depend>hardware_interface</run_depend>
+ <run_depend>joint_state_controller</run_depend>
+ <run_depend>roscpp</run_depend>
+ <run_depend>rospy</run_depend>
+
+
+ <!-- The export tag contains other, unspecified, tags -->
+ <export>
+ <!-- Other tools can request additional information be placed here -->
+
+ </export>
+</package>
--- /dev/null
+#!/bin/bash
+
+echo "USAGE: ./export_motor_test -f file.bag -t topic_root -o output_root"
+
+while getopts f:t:o: option
+do
+ case "${option}"
+ in
+ f) FILE=${OPTARG};;
+ t) TOPIC_ROOT=${OPTARG};;
+ o) OUTPUT=${OPTARG};;
+ esac
+done
+
+set -x
+echo "Joint_States_File:" $OUTPUT"_joint_states.dat"
+rostopic echo -b $FILE -p /$TOPIC_ROOT/joint_states > $OUTPUT"_joint_states.dat"
+echo "Left_Motor_CMD_File:" $OUTPUT"_left_wheel.dat"
+rostopic echo -b $FILE -p /$TOPIC_ROOT/left_wheel_joint_effort_controller/command > $OUTPUT"_left_wheel.dat"
+echo "Right_Motor_CMD_File:" $OUTPUT"_right_wheel.dat"
+rostopic echo -b $FILE -p /$TOPIC_ROOT/right_wheel_joint_effort_controller/command > $OUTPUT"_right_wheel.dat"
--- /dev/null
+#include "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];
+}
+
+}
--- /dev/null
+#include <ros/ros.h>
+#include <sstream>
+#include <std_msgs/Duration.h>
+#include <controller_manager/controller_manager.h>
+#include <realtime_tools/realtime_publisher.h>
+#include <boost/scoped_ptr.hpp>
+#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<std_msgs::Duration> >ts_pub;
+ ts_pub.reset(new realtime_tools::RealtimePublisher<std_msgs::Duration>(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;
+}
--- /dev/null
+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)
--- /dev/null
+#ifndef CIRCLE_PATH_H
+#define CIRCLE_PATH_H
+
+#include <Eigen/Dense>
+
+/** Circle path
+* @author Walter Fetter Lages <w.fetter@ieee.org>
+*/
+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
--- /dev/null
+#ifndef EIGHT_PATH_H
+#define EIGHT_PATH_H
+
+#include <twil_trajectories/circle_path.h>
+
+/** 8 path
+* @author Walter Fetter Lages <w.fetter@ieee.org>
+*/
+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
--- /dev/null
+<?xml version="1.0"?>
+<package>
+ <name>twil_trajectories</name>
+ <version>2.0.0</version>
+ <description>The twil_trajectories package</description>
+
+ <!-- One maintainer tag required, multiple allowed, one person per tag -->
+ <!-- Example: -->
+ <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+ <maintainer email="fetter@ece.ufrgs.br">Walter Fetter Lages</maintainer>
+
+
+ <!-- One license tag required, multiple allowed, one license per tag -->
+ <!-- Commonly used license strings: -->
+ <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+ <license>GPLv3</license>
+
+
+ <!-- Url tags are optional, but mutiple are allowed, one per tag -->
+ <!-- Optional attribute type can be: website, bugtracker, or repository -->
+ <!-- Example: -->
+ <!-- <url type="website">http://wiki.ros.org/twil_ident</url> -->
+
+
+ <!-- Author tags are optional, mutiple are allowed, one per tag -->
+ <!-- Authors do not have to be maintianers, but could be -->
+ <!-- Example: -->
+ <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+ <author email="fetter@ece.ufrgs.br">Walter Fetter Lages</author>
+
+
+ <!-- The *_depend tags are used to specify dependencies -->
+ <!-- Dependencies can be catkin packages or system dependencies -->
+ <!-- Examples: -->
+ <!-- Use build_depend for packages you need at compile time: -->
+ <!-- <build_depend>message_generation</build_depend> -->
+ <!-- Use buildtool_depend for build tool packages: -->
+ <!-- <buildtool_depend>catkin</buildtool_depend> -->
+ <!-- Use run_depend for packages you need at runtime: -->
+ <!-- <run_depend>message_runtime</run_depend> -->
+ <!-- Use test_depend for packages you need only for testing: -->
+ <!-- <test_depend>gtest</test_depend> -->
+ <buildtool_depend>catkin</buildtool_depend>
+ <build_depend>eigen</build_depend>
+ <build_depend>geometry_msgs</build_depend>
+
+ <run_depend>eigen</run_depend>
+ <run_depend>geometry_msgs</run_depend>
+
+ <!-- The export tag contains other, unspecified, tags -->
+ <export>
+ <!-- You can specify that this package is a metapackage here: -->
+ <!-- <metapackage/> -->
+
+ <!-- Other tools can request additional information be placed here -->
+
+ </export>
+</package>
--- /dev/null
+#include <twil_trajectories/circle_path.h>
+
+#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;
+}
--- /dev/null
+#include <ros/ros.h>
+#include <geometry_msgs/Pose2D.h>
+
+#include <twil_trajectories/circle_path.h>
+
+
+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<geometry_msgs::Pose2D>("/"+robot_config+"/twist_pid_backstepping_controller/cmd_pos2d",1);
+ commandPublisher2=node_.advertise<geometry_msgs::Pose2D>("/"+robot_config+"/dynamics_pid_backstepping_controller/cmd_pos2d",1);
+
+ 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;
+}
--- /dev/null
+#include <twil_trajectories/eight_path.h>
+
+#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);
+}
--- /dev/null
+#include <ros/ros.h>
+#include <geometry_msgs/Pose2D.h>
+
+#include <twil_trajectories/eight_path.h>
+
+
+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<geometry_msgs::Pose2D>("/"+robot_config+"/twist_pid_backstepping_controller/cmd_pos2d",1);
+ commandPublisher2=node_.advertise<geometry_msgs::Pose2D>("/"+robot_config+"/dynamics_pid_backstepping_controller/cmd_pos2d",1);
+
+ 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;
+}
--- /dev/null
+#include <ros/ros.h>
+
+#include <Eigen/Dense>
+
+#include <geometry_msgs/Pose2D.h>
+#include <geometry_msgs/PoseStamped.h>
+
+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<geometry_msgs::PoseStamped>("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;
+}
projects / twil.git / commitdiff